diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000000000000000000000000000000000000..4b9655036c6956c971a0a99239b8710764e4c189
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,55 @@
+*~
+.DS_Store
+
+Mock
+Output
+MR-Leo-server
+ORBvoc.txt
+*.csv
+
+# C++ objects and libs
+*.slo
+*.lo
+*.o
+*.a
+*.la
+*.lai
+*.so
+*.dll
+*.dylib
+
+# Qt-es
+object_script.*.Release
+object_script.*.Debug
+*_plugin_import.cpp
+/.qmake.cache
+/.qmake.stash
+*.pro.user
+*.pro.user.*
+*.qbs.user
+*.qbs.user.*
+*.moc
+moc_*.cpp
+moc_*.h
+qrc_*.cpp
+ui_*.h
+*.qmlc
+*.jsc
+Makefile*
+*build-*
+
+# Qt unit tests
+target_wrapper.*
+
+# QtCreator
+*.autosave
+
+# QtCreator Qml
+*.qmlproject.user
+*.qmlproject.user.*
+
+# QtCreator CMake
+CMakeLists.txt.user*
+CMakeFiles
+CMakeCache.txt
+*.cmake
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/MR-Leo-server.pro b/MR-Leo-server.pro
new file mode 100644
index 0000000000000000000000000000000000000000..85b1a0e910adedcad5828cc7a575581bc992c216
--- /dev/null
+++ b/MR-Leo-server.pro
@@ -0,0 +1,289 @@
+#===========================================================
+#
+# MR-Leo
+# Server for creating mixed reality using edge computing.
+# Authors:
+# Johan Lindqvist (johli392@student.liu.se)
+#
+# --------------------------------------------------------
+#
+# > For testing: Enable this to include support for
+# creating GUI windows that display the video source
+# feed and generated output, both for connected
+# devices and the local mock camera client.
+# Note that this also adds dependencies on Qt5 widgets
+# and the surrounding environment.
+# > Uncomment following line:
+CONFIG += ENABLE_GUI_SUPPORT
+#
+# > Use Pangolin 3D library
+# This requires that one has the Pangolin library
+# is is available in the lib path.
+# (https://github.com/stevenlovegrove/Pangolin)
+# > Uncomment following line:
+#CONFIG += USE_PANGOLIN
+#CONFIG += SET_PANGOLIN_HEADLESS_MODE
+#
+# > No graphics is sent, only limited information
+# about pointcloud and AR object.
+# > Uncomment following line:
+#CONFIG += DISABLE_IMAGE_OUTPUT
+#
+# > Limit rendering to 1 frame out per 1 frame in.
+# > If not set it will render as many frames as possible.
+# > Uncomment following line:
+CONFIG += LIMIT_RENDERING
+#
+#
+#===========================================================
+
+TEMPLATE = app
+CONFIG += qt
+
+QT += network \
+ gui \
+ core
+
+PKGCONFIG += opencv
+PKGCONFIG += gstreamer-1.0
+PKGCONFIG += gstreamer-plugins-base-1.0
+PKGCONFIG += gstreamer-app-1.0
+PKGCONFIG += eigen3
+PKGCONFIG += glew
+
+INCLUDEPATH += externals/ORB_SLAM2
+INCLUDEPATH += externals/ORB_SLAM2/include
+INCLUDEPATH += externals/Pangolin/include
+INCLUDEPATH += externals/Pangolin-config
+
+#----------------------------------------
+
+LIMIT_RENDERING {
+ # Enable use of QMutex in rendering thread.
+ DEFINES += "USE_QMUTEX_AR=TRUE"
+ # Enable use of QMutex in local mapping thread.
+ #DEFINES += "USE_QMUTEX_LOC_MAP=TRUE"
+}
+
+DISABLE_IMAGE_OUTPUT {
+ DEFINES += "DISABLE_IMAGE_OUTPUT=TRUE"
+}
+
+USE_PANGOLIN {
+ DEFINES += "RENDER_WITH_PANGOLIN=TRUE"
+ LIBS += -lpangolin
+} else {
+ SOURCES += externals/Pangolin/src/display/opengl_render_state.cpp
+ PKGCONFIG += osmesa
+ LIBS += -lGLU
+ LIBS += -lOSMesa
+ LIBS += -lm
+}
+
+SET_PANGOLIN_HEADLESS_MODE {
+ DEFINES += "RENDER_PANGOLIN_HEADLESS=TRUE"
+}
+
+ENABLE_GUI_SUPPORT {
+ DEFINES += "ENABLE_WIDGET_SUPPORT=TRUE"
+ QT += widgets
+}
+
+#----------------------------------------
+
+DEFINES += "EIGEN_NO_DEPRECATED_WARNING"
+
+SOURCES += \
+ source/cannyfilter.cpp \
+ source/imageprocesser.cpp \
+ source/ViewerAR.cc \
+ source/main.cpp \
+ source/mrserver.cpp \
+ source/udpconnection.cpp \
+ source/udpbuilder.cpp \
+ source/tcpconnection.cpp \
+ source/tcpbuilder.cpp \
+ source/mockclient.cpp \
+ source/imagewriter.cpp \
+ source/udpsender.cpp \
+ source/networkconnection.cpp \
+ source/videoreceiver.cpp \
+ source/videotransmitter.cpp \
+ source/orbslamprocesser.cpp \
+ source/echoimage.cpp
+
+
+HEADERS += \
+ source/cannyfilter.h \
+ source/imageprocesser.h \
+ source/ViewerAR.h \
+ source/mrserver.h \
+ source/global.h \
+ source/udpconnection.h \
+ source/networkconnection.h \
+ source/udpbuilder.h \
+ source/tcpconnection.h \
+ source/tcpbuilder.h \
+ source/mockclient.h \
+ source/imagewriter.h \
+ source/udpsender.h \
+ source/videoreceiver.h \
+ source/videostreamer.h \
+ source/videotransmitter.h \
+ source/orbslamprocesser.h \
+ source/echoimage.h
+
+
+SOURCES += \
+ externals/ORB_SLAM2/src/KeyFrameDatabase.cc \
+ externals/ORB_SLAM2/src/Tracking.cc \
+ externals/ORB_SLAM2/src/ORBmatcher.cc \
+ externals/ORB_SLAM2/src/Sim3Solver.cc \
+ externals/ORB_SLAM2/src/ORBextractor.cc \
+ externals/ORB_SLAM2/src/Map.cc \
+ externals/ORB_SLAM2/src/Initializer.cc \
+ externals/ORB_SLAM2/src/MapPoint.cc \
+ externals/ORB_SLAM2/src/LocalMapping.cc \
+ externals/ORB_SLAM2/src/Optimizer.cc \
+ externals/ORB_SLAM2/src/LoopClosing.cc \
+ externals/ORB_SLAM2/src/Frame.cc \
+ externals/ORB_SLAM2/src/KeyFrame.cc \
+ externals/ORB_SLAM2/src/MapDrawer.cc \
+ externals/ORB_SLAM2/src/Converter.cc \
+ externals/ORB_SLAM2/src/System.cc \
+ externals/ORB_SLAM2/src/PnPsolver.cc \
+ externals/ORB_SLAM2/src/Viewer.cc \
+ externals/ORB_SLAM2/src/FrameDrawer.cc \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.hpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.hpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.cpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.cpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.cpp \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.cpp \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.cpp
+
+HEADERS += \
+ externals/ORB_SLAM2/include/FrameDrawer.h \
+ externals/ORB_SLAM2/include/MapPoint.h \
+ externals/ORB_SLAM2/include/KeyFrame.h \
+ externals/ORB_SLAM2/include/Viewer.h \
+ externals/ORB_SLAM2/include/KeyFrameDatabase.h \
+ externals/ORB_SLAM2/include/Map.h \
+ externals/ORB_SLAM2/include/ORBextractor.h \
+ externals/ORB_SLAM2/include/ORBmatcher.h \
+ externals/ORB_SLAM2/include/Frame.h \
+ externals/ORB_SLAM2/include/LocalMapping.h \
+ externals/ORB_SLAM2/include/Converter.h \
+ externals/ORB_SLAM2/include/PnPsolver.h \
+ externals/ORB_SLAM2/include/Optimizer.h \
+ externals/ORB_SLAM2/include/LoopClosing.h \
+ externals/ORB_SLAM2/include/Tracking.h \
+ externals/ORB_SLAM2/include/Initializer.h \
+ externals/ORB_SLAM2/include/ORBVocabulary.h \
+ externals/ORB_SLAM2/include/System.h \
+ externals/ORB_SLAM2/include/Sim3Solver.h \
+ externals/ORB_SLAM2/include/MapDrawer.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FClass.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.h \
+ externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3quat.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/sim3.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_operations.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/creators.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/openmp_mutex.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_edge.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/eigen_types.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/linear_solver.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/misc.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/macros.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.h \
+ externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/color_macros.h
+
+
+CONFIG -= no-pkg-config
+CONFIG += link_pkgconfig
+
+
diff --git a/README.md b/README.md
index e69de29bb2d1d6434b8b29ae775ad8c2e48c5391..4c91f4bd1098b6181fbaff5bdd6ee5cb5894bde6 100644
--- a/README.md
+++ b/README.md
@@ -0,0 +1,51 @@
+MR-Leo - Server
+========================
+
+### Introduction
+This repository contains a Qt C++ project.
+
+Tested with Ubuntu 18.04.
+
+### Screenshot
+
+
+### Setting up the build environment on Ubuntu 18.04
+* Install and configure Qt. https://wiki.qt.io/Install_Qt_5_on_Ubuntu
+ ```sudo apt install qt5-default```
+* Install GStreamer:
+ ```sudo apt install libgstreamer1.0-dev```
+ ```sudo apt install libgstreamer-plugins-base1.0-dev```
+ ```sudo apt install gstreamer1.0-plugins-bad```
+* Install OpenCV:
+ ```sudo apt install libopencv-dev```
+* Install Eigen:
+ ```sudo apt install libeigen3-dev```
+* Install GLEW:
+ ```sudo apt install libglew-dev```
+* Install OSMesa:
+ ```sudo apt install libosmesa6-dev```
+* Install GLU:
+ ```sudo apt install libglu1-mesa-dev```
+
+### Building
+1. Make sure that you have the build environment set up.
+2. Go to the directory with the project file _MR-Leo-server.pro_.
+3. Run Qt _qmake_ and the system's _make_: ```qmake && make```
+4. If compilation is successful, the executable file is placed in the current directory.
+
+Note: Performance related issues have been observed when running debug builds of the software. If compiling with Qt Creator, test with a release build.
+
+
+### Running
+1. Extract the file _ORBVoc.text_ from _externals/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz_ to the directory where the executable was placed.
+2. Start the server software with:
+```./MR-Leo-server```
+To see how you can change the configuration, start the server with:
+```./MR-Leo-server --help```
+Two configuration options that are useful for testing are ```./MR-Leo-server --display``` to have GUI windows with the content and ```./MR-Leo-server --camera <0,1,..>``` to add a local client with video streams from a local webcam. Clicking with the mouse in the windows has the same effect as pressing the _Add_ button in the Android client.
+
+### Authors
+Johan Lindqvist
+johli392@student.liu.se
+johan.lindqvist@gmail.com
+
diff --git a/externals/ORB_SLAM2/.gitignore b/externals/ORB_SLAM2/.gitignore
new file mode 100755
index 0000000000000000000000000000000000000000..ffeb9cd8d922ff9f43d3c808a5019cef987a47f6
--- /dev/null
+++ b/externals/ORB_SLAM2/.gitignore
@@ -0,0 +1,23 @@
+CMakeLists.txt.user
+Examples/Monocular/mono_euroc
+Examples/Monocular/mono_kitti
+Examples/Monocular/mono_tum
+Examples/RGB-D/rgbd_tum
+Examples/ROS/ORB_SLAM2/CMakeLists.txt.user
+Examples/ROS/ORB_SLAM2/Mono
+Examples/ROS/ORB_SLAM2/MonoAR
+Examples/ROS/ORB_SLAM2/RGBD
+Examples/ROS/ORB_SLAM2/Stereo
+Examples/ROS/ORB_SLAM2/build/
+Examples/Stereo/stereo_euroc
+Examples/Stereo/stereo_kitti
+KeyFrameTrajectory.txt
+Thirdparty/DBoW2/build/
+Thirdparty/DBoW2/lib/
+Thirdparty/g2o/build/
+Thirdparty/g2o/lib/
+Vocabulary/ORBvoc.txt
+build/
+*~
+lib/
+
diff --git a/externals/ORB_SLAM2/CMakeLists.txt b/externals/ORB_SLAM2/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..08a8af468ec0228cded295de4a30f0826ef279ee
--- /dev/null
+++ b/externals/ORB_SLAM2/CMakeLists.txt
@@ -0,0 +1,113 @@
+cmake_minimum_required(VERSION 2.8)
+project(ORB_SLAM2)
+
+IF(NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE Release)
+ENDIF()
+
+MESSAGE("Build type: " ${CMAKE_BUILD_TYPE})
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
+
+# Check C++11 or C++0x support
+include(CheckCXXCompilerFlag)
+CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
+CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
+if(COMPILER_SUPPORTS_CXX11)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+ add_definitions(-DCOMPILEDWITHC11)
+ message(STATUS "Using flag -std=c++11.")
+elseif(COMPILER_SUPPORTS_CXX0X)
+ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
+ add_definitions(-DCOMPILEDWITHC0X)
+ message(STATUS "Using flag -std=c++0x.")
+else()
+ message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
+endif()
+
+LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
+
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+ find_package(OpenCV 2.4.3 QUIET)
+ if(NOT OpenCV_FOUND)
+ message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+ endif()
+endif()
+
+find_package(Eigen3 3.1.0 REQUIRED)
+find_package(Pangolin REQUIRED)
+
+include_directories(
+${PROJECT_SOURCE_DIR}
+${PROJECT_SOURCE_DIR}/include
+${EIGEN3_INCLUDE_DIR}
+${Pangolin_INCLUDE_DIRS}
+)
+
+set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
+
+add_library(${PROJECT_NAME} SHARED
+src/System.cc
+src/Tracking.cc
+src/LocalMapping.cc
+src/LoopClosing.cc
+src/ORBextractor.cc
+src/ORBmatcher.cc
+src/FrameDrawer.cc
+src/Converter.cc
+src/MapPoint.cc
+src/KeyFrame.cc
+src/Map.cc
+src/MapDrawer.cc
+src/Optimizer.cc
+src/PnPsolver.cc
+src/Frame.cc
+src/KeyFrameDatabase.cc
+src/Sim3Solver.cc
+src/Initializer.cc
+src/Viewer.cc
+)
+
+target_link_libraries(${PROJECT_NAME}
+${OpenCV_LIBS}
+${EIGEN3_LIBS}
+${Pangolin_LIBRARIES}
+${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
+${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
+)
+
+# Build examples
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/RGB-D)
+
+add_executable(rgbd_tum
+Examples/RGB-D/rgbd_tum.cc)
+target_link_libraries(rgbd_tum ${PROJECT_NAME})
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Stereo)
+
+add_executable(stereo_kitti
+Examples/Stereo/stereo_kitti.cc)
+target_link_libraries(stereo_kitti ${PROJECT_NAME})
+
+add_executable(stereo_euroc
+Examples/Stereo/stereo_euroc.cc)
+target_link_libraries(stereo_euroc ${PROJECT_NAME})
+
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/Examples/Monocular)
+
+add_executable(mono_tum
+Examples/Monocular/mono_tum.cc)
+target_link_libraries(mono_tum ${PROJECT_NAME})
+
+add_executable(mono_kitti
+Examples/Monocular/mono_kitti.cc)
+target_link_libraries(mono_kitti ${PROJECT_NAME})
+
+add_executable(mono_euroc
+Examples/Monocular/mono_euroc.cc)
+target_link_libraries(mono_euroc ${PROJECT_NAME})
+
diff --git a/externals/ORB_SLAM2/Dependencies.md b/externals/ORB_SLAM2/Dependencies.md
new file mode 100755
index 0000000000000000000000000000000000000000..733d3600a666f9a1eee1a6c5d1555904b0f18b0e
--- /dev/null
+++ b/externals/ORB_SLAM2/Dependencies.md
@@ -0,0 +1,44 @@
+##List of Known Dependencies
+###ORB-SLAM2 version 1.0
+
+In this document we list all the pieces of code included by ORB-SLAM2 and linked libraries which are not property of the authors of ORB-SLAM2.
+
+
+#####Code in **src** and **include** folders
+
+* *ORBextractor.cc*.
+This is a modified version of orb.cpp of OpenCV library. The original code is BSD licensed.
+
+* *PnPsolver.h, PnPsolver.cc*.
+This is a modified version of the epnp.h and epnp.cc of Vincent Lepetit.
+This code can be found in popular BSD licensed computer vision libraries as [OpenCV](https://github.com/Itseez/opencv/blob/master/modules/calib3d/src/epnp.cpp) and [OpenGV](https://github.com/laurentkneip/opengv/blob/master/src/absolute_pose/modules/Epnp.cpp). The original code is FreeBSD.
+
+* Function *ORBmatcher::DescriptorDistance* in *ORBmatcher.cc*.
+The code is from: http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel.
+The code is in the public domain.
+
+#####Code in Thirdparty folder
+
+* All code in **DBoW2** folder.
+This is a modified version of [DBoW2](https://github.com/dorian3d/DBoW2) and [DLib](https://github.com/dorian3d/DLib) library. All files included are BSD licensed.
+
+* All code in **g2o** folder.
+This is a modified version of [g2o](https://github.com/RainerKuemmerle/g2o). All files included are BSD licensed.
+
+#####Library dependencies
+
+* **Pangolin (visualization and user interface)**.
+[MIT license](https://en.wikipedia.org/wiki/MIT_License).
+
+* **OpenCV**.
+BSD license.
+
+* **Eigen3**.
+For versions greater than 3.1.1 is MPL2, earlier versions are LGPLv3.
+
+* **ROS (Optional, only if you build Examples/ROS)**.
+BSD license. In the manifest.xml the only declared package dependencies are roscpp, tf, sensor_msgs, image_transport, cv_bridge, which are all BSD licensed.
+
+
+
+
diff --git a/externals/ORB_SLAM2/LICENSE.txt b/externals/ORB_SLAM2/LICENSE.txt
new file mode 100755
index 0000000000000000000000000000000000000000..04e69088674570513ae829fd271c939798930478
--- /dev/null
+++ b/externals/ORB_SLAM2/LICENSE.txt
@@ -0,0 +1,11 @@
+ORB-SLAM2 is released under a GPLv3 license (see License-gpl.txt).
+Please see Dependencies.md for a list of all included code and library dependencies which are not property of the authors of ORB-SLAM2.
+
+For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors.
+
+If you use ORB-SLAM in an academic work, please cite the most relevant publication associated by visiting:
+https://github.com/raulmur/ORB_SLAM2
+
+
+
+
diff --git a/externals/ORB_SLAM2/License-gpl.txt b/externals/ORB_SLAM2/License-gpl.txt
new file mode 100755
index 0000000000000000000000000000000000000000..94a9ed024d3859793618152ea559a168bbcbb5e2
--- /dev/null
+++ b/externals/ORB_SLAM2/License-gpl.txt
@@ -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>.
diff --git a/externals/ORB_SLAM2/README.md b/externals/ORB_SLAM2/README.md
new file mode 100755
index 0000000000000000000000000000000000000000..acf42ee7e0c1d452cc2d4d0bb7a4da0cafa6a253
--- /dev/null
+++ b/externals/ORB_SLAM2/README.md
@@ -0,0 +1,240 @@
+# ORB-SLAM2
+**Authors:** [Raul Mur-Artal](http://webdiis.unizar.es/~raulmur/), [Juan D. Tardos](http://webdiis.unizar.es/~jdtardos/), [J. M. M. Montiel](http://webdiis.unizar.es/~josemari/) and [Dorian Galvez-Lopez](http://doriangalvez.com/) ([DBoW2](https://github.com/dorian3d/DBoW2))
+
+**13 Jan 2017**: OpenCV 3 and Eigen 3.3 are now supported.
+
+**22 Dec 2016**: Added AR demo (see section 7).
+
+ORB-SLAM2 is a real-time SLAM library for **Monocular**, **Stereo** and **RGB-D** cameras that computes the camera trajectory and a sparse 3D reconstruction (in the stereo and RGB-D case with true scale). It is able to detect loops and relocalize the camera in real time. We provide examples to run the SLAM system in the [KITTI dataset](http://www.cvlibs.net/datasets/kitti/eval_odometry.php) as stereo or monocular, in the [TUM dataset](http://vision.in.tum.de/data/datasets/rgbd-dataset) as RGB-D or monocular, and in the [EuRoC dataset](http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets) as stereo or monocular. We also provide a ROS node to process live monocular, stereo or RGB-D streams. **The library can be compiled without ROS**. ORB-SLAM2 provides a GUI to change between a *SLAM Mode* and *Localization Mode*, see section 9 of this document.
+
+<a href="https://www.youtube.com/embed/ufvPS5wJAx0" target="_blank"><img src="http://img.youtube.com/vi/ufvPS5wJAx0/0.jpg"
+alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
+<a href="https://www.youtube.com/embed/T-9PYCKhDLM" target="_blank"><img src="http://img.youtube.com/vi/T-9PYCKhDLM/0.jpg"
+alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
+<a href="https://www.youtube.com/embed/kPwy8yA4CKM" target="_blank"><img src="http://img.youtube.com/vi/kPwy8yA4CKM/0.jpg"
+alt="ORB-SLAM2" width="240" height="180" border="10" /></a>
+
+
+### Related Publications:
+
+[Monocular] Raúl Mur-Artal, J. M. M. Montiel and Juan D. Tardós. **ORB-SLAM: A Versatile and Accurate Monocular SLAM System**. *IEEE Transactions on Robotics,* vol. 31, no. 5, pp. 1147-1163, 2015. (**2015 IEEE Transactions on Robotics Best Paper Award**). **[PDF](http://webdiis.unizar.es/~raulmur/MurMontielTardosTRO15.pdf)**.
+
+[Stereo and RGB-D] Raúl Mur-Artal and Juan D. Tardós. **ORB-SLAM2: an Open-Source SLAM System for Monocular, Stereo and RGB-D Cameras**. *IEEE Transactions on Robotics,* vol. 33, no. 5, pp. 1255-1262, 2017. **[PDF](https://128.84.21.199/pdf/1610.06475.pdf)**.
+
+[DBoW2 Place Recognizer] Dorian Gálvez-López and Juan D. Tardós. **Bags of Binary Words for Fast Place Recognition in Image Sequences**. *IEEE Transactions on Robotics,* vol. 28, no. 5, pp. 1188-1197, 2012. **[PDF](http://doriangalvez.com/php/dl.php?dlp=GalvezTRO12.pdf)**
+
+# 1. License
+
+ORB-SLAM2 is released under a [GPLv3 license](https://github.com/raulmur/ORB_SLAM2/blob/master/License-gpl.txt). For a list of all code/library dependencies (and associated licenses), please see [Dependencies.md](https://github.com/raulmur/ORB_SLAM2/blob/master/Dependencies.md).
+
+For a closed-source version of ORB-SLAM2 for commercial purposes, please contact the authors: orbslam (at) unizar (dot) es.
+
+If you use ORB-SLAM2 (Monocular) in an academic work, please cite:
+
+ @article{murTRO2015,
+ title={{ORB-SLAM}: a Versatile and Accurate Monocular {SLAM} System},
+ author={Mur-Artal, Ra\'ul, Montiel, J. M. M. and Tard\'os, Juan D.},
+ journal={IEEE Transactions on Robotics},
+ volume={31},
+ number={5},
+ pages={1147--1163},
+ doi = {10.1109/TRO.2015.2463671},
+ year={2015}
+ }
+
+if you use ORB-SLAM2 (Stereo or RGB-D) in an academic work, please cite:
+
+ @article{murORB2,
+ title={{ORB-SLAM2}: an Open-Source {SLAM} System for Monocular, Stereo and {RGB-D} Cameras},
+ author={Mur-Artal, Ra\'ul and Tard\'os, Juan D.},
+ journal={IEEE Transactions on Robotics},
+ volume={33},
+ number={5},
+ pages={1255--1262},
+ doi = {10.1109/TRO.2017.2705103},
+ year={2017}
+ }
+
+# 2. Prerequisites
+We have tested the library in **Ubuntu 12.04**, **14.04** and **16.04**, but it should be easy to compile in other platforms. A powerful computer (e.g. i7) will ensure real-time performance and provide more stable and accurate results.
+
+## C++11 or C++0x Compiler
+We use the new thread and chrono functionalities of C++11.
+
+## Pangolin
+We use [Pangolin](https://github.com/stevenlovegrove/Pangolin) for visualization and user interface. Dowload and install instructions can be found at: https://github.com/stevenlovegrove/Pangolin.
+
+## OpenCV
+We use [OpenCV](http://opencv.org) to manipulate images and features. Dowload and install instructions can be found at: http://opencv.org. **Required at leat 2.4.3. Tested with OpenCV 2.4.11 and OpenCV 3.2**.
+
+## Eigen3
+Required by g2o (see below). Download and install instructions can be found at: http://eigen.tuxfamily.org. **Required at least 3.1.0**.
+
+## DBoW2 and g2o (Included in Thirdparty folder)
+We use modified versions of the [DBoW2](https://github.com/dorian3d/DBoW2) library to perform place recognition and [g2o](https://github.com/RainerKuemmerle/g2o) library to perform non-linear optimizations. Both modified libraries (which are BSD) are included in the *Thirdparty* folder.
+
+## ROS (optional)
+We provide some examples to process the live input of a monocular, stereo or RGB-D camera using [ROS](ros.org). Building these examples is optional. In case you want to use ROS, a version Hydro or newer is needed.
+
+# 3. Building ORB-SLAM2 library and examples
+
+Clone the repository:
+```
+git clone https://github.com/raulmur/ORB_SLAM2.git ORB_SLAM2
+```
+
+We provide a script `build.sh` to build the *Thirdparty* libraries and *ORB-SLAM2*. Please make sure you have installed all required dependencies (see section 2). Execute:
+```
+cd ORB_SLAM2
+chmod +x build.sh
+./build.sh
+```
+
+This will create **libORB_SLAM2.so** at *lib* folder and the executables **mono_tum**, **mono_kitti**, **rgbd_tum**, **stereo_kitti**, **mono_euroc** and **stereo_euroc** in *Examples* folder.
+
+# 4. Monocular Examples
+
+## TUM Dataset
+
+1. Download a sequence from http://vision.in.tum.de/data/datasets/rgbd-dataset/download and uncompress it.
+
+2. Execute the following command. Change `TUMX.yaml` to TUM1.yaml,TUM2.yaml or TUM3.yaml for freiburg1, freiburg2 and freiburg3 sequences respectively. Change `PATH_TO_SEQUENCE_FOLDER`to the uncompressed sequence folder.
+```
+./Examples/Monocular/mono_tum Vocabulary/ORBvoc.txt Examples/Monocular/TUMX.yaml PATH_TO_SEQUENCE_FOLDER
+```
+
+## KITTI Dataset
+
+1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php
+
+2. Execute the following command. Change `KITTIX.yaml`by KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11.
+```
+./Examples/Monocular/mono_kitti Vocabulary/ORBvoc.txt Examples/Monocular/KITTIX.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/SEQUENCE_NUMBER
+```
+
+## EuRoC Dataset
+
+1. Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
+
+2. Execute the following first command for V1 and V2 sequences, or the second command for MH sequences. Change PATH_TO_SEQUENCE_FOLDER and SEQUENCE according to the sequence you want to run.
+```
+./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml PATH_TO_SEQUENCE_FOLDER/mav0/cam0/data Examples/Monocular/EuRoC_TimeStamps/SEQUENCE.txt
+```
+
+```
+./Examples/Monocular/mono_euroc Vocabulary/ORBvoc.txt Examples/Monocular/EuRoC.yaml PATH_TO_SEQUENCE/cam0/data Examples/Monocular/EuRoC_TimeStamps/SEQUENCE.txt
+```
+
+# 5. Stereo Examples
+
+## KITTI Dataset
+
+1. Download the dataset (grayscale images) from http://www.cvlibs.net/datasets/kitti/eval_odometry.php
+
+2. Execute the following command. Change `KITTIX.yaml`to KITTI00-02.yaml, KITTI03.yaml or KITTI04-12.yaml for sequence 0 to 2, 3, and 4 to 12 respectively. Change `PATH_TO_DATASET_FOLDER` to the uncompressed dataset folder. Change `SEQUENCE_NUMBER` to 00, 01, 02,.., 11.
+```
+./Examples/Stereo/stereo_kitti Vocabulary/ORBvoc.txt Examples/Stereo/KITTIX.yaml PATH_TO_DATASET_FOLDER/dataset/sequences/SEQUENCE_NUMBER
+```
+
+## EuRoC Dataset
+
+1. Download a sequence (ASL format) from http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets
+
+2. Execute the following first command for V1 and V2 sequences, or the second command for MH sequences. Change PATH_TO_SEQUENCE_FOLDER and SEQUENCE according to the sequence you want to run.
+```
+./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml PATH_TO_SEQUENCE/mav0/cam0/data PATH_TO_SEQUENCE/mav0/cam1/data Examples/Stereo/EuRoC_TimeStamps/SEQUENCE.txt
+```
+```
+./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml PATH_TO_SEQUENCE/cam0/data PATH_TO_SEQUENCE/cam1/data Examples/Stereo/EuRoC_TimeStamps/SEQUENCE.txt
+```
+
+# 6. RGB-D Example
+
+## TUM Dataset
+
+1. Download a sequence from http://vision.in.tum.de/data/datasets/rgbd-dataset/download and uncompress it.
+
+2. Associate RGB images and depth images using the python script [associate.py](http://vision.in.tum.de/data/datasets/rgbd-dataset/tools). We already provide associations for some of the sequences in *Examples/RGB-D/associations/*. You can generate your own associations file executing:
+
+ ```
+ python associate.py PATH_TO_SEQUENCE/rgb.txt PATH_TO_SEQUENCE/depth.txt > associations.txt
+ ```
+
+3. Execute the following command. Change `TUMX.yaml` to TUM1.yaml,TUM2.yaml or TUM3.yaml for freiburg1, freiburg2 and freiburg3 sequences respectively. Change `PATH_TO_SEQUENCE_FOLDER`to the uncompressed sequence folder. Change `ASSOCIATIONS_FILE` to the path to the corresponding associations file.
+
+ ```
+ ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUMX.yaml PATH_TO_SEQUENCE_FOLDER ASSOCIATIONS_FILE
+ ```
+
+# 7. ROS Examples
+
+### Building the nodes for mono, monoAR, stereo and RGB-D
+1. Add the path including *Examples/ROS/ORB_SLAM2* to the ROS_PACKAGE_PATH environment variable. Open .bashrc file and add at the end the following line. Replace PATH by the folder where you cloned ORB_SLAM2:
+
+ ```
+ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
+ ```
+
+2. Execute `build_ros.sh` script:
+
+ ```
+ chmod +x build_ros.sh
+ ./build_ros.sh
+ ```
+
+### Running Monocular Node
+For a monocular input from topic `/camera/image_raw` run node ORB_SLAM2/Mono. You will need to provide the vocabulary file and a settings file. See the monocular examples above.
+
+ ```
+ rosrun ORB_SLAM2 Mono PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+ ```
+
+### Running Monocular Augmented Reality Demo
+This is a demo of augmented reality where you can use an interface to insert virtual cubes in planar regions of the scene.
+The node reads images from topic `/camera/image_raw`.
+
+ ```
+ rosrun ORB_SLAM2 MonoAR PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+ ```
+
+### Running Stereo Node
+For a stereo input from topic `/camera/left/image_raw` and `/camera/right/image_raw` run node ORB_SLAM2/Stereo. You will need to provide the vocabulary file and a settings file. If you **provide rectification matrices** (see Examples/Stereo/EuRoC.yaml example), the node will recitify the images online, **otherwise images must be pre-rectified**.
+
+ ```
+ rosrun ORB_SLAM2 Stereo PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE ONLINE_RECTIFICATION
+ ```
+
+**Example**: Download a rosbag (e.g. V1_01_easy.bag) from the EuRoC dataset (http://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets). Open 3 tabs on the terminal and run the following command at each tab:
+ ```
+ roscore
+ ```
+
+ ```
+ rosrun ORB_SLAM2 Stereo Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml true
+ ```
+
+ ```
+ rosbag play --pause V1_01_easy.bag /cam0/image_raw:=/camera/left/image_raw /cam1/image_raw:=/camera/right/image_raw
+ ```
+
+Once ORB-SLAM2 has loaded the vocabulary, press space in the rosbag tab. Enjoy!. Note: a powerful computer is required to run the most exigent sequences of this dataset.
+
+### Running RGB_D Node
+For an RGB-D input from topics `/camera/rgb/image_raw` and `/camera/depth_registered/image_raw`, run node ORB_SLAM2/RGBD. You will need to provide the vocabulary file and a settings file. See the RGB-D example above.
+
+ ```
+ rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY PATH_TO_SETTINGS_FILE
+ ```
+
+# 8. Processing your own sequences
+You will need to create a settings file with the calibration of your camera. See the settings file provided for the TUM and KITTI datasets for monocular, stereo and RGB-D cameras. We use the calibration model of OpenCV. See the examples to learn how to create a program that makes use of the ORB-SLAM2 library and how to pass images to the SLAM system. Stereo input must be synchronized and rectified. RGB-D input must be synchronized and depth registered.
+
+# 9. SLAM and Localization Modes
+You can change between the *SLAM* and *Localization mode* using the GUI of the map viewer.
+
+### SLAM Mode
+This is the default mode. The system runs in parallal three threads: Tracking, Local Mapping and Loop Closing. The system localizes the camera, builds new map and tries to close loops.
+
+### Localization Mode
+This mode can be used when you have a good map of your working area. In this mode the Local Mapping and Loop Closing are deactivated. The system localizes the camera in the map (which is no longer updated), using relocalization if needed.
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/CMakeLists.txt b/externals/ORB_SLAM2/Thirdparty/DBoW2/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..0eb512600a2b92d415e196a3b642f2410896385a
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/CMakeLists.txt
@@ -0,0 +1,40 @@
+cmake_minimum_required(VERSION 2.8)
+project(DBoW2)
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -march=native ")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
+
+set(HDRS_DBOW2
+ DBoW2/BowVector.h
+ DBoW2/FORB.h
+ DBoW2/FClass.h
+ DBoW2/FeatureVector.h
+ DBoW2/ScoringObject.h
+ DBoW2/TemplatedVocabulary.h)
+set(SRCS_DBOW2
+ DBoW2/BowVector.cpp
+ DBoW2/FORB.cpp
+ DBoW2/FeatureVector.cpp
+ DBoW2/ScoringObject.cpp)
+
+set(HDRS_DUTILS
+ DUtils/Random.h
+ DUtils/Timestamp.h)
+set(SRCS_DUTILS
+ DUtils/Random.cpp
+ DUtils/Timestamp.cpp)
+
+find_package(OpenCV 3.0 QUIET)
+if(NOT OpenCV_FOUND)
+ find_package(OpenCV 2.4.3 QUIET)
+ if(NOT OpenCV_FOUND)
+ message(FATAL_ERROR "OpenCV > 2.4.3 not found.")
+ endif()
+endif()
+
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+include_directories(${OpenCV_INCLUDE_DIRS})
+add_library(DBoW2 SHARED ${SRCS_DBOW2} ${SRCS_DUTILS})
+target_link_libraries(DBoW2 ${OpenCV_LIBS})
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1337fa3e5b942564cce3317ecfdad7e3e8b5c952
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.cpp
@@ -0,0 +1,130 @@
+/**
+ * File: BowVector.cpp
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <iostream>
+#include <fstream>
+#include <vector>
+#include <algorithm>
+#include <cmath>
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+BowVector::BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+BowVector::~BowVector(void)
+{
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addWeight(WordId id, WordValue v)
+{
+ BowVector::iterator vit = this->lower_bound(id);
+
+ if(vit != this->end() && !(this->key_comp()(id, vit->first)))
+ {
+ vit->second += v;
+ }
+ else
+ {
+ this->insert(vit, BowVector::value_type(id, v));
+ }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::addIfNotExist(WordId id, WordValue v)
+{
+ BowVector::iterator vit = this->lower_bound(id);
+
+ if(vit == this->end() || (this->key_comp()(id, vit->first)))
+ {
+ this->insert(vit, BowVector::value_type(id, v));
+ }
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::normalize(LNorm norm_type)
+{
+ double norm = 0.0;
+ BowVector::iterator it;
+
+ if(norm_type == DBoW2::L1)
+ {
+ for(it = begin(); it != end(); ++it)
+ norm += fabs(it->second);
+ }
+ else
+ {
+ for(it = begin(); it != end(); ++it)
+ norm += it->second * it->second;
+ norm = sqrt(norm);
+ }
+
+ if(norm > 0.0)
+ {
+ for(it = begin(); it != end(); ++it)
+ it->second /= norm;
+ }
+}
+
+// --------------------------------------------------------------------------
+
+std::ostream& operator<< (std::ostream &out, const BowVector &v)
+{
+ BowVector::const_iterator vit;
+ std::vector<unsigned int>::const_iterator iit;
+ unsigned int i = 0;
+ const unsigned int N = v.size();
+ for(vit = v.begin(); vit != v.end(); ++vit, ++i)
+ {
+ out << "<" << vit->first << ", " << vit->second << ">";
+
+ if(i < N-1) out << ", ";
+ }
+ return out;
+}
+
+// --------------------------------------------------------------------------
+
+void BowVector::saveM(const std::string &filename, size_t W) const
+{
+ std::fstream f(filename.c_str(), std::ios::out);
+
+ WordId last = 0;
+ BowVector::const_iterator bit;
+ for(bit = this->begin(); bit != this->end(); ++bit)
+ {
+ for(; last < bit->first; ++last)
+ {
+ f << "0 ";
+ }
+ f << bit->second << " ";
+
+ last = bit->first + 1;
+ }
+ for(; last < (WordId)W; ++last)
+ f << "0 ";
+
+ f.close();
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.h
new file mode 100644
index 0000000000000000000000000000000000000000..f559811debd7fc60ec9370208157bbe1bdc0de30
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/BowVector.h
@@ -0,0 +1,109 @@
+/**
+ * File: BowVector.h
+ * Date: March 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: bag of words vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_BOW_VECTOR__
+#define __D_T_BOW_VECTOR__
+
+#include <iostream>
+#include <map>
+#include <vector>
+
+namespace DBoW2 {
+
+/// Id of words
+typedef unsigned int WordId;
+
+/// Value of a word
+typedef double WordValue;
+
+/// Id of nodes in the vocabulary treee
+typedef unsigned int NodeId;
+
+/// L-norms for normalization
+enum LNorm
+{
+ L1,
+ L2
+};
+
+/// Weighting type
+enum WeightingType
+{
+ TF_IDF,
+ TF,
+ IDF,
+ BINARY
+};
+
+/// Scoring type
+enum ScoringType
+{
+ L1_NORM,
+ L2_NORM,
+ CHI_SQUARE,
+ KL,
+ BHATTACHARYYA,
+ DOT_PRODUCT,
+};
+
+/// Vector of words to represent images
+class BowVector:
+ public std::map<WordId, WordValue>
+{
+public:
+
+ /**
+ * Constructor
+ */
+ BowVector(void);
+
+ /**
+ * Destructor
+ */
+ ~BowVector(void);
+
+ /**
+ * Adds a value to a word value existing in the vector, or creates a new
+ * word with the given value
+ * @param id word id to look for
+ * @param v value to create the word with, or to add to existing word
+ */
+ void addWeight(WordId id, WordValue v);
+
+ /**
+ * Adds a word with a value to the vector only if this does not exist yet
+ * @param id word id to look for
+ * @param v value to give to the word if this does not exist
+ */
+ void addIfNotExist(WordId id, WordValue v);
+
+ /**
+ * L1-Normalizes the values in the vector
+ * @param norm_type norm used
+ */
+ void normalize(LNorm norm_type);
+
+ /**
+ * Prints the content of the bow vector
+ * @param out stream
+ * @param v
+ */
+ friend std::ostream& operator<<(std::ostream &out, const BowVector &v);
+
+ /**
+ * Saves the bow vector as a vector in a matlab file
+ * @param filename
+ * @param W number of words in the vocabulary
+ */
+ void saveM(const std::string &filename, size_t W) const;
+};
+
+} // namespace DBoW2
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FClass.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FClass.h
new file mode 100644
index 0000000000000000000000000000000000000000..13be53d753e9786032eafedfda77c8ff36c522ad
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FClass.h
@@ -0,0 +1,71 @@
+/**
+ * File: FClass.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: generic FClass to instantiate templated classes
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FCLASS__
+#define __D_T_FCLASS__
+
+#include <opencv2/core/core.hpp>
+#include <vector>
+#include <string>
+
+namespace DBoW2 {
+
+/// Generic class to encapsulate functions to manage descriptors.
+/**
+ * This class must be inherited. Derived classes can be used as the
+ * parameter F when creating Templated structures
+ * (TemplatedVocabulary, TemplatedDatabase, ...)
+ */
+class FClass
+{
+ class TDescriptor;
+ typedef const TDescriptor *pDescriptor;
+
+ /**
+ * Calculates the mean value of a set of descriptors
+ * @param descriptors
+ * @param mean mean descriptor
+ */
+ virtual void meanValue(const std::vector<pDescriptor> &descriptors,
+ TDescriptor &mean) = 0;
+
+ /**
+ * Calculates the distance between two descriptors
+ * @param a
+ * @param b
+ * @return distance
+ */
+ static double distance(const TDescriptor &a, const TDescriptor &b);
+
+ /**
+ * Returns a string version of the descriptor
+ * @param a descriptor
+ * @return string version
+ */
+ static std::string toString(const TDescriptor &a);
+
+ /**
+ * Returns a descriptor from a string
+ * @param a descriptor
+ * @param s string version
+ */
+ static void fromString(TDescriptor &a, const std::string &s);
+
+ /**
+ * Returns a mat with the descriptors in float format
+ * @param descriptors
+ * @param mat (out) NxL 32F matrix
+ */
+ static void toMat32F(const std::vector<TDescriptor> &descriptors,
+ cv::Mat &mat);
+};
+
+} // namespace DBoW2
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1f1990c2f7ae54eeb18a8b843cbdece94e2c3493
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.cpp
@@ -0,0 +1,193 @@
+/**
+ * File: FORB.cpp
+ * Date: June 2012
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for ORB descriptors
+ * License: see the LICENSE.txt file
+ *
+ * Distance function has been modified
+ *
+ */
+
+
+#include <vector>
+#include <string>
+#include <sstream>
+#include <stdint-gcc.h>
+
+#include "FORB.h"
+
+using namespace std;
+
+namespace DBoW2 {
+
+// --------------------------------------------------------------------------
+
+const int FORB::L=32;
+
+void FORB::meanValue(const std::vector<FORB::pDescriptor> &descriptors,
+ FORB::TDescriptor &mean)
+{
+ if(descriptors.empty())
+ {
+ mean.release();
+ return;
+ }
+ else if(descriptors.size() == 1)
+ {
+ mean = descriptors[0]->clone();
+ }
+ else
+ {
+ vector<int> sum(FORB::L * 8, 0);
+
+ for(size_t i = 0; i < descriptors.size(); ++i)
+ {
+ const cv::Mat &d = *descriptors[i];
+ const unsigned char *p = d.ptr<unsigned char>();
+
+ for(int j = 0; j < d.cols; ++j, ++p)
+ {
+ if(*p & (1 << 7)) ++sum[ j*8 ];
+ if(*p & (1 << 6)) ++sum[ j*8 + 1 ];
+ if(*p & (1 << 5)) ++sum[ j*8 + 2 ];
+ if(*p & (1 << 4)) ++sum[ j*8 + 3 ];
+ if(*p & (1 << 3)) ++sum[ j*8 + 4 ];
+ if(*p & (1 << 2)) ++sum[ j*8 + 5 ];
+ if(*p & (1 << 1)) ++sum[ j*8 + 6 ];
+ if(*p & (1)) ++sum[ j*8 + 7 ];
+ }
+ }
+
+ mean = cv::Mat::zeros(1, FORB::L, CV_8U);
+ unsigned char *p = mean.ptr<unsigned char>();
+
+ const int N2 = (int)descriptors.size() / 2 + descriptors.size() % 2;
+ for(size_t i = 0; i < sum.size(); ++i)
+ {
+ if(sum[i] >= N2)
+ {
+ // set bit
+ *p |= 1 << (7 - (i % 8));
+ }
+
+ if(i % 8 == 7) ++p;
+ }
+ }
+}
+
+// --------------------------------------------------------------------------
+
+int FORB::distance(const FORB::TDescriptor &a,
+ const FORB::TDescriptor &b)
+{
+ // Bit set count operation from
+ // http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
+
+ const int *pa = a.ptr<int32_t>();
+ const int *pb = b.ptr<int32_t>();
+
+ int dist=0;
+
+ for(int i=0; i<8; i++, pa++, pb++)
+ {
+ unsigned int v = *pa ^ *pb;
+ v = v - ((v >> 1) & 0x55555555);
+ v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
+ dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
+ }
+
+ return dist;
+}
+
+// --------------------------------------------------------------------------
+
+std::string FORB::toString(const FORB::TDescriptor &a)
+{
+ stringstream ss;
+ const unsigned char *p = a.ptr<unsigned char>();
+
+ for(int i = 0; i < a.cols; ++i, ++p)
+ {
+ ss << (int)*p << " ";
+ }
+
+ return ss.str();
+}
+
+// --------------------------------------------------------------------------
+
+void FORB::fromString(FORB::TDescriptor &a, const std::string &s)
+{
+ a.create(1, FORB::L, CV_8U);
+ unsigned char *p = a.ptr<unsigned char>();
+
+ stringstream ss(s);
+ for(int i = 0; i < FORB::L; ++i, ++p)
+ {
+ int n;
+ ss >> n;
+
+ if(!ss.fail())
+ *p = (unsigned char)n;
+ }
+
+}
+
+// --------------------------------------------------------------------------
+
+void FORB::toMat32F(const std::vector<TDescriptor> &descriptors,
+ cv::Mat &mat)
+{
+ if(descriptors.empty())
+ {
+ mat.release();
+ return;
+ }
+
+ const size_t N = descriptors.size();
+
+ mat.create(N, FORB::L*8, CV_32F);
+ float *p = mat.ptr<float>();
+
+ for(size_t i = 0; i < N; ++i)
+ {
+ const int C = descriptors[i].cols;
+ const unsigned char *desc = descriptors[i].ptr<unsigned char>();
+
+ for(int j = 0; j < C; ++j, p += 8)
+ {
+ p[0] = (desc[j] & (1 << 7) ? 1 : 0);
+ p[1] = (desc[j] & (1 << 6) ? 1 : 0);
+ p[2] = (desc[j] & (1 << 5) ? 1 : 0);
+ p[3] = (desc[j] & (1 << 4) ? 1 : 0);
+ p[4] = (desc[j] & (1 << 3) ? 1 : 0);
+ p[5] = (desc[j] & (1 << 2) ? 1 : 0);
+ p[6] = (desc[j] & (1 << 1) ? 1 : 0);
+ p[7] = desc[j] & (1);
+ }
+ }
+}
+
+// --------------------------------------------------------------------------
+
+void FORB::toMat8U(const std::vector<TDescriptor> &descriptors,
+ cv::Mat &mat)
+{
+ mat.create(descriptors.size(), 32, CV_8U);
+
+ unsigned char *p = mat.ptr<unsigned char>();
+
+ for(size_t i = 0; i < descriptors.size(); ++i, p += 32)
+ {
+ const unsigned char *d = descriptors[i].ptr<unsigned char>();
+ std::copy(d, d+32, p);
+ }
+
+}
+
+// --------------------------------------------------------------------------
+
+} // namespace DBoW2
+
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.h
new file mode 100644
index 0000000000000000000000000000000000000000..a39599f20e1598181064bc18f4853dbdccad0beb
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FORB.h
@@ -0,0 +1,79 @@
+/**
+ * File: FORB.h
+ * Date: June 2012
+ * Author: Dorian Galvez-Lopez
+ * Description: functions for ORB descriptors
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_F_ORB__
+#define __D_T_F_ORB__
+
+#include <opencv2/core/core.hpp>
+#include <vector>
+#include <string>
+
+#include "FClass.h"
+
+namespace DBoW2 {
+
+/// Functions to manipulate ORB descriptors
+class FORB: protected FClass
+{
+public:
+
+ /// Descriptor type
+ typedef cv::Mat TDescriptor; // CV_8U
+ /// Pointer to a single descriptor
+ typedef const TDescriptor *pDescriptor;
+ /// Descriptor length (in bytes)
+ static const int L;
+
+ /**
+ * Calculates the mean value of a set of descriptors
+ * @param descriptors
+ * @param mean mean descriptor
+ */
+ static void meanValue(const std::vector<pDescriptor> &descriptors,
+ TDescriptor &mean);
+
+ /**
+ * Calculates the distance between two descriptors
+ * @param a
+ * @param b
+ * @return distance
+ */
+ static int distance(const TDescriptor &a, const TDescriptor &b);
+
+ /**
+ * Returns a string version of the descriptor
+ * @param a descriptor
+ * @return string version
+ */
+ static std::string toString(const TDescriptor &a);
+
+ /**
+ * Returns a descriptor from a string
+ * @param a descriptor
+ * @param s string version
+ */
+ static void fromString(TDescriptor &a, const std::string &s);
+
+ /**
+ * Returns a mat with the descriptors in float format
+ * @param descriptors
+ * @param mat (out) NxL 32F matrix
+ */
+ static void toMat32F(const std::vector<TDescriptor> &descriptors,
+ cv::Mat &mat);
+
+ static void toMat8U(const std::vector<TDescriptor> &descriptors,
+ cv::Mat &mat);
+
+};
+
+} // namespace DBoW2
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c055a15767318cd3139fa93e3a24f642383479cd
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.cpp
@@ -0,0 +1,85 @@
+/**
+ * File: FeatureVector.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "FeatureVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+FeatureVector::~FeatureVector(void)
+{
+}
+
+// ---------------------------------------------------------------------------
+
+void FeatureVector::addFeature(NodeId id, unsigned int i_feature)
+{
+ FeatureVector::iterator vit = this->lower_bound(id);
+
+ if(vit != this->end() && vit->first == id)
+ {
+ vit->second.push_back(i_feature);
+ }
+ else
+ {
+ vit = this->insert(vit, FeatureVector::value_type(id,
+ std::vector<unsigned int>() ));
+ vit->second.push_back(i_feature);
+ }
+}
+
+// ---------------------------------------------------------------------------
+
+std::ostream& operator<<(std::ostream &out,
+ const FeatureVector &v)
+{
+ if(!v.empty())
+ {
+ FeatureVector::const_iterator vit = v.begin();
+
+ const std::vector<unsigned int>* f = &vit->second;
+
+ out << "<" << vit->first << ": [";
+ if(!f->empty()) out << (*f)[0];
+ for(unsigned int i = 1; i < f->size(); ++i)
+ {
+ out << ", " << (*f)[i];
+ }
+ out << "]>";
+
+ for(++vit; vit != v.end(); ++vit)
+ {
+ f = &vit->second;
+
+ out << ", <" << vit->first << ": [";
+ if(!f->empty()) out << (*f)[0];
+ for(unsigned int i = 1; i < f->size(); ++i)
+ {
+ out << ", " << (*f)[i];
+ }
+ out << "]>";
+ }
+ }
+
+ return out;
+}
+
+// ---------------------------------------------------------------------------
+
+} // namespace DBoW2
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.h
new file mode 100644
index 0000000000000000000000000000000000000000..08a91def7cfabf4a2934784d8bcd2995c26ac787
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/FeatureVector.h
@@ -0,0 +1,56 @@
+/**
+ * File: FeatureVector.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: feature vector
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_FEATURE_VECTOR__
+#define __D_T_FEATURE_VECTOR__
+
+#include "BowVector.h"
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace DBoW2 {
+
+/// Vector of nodes with indexes of local features
+class FeatureVector:
+ public std::map<NodeId, std::vector<unsigned int> >
+{
+public:
+
+ /**
+ * Constructor
+ */
+ FeatureVector(void);
+
+ /**
+ * Destructor
+ */
+ ~FeatureVector(void);
+
+ /**
+ * Adds a feature to an existing node, or adds a new node with an initial
+ * feature
+ * @param id node id to add or to modify
+ * @param i_feature index of feature to add to the given node
+ */
+ void addFeature(NodeId id, unsigned int i_feature);
+
+ /**
+ * Sends a string versions of the feature vector through the stream
+ * @param out stream
+ * @param v feature vector
+ */
+ friend std::ostream& operator<<(std::ostream &out, const FeatureVector &v);
+
+};
+
+} // namespace DBoW2
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..063a96e87d6d807294c7728cc650ae4c15cebf57
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.cpp
@@ -0,0 +1,315 @@
+/**
+ * File: ScoringObject.cpp
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cfloat>
+#include "TemplatedVocabulary.h"
+#include "BowVector.h"
+
+using namespace DBoW2;
+
+// If you change the type of WordValue, make sure you change also the
+// epsilon value (this is needed by the KL method)
+const double GeneralScoring::LOG_EPS = log(DBL_EPSILON); // FLT_EPSILON
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L1Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ score += fabs(vi - wi) - fabs(vi) - fabs(wi);
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ v1_it = v1.lower_bound(v2_it->first);
+ // v1_it = (first element >= v2_it.id)
+ }
+ else
+ {
+ // move v2 forward
+ v2_it = v2.lower_bound(v1_it->first);
+ // v2_it = (first element >= v1_it.id)
+ }
+ }
+
+ // ||v - w||_{L1} = 2 + Sum(|v_i - w_i| - |v_i| - |w_i|)
+ // for all i | v_i != 0 and w_i != 0
+ // (Nister, 2006)
+ // scaled_||v - w||_{L1} = 1 - 0.5 * ||v - w||_{L1}
+ score = -score/2.0;
+
+ return score; // [0..1]
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double L2Scoring::score(const BowVector &v1, const BowVector &v2) const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ score += vi * wi;
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ v1_it = v1.lower_bound(v2_it->first);
+ // v1_it = (first element >= v2_it.id)
+ }
+ else
+ {
+ // move v2 forward
+ v2_it = v2.lower_bound(v1_it->first);
+ // v2_it = (first element >= v1_it.id)
+ }
+ }
+
+ // ||v - w||_{L2} = sqrt( 2 - 2 * Sum(v_i * w_i) )
+ // for all i | v_i != 0 and w_i != 0 )
+ // (Nister, 2006)
+ if(score >= 1) // rounding errors
+ score = 1.0;
+ else
+ score = 1.0 - sqrt(1.0 - score); // [0..1]
+
+ return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double ChiSquareScoring::score(const BowVector &v1, const BowVector &v2)
+ const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ // all the items are taken into account
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ // (v-w)^2/(v+w) - v - w = -4 vw/(v+w)
+ // we move the -4 out
+ if(vi + wi != 0.0) score += vi * wi / (vi + wi);
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ v1_it = v1.lower_bound(v2_it->first);
+ }
+ else
+ {
+ // move v2 forward
+ v2_it = v2.lower_bound(v1_it->first);
+ }
+ }
+
+ // this takes the -4 into account
+ score = 2. * score; // [0..1]
+
+ return score;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double KLScoring::score(const BowVector &v1, const BowVector &v2) const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ // all the items or v are taken into account
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ if(vi != 0 && wi != 0) score += vi * log(vi/wi);
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ score += vi * (log(vi) - LOG_EPS);
+ ++v1_it;
+ }
+ else
+ {
+ // move v2_it forward, do not add any score
+ v2_it = v2.lower_bound(v1_it->first);
+ // v2_it = (first element >= v1_it.id)
+ }
+ }
+
+ // sum rest of items of v
+ for(; v1_it != v1_end; ++v1_it)
+ if(v1_it->second != 0)
+ score += v1_it->second * (log(v1_it->second) - LOG_EPS);
+
+ return score; // cannot be scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double BhattacharyyaScoring::score(const BowVector &v1,
+ const BowVector &v2) const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ score += sqrt(vi * wi);
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ v1_it = v1.lower_bound(v2_it->first);
+ // v1_it = (first element >= v2_it.id)
+ }
+ else
+ {
+ // move v2 forward
+ v2_it = v2.lower_bound(v1_it->first);
+ // v2_it = (first element >= v1_it.id)
+ }
+ }
+
+ return score; // already scaled
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+double DotProductScoring::score(const BowVector &v1,
+ const BowVector &v2) const
+{
+ BowVector::const_iterator v1_it, v2_it;
+ const BowVector::const_iterator v1_end = v1.end();
+ const BowVector::const_iterator v2_end = v2.end();
+
+ v1_it = v1.begin();
+ v2_it = v2.begin();
+
+ double score = 0;
+
+ while(v1_it != v1_end && v2_it != v2_end)
+ {
+ const WordValue& vi = v1_it->second;
+ const WordValue& wi = v2_it->second;
+
+ if(v1_it->first == v2_it->first)
+ {
+ score += vi * wi;
+
+ // move v1 and v2 forward
+ ++v1_it;
+ ++v2_it;
+ }
+ else if(v1_it->first < v2_it->first)
+ {
+ // move v1 forward
+ v1_it = v1.lower_bound(v2_it->first);
+ // v1_it = (first element >= v2_it.id)
+ }
+ else
+ {
+ // move v2 forward
+ v2_it = v2.lower_bound(v1_it->first);
+ // v2_it = (first element >= v1_it.id)
+ }
+ }
+
+ return score; // cannot scale
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.h
new file mode 100644
index 0000000000000000000000000000000000000000..8d5b82192a18d23dce7e9b917e2d4d2a3b1ee80d
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/ScoringObject.h
@@ -0,0 +1,96 @@
+/**
+ * File: ScoringObject.h
+ * Date: November 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: functions to compute bow scores
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_SCORING_OBJECT__
+#define __D_T_SCORING_OBJECT__
+
+#include "BowVector.h"
+
+namespace DBoW2 {
+
+/// Base class of scoring functions
+class GeneralScoring
+{
+public:
+ /**
+ * Computes the score between two vectors. Vectors must be sorted and
+ * normalized if necessary
+ * @param v (in/out)
+ * @param w (in/out)
+ * @return score
+ */
+ virtual double score(const BowVector &v, const BowVector &w) const = 0;
+
+ /**
+ * Returns whether a vector must be normalized before scoring according
+ * to the scoring scheme
+ * @param norm norm to use
+ * @return true iff must normalize
+ */
+ virtual bool mustNormalize(LNorm &norm) const = 0;
+
+ /// Log of epsilon
+ static const double LOG_EPS;
+ // If you change the type of WordValue, make sure you change also the
+ // epsilon value (this is needed by the KL method)
+
+ virtual ~GeneralScoring() {} //!< Required for virtual base classes
+
+};
+
+/**
+ * Macro for defining Scoring classes
+ * @param NAME name of class
+ * @param MUSTNORMALIZE if vectors must be normalized to compute the score
+ * @param NORM type of norm to use when MUSTNORMALIZE
+ */
+#define __SCORING_CLASS(NAME, MUSTNORMALIZE, NORM) \
+ NAME: public GeneralScoring \
+ { public: \
+ /** \
+ * Computes score between two vectors \
+ * @param v \
+ * @param w \
+ * @return score between v and w \
+ */ \
+ virtual double score(const BowVector &v, const BowVector &w) const; \
+ \
+ /** \
+ * Says if a vector must be normalized according to the scoring function \
+ * @param norm (out) if true, norm to use
+ * @return true iff vectors must be normalized \
+ */ \
+ virtual inline bool mustNormalize(LNorm &norm) const \
+ { norm = NORM; return MUSTNORMALIZE; } \
+ }
+
+/// L1 Scoring object
+class __SCORING_CLASS(L1Scoring, true, L1);
+
+/// L2 Scoring object
+class __SCORING_CLASS(L2Scoring, true, L2);
+
+/// Chi square Scoring object
+class __SCORING_CLASS(ChiSquareScoring, true, L1);
+
+/// KL divergence Scoring object
+class __SCORING_CLASS(KLScoring, true, L1);
+
+/// Bhattacharyya Scoring object
+class __SCORING_CLASS(BhattacharyyaScoring, true, L1);
+
+/// Dot product Scoring object
+class __SCORING_CLASS(DotProductScoring, false, L1);
+
+#undef __SCORING_CLASS
+
+} // namespace DBoW2
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
new file mode 100644
index 0000000000000000000000000000000000000000..01959344ed2d4fc2b90c174fb3bc5caabce96fe8
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h
@@ -0,0 +1,1665 @@
+/**
+ * This is a modified version of TemplatedVocabulary.h from DBoW2 (see below).
+ * Added functions: Save and Load from text files without using cv::FileStorage.
+ * Date: August 2015
+ * Raúl Mur-Artal
+ */
+
+/**
+ * File: TemplatedVocabulary.h
+ * Date: February 2011
+ * Author: Dorian Galvez-Lopez
+ * Description: templated vocabulary
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_T_TEMPLATED_VOCABULARY__
+#define __D_T_TEMPLATED_VOCABULARY__
+
+#include <cassert>
+
+#include <vector>
+#include <numeric>
+#include <fstream>
+#include <string>
+#include <algorithm>
+#include <opencv2/core/core.hpp>
+#include <limits>
+
+#include "FeatureVector.h"
+#include "BowVector.h"
+#include "ScoringObject.h"
+
+#include "../DUtils/Random.h"
+
+using namespace std;
+
+namespace DBoW2 {
+
+/// @param TDescriptor class of descriptor
+/// @param F class of descriptor functions
+template<class TDescriptor, class F>
+/// Generic Vocabulary
+class TemplatedVocabulary
+{
+public:
+
+ /**
+ * Initiates an empty vocabulary
+ * @param k branching factor
+ * @param L depth levels
+ * @param weighting weighting type
+ * @param scoring scoring type
+ */
+ TemplatedVocabulary(int k = 10, int L = 5,
+ WeightingType weighting = TF_IDF, ScoringType scoring = L1_NORM);
+
+ /**
+ * Creates the vocabulary by loading a file
+ * @param filename
+ */
+ TemplatedVocabulary(const std::string &filename);
+
+ /**
+ * Creates the vocabulary by loading a file
+ * @param filename
+ */
+ TemplatedVocabulary(const char *filename);
+
+ /**
+ * Copy constructor
+ * @param voc
+ */
+ TemplatedVocabulary(const TemplatedVocabulary<TDescriptor, F> &voc);
+
+ /**
+ * Destructor
+ */
+ virtual ~TemplatedVocabulary();
+
+ /**
+ * Assigns the given vocabulary to this by copying its data and removing
+ * all the data contained by this vocabulary before
+ * @param voc
+ * @return reference to this vocabulary
+ */
+ TemplatedVocabulary<TDescriptor, F>& operator=(
+ const TemplatedVocabulary<TDescriptor, F> &voc);
+
+ /**
+ * Creates a vocabulary from the training features with the already
+ * defined parameters
+ * @param training_features
+ */
+ virtual void create
+ (const std::vector<std::vector<TDescriptor> > &training_features);
+
+ /**
+ * Creates a vocabulary from the training features, setting the branching
+ * factor and the depth levels of the tree
+ * @param training_features
+ * @param k branching factor
+ * @param L depth levels
+ */
+ virtual void create
+ (const std::vector<std::vector<TDescriptor> > &training_features,
+ int k, int L);
+
+ /**
+ * Creates a vocabulary from the training features, setting the branching
+ * factor nad the depth levels of the tree, and the weighting and scoring
+ * schemes
+ */
+ virtual void create
+ (const std::vector<std::vector<TDescriptor> > &training_features,
+ int k, int L, WeightingType weighting, ScoringType scoring);
+
+ /**
+ * Returns the number of words in the vocabulary
+ * @return number of words
+ */
+ virtual inline unsigned int size() const;
+
+ /**
+ * Returns whether the vocabulary is empty (i.e. it has not been trained)
+ * @return true iff the vocabulary is empty
+ */
+ virtual inline bool empty() const;
+
+ /**
+ * Transforms a set of descriptores into a bow vector
+ * @param features
+ * @param v (out) bow vector of weighted words
+ */
+ virtual void transform(const std::vector<TDescriptor>& features, BowVector &v)
+ const;
+
+ /**
+ * Transform a set of descriptors into a bow vector and a feature vector
+ * @param features
+ * @param v (out) bow vector
+ * @param fv (out) feature vector of nodes and feature indexes
+ * @param levelsup levels to go up the vocabulary tree to get the node index
+ */
+ virtual void transform(const std::vector<TDescriptor>& features,
+ BowVector &v, FeatureVector &fv, int levelsup) const;
+
+ /**
+ * Transforms a single feature into a word (without weight)
+ * @param feature
+ * @return word id
+ */
+ virtual WordId transform(const TDescriptor& feature) const;
+
+ /**
+ * Returns the score of two vectors
+ * @param a vector
+ * @param b vector
+ * @return score between vectors
+ * @note the vectors must be already sorted and normalized if necessary
+ */
+ inline double score(const BowVector &a, const BowVector &b) const;
+
+ /**
+ * Returns the id of the node that is "levelsup" levels from the word given
+ * @param wid word id
+ * @param levelsup 0..L
+ * @return node id. if levelsup is 0, returns the node id associated to the
+ * word id
+ */
+ virtual NodeId getParentNode(WordId wid, int levelsup) const;
+
+ /**
+ * Returns the ids of all the words that are under the given node id,
+ * by traversing any of the branches that goes down from the node
+ * @param nid starting node id
+ * @param words ids of words
+ */
+ void getWordsFromNode(NodeId nid, std::vector<WordId> &words) const;
+
+ /**
+ * Returns the branching factor of the tree (k)
+ * @return k
+ */
+ inline int getBranchingFactor() const { return m_k; }
+
+ /**
+ * Returns the depth levels of the tree (L)
+ * @return L
+ */
+ inline int getDepthLevels() const { return m_L; }
+
+ /**
+ * Returns the real depth levels of the tree on average
+ * @return average of depth levels of leaves
+ */
+ float getEffectiveLevels() const;
+
+ /**
+ * Returns the descriptor of a word
+ * @param wid word id
+ * @return descriptor
+ */
+ virtual inline TDescriptor getWord(WordId wid) const;
+
+ /**
+ * Returns the weight of a word
+ * @param wid word id
+ * @return weight
+ */
+ virtual inline WordValue getWordWeight(WordId wid) const;
+
+ /**
+ * Returns the weighting method
+ * @return weighting method
+ */
+ inline WeightingType getWeightingType() const { return m_weighting; }
+
+ /**
+ * Returns the scoring method
+ * @return scoring method
+ */
+ inline ScoringType getScoringType() const { return m_scoring; }
+
+ /**
+ * Changes the weighting method
+ * @param type new weighting type
+ */
+ inline void setWeightingType(WeightingType type);
+
+ /**
+ * Changes the scoring method
+ * @param type new scoring type
+ */
+ void setScoringType(ScoringType type);
+
+ /**
+ * Loads the vocabulary from a text file
+ * @param filename
+ */
+ bool loadFromTextFile(const std::string &filename);
+
+ /**
+ * Saves the vocabulary into a text file
+ * @param filename
+ */
+ void saveToTextFile(const std::string &filename) const;
+
+ /**
+ * Saves the vocabulary into a file
+ * @param filename
+ */
+ void save(const std::string &filename) const;
+
+ /**
+ * Loads the vocabulary from a file
+ * @param filename
+ */
+ void load(const std::string &filename);
+
+ /**
+ * Saves the vocabulary to a file storage structure
+ * @param fn node in file storage
+ */
+ virtual void save(cv::FileStorage &fs,
+ const std::string &name = "vocabulary") const;
+
+ /**
+ * Loads the vocabulary from a file storage node
+ * @param fn first node
+ * @param subname name of the child node of fn where the tree is stored.
+ * If not given, the fn node is used instead
+ */
+ virtual void load(const cv::FileStorage &fs,
+ const std::string &name = "vocabulary");
+
+ /**
+ * Stops those words whose weight is below minWeight.
+ * Words are stopped by setting their weight to 0. There are not returned
+ * later when transforming image features into vectors.
+ * Note that when using IDF or TF_IDF, the weight is the idf part, which
+ * is equivalent to -log(f), where f is the frequency of the word
+ * (f = Ni/N, Ni: number of training images where the word is present,
+ * N: number of training images).
+ * Note that the old weight is forgotten, and subsequent calls to this
+ * function with a lower minWeight have no effect.
+ * @return number of words stopped now
+ */
+ virtual int stopWords(double minWeight);
+
+protected:
+
+ /// Pointer to descriptor
+ typedef const TDescriptor *pDescriptor;
+
+ /// Tree node
+ struct Node
+ {
+ /// Node id
+ NodeId id;
+ /// Weight if the node is a word
+ WordValue weight;
+ /// Children
+ vector<NodeId> children;
+ /// Parent node (undefined in case of root)
+ NodeId parent;
+ /// Node descriptor
+ TDescriptor descriptor;
+
+ /// Word id if the node is a word
+ WordId word_id;
+
+ /**
+ * Empty constructor
+ */
+ Node(): id(0), weight(0), parent(0), word_id(0){}
+
+ /**
+ * Constructor
+ * @param _id node id
+ */
+ Node(NodeId _id): id(_id), weight(0), parent(0), word_id(0){}
+
+ /**
+ * Returns whether the node is a leaf node
+ * @return true iff the node is a leaf
+ */
+ inline bool isLeaf() const { return children.empty(); }
+ };
+
+protected:
+
+ /**
+ * Creates an instance of the scoring object accoring to m_scoring
+ */
+ void createScoringObject();
+
+ /**
+ * Returns a set of pointers to descriptores
+ * @param training_features all the features
+ * @param features (out) pointers to the training features
+ */
+ void getFeatures(
+ const vector<vector<TDescriptor> > &training_features,
+ vector<pDescriptor> &features) const;
+
+ /**
+ * Returns the word id associated to a feature
+ * @param feature
+ * @param id (out) word id
+ * @param weight (out) word weight
+ * @param nid (out) if given, id of the node "levelsup" levels up
+ * @param levelsup
+ */
+ virtual void transform(const TDescriptor &feature,
+ WordId &id, WordValue &weight, NodeId* nid = NULL, int levelsup = 0) const;
+
+ /**
+ * Returns the word id associated to a feature
+ * @param feature
+ * @param id (out) word id
+ */
+ virtual void transform(const TDescriptor &feature, WordId &id) const;
+
+ /**
+ * Creates a level in the tree, under the parent, by running kmeans with
+ * a descriptor set, and recursively creates the subsequent levels too
+ * @param parent_id id of parent node
+ * @param descriptors descriptors to run the kmeans on
+ * @param current_level current level in the tree
+ */
+ void HKmeansStep(NodeId parent_id, const vector<pDescriptor> &descriptors,
+ int current_level);
+
+ /**
+ * Creates k clusters from the given descriptors with some seeding algorithm.
+ * @note In this class, kmeans++ is used, but this function should be
+ * overriden by inherited classes.
+ */
+ virtual void initiateClusters(const vector<pDescriptor> &descriptors,
+ vector<TDescriptor> &clusters) const;
+
+ /**
+ * Creates k clusters from the given descriptor sets by running the
+ * initial step of kmeans++
+ * @param descriptors
+ * @param clusters resulting clusters
+ */
+ void initiateClustersKMpp(const vector<pDescriptor> &descriptors,
+ vector<TDescriptor> &clusters) const;
+
+ /**
+ * Create the words of the vocabulary once the tree has been built
+ */
+ void createWords();
+
+ /**
+ * Sets the weights of the nodes of tree according to the given features.
+ * Before calling this function, the nodes and the words must be already
+ * created (by calling HKmeansStep and createWords)
+ * @param features
+ */
+ void setNodeWeights(const vector<vector<TDescriptor> > &features);
+
+protected:
+
+ /// Branching factor
+ int m_k;
+
+ /// Depth levels
+ int m_L;
+
+ /// Weighting method
+ WeightingType m_weighting;
+
+ /// Scoring method
+ ScoringType m_scoring;
+
+ /// Object for computing scores
+ GeneralScoring* m_scoring_object;
+
+ /// Tree nodes
+ std::vector<Node> m_nodes;
+
+ /// Words of the vocabulary (tree leaves)
+ /// this condition holds: m_words[wid]->word_id == wid
+ std::vector<Node*> m_words;
+
+};
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
+ (int k, int L, WeightingType weighting, ScoringType scoring)
+ : m_k(k), m_L(L), m_weighting(weighting), m_scoring(scoring),
+ m_scoring_object(NULL)
+{
+ createScoringObject();
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
+ (const std::string &filename): m_scoring_object(NULL)
+{
+ load(filename);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary
+ (const char *filename): m_scoring_object(NULL)
+{
+ load(filename);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::createScoringObject()
+{
+ delete m_scoring_object;
+ m_scoring_object = NULL;
+
+ switch(m_scoring)
+ {
+ case L1_NORM:
+ m_scoring_object = new L1Scoring;
+ break;
+
+ case L2_NORM:
+ m_scoring_object = new L2Scoring;
+ break;
+
+ case CHI_SQUARE:
+ m_scoring_object = new ChiSquareScoring;
+ break;
+
+ case KL:
+ m_scoring_object = new KLScoring;
+ break;
+
+ case BHATTACHARYYA:
+ m_scoring_object = new BhattacharyyaScoring;
+ break;
+
+ case DOT_PRODUCT:
+ m_scoring_object = new DotProductScoring;
+ break;
+
+ }
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::setScoringType(ScoringType type)
+{
+ m_scoring = type;
+ createScoringObject();
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::setWeightingType(WeightingType type)
+{
+ this->m_weighting = type;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor,F>::TemplatedVocabulary(
+ const TemplatedVocabulary<TDescriptor, F> &voc)
+ : m_scoring_object(NULL)
+{
+ *this = voc;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor,F>::~TemplatedVocabulary()
+{
+ delete m_scoring_object;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TemplatedVocabulary<TDescriptor, F>&
+TemplatedVocabulary<TDescriptor,F>::operator=
+ (const TemplatedVocabulary<TDescriptor, F> &voc)
+{
+ this->m_k = voc.m_k;
+ this->m_L = voc.m_L;
+ this->m_scoring = voc.m_scoring;
+ this->m_weighting = voc.m_weighting;
+
+ this->createScoringObject();
+
+ this->m_nodes.clear();
+ this->m_words.clear();
+
+ this->m_nodes = voc.m_nodes;
+ this->createWords();
+
+ return *this;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::create(
+ const std::vector<std::vector<TDescriptor> > &training_features)
+{
+ m_nodes.clear();
+ m_words.clear();
+
+ // expected_nodes = Sum_{i=0..L} ( k^i )
+ int expected_nodes =
+ (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
+
+ m_nodes.reserve(expected_nodes); // avoid allocations when creating the tree
+
+
+ vector<pDescriptor> features;
+ getFeatures(training_features, features);
+
+
+ // create root
+ m_nodes.push_back(Node(0)); // root
+
+ // create the tree
+ HKmeansStep(0, features, 1);
+
+ // create the words
+ createWords();
+
+ // and set the weight of each node of the tree
+ setNodeWeights(training_features);
+
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::create(
+ const std::vector<std::vector<TDescriptor> > &training_features,
+ int k, int L)
+{
+ m_k = k;
+ m_L = L;
+
+ create(training_features);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::create(
+ const std::vector<std::vector<TDescriptor> > &training_features,
+ int k, int L, WeightingType weighting, ScoringType scoring)
+{
+ m_k = k;
+ m_L = L;
+ m_weighting = weighting;
+ m_scoring = scoring;
+ createScoringObject();
+
+ create(training_features);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::getFeatures(
+ const vector<vector<TDescriptor> > &training_features,
+ vector<pDescriptor> &features) const
+{
+ features.resize(0);
+
+ typename vector<vector<TDescriptor> >::const_iterator vvit;
+ typename vector<TDescriptor>::const_iterator vit;
+ for(vvit = training_features.begin(); vvit != training_features.end(); ++vvit)
+ {
+ features.reserve(features.size() + vvit->size());
+ for(vit = vvit->begin(); vit != vvit->end(); ++vit)
+ {
+ features.push_back(&(*vit));
+ }
+ }
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::HKmeansStep(NodeId parent_id,
+ const vector<pDescriptor> &descriptors, int current_level)
+{
+ if(descriptors.empty()) return;
+
+ // features associated to each cluster
+ vector<TDescriptor> clusters;
+ vector<vector<unsigned int> > groups; // groups[i] = [j1, j2, ...]
+ // j1, j2, ... indices of descriptors associated to cluster i
+
+ clusters.reserve(m_k);
+ groups.reserve(m_k);
+
+ //const int msizes[] = { m_k, descriptors.size() };
+ //cv::SparseMat assoc(2, msizes, CV_8U);
+ //cv::SparseMat last_assoc(2, msizes, CV_8U);
+ //// assoc.row(cluster_idx).col(descriptor_idx) = 1 iif associated
+
+ if((int)descriptors.size() <= m_k)
+ {
+ // trivial case: one cluster per feature
+ groups.resize(descriptors.size());
+
+ for(unsigned int i = 0; i < descriptors.size(); i++)
+ {
+ groups[i].push_back(i);
+ clusters.push_back(*descriptors[i]);
+ }
+ }
+ else
+ {
+ // select clusters and groups with kmeans
+
+ bool first_time = true;
+ bool goon = true;
+
+ // to check if clusters move after iterations
+ vector<int> last_association, current_association;
+
+ while(goon)
+ {
+ // 1. Calculate clusters
+
+ if(first_time)
+ {
+ // random sample
+ initiateClusters(descriptors, clusters);
+ }
+ else
+ {
+ // calculate cluster centres
+
+ for(unsigned int c = 0; c < clusters.size(); ++c)
+ {
+ vector<pDescriptor> cluster_descriptors;
+ cluster_descriptors.reserve(groups[c].size());
+
+ /*
+ for(unsigned int d = 0; d < descriptors.size(); ++d)
+ {
+ if( assoc.find<unsigned char>(c, d) )
+ {
+ cluster_descriptors.push_back(descriptors[d]);
+ }
+ }
+ */
+
+ vector<unsigned int>::const_iterator vit;
+ for(vit = groups[c].begin(); vit != groups[c].end(); ++vit)
+ {
+ cluster_descriptors.push_back(descriptors[*vit]);
+ }
+
+
+ F::meanValue(cluster_descriptors, clusters[c]);
+ }
+
+ } // if(!first_time)
+
+ // 2. Associate features with clusters
+
+ // calculate distances to cluster centers
+ groups.clear();
+ groups.resize(clusters.size(), vector<unsigned int>());
+ current_association.resize(descriptors.size());
+
+ //assoc.clear();
+
+ typename vector<pDescriptor>::const_iterator fit;
+ //unsigned int d = 0;
+ for(fit = descriptors.begin(); fit != descriptors.end(); ++fit)//, ++d)
+ {
+ double best_dist = F::distance(*(*fit), clusters[0]);
+ unsigned int icluster = 0;
+
+ for(unsigned int c = 1; c < clusters.size(); ++c)
+ {
+ double dist = F::distance(*(*fit), clusters[c]);
+ if(dist < best_dist)
+ {
+ best_dist = dist;
+ icluster = c;
+ }
+ }
+
+ //assoc.ref<unsigned char>(icluster, d) = 1;
+
+ groups[icluster].push_back(fit - descriptors.begin());
+ current_association[ fit - descriptors.begin() ] = icluster;
+ }
+
+ // kmeans++ ensures all the clusters has any feature associated with them
+
+ // 3. check convergence
+ if(first_time)
+ {
+ first_time = false;
+ }
+ else
+ {
+ //goon = !eqUChar(last_assoc, assoc);
+
+ goon = false;
+ for(unsigned int i = 0; i < current_association.size(); i++)
+ {
+ if(current_association[i] != last_association[i]){
+ goon = true;
+ break;
+ }
+ }
+ }
+
+ if(goon)
+ {
+ // copy last feature-cluster association
+ last_association = current_association;
+ //last_assoc = assoc.clone();
+ }
+
+ } // while(goon)
+
+ } // if must run kmeans
+
+ // create nodes
+ for(unsigned int i = 0; i < clusters.size(); ++i)
+ {
+ NodeId id = m_nodes.size();
+ m_nodes.push_back(Node(id));
+ m_nodes.back().descriptor = clusters[i];
+ m_nodes.back().parent = parent_id;
+ m_nodes[parent_id].children.push_back(id);
+ }
+
+ // go on with the next level
+ if(current_level < m_L)
+ {
+ // iterate again with the resulting clusters
+ const vector<NodeId> &children_ids = m_nodes[parent_id].children;
+ for(unsigned int i = 0; i < clusters.size(); ++i)
+ {
+ NodeId id = children_ids[i];
+
+ vector<pDescriptor> child_features;
+ child_features.reserve(groups[i].size());
+
+ vector<unsigned int>::const_iterator vit;
+ for(vit = groups[i].begin(); vit != groups[i].end(); ++vit)
+ {
+ child_features.push_back(descriptors[*vit]);
+ }
+
+ if(child_features.size() > 1)
+ {
+ HKmeansStep(id, child_features, current_level + 1);
+ }
+ }
+ }
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor, F>::initiateClusters
+ (const vector<pDescriptor> &descriptors, vector<TDescriptor> &clusters) const
+{
+ initiateClustersKMpp(descriptors, clusters);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::initiateClustersKMpp(
+ const vector<pDescriptor> &pfeatures, vector<TDescriptor> &clusters) const
+{
+ // Implements kmeans++ seeding algorithm
+ // Algorithm:
+ // 1. Choose one center uniformly at random from among the data points.
+ // 2. For each data point x, compute D(x), the distance between x and the nearest
+ // center that has already been chosen.
+ // 3. Add one new data point as a center. Each point x is chosen with probability
+ // proportional to D(x)^2.
+ // 4. Repeat Steps 2 and 3 until k centers have been chosen.
+ // 5. Now that the initial centers have been chosen, proceed using standard k-means
+ // clustering.
+
+ DUtils::Random::SeedRandOnce();
+
+ clusters.resize(0);
+ clusters.reserve(m_k);
+ vector<double> min_dists(pfeatures.size(), std::numeric_limits<double>::max());
+
+ // 1.
+
+ int ifeature = DUtils::Random::RandomInt(0, pfeatures.size()-1);
+
+ // create first cluster
+ clusters.push_back(*pfeatures[ifeature]);
+
+ // compute the initial distances
+ typename vector<pDescriptor>::const_iterator fit;
+ vector<double>::iterator dit;
+ dit = min_dists.begin();
+ for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)
+ {
+ *dit = F::distance(*(*fit), clusters.back());
+ }
+
+ while((int)clusters.size() < m_k)
+ {
+ // 2.
+ dit = min_dists.begin();
+ for(fit = pfeatures.begin(); fit != pfeatures.end(); ++fit, ++dit)
+ {
+ if(*dit > 0)
+ {
+ double dist = F::distance(*(*fit), clusters.back());
+ if(dist < *dit) *dit = dist;
+ }
+ }
+
+ // 3.
+ double dist_sum = std::accumulate(min_dists.begin(), min_dists.end(), 0.0);
+
+ if(dist_sum > 0)
+ {
+ double cut_d;
+ do
+ {
+ cut_d = DUtils::Random::RandomValue<double>(0, dist_sum);
+ } while(cut_d == 0.0);
+
+ double d_up_now = 0;
+ for(dit = min_dists.begin(); dit != min_dists.end(); ++dit)
+ {
+ d_up_now += *dit;
+ if(d_up_now >= cut_d) break;
+ }
+
+ if(dit == min_dists.end())
+ ifeature = pfeatures.size()-1;
+ else
+ ifeature = dit - min_dists.begin();
+
+ clusters.push_back(*pfeatures[ifeature]);
+
+ } // if dist_sum > 0
+ else
+ break;
+
+ } // while(used_clusters < m_k)
+
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::createWords()
+{
+ m_words.resize(0);
+
+ if(!m_nodes.empty())
+ {
+ m_words.reserve( (int)pow((double)m_k, (double)m_L) );
+
+ typename vector<Node>::iterator nit;
+
+ nit = m_nodes.begin(); // ignore root
+ for(++nit; nit != m_nodes.end(); ++nit)
+ {
+ if(nit->isLeaf())
+ {
+ nit->word_id = m_words.size();
+ m_words.push_back( &(*nit) );
+ }
+ }
+ }
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::setNodeWeights
+ (const vector<vector<TDescriptor> > &training_features)
+{
+ const unsigned int NWords = m_words.size();
+ const unsigned int NDocs = training_features.size();
+
+ if(m_weighting == TF || m_weighting == BINARY)
+ {
+ // idf part must be 1 always
+ for(unsigned int i = 0; i < NWords; i++)
+ m_words[i]->weight = 1;
+ }
+ else if(m_weighting == IDF || m_weighting == TF_IDF)
+ {
+ // IDF and TF-IDF: we calculte the idf path now
+
+ // Note: this actually calculates the idf part of the tf-idf score.
+ // The complete tf-idf score is calculated in ::transform
+
+ vector<unsigned int> Ni(NWords, 0);
+ vector<bool> counted(NWords, false);
+
+ typename vector<vector<TDescriptor> >::const_iterator mit;
+ typename vector<TDescriptor>::const_iterator fit;
+
+ for(mit = training_features.begin(); mit != training_features.end(); ++mit)
+ {
+ fill(counted.begin(), counted.end(), false);
+
+ for(fit = mit->begin(); fit < mit->end(); ++fit)
+ {
+ WordId word_id;
+ transform(*fit, word_id);
+
+ if(!counted[word_id])
+ {
+ Ni[word_id]++;
+ counted[word_id] = true;
+ }
+ }
+ }
+
+ // set ln(N/Ni)
+ for(unsigned int i = 0; i < NWords; i++)
+ {
+ if(Ni[i] > 0)
+ {
+ m_words[i]->weight = log((double)NDocs / (double)Ni[i]);
+ }// else // This cannot occur if using kmeans++
+ }
+
+ }
+
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+inline unsigned int TemplatedVocabulary<TDescriptor,F>::size() const
+{
+ return m_words.size();
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+inline bool TemplatedVocabulary<TDescriptor,F>::empty() const
+{
+ return m_words.empty();
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+float TemplatedVocabulary<TDescriptor,F>::getEffectiveLevels() const
+{
+ long sum = 0;
+ typename std::vector<Node*>::const_iterator wit;
+ for(wit = m_words.begin(); wit != m_words.end(); ++wit)
+ {
+ const Node *p = *wit;
+
+ for(; p->id != 0; sum++) p = &m_nodes[p->parent];
+ }
+
+ return (float)((double)sum / (double)m_words.size());
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+TDescriptor TemplatedVocabulary<TDescriptor,F>::getWord(WordId wid) const
+{
+ return m_words[wid]->descriptor;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+WordValue TemplatedVocabulary<TDescriptor, F>::getWordWeight(WordId wid) const
+{
+ return m_words[wid]->weight;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+WordId TemplatedVocabulary<TDescriptor, F>::transform
+ (const TDescriptor& feature) const
+{
+ if(empty())
+ {
+ return 0;
+ }
+
+ WordId wid;
+ transform(feature, wid);
+ return wid;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::transform(
+ const std::vector<TDescriptor>& features, BowVector &v) const
+{
+ v.clear();
+
+ if(empty())
+ {
+ return;
+ }
+
+ // normalize
+ LNorm norm;
+ bool must = m_scoring_object->mustNormalize(norm);
+
+ typename vector<TDescriptor>::const_iterator fit;
+
+ if(m_weighting == TF || m_weighting == TF_IDF)
+ {
+ for(fit = features.begin(); fit < features.end(); ++fit)
+ {
+ WordId id;
+ WordValue w;
+ // w is the idf value if TF_IDF, 1 if TF
+
+ transform(*fit, id, w);
+
+ // not stopped
+ if(w > 0) v.addWeight(id, w);
+ }
+
+ if(!v.empty() && !must)
+ {
+ // unnecessary when normalizing
+ const double nd = v.size();
+ for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
+ vit->second /= nd;
+ }
+
+ }
+ else // IDF || BINARY
+ {
+ for(fit = features.begin(); fit < features.end(); ++fit)
+ {
+ WordId id;
+ WordValue w;
+ // w is idf if IDF, or 1 if BINARY
+
+ transform(*fit, id, w);
+
+ // not stopped
+ if(w > 0) v.addIfNotExist(id, w);
+
+ } // if add_features
+ } // if m_weighting == ...
+
+ if(must) v.normalize(norm);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::transform(
+ const std::vector<TDescriptor>& features,
+ BowVector &v, FeatureVector &fv, int levelsup) const
+{
+ v.clear();
+ fv.clear();
+
+ if(empty()) // safe for subclasses
+ {
+ return;
+ }
+
+ // normalize
+ LNorm norm;
+ bool must = m_scoring_object->mustNormalize(norm);
+
+ typename vector<TDescriptor>::const_iterator fit;
+
+ if(m_weighting == TF || m_weighting == TF_IDF)
+ {
+ unsigned int i_feature = 0;
+ for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
+ {
+ WordId id;
+ NodeId nid;
+ WordValue w;
+ // w is the idf value if TF_IDF, 1 if TF
+
+ transform(*fit, id, w, &nid, levelsup);
+
+ if(w > 0) // not stopped
+ {
+ v.addWeight(id, w);
+ fv.addFeature(nid, i_feature);
+ }
+ }
+
+ if(!v.empty() && !must)
+ {
+ // unnecessary when normalizing
+ const double nd = v.size();
+ for(BowVector::iterator vit = v.begin(); vit != v.end(); vit++)
+ vit->second /= nd;
+ }
+
+ }
+ else // IDF || BINARY
+ {
+ unsigned int i_feature = 0;
+ for(fit = features.begin(); fit < features.end(); ++fit, ++i_feature)
+ {
+ WordId id;
+ NodeId nid;
+ WordValue w;
+ // w is idf if IDF, or 1 if BINARY
+
+ transform(*fit, id, w, &nid, levelsup);
+
+ if(w > 0) // not stopped
+ {
+ v.addIfNotExist(id, w);
+ fv.addFeature(nid, i_feature);
+ }
+ }
+ } // if m_weighting == ...
+
+ if(must) v.normalize(norm);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+inline double TemplatedVocabulary<TDescriptor,F>::score
+ (const BowVector &v1, const BowVector &v2) const
+{
+ return m_scoring_object->score(v1, v2);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::transform
+ (const TDescriptor &feature, WordId &id) const
+{
+ WordValue weight;
+ transform(feature, id, weight);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::transform(const TDescriptor &feature,
+ WordId &word_id, WordValue &weight, NodeId *nid, int levelsup) const
+{
+ // propagate the feature down the tree
+ vector<NodeId> nodes;
+ typename vector<NodeId>::const_iterator nit;
+
+ // level at which the node must be stored in nid, if given
+ const int nid_level = m_L - levelsup;
+ if(nid_level <= 0 && nid != NULL) *nid = 0; // root
+
+ NodeId final_id = 0; // root
+ int current_level = 0;
+
+ do
+ {
+ ++current_level;
+ nodes = m_nodes[final_id].children;
+ final_id = nodes[0];
+
+ double best_d = F::distance(feature, m_nodes[final_id].descriptor);
+
+ for(nit = nodes.begin() + 1; nit != nodes.end(); ++nit)
+ {
+ NodeId id = *nit;
+ double d = F::distance(feature, m_nodes[id].descriptor);
+ if(d < best_d)
+ {
+ best_d = d;
+ final_id = id;
+ }
+ }
+
+ if(nid != NULL && current_level == nid_level)
+ *nid = final_id;
+
+ } while( !m_nodes[final_id].isLeaf() );
+
+ // turn node id into word id
+ word_id = m_nodes[final_id].word_id;
+ weight = m_nodes[final_id].weight;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+NodeId TemplatedVocabulary<TDescriptor,F>::getParentNode
+ (WordId wid, int levelsup) const
+{
+ NodeId ret = m_words[wid]->id; // node id
+ while(levelsup > 0 && ret != 0) // ret == 0 --> root
+ {
+ --levelsup;
+ ret = m_nodes[ret].parent;
+ }
+ return ret;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::getWordsFromNode
+ (NodeId nid, std::vector<WordId> &words) const
+{
+ words.clear();
+
+ if(m_nodes[nid].isLeaf())
+ {
+ words.push_back(m_nodes[nid].word_id);
+ }
+ else
+ {
+ words.reserve(m_k); // ^1, ^2, ...
+
+ vector<NodeId> parents;
+ parents.push_back(nid);
+
+ while(!parents.empty())
+ {
+ NodeId parentid = parents.back();
+ parents.pop_back();
+
+ const vector<NodeId> &child_ids = m_nodes[parentid].children;
+ vector<NodeId>::const_iterator cit;
+
+ for(cit = child_ids.begin(); cit != child_ids.end(); ++cit)
+ {
+ const Node &child_node = m_nodes[*cit];
+
+ if(child_node.isLeaf())
+ words.push_back(child_node.word_id);
+ else
+ parents.push_back(*cit);
+
+ } // for each child
+ } // while !parents.empty
+ }
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+int TemplatedVocabulary<TDescriptor,F>::stopWords(double minWeight)
+{
+ int c = 0;
+ typename vector<Node*>::iterator wit;
+ for(wit = m_words.begin(); wit != m_words.end(); ++wit)
+ {
+ if((*wit)->weight < minWeight)
+ {
+ ++c;
+ (*wit)->weight = 0;
+ }
+ }
+ return c;
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+bool TemplatedVocabulary<TDescriptor,F>::loadFromTextFile(const std::string &filename)
+{
+ ifstream f;
+ f.open(filename.c_str());
+
+ if(f.eof())
+ return false;
+
+ m_words.clear();
+ m_nodes.clear();
+
+ string s;
+ getline(f,s);
+ stringstream ss;
+ ss << s;
+ ss >> m_k;
+ ss >> m_L;
+ int n1, n2;
+ ss >> n1;
+ ss >> n2;
+
+ if(m_k<0 || m_k>20 || m_L<1 || m_L>10 || n1<0 || n1>5 || n2<0 || n2>3)
+ {
+ std::cerr << "Vocabulary loading failure: This is not a correct text file!" << endl;
+ return false;
+ }
+
+ m_scoring = (ScoringType)n1;
+ m_weighting = (WeightingType)n2;
+ createScoringObject();
+
+ // nodes
+ int expected_nodes =
+ (int)((pow((double)m_k, (double)m_L + 1) - 1)/(m_k - 1));
+ m_nodes.reserve(expected_nodes);
+
+ m_words.reserve(pow((double)m_k, (double)m_L + 1));
+
+ m_nodes.resize(1);
+ m_nodes[0].id = 0;
+ while(!f.eof())
+ {
+ string snode;
+ getline(f,snode);
+ stringstream ssnode;
+ ssnode << snode;
+
+ int nid = m_nodes.size();
+ m_nodes.resize(m_nodes.size()+1);
+ m_nodes[nid].id = nid;
+
+ int pid ;
+ ssnode >> pid;
+ m_nodes[nid].parent = pid;
+ m_nodes[pid].children.push_back(nid);
+
+ int nIsLeaf;
+ ssnode >> nIsLeaf;
+
+ stringstream ssd;
+ for(int iD=0;iD<F::L;iD++)
+ {
+ string sElement;
+ ssnode >> sElement;
+ ssd << sElement << " ";
+ }
+ F::fromString(m_nodes[nid].descriptor, ssd.str());
+
+ ssnode >> m_nodes[nid].weight;
+
+ if(nIsLeaf>0)
+ {
+ int wid = m_words.size();
+ m_words.resize(wid+1);
+
+ m_nodes[nid].word_id = wid;
+ m_words[wid] = &m_nodes[nid];
+ }
+ else
+ {
+ m_nodes[nid].children.reserve(m_k);
+ }
+ }
+
+ return true;
+
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::saveToTextFile(const std::string &filename) const
+{
+ fstream f;
+ f.open(filename.c_str(),ios_base::out);
+ f << m_k << " " << m_L << " " << " " << m_scoring << " " << m_weighting << endl;
+
+ for(size_t i=1; i<m_nodes.size();i++)
+ {
+ const Node& node = m_nodes[i];
+
+ f << node.parent << " ";
+ if(node.isLeaf())
+ f << 1 << " ";
+ else
+ f << 0 << " ";
+
+ f << F::toString(node.descriptor) << " " << (double)node.weight << endl;
+ }
+
+ f.close();
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::save(const std::string &filename) const
+{
+ cv::FileStorage fs(filename.c_str(), cv::FileStorage::WRITE);
+ if(!fs.isOpened()) throw string("Could not open file ") + filename;
+
+ save(fs);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::load(const std::string &filename)
+{
+ cv::FileStorage fs(filename.c_str(), cv::FileStorage::READ);
+ if(!fs.isOpened()) throw string("Could not open file ") + filename;
+
+ this->load(fs);
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::save(cv::FileStorage &f,
+ const std::string &name) const
+{
+ // Format YAML:
+ // vocabulary
+ // {
+ // k:
+ // L:
+ // scoringType:
+ // weightingType:
+ // nodes
+ // [
+ // {
+ // nodeId:
+ // parentId:
+ // weight:
+ // descriptor:
+ // }
+ // ]
+ // words
+ // [
+ // {
+ // wordId:
+ // nodeId:
+ // }
+ // ]
+ // }
+ //
+ // The root node (index 0) is not included in the node vector
+ //
+
+ f << name << "{";
+
+ f << "k" << m_k;
+ f << "L" << m_L;
+ f << "scoringType" << m_scoring;
+ f << "weightingType" << m_weighting;
+
+ // tree
+ f << "nodes" << "[";
+ vector<NodeId> parents, children;
+ vector<NodeId>::const_iterator pit;
+
+ parents.push_back(0); // root
+
+ while(!parents.empty())
+ {
+ NodeId pid = parents.back();
+ parents.pop_back();
+
+ const Node& parent = m_nodes[pid];
+ children = parent.children;
+
+ for(pit = children.begin(); pit != children.end(); pit++)
+ {
+ const Node& child = m_nodes[*pit];
+
+ // save node data
+ f << "{:";
+ f << "nodeId" << (int)child.id;
+ f << "parentId" << (int)pid;
+ f << "weight" << (double)child.weight;
+ f << "descriptor" << F::toString(child.descriptor);
+ f << "}";
+
+ // add to parent list
+ if(!child.isLeaf())
+ {
+ parents.push_back(*pit);
+ }
+ }
+ }
+
+ f << "]"; // nodes
+
+ // words
+ f << "words" << "[";
+
+ typename vector<Node*>::const_iterator wit;
+ for(wit = m_words.begin(); wit != m_words.end(); wit++)
+ {
+ WordId id = wit - m_words.begin();
+ f << "{:";
+ f << "wordId" << (int)id;
+ f << "nodeId" << (int)(*wit)->id;
+ f << "}";
+ }
+
+ f << "]"; // words
+
+ f << "}";
+
+}
+
+// --------------------------------------------------------------------------
+
+template<class TDescriptor, class F>
+void TemplatedVocabulary<TDescriptor,F>::load(const cv::FileStorage &fs,
+ const std::string &name)
+{
+ m_words.clear();
+ m_nodes.clear();
+
+ cv::FileNode fvoc = fs[name];
+
+ m_k = (int)fvoc["k"];
+ m_L = (int)fvoc["L"];
+ m_scoring = (ScoringType)((int)fvoc["scoringType"]);
+ m_weighting = (WeightingType)((int)fvoc["weightingType"]);
+
+ createScoringObject();
+
+ // nodes
+ cv::FileNode fn = fvoc["nodes"];
+
+ m_nodes.resize(fn.size() + 1); // +1 to include root
+ m_nodes[0].id = 0;
+
+ for(unsigned int i = 0; i < fn.size(); ++i)
+ {
+ NodeId nid = (int)fn[i]["nodeId"];
+ NodeId pid = (int)fn[i]["parentId"];
+ WordValue weight = (WordValue)fn[i]["weight"];
+ string d = (string)fn[i]["descriptor"];
+
+ m_nodes[nid].id = nid;
+ m_nodes[nid].parent = pid;
+ m_nodes[nid].weight = weight;
+ m_nodes[pid].children.push_back(nid);
+
+ F::fromString(m_nodes[nid].descriptor, d);
+ }
+
+ // words
+ fn = fvoc["words"];
+
+ m_words.resize(fn.size());
+
+ for(unsigned int i = 0; i < fn.size(); ++i)
+ {
+ NodeId wid = (int)fn[i]["wordId"];
+ NodeId nid = (int)fn[i]["nodeId"];
+
+ m_nodes[nid].word_id = wid;
+ m_words[wid] = &m_nodes[nid];
+ }
+}
+
+// --------------------------------------------------------------------------
+
+/**
+ * Writes printable information of the vocabulary
+ * @param os stream to write to
+ * @param voc
+ */
+template<class TDescriptor, class F>
+std::ostream& operator<<(std::ostream &os,
+ const TemplatedVocabulary<TDescriptor,F> &voc)
+{
+ os << "Vocabulary: k = " << voc.getBranchingFactor()
+ << ", L = " << voc.getDepthLevels()
+ << ", Weighting = ";
+
+ switch(voc.getWeightingType())
+ {
+ case TF_IDF: os << "tf-idf"; break;
+ case TF: os << "tf"; break;
+ case IDF: os << "idf"; break;
+ case BINARY: os << "binary"; break;
+ }
+
+ os << ", Scoring = ";
+ switch(voc.getScoringType())
+ {
+ case L1_NORM: os << "L1-norm"; break;
+ case L2_NORM: os << "L2-norm"; break;
+ case CHI_SQUARE: os << "Chi square distance"; break;
+ case KL: os << "KL-divergence"; break;
+ case BHATTACHARYYA: os << "Bhattacharyya coefficient"; break;
+ case DOT_PRODUCT: os << "Dot product"; break;
+ }
+
+ os << ", Number of words = " << voc.size();
+
+ return os;
+}
+
+} // namespace DBoW2
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..79df2a697822cc9610f49d6aad0b7116cc32f246
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.cpp
@@ -0,0 +1,129 @@
+/*
+ * File: Random.cpp
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include "Random.h"
+#include "Timestamp.h"
+#include <cstdlib>
+using namespace std;
+
+bool DUtils::Random::m_already_seeded = false;
+
+void DUtils::Random::SeedRand(){
+ Timestamp time;
+ time.setToCurrentTime();
+ srand((unsigned)time.getFloatTime());
+}
+
+void DUtils::Random::SeedRandOnce()
+{
+ if(!m_already_seeded)
+ {
+ DUtils::Random::SeedRand();
+ m_already_seeded = true;
+ }
+}
+
+void DUtils::Random::SeedRand(int seed)
+{
+ srand(seed);
+}
+
+void DUtils::Random::SeedRandOnce(int seed)
+{
+ if(!m_already_seeded)
+ {
+ DUtils::Random::SeedRand(seed);
+ m_already_seeded = true;
+ }
+}
+
+int DUtils::Random::RandomInt(int min, int max){
+ int d = max - min + 1;
+ return int(((double)rand()/((double)RAND_MAX + 1.0)) * d) + min;
+}
+
+// ---------------------------------------------------------------------------
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer(int min, int max)
+{
+ if(min <= max)
+ {
+ m_min = min;
+ m_max = max;
+ }
+ else
+ {
+ m_min = max;
+ m_max = min;
+ }
+
+ createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer::UnrepeatedRandomizer
+ (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+ *this = rnd;
+}
+
+// ---------------------------------------------------------------------------
+
+int DUtils::Random::UnrepeatedRandomizer::get()
+{
+ if(empty()) createValues();
+
+ DUtils::Random::SeedRandOnce();
+
+ int k = DUtils::Random::RandomInt(0, m_values.size()-1);
+ int ret = m_values[k];
+ m_values[k] = m_values.back();
+ m_values.pop_back();
+
+ return ret;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::createValues()
+{
+ int n = m_max - m_min + 1;
+
+ m_values.resize(n);
+ for(int i = 0; i < n; ++i) m_values[i] = m_min + i;
+}
+
+// ---------------------------------------------------------------------------
+
+void DUtils::Random::UnrepeatedRandomizer::reset()
+{
+ if((int)m_values.size() != m_max - m_min + 1) createValues();
+}
+
+// ---------------------------------------------------------------------------
+
+DUtils::Random::UnrepeatedRandomizer&
+DUtils::Random::UnrepeatedRandomizer::operator=
+ (const DUtils::Random::UnrepeatedRandomizer& rnd)
+{
+ if(this != &rnd)
+ {
+ this->m_min = rnd.m_min;
+ this->m_max = rnd.m_max;
+ this->m_values = rnd.m_values;
+ }
+ return *this;
+}
+
+// ---------------------------------------------------------------------------
+
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.h
new file mode 100644
index 0000000000000000000000000000000000000000..5115dc47c2f13c564241e3bda4eca29954518029
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Random.h
@@ -0,0 +1,184 @@
+/*
+ * File: Random.h
+ * Project: DUtils library
+ * Author: Dorian Galvez-Lopez
+ * Date: April 2010, November 2011
+ * Description: manages pseudo-random numbers
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#pragma once
+#ifndef __D_RANDOM__
+#define __D_RANDOM__
+
+#include <cstdlib>
+#include <vector>
+
+namespace DUtils {
+
+/// Functions to generate pseudo-random numbers
+class Random
+{
+public:
+ class UnrepeatedRandomizer;
+
+public:
+ /**
+ * Sets the random number seed to the current time
+ */
+ static void SeedRand();
+
+ /**
+ * Sets the random number seed to the current time only the first
+ * time this function is called
+ */
+ static void SeedRandOnce();
+
+ /**
+ * Sets the given random number seed
+ * @param seed
+ */
+ static void SeedRand(int seed);
+
+ /**
+ * Sets the given random number seed only the first time this function
+ * is called
+ * @param seed
+ */
+ static void SeedRandOnce(int seed);
+
+ /**
+ * Returns a random number in the range [0..1]
+ * @return random T number in [0..1]
+ */
+ template <class T>
+ static T RandomValue(){
+ return (T)rand()/(T)RAND_MAX;
+ }
+
+ /**
+ * Returns a random number in the range [min..max]
+ * @param min
+ * @param max
+ * @return random T number in [min..max]
+ */
+ template <class T>
+ static T RandomValue(T min, T max){
+ return Random::RandomValue<T>() * (max - min) + min;
+ }
+
+ /**
+ * Returns a random int in the range [min..max]
+ * @param min
+ * @param max
+ * @return random int in [min..max]
+ */
+ static int RandomInt(int min, int max);
+
+ /**
+ * Returns a random number from a gaussian distribution
+ * @param mean
+ * @param sigma standard deviation
+ */
+ template <class T>
+ static T RandomGaussianValue(T mean, T sigma)
+ {
+ // Box-Muller transformation
+ T x1, x2, w, y1;
+
+ do {
+ x1 = (T)2. * RandomValue<T>() - (T)1.;
+ x2 = (T)2. * RandomValue<T>() - (T)1.;
+ w = x1 * x1 + x2 * x2;
+ } while ( w >= (T)1. || w == (T)0. );
+
+ w = sqrt( ((T)-2.0 * log( w ) ) / w );
+ y1 = x1 * w;
+
+ return( mean + y1 * sigma );
+ }
+
+private:
+
+ /// If SeedRandOnce() or SeedRandOnce(int) have already been called
+ static bool m_already_seeded;
+
+};
+
+// ---------------------------------------------------------------------------
+
+/// Provides pseudo-random numbers with no repetitions
+class Random::UnrepeatedRandomizer
+{
+public:
+
+ /**
+ * Creates a randomizer that returns numbers in the range [min, max]
+ * @param min
+ * @param max
+ */
+ UnrepeatedRandomizer(int min, int max);
+ ~UnrepeatedRandomizer(){}
+
+ /**
+ * Copies a randomizer
+ * @param rnd
+ */
+ UnrepeatedRandomizer(const UnrepeatedRandomizer& rnd);
+
+ /**
+ * Copies a randomizer
+ * @param rnd
+ */
+ UnrepeatedRandomizer& operator=(const UnrepeatedRandomizer& rnd);
+
+ /**
+ * Returns a random number not given before. If all the possible values
+ * were already given, the process starts again
+ * @return unrepeated random number
+ */
+ int get();
+
+ /**
+ * Returns whether all the possible values between min and max were
+ * already given. If get() is called when empty() is true, the behaviour
+ * is the same than after creating the randomizer
+ * @return true iff all the values were returned
+ */
+ inline bool empty() const { return m_values.empty(); }
+
+ /**
+ * Returns the number of values still to be returned
+ * @return amount of values to return
+ */
+ inline unsigned int left() const { return m_values.size(); }
+
+ /**
+ * Resets the randomizer as it were just created
+ */
+ void reset();
+
+protected:
+
+ /**
+ * Creates the vector with available values
+ */
+ void createValues();
+
+protected:
+
+ /// Min of range of values
+ int m_min;
+ /// Max of range of values
+ int m_max;
+
+ /// Available values
+ std::vector<int> m_values;
+
+};
+
+}
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.cpp b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4551839f177e35c1d702088cdb9f6086d20f7545
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.cpp
@@ -0,0 +1,246 @@
+/*
+ * File: Timestamp.cpp
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ *
+ * Note: in windows, this class has a 1ms resolution
+ *
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#include <cstdio>
+#include <cstdlib>
+#include <ctime>
+#include <cmath>
+#include <sstream>
+#include <iomanip>
+
+#ifdef _WIN32
+#ifndef WIN32
+#define WIN32
+#endif
+#endif
+
+#ifdef WIN32
+#include <sys/timeb.h>
+#define sprintf sprintf_s
+#else
+#include <sys/time.h>
+#endif
+
+#include "Timestamp.h"
+
+using namespace std;
+
+using namespace DUtils;
+
+Timestamp::Timestamp(Timestamp::tOptions option)
+{
+ if(option & CURRENT_TIME)
+ setToCurrentTime();
+ else if(option & ZERO)
+ setTime(0.);
+}
+
+Timestamp::~Timestamp(void)
+{
+}
+
+bool Timestamp::empty() const
+{
+ return m_secs == 0 && m_usecs == 0;
+}
+
+void Timestamp::setToCurrentTime(){
+
+#ifdef WIN32
+ struct __timeb32 timebuffer;
+ _ftime32_s( &timebuffer ); // C4996
+ // Note: _ftime is deprecated; consider using _ftime_s instead
+ m_secs = timebuffer.time;
+ m_usecs = timebuffer.millitm * 1000;
+#else
+ struct timeval now;
+ gettimeofday(&now, NULL);
+ m_secs = now.tv_sec;
+ m_usecs = now.tv_usec;
+#endif
+
+}
+
+void Timestamp::setTime(const string &stime){
+ string::size_type p = stime.find('.');
+ if(p == string::npos){
+ m_secs = atol(stime.c_str());
+ m_usecs = 0;
+ }else{
+ m_secs = atol(stime.substr(0, p).c_str());
+
+ string s_usecs = stime.substr(p+1, 6);
+ m_usecs = atol(stime.substr(p+1).c_str());
+ m_usecs *= (unsigned long)pow(10.0, double(6 - s_usecs.length()));
+ }
+}
+
+void Timestamp::setTime(double s)
+{
+ m_secs = (unsigned long)s;
+ m_usecs = (s - (double)m_secs) * 1e6;
+}
+
+double Timestamp::getFloatTime() const {
+ return double(m_secs) + double(m_usecs)/1000000.0;
+}
+
+string Timestamp::getStringTime() const {
+ char buf[32];
+ sprintf(buf, "%.6lf", this->getFloatTime());
+ return string(buf);
+}
+
+double Timestamp::operator- (const Timestamp &t) const {
+ return this->getFloatTime() - t.getFloatTime();
+}
+
+Timestamp& Timestamp::operator+= (double s)
+{
+ *this = *this + s;
+ return *this;
+}
+
+Timestamp& Timestamp::operator-= (double s)
+{
+ *this = *this - s;
+ return *this;
+}
+
+Timestamp Timestamp::operator+ (double s) const
+{
+ unsigned long secs = (long)floor(s);
+ unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+ return this->plus(secs, usecs);
+}
+
+Timestamp Timestamp::plus(unsigned long secs, unsigned long usecs) const
+{
+ Timestamp t;
+
+ const unsigned long max = 1000000ul;
+
+ if(m_usecs + usecs >= max)
+ t.setTime(m_secs + secs + 1, m_usecs + usecs - max);
+ else
+ t.setTime(m_secs + secs, m_usecs + usecs);
+
+ return t;
+}
+
+Timestamp Timestamp::operator- (double s) const
+{
+ unsigned long secs = (long)floor(s);
+ unsigned long usecs = (long)((s - (double)secs) * 1e6);
+
+ return this->minus(secs, usecs);
+}
+
+Timestamp Timestamp::minus(unsigned long secs, unsigned long usecs) const
+{
+ Timestamp t;
+
+ const unsigned long max = 1000000ul;
+
+ if(m_usecs < usecs)
+ t.setTime(m_secs - secs - 1, max - (usecs - m_usecs));
+ else
+ t.setTime(m_secs - secs, m_usecs - usecs);
+
+ return t;
+}
+
+bool Timestamp::operator> (const Timestamp &t) const
+{
+ if(m_secs > t.m_secs) return true;
+ else if(m_secs == t.m_secs) return m_usecs > t.m_usecs;
+ else return false;
+}
+
+bool Timestamp::operator>= (const Timestamp &t) const
+{
+ if(m_secs > t.m_secs) return true;
+ else if(m_secs == t.m_secs) return m_usecs >= t.m_usecs;
+ else return false;
+}
+
+bool Timestamp::operator< (const Timestamp &t) const
+{
+ if(m_secs < t.m_secs) return true;
+ else if(m_secs == t.m_secs) return m_usecs < t.m_usecs;
+ else return false;
+}
+
+bool Timestamp::operator<= (const Timestamp &t) const
+{
+ if(m_secs < t.m_secs) return true;
+ else if(m_secs == t.m_secs) return m_usecs <= t.m_usecs;
+ else return false;
+}
+
+bool Timestamp::operator== (const Timestamp &t) const
+{
+ return(m_secs == t.m_secs && m_usecs == t.m_usecs);
+}
+
+
+string Timestamp::Format(bool machine_friendly) const
+{
+ struct tm tm_time;
+
+ time_t t = (time_t)getFloatTime();
+
+#ifdef WIN32
+ localtime_s(&tm_time, &t);
+#else
+ localtime_r(&t, &tm_time);
+#endif
+
+ char buffer[128];
+
+ if(machine_friendly)
+ {
+ strftime(buffer, 128, "%Y%m%d_%H%M%S", &tm_time);
+ }
+ else
+ {
+ strftime(buffer, 128, "%c", &tm_time); // Thu Aug 23 14:55:02 2001
+ }
+
+ return string(buffer);
+}
+
+string Timestamp::Format(double s) {
+ int days = int(s / (24. * 3600.0));
+ s -= days * (24. * 3600.0);
+ int hours = int(s / 3600.0);
+ s -= hours * 3600;
+ int minutes = int(s / 60.0);
+ s -= minutes * 60;
+ int seconds = int(s);
+ int ms = int((s - seconds)*1e6);
+
+ stringstream ss;
+ ss.fill('0');
+ bool b;
+ if((b = (days > 0))) ss << days << "d ";
+ if((b = (b || hours > 0))) ss << setw(2) << hours << ":";
+ if((b = (b || minutes > 0))) ss << setw(2) << minutes << ":";
+ if(b) ss << setw(2);
+ ss << seconds;
+ if(!b) ss << "." << setw(6) << ms;
+
+ return ss.str();
+}
+
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.h b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.h
new file mode 100644
index 0000000000000000000000000000000000000000..b92f89f872988600e4d81257aa613d5687a6b1b6
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/DUtils/Timestamp.h
@@ -0,0 +1,204 @@
+/*
+ * File: Timestamp.h
+ * Author: Dorian Galvez-Lopez
+ * Date: March 2009
+ * Description: timestamping functions
+ * License: see the LICENSE.txt file
+ *
+ */
+
+#ifndef __D_TIMESTAMP__
+#define __D_TIMESTAMP__
+
+#include <iostream>
+using namespace std;
+
+namespace DUtils {
+
+/// Timestamp
+class Timestamp
+{
+public:
+
+ /// Options to initiate a timestamp
+ enum tOptions
+ {
+ NONE = 0,
+ CURRENT_TIME = 0x1,
+ ZERO = 0x2
+ };
+
+public:
+
+ /**
+ * Creates a timestamp
+ * @param option option to set the initial time stamp
+ */
+ Timestamp(Timestamp::tOptions option = NONE);
+
+ /**
+ * Destructor
+ */
+ virtual ~Timestamp(void);
+
+ /**
+ * Says if the timestamp is "empty": seconds and usecs are both 0, as
+ * when initiated with the ZERO flag
+ * @return true iif secs == usecs == 0
+ */
+ bool empty() const;
+
+ /**
+ * Sets this instance to the current time
+ */
+ void setToCurrentTime();
+
+ /**
+ * Sets the timestamp from seconds and microseconds
+ * @param secs: seconds
+ * @param usecs: microseconds
+ */
+ inline void setTime(unsigned long secs, unsigned long usecs){
+ m_secs = secs;
+ m_usecs = usecs;
+ }
+
+ /**
+ * Returns the timestamp in seconds and microseconds
+ * @param secs seconds
+ * @param usecs microseconds
+ */
+ inline void getTime(unsigned long &secs, unsigned long &usecs) const
+ {
+ secs = m_secs;
+ usecs = m_usecs;
+ }
+
+ /**
+ * Sets the timestamp from a string with the time in seconds
+ * @param stime: string such as "1235603336.036609"
+ */
+ void setTime(const string &stime);
+
+ /**
+ * Sets the timestamp from a number of seconds from the epoch
+ * @param s seconds from the epoch
+ */
+ void setTime(double s);
+
+ /**
+ * Returns this timestamp as the number of seconds in (long) float format
+ */
+ double getFloatTime() const;
+
+ /**
+ * Returns this timestamp as the number of seconds in fixed length string format
+ */
+ string getStringTime() const;
+
+ /**
+ * Returns the difference in seconds between this timestamp (greater) and t (smaller)
+ * If the order is swapped, a negative number is returned
+ * @param t: timestamp to subtract from this timestamp
+ * @return difference in seconds
+ */
+ double operator- (const Timestamp &t) const;
+
+ /**
+ * Returns a copy of this timestamp + s seconds + us microseconds
+ * @param s seconds
+ * @param us microseconds
+ */
+ Timestamp plus(unsigned long s, unsigned long us) const;
+
+ /**
+ * Returns a copy of this timestamp - s seconds - us microseconds
+ * @param s seconds
+ * @param us microseconds
+ */
+ Timestamp minus(unsigned long s, unsigned long us) const;
+
+ /**
+ * Adds s seconds to this timestamp and returns a reference to itself
+ * @param s seconds
+ * @return reference to this timestamp
+ */
+ Timestamp& operator+= (double s);
+
+ /**
+ * Substracts s seconds to this timestamp and returns a reference to itself
+ * @param s seconds
+ * @return reference to this timestamp
+ */
+ Timestamp& operator-= (double s);
+
+ /**
+ * Returns a copy of this timestamp + s seconds
+ * @param s: seconds
+ */
+ Timestamp operator+ (double s) const;
+
+ /**
+ * Returns a copy of this timestamp - s seconds
+ * @param s: seconds
+ */
+ Timestamp operator- (double s) const;
+
+ /**
+ * Returns whether this timestamp is at the future of t
+ * @param t
+ */
+ bool operator> (const Timestamp &t) const;
+
+ /**
+ * Returns whether this timestamp is at the future of (or is the same as) t
+ * @param t
+ */
+ bool operator>= (const Timestamp &t) const;
+
+ /**
+ * Returns whether this timestamp and t represent the same instant
+ * @param t
+ */
+ bool operator== (const Timestamp &t) const;
+
+ /**
+ * Returns whether this timestamp is at the past of t
+ * @param t
+ */
+ bool operator< (const Timestamp &t) const;
+
+ /**
+ * Returns whether this timestamp is at the past of (or is the same as) t
+ * @param t
+ */
+ bool operator<= (const Timestamp &t) const;
+
+ /**
+ * Returns the timestamp in a human-readable string
+ * @param machine_friendly if true, the returned string is formatted
+ * to yyyymmdd_hhmmss, without weekday or spaces
+ * @note This has not been tested under Windows
+ * @note The timestamp is truncated to seconds
+ */
+ string Format(bool machine_friendly = false) const;
+
+ /**
+ * Returns a string version of the elapsed time in seconds, with the format
+ * xd hh:mm:ss, hh:mm:ss, mm:ss or s.us
+ * @param s: elapsed seconds (given by getFloatTime) to format
+ */
+ static string Format(double s);
+
+
+protected:
+ /// Seconds
+ unsigned long m_secs; // seconds
+ /// Microseconds
+ unsigned long m_usecs; // microseconds
+};
+
+}
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/LICENSE.txt b/externals/ORB_SLAM2/Thirdparty/DBoW2/LICENSE.txt
new file mode 100644
index 0000000000000000000000000000000000000000..18e3160b9cfd9846855365eb9ae69316c67654b3
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/LICENSE.txt
@@ -0,0 +1,44 @@
+DBoW2: bag-of-words library for C++ with generic descriptors
+
+Copyright (c) 2015 Dorian Galvez-Lopez <http://doriangalvez.com> (Universidad de Zaragoza)
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions
+are met:
+1. Redistributions of source code must retain the above copyright
+ notice, this list of conditions and the following disclaimer.
+2. Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+3. Neither the name of copyright holders nor the names of its
+ contributors may be used to endorse or promote products derived
+ from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL COPYRIGHT HOLDERS OR CONTRIBUTORS
+BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+
+If you use it in an academic work, please cite:
+
+ @ARTICLE{GalvezTRO12,
+ author={G\'alvez-L\'opez, Dorian and Tard\'os, J. D.},
+ journal={IEEE Transactions on Robotics},
+ title={Bags of Binary Words for Fast Place Recognition in Image Sequences},
+ year={2012},
+ month={October},
+ volume={28},
+ number={5},
+ pages={1188--1197},
+ doi={10.1109/TRO.2012.2197158},
+ ISSN={1552-3098}
+ }
+
diff --git a/externals/ORB_SLAM2/Thirdparty/DBoW2/README.txt b/externals/ORB_SLAM2/Thirdparty/DBoW2/README.txt
new file mode 100644
index 0000000000000000000000000000000000000000..71827f06cf321c9f797683385faa8168f208543a
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/DBoW2/README.txt
@@ -0,0 +1,7 @@
+You should have received this DBoW2 version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
+See the original DBoW2 library at: https://github.com/dorian3d/DBoW2
+All files included in this version are BSD, see LICENSE.txt
+
+We also use Random.h, Random.cpp, Timestamp.pp and Timestamp.h from DLib/DUtils.
+See the original DLib library at: https://github.com/dorian3d/DLib
+All files included in this version are BSD, see LICENSE.txt
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/CMakeLists.txt b/externals/ORB_SLAM2/Thirdparty/g2o/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..620e68876e7deefc2ede8dc817d4c8c170fd7ade
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/CMakeLists.txt
@@ -0,0 +1,174 @@
+CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
+SET(CMAKE_LEGACY_CYGWIN_WIN32 0)
+
+PROJECT(g2o)
+
+SET(g2o_C_FLAGS)
+SET(g2o_CXX_FLAGS)
+
+# default built type
+IF(NOT CMAKE_BUILD_TYPE)
+ SET(CMAKE_BUILD_TYPE Release)
+ENDIF()
+
+MESSAGE(STATUS "BUILD TYPE:" ${CMAKE_BUILD_TYPE})
+
+SET (G2O_LIB_TYPE SHARED)
+
+# There seems to be an issue with MSVC8
+# see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=83
+if(MSVC90)
+ add_definitions(-DEIGEN_DONT_ALIGN_STATICALLY=1)
+ message(STATUS "Disabling memory alignment for MSVC8")
+endif(MSVC90)
+
+# Set the output directory for the build executables and libraries
+IF(WIN32)
+ SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/bin CACHE PATH "Target for the libraries")
+ELSE(WIN32)
+ SET(g2o_LIBRARY_OUTPUT_DIRECTORY ${g2o_SOURCE_DIR}/lib CACHE PATH "Target for the libraries")
+ENDIF(WIN32)
+SET(CMAKE_ARCHIVE_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${g2o_LIBRARY_OUTPUT_DIRECTORY})
+SET(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${g2o_RUNTIME_OUTPUT_DIRECTORY})
+
+# Set search directory for looking for our custom CMake scripts to
+# look for SuiteSparse, QGLViewer, and Eigen3.
+LIST(APPEND CMAKE_MODULE_PATH ${g2o_SOURCE_DIR}/cmake_modules)
+
+# Detect OS and define macros appropriately
+IF(UNIX)
+ ADD_DEFINITIONS(-DUNIX)
+ MESSAGE(STATUS "Compiling on Unix")
+ENDIF(UNIX)
+
+# Eigen library parallelise itself, though, presumably due to performance issues
+# OPENMP is experimental. We experienced some slowdown with it
+FIND_PACKAGE(OpenMP)
+SET(G2O_USE_OPENMP OFF CACHE BOOL "Build g2o with OpenMP support (EXPERIMENTAL)")
+IF(OPENMP_FOUND AND G2O_USE_OPENMP)
+ SET (G2O_OPENMP 1)
+ SET(g2o_C_FLAGS "${g2o_C_FLAGS} ${OpenMP_C_FLAGS}")
+ SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -DEIGEN_DONT_PARALLELIZE ${OpenMP_CXX_FLAGS}")
+ MESSAGE(STATUS "Compiling with OpenMP support")
+ENDIF(OPENMP_FOUND AND G2O_USE_OPENMP)
+
+# Compiler specific options for gcc
+SET(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -O3 -march=native")
+SET(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} -O3 -march=native")
+
+# activate warnings !!!
+SET(g2o_C_FLAGS "${g2o_C_FLAGS} -Wall -W")
+SET(g2o_CXX_FLAGS "${g2o_CXX_FLAGS} -Wall -W")
+
+# specifying compiler flags
+SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${g2o_CXX_FLAGS}")
+SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${g2o_C_FLAGS}")
+
+# Find Eigen3
+SET(EIGEN3_INCLUDE_DIR ${G2O_EIGEN3_INCLUDE})
+FIND_PACKAGE(Eigen3 3.1.0 REQUIRED)
+IF(EIGEN3_FOUND)
+ SET(G2O_EIGEN3_INCLUDE ${EIGEN3_INCLUDE_DIR} CACHE PATH "Directory of Eigen3")
+ELSE(EIGEN3_FOUND)
+ SET(G2O_EIGEN3_INCLUDE "" CACHE PATH "Directory of Eigen3")
+ENDIF(EIGEN3_FOUND)
+
+# Generate config.h
+SET(G2O_CXX_COMPILER "${CMAKE_CXX_COMPILER_ID} ${CMAKE_CXX_COMPILER}")
+configure_file(config.h.in ${g2o_SOURCE_DIR}/config.h)
+
+# Set up the top-level include directories
+INCLUDE_DIRECTORIES(
+${g2o_SOURCE_DIR}/core
+${g2o_SOURCE_DIR}/types
+${g2o_SOURCE_DIR}/stuff
+${G2O_EIGEN3_INCLUDE})
+
+# Include the subdirectories
+ADD_LIBRARY(g2o ${G2O_LIB_TYPE}
+#types
+g2o/types/types_sba.h
+g2o/types/types_six_dof_expmap.h
+g2o/types/types_sba.cpp
+g2o/types/types_six_dof_expmap.cpp
+g2o/types/types_seven_dof_expmap.cpp
+g2o/types/types_seven_dof_expmap.h
+g2o/types/se3quat.h
+g2o/types/se3_ops.h
+g2o/types/se3_ops.hpp
+#core
+g2o/core/base_edge.h
+g2o/core/base_binary_edge.h
+g2o/core/hyper_graph_action.cpp
+g2o/core/base_binary_edge.hpp
+g2o/core/hyper_graph_action.h
+g2o/core/base_multi_edge.h
+g2o/core/hyper_graph.cpp
+g2o/core/base_multi_edge.hpp
+g2o/core/hyper_graph.h
+g2o/core/base_unary_edge.h
+g2o/core/linear_solver.h
+g2o/core/base_unary_edge.hpp
+g2o/core/marginal_covariance_cholesky.cpp
+g2o/core/base_vertex.h
+g2o/core/marginal_covariance_cholesky.h
+g2o/core/base_vertex.hpp
+g2o/core/matrix_structure.cpp
+g2o/core/batch_stats.cpp
+g2o/core/matrix_structure.h
+g2o/core/batch_stats.h
+g2o/core/openmp_mutex.h
+g2o/core/block_solver.h
+g2o/core/block_solver.hpp
+g2o/core/parameter.cpp
+g2o/core/parameter.h
+g2o/core/cache.cpp
+g2o/core/cache.h
+g2o/core/optimizable_graph.cpp
+g2o/core/optimizable_graph.h
+g2o/core/solver.cpp
+g2o/core/solver.h
+g2o/core/creators.h
+g2o/core/optimization_algorithm_factory.cpp
+g2o/core/estimate_propagator.cpp
+g2o/core/optimization_algorithm_factory.h
+g2o/core/estimate_propagator.h
+g2o/core/factory.cpp
+g2o/core/optimization_algorithm_property.h
+g2o/core/factory.h
+g2o/core/sparse_block_matrix.h
+g2o/core/sparse_optimizer.cpp
+g2o/core/sparse_block_matrix.hpp
+g2o/core/sparse_optimizer.h
+g2o/core/hyper_dijkstra.cpp
+g2o/core/hyper_dijkstra.h
+g2o/core/parameter_container.cpp
+g2o/core/parameter_container.h
+g2o/core/optimization_algorithm.cpp
+g2o/core/optimization_algorithm.h
+g2o/core/optimization_algorithm_with_hessian.cpp
+g2o/core/optimization_algorithm_with_hessian.h
+g2o/core/optimization_algorithm_levenberg.cpp
+g2o/core/optimization_algorithm_levenberg.h
+g2o/core/jacobian_workspace.cpp
+g2o/core/jacobian_workspace.h
+g2o/core/robust_kernel.cpp
+g2o/core/robust_kernel.h
+g2o/core/robust_kernel_factory.cpp
+g2o/core/robust_kernel_factory.h
+g2o/core/robust_kernel_impl.cpp
+g2o/core/robust_kernel_impl.h
+#stuff
+g2o/stuff/string_tools.h
+g2o/stuff/color_macros.h
+g2o/stuff/macros.h
+g2o/stuff/timeutil.cpp
+g2o/stuff/misc.h
+g2o/stuff/timeutil.h
+g2o/stuff/os_specific.c
+g2o/stuff/os_specific.h
+g2o/stuff/string_tools.cpp
+g2o/stuff/property.cpp
+g2o/stuff/property.h
+)
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/README.txt b/externals/ORB_SLAM2/Thirdparty/g2o/README.txt
new file mode 100644
index 0000000000000000000000000000000000000000..82641a8a1b06dcd24be8556c3e282985cb925897
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/README.txt
@@ -0,0 +1,3 @@
+You should have received this g2o version along with ORB-SLAM2 (https://github.com/raulmur/ORB_SLAM2).
+See the original g2o library at: https://github.com/RainerKuemmerle/g2o
+All files included in this g2o version are BSD, see license-bsd.txt
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/config.h b/externals/ORB_SLAM2/Thirdparty/g2o/config.h
new file mode 100644
index 0000000000000000000000000000000000000000..5346723ccdd20c76d1cf66b8bfb3fc2e5cfd7ff3
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/config.h
@@ -0,0 +1,13 @@
+#ifndef G2O_CONFIG_H
+#define G2O_CONFIG_H
+
+//#define G2O_OPENMP 1
+//#define G2O_SHARED_LIBS 1
+
+// give a warning if Eigen defaults to row-major matrices.
+// We internally assume column-major matrices throughout the code.
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+# error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)"
+#endif
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/config.h.in b/externals/ORB_SLAM2/Thirdparty/g2o/config.h.in
new file mode 100644
index 0000000000000000000000000000000000000000..49c686b1c06171ec497c2abf7cc13415297d26a6
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/config.h.in
@@ -0,0 +1,13 @@
+#ifndef G2O_CONFIG_H
+#define G2O_CONFIG_H
+
+#cmakedefine G2O_OPENMP 1
+#cmakedefine G2O_SHARED_LIBS 1
+
+// give a warning if Eigen defaults to row-major matrices.
+// We internally assume column-major matrices throughout the code.
+#ifdef EIGEN_DEFAULT_TO_ROW_MAJOR
+# error "g2o requires column major Eigen matrices (see http://eigen.tuxfamily.org/bz/show_bug.cgi?id=422)"
+#endif
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.h
new file mode 100644
index 0000000000000000000000000000000000000000..660e83afd62e0b24d42bf46f4b6aa3bc1a14688c
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.h
@@ -0,0 +1,119 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_BINARY_EDGE_H
+#define G2O_BASE_BINARY_EDGE_H
+
+#include <iostream>
+#include <limits>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+ using namespace Eigen;
+
+ template <int D, typename E, typename VertexXi, typename VertexXj>
+ class BaseBinaryEdge : public BaseEdge<D, E>
+ {
+ public:
+
+ typedef VertexXi VertexXiType;
+ typedef VertexXj VertexXjType;
+
+ static const int Di = VertexXiType::Dimension;
+ static const int Dj = VertexXjType::Dimension;
+
+ static const int Dimension = BaseEdge<D, E>::Dimension;
+ typedef typename BaseEdge<D,E>::Measurement Measurement;
+ typedef typename Matrix<double, D, Di>::AlignedMapType JacobianXiOplusType;
+ typedef typename Matrix<double, D, Dj>::AlignedMapType JacobianXjOplusType;
+ typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+ typedef typename BaseEdge<D,E>::InformationType InformationType;
+
+ typedef Eigen::Map<Matrix<double, Di, Dj>, Matrix<double, Di, Dj>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+ typedef Eigen::Map<Matrix<double, Dj, Di>, Matrix<double, Dj, Di>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockTransposedType;
+
+ BaseBinaryEdge() : BaseEdge<D,E>(),
+ _hessianRowMajor(false),
+ _hessian(0, VertexXiType::Dimension, VertexXjType::Dimension), // HACK we map to the null pointer for initializing the Maps
+ _hessianTransposed(0, VertexXjType::Dimension, VertexXiType::Dimension),
+ _jacobianOplusXi(0, D, Di), _jacobianOplusXj(0, D, Dj)
+ {
+ _vertices.resize(2);
+ }
+
+ virtual OptimizableGraph::Vertex* createFrom();
+ virtual OptimizableGraph::Vertex* createTo();
+
+ virtual void resize(size_t size);
+
+ virtual bool allVerticesFixed() const;
+
+ virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+ /**
+ * Linearizes the oplus operator in the vertex, and stores
+ * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
+ */
+ virtual void linearizeOplus();
+
+ //! returns the result of the linearization in the manifold space for the node xi
+ const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
+ //! returns the result of the linearization in the manifold space for the node xj
+ const JacobianXjOplusType& jacobianOplusXj() const { return _jacobianOplusXj;}
+
+ virtual void constructQuadraticForm() ;
+
+ virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
+
+ using BaseEdge<D,E>::resize;
+ using BaseEdge<D,E>::computeError;
+
+ protected:
+ using BaseEdge<D,E>::_measurement;
+ using BaseEdge<D,E>::_information;
+ using BaseEdge<D,E>::_error;
+ using BaseEdge<D,E>::_vertices;
+ using BaseEdge<D,E>::_dimension;
+
+ bool _hessianRowMajor;
+ HessianBlockType _hessian;
+ HessianBlockTransposedType _hessianTransposed;
+ JacobianXiOplusType _jacobianOplusXi;
+ JacobianXjOplusType _jacobianOplusXj;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+#include "base_binary_edge.hpp"
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..7542a9fd617364c566bb8542a426d093720090e2
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_binary_edge.hpp
@@ -0,0 +1,218 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createFrom(){
+ return new VertexXiType();
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+OptimizableGraph::Vertex* BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::createTo(){
+ return new VertexXjType();
+}
+
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::resize(size_t size)
+{
+ if (size != 2) {
+ std::cerr << "WARNING, attempting to resize binary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
+ }
+ BaseEdge<D, E>::resize(size);
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+bool BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::allVerticesFixed() const
+{
+ return (static_cast<const VertexXiType*> (_vertices[0])->fixed() &&
+ static_cast<const VertexXjType*> (_vertices[1])->fixed());
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::constructQuadraticForm()
+{
+ VertexXiType* from = static_cast<VertexXiType*>(_vertices[0]);
+ VertexXjType* to = static_cast<VertexXjType*>(_vertices[1]);
+
+ // get the Jacobian of the nodes in the manifold domain
+ const JacobianXiOplusType& A = jacobianOplusXi();
+ const JacobianXjOplusType& B = jacobianOplusXj();
+
+
+ bool fromNotFixed = !(from->fixed());
+ bool toNotFixed = !(to->fixed());
+
+ if (fromNotFixed || toNotFixed) {
+#ifdef G2O_OPENMP
+ from->lockQuadraticForm();
+ to->lockQuadraticForm();
+#endif
+ const InformationType& omega = _information;
+ Matrix<double, D, 1> omega_r = - omega * _error;
+ if (this->robustKernel() == 0) {
+ if (fromNotFixed) {
+ Matrix<double, VertexXiType::Dimension, D> AtO = A.transpose() * omega;
+ from->b().noalias() += A.transpose() * omega_r;
+ from->A().noalias() += AtO*A;
+ if (toNotFixed ) {
+ if (_hessianRowMajor) // we have to write to the block as transposed
+ _hessianTransposed.noalias() += B.transpose() * AtO.transpose();
+ else
+ _hessian.noalias() += AtO * B;
+ }
+ }
+ if (toNotFixed) {
+ to->b().noalias() += B.transpose() * omega_r;
+ to->A().noalias() += B.transpose() * omega * B;
+ }
+ } else { // robust (weighted) error according to some kernel
+ double error = this->chi2();
+ Eigen::Vector3d rho;
+ this->robustKernel()->robustify(error, rho);
+ InformationType weightedOmega = this->robustInformation(rho);
+ //std::cout << PVAR(rho.transpose()) << std::endl;
+ //std::cout << PVAR(weightedOmega) << std::endl;
+
+ omega_r *= rho[1];
+ if (fromNotFixed) {
+ from->b().noalias() += A.transpose() * omega_r;
+ from->A().noalias() += A.transpose() * weightedOmega * A;
+ if (toNotFixed ) {
+ if (_hessianRowMajor) // we have to write to the block as transposed
+ _hessianTransposed.noalias() += B.transpose() * weightedOmega * A;
+ else
+ _hessian.noalias() += A.transpose() * weightedOmega * B;
+ }
+ }
+ if (toNotFixed) {
+ to->b().noalias() += B.transpose() * omega_r;
+ to->A().noalias() += B.transpose() * weightedOmega * B;
+ }
+ }
+#ifdef G2O_OPENMP
+ to->unlockQuadraticForm();
+ from->unlockQuadraticForm();
+#endif
+ }
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+ new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, Di);
+ new (&_jacobianOplusXj) JacobianXjOplusType(jacobianWorkspace.workspaceForVertex(1), D, Dj);
+ linearizeOplus();
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::linearizeOplus()
+{
+ VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
+ VertexXjType* vj = static_cast<VertexXjType*>(_vertices[1]);
+
+ bool iNotFixed = !(vi->fixed());
+ bool jNotFixed = !(vj->fixed());
+
+ if (!iNotFixed && !jNotFixed)
+ return;
+
+#ifdef G2O_OPENMP
+ vi->lockQuadraticForm();
+ vj->lockQuadraticForm();
+#endif
+
+ const double delta = 1e-9;
+ const double scalar = 1.0 / (2*delta);
+ ErrorVector errorBak;
+ ErrorVector errorBeforeNumeric = _error;
+
+ if (iNotFixed) {
+ //Xi - estimate the jacobian numerically
+ double add_vi[VertexXiType::Dimension];
+ std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
+ // add small step along the unit vector in each dimension
+ for (int d = 0; d < VertexXiType::Dimension; ++d) {
+ vi->push();
+ add_vi[d] = delta;
+ vi->oplus(add_vi);
+ computeError();
+ errorBak = _error;
+ vi->pop();
+ vi->push();
+ add_vi[d] = -delta;
+ vi->oplus(add_vi);
+ computeError();
+ errorBak -= _error;
+ vi->pop();
+ add_vi[d] = 0.0;
+
+ _jacobianOplusXi.col(d) = scalar * errorBak;
+ } // end dimension
+ }
+
+ if (jNotFixed) {
+ //Xj - estimate the jacobian numerically
+ double add_vj[VertexXjType::Dimension];
+ std::fill(add_vj, add_vj + VertexXjType::Dimension, 0.0);
+ // add small step along the unit vector in each dimension
+ for (int d = 0; d < VertexXjType::Dimension; ++d) {
+ vj->push();
+ add_vj[d] = delta;
+ vj->oplus(add_vj);
+ computeError();
+ errorBak = _error;
+ vj->pop();
+ vj->push();
+ add_vj[d] = -delta;
+ vj->oplus(add_vj);
+ computeError();
+ errorBak -= _error;
+ vj->pop();
+ add_vj[d] = 0.0;
+
+ _jacobianOplusXj.col(d) = scalar * errorBak;
+ }
+ } // end dimension
+
+ _error = errorBeforeNumeric;
+#ifdef G2O_OPENMP
+ vj->unlockQuadraticForm();
+ vi->unlockQuadraticForm();
+#endif
+}
+
+template <int D, typename E, typename VertexXiType, typename VertexXjType>
+void BaseBinaryEdge<D, E, VertexXiType, VertexXjType>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
+{
+ (void) i; (void) j;
+ //assert(i == 0 && j == 1);
+ if (rowMajor) {
+ new (&_hessianTransposed) HessianBlockTransposedType(d, VertexXjType::Dimension, VertexXiType::Dimension);
+ } else {
+ new (&_hessian) HessianBlockType(d, VertexXiType::Dimension, VertexXjType::Dimension);
+ }
+ _hessianRowMajor = rowMajor;
+}
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_edge.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_edge.h
new file mode 100644
index 0000000000000000000000000000000000000000..91139e4f0d7cb561642f899ffa0ab6a08ecec465
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_edge.h
@@ -0,0 +1,110 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_EDGE_H
+#define G2O_BASE_EDGE_H
+
+#include <iostream>
+#include <limits>
+
+#include <Eigen/Core>
+
+#include "optimizable_graph.h"
+
+namespace g2o {
+
+ using namespace Eigen;
+
+ template <int D, typename E>
+ class BaseEdge : public OptimizableGraph::Edge
+ {
+ public:
+
+ static const int Dimension = D;
+ typedef E Measurement;
+ typedef Matrix<double, D, 1> ErrorVector;
+ typedef Matrix<double, D, D> InformationType;
+
+ BaseEdge() : OptimizableGraph::Edge()
+ {
+ _dimension = D;
+ }
+
+ virtual ~BaseEdge() {}
+
+ virtual double chi2() const
+ {
+ return _error.dot(information()*_error);
+ }
+
+ virtual const double* errorData() const { return _error.data();}
+ virtual double* errorData() { return _error.data();}
+ const ErrorVector& error() const { return _error;}
+ ErrorVector& error() { return _error;}
+
+ //! information matrix of the constraint
+ const InformationType& information() const { return _information;}
+ InformationType& information() { return _information;}
+ void setInformation(const InformationType& information) { _information = information;}
+
+ virtual const double* informationData() const { return _information.data();}
+ virtual double* informationData() { return _information.data();}
+
+ //! accessor functions for the measurement represented by the edge
+ const Measurement& measurement() const { return _measurement;}
+ virtual void setMeasurement(const Measurement& m) { _measurement = m;}
+
+ virtual int rank() const {return _dimension;}
+
+ virtual void initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
+ {
+ std::cerr << "inititialEstimate() is not implemented, please give implementation in your derived class" << std::endl;
+ }
+
+ protected:
+
+ Measurement _measurement;
+ InformationType _information;
+ ErrorVector _error;
+
+ /**
+ * calculate the robust information matrix by updating the information matrix of the error
+ */
+ InformationType robustInformation(const Eigen::Vector3d& rho)
+ {
+ InformationType result = rho[1] * _information;
+ //ErrorVector weightedErrror = _information * _error;
+ //result.noalias() += 2 * rho[2] * (weightedErrror * weightedErrror.transpose());
+ return result;
+ }
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd2261fd4874475c0221190b4ea21f670416d050
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.h
@@ -0,0 +1,113 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_MULTI_EDGE_H
+#define G2O_BASE_MULTI_EDGE_H
+
+#include <iostream>
+#include <iomanip>
+#include <limits>
+
+#include <Eigen/StdVector>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+ using namespace Eigen;
+
+ /**
+ * \brief base class to represent an edge connecting an arbitrary number of nodes
+ *
+ * D - Dimension of the measurement
+ * E - type to represent the measurement
+ */
+ template <int D, typename E>
+ class BaseMultiEdge : public BaseEdge<D,E>
+ {
+ public:
+ /**
+ * \brief helper for mapping the Hessian memory of the upper triangular block
+ */
+ struct HessianHelper {
+ Eigen::Map<MatrixXd> matrix; ///< the mapped memory
+ bool transposed; ///< the block has to be transposed
+ HessianHelper() : matrix(0, 0, 0), transposed(false) {}
+ };
+
+ public:
+ static const int Dimension = BaseEdge<D,E>::Dimension;
+ typedef typename BaseEdge<D,E>::Measurement Measurement;
+ typedef MatrixXd::MapType JacobianType;
+ typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+ typedef typename BaseEdge<D,E>::InformationType InformationType;
+ typedef Eigen::Map<MatrixXd, MatrixXd::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+
+ BaseMultiEdge() : BaseEdge<D,E>()
+ {
+ }
+
+ virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+ /**
+ * Linearizes the oplus operator in the vertex, and stores
+ * the result in temporary variable vector _jacobianOplus
+ */
+ virtual void linearizeOplus();
+
+ virtual void resize(size_t size);
+
+ virtual bool allVerticesFixed() const;
+
+ virtual void constructQuadraticForm() ;
+
+ virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor);
+
+ using BaseEdge<D,E>::computeError;
+
+ protected:
+ using BaseEdge<D,E>::_measurement;
+ using BaseEdge<D,E>::_information;
+ using BaseEdge<D,E>::_error;
+ using BaseEdge<D,E>::_vertices;
+ using BaseEdge<D,E>::_dimension;
+
+ std::vector<HessianHelper> _hessian;
+ std::vector<JacobianType, aligned_allocator<JacobianType> > _jacobianOplus; ///< jacobians of the edge (w.r.t. oplus)
+
+ void computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError);
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+#include "base_multi_edge.hpp"
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2fa66d955816a66db5399eb9d611f2fc9c33158
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_multi_edge.hpp
@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+namespace internal {
+ inline int computeUpperTriangleIndex(int i, int j)
+ {
+ int elemsUpToCol = ((j-1) * j) / 2;
+ return elemsUpToCol + i;
+ }
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::constructQuadraticForm()
+{
+ if (this->robustKernel()) {
+ double error = this->chi2();
+ Eigen::Vector3d rho;
+ this->robustKernel()->robustify(error, rho);
+ Matrix<double, D, 1> omega_r = - _information * _error;
+ omega_r *= rho[1];
+ computeQuadraticForm(this->robustInformation(rho), omega_r);
+ } else {
+ computeQuadraticForm(_information, - _information * _error);
+ }
+}
+
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+ for (size_t i = 0; i < _vertices.size(); ++i) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+ assert(v->dimension() >= 0);
+ new (&_jacobianOplus[i]) JacobianType(jacobianWorkspace.workspaceForVertex(i), D, v->dimension());
+ }
+ linearizeOplus();
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::linearizeOplus()
+{
+#ifdef G2O_OPENMP
+ for (size_t i = 0; i < _vertices.size(); ++i) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+ v->lockQuadraticForm();
+ }
+#endif
+
+ const double delta = 1e-9;
+ const double scalar = 1.0 / (2*delta);
+ ErrorVector errorBak;
+ ErrorVector errorBeforeNumeric = _error;
+
+ for (size_t i = 0; i < _vertices.size(); ++i) {
+ //Xi - estimate the jacobian numerically
+ OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+
+ if (vi->fixed())
+ continue;
+
+ const int vi_dim = vi->dimension();
+ assert(vi_dim >= 0);
+#ifdef _MSC_VER
+ double* add_vi = new double[vi_dim];
+#else
+ double add_vi[vi_dim];
+#endif
+ std::fill(add_vi, add_vi + vi_dim, 0.0);
+ assert(_dimension >= 0);
+ assert(_jacobianOplus[i].rows() == _dimension && _jacobianOplus[i].cols() == vi_dim && "jacobian cache dimension does not match");
+ _jacobianOplus[i].resize(_dimension, vi_dim);
+ // add small step along the unit vector in each dimension
+ for (int d = 0; d < vi_dim; ++d) {
+ vi->push();
+ add_vi[d] = delta;
+ vi->oplus(add_vi);
+ computeError();
+ errorBak = _error;
+ vi->pop();
+ vi->push();
+ add_vi[d] = -delta;
+ vi->oplus(add_vi);
+ computeError();
+ errorBak -= _error;
+ vi->pop();
+ add_vi[d] = 0.0;
+
+ _jacobianOplus[i].col(d) = scalar * errorBak;
+ } // end dimension
+#ifdef _MSC_VER
+ delete[] add_vi;
+#endif
+ }
+ _error = errorBeforeNumeric;
+
+#ifdef G2O_OPENMP
+ for (int i = (int)(_vertices.size()) - 1; i >= 0; --i) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+ v->unlockQuadraticForm();
+ }
+#endif
+
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::mapHessianMemory(double* d, int i, int j, bool rowMajor)
+{
+ int idx = internal::computeUpperTriangleIndex(i, j);
+ assert(idx < (int)_hessian.size());
+ OptimizableGraph::Vertex* vi = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(i));
+ OptimizableGraph::Vertex* vj = static_cast<OptimizableGraph::Vertex*>(HyperGraph::Edge::vertex(j));
+ assert(vi->dimension() >= 0);
+ assert(vj->dimension() >= 0);
+ HessianHelper& h = _hessian[idx];
+ if (rowMajor) {
+ if (h.matrix.data() != d || h.transposed != rowMajor)
+ new (&h.matrix) HessianBlockType(d, vj->dimension(), vi->dimension());
+ } else {
+ if (h.matrix.data() != d || h.transposed != rowMajor)
+ new (&h.matrix) HessianBlockType(d, vi->dimension(), vj->dimension());
+ }
+ h.transposed = rowMajor;
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::resize(size_t size)
+{
+ BaseEdge<D,E>::resize(size);
+ int n = (int)_vertices.size();
+ int maxIdx = (n * (n-1))/2;
+ assert(maxIdx >= 0);
+ _hessian.resize(maxIdx);
+ _jacobianOplus.resize(size, JacobianType(0,0,0));
+}
+
+template <int D, typename E>
+bool BaseMultiEdge<D, E>::allVerticesFixed() const
+{
+ for (size_t i = 0; i < _vertices.size(); ++i) {
+ if (!static_cast<const OptimizableGraph::Vertex*> (_vertices[i])->fixed()) {
+ return false;
+ }
+ }
+ return true;
+}
+
+template <int D, typename E>
+void BaseMultiEdge<D, E>::computeQuadraticForm(const InformationType& omega, const ErrorVector& weightedError)
+{
+ for (size_t i = 0; i < _vertices.size(); ++i) {
+ OptimizableGraph::Vertex* from = static_cast<OptimizableGraph::Vertex*>(_vertices[i]);
+ bool istatus = !(from->fixed());
+
+ if (istatus) {
+ const MatrixXd& A = _jacobianOplus[i];
+
+ MatrixXd AtO = A.transpose() * omega;
+ int fromDim = from->dimension();
+ assert(fromDim >= 0);
+ Eigen::Map<MatrixXd> fromMap(from->hessianData(), fromDim, fromDim);
+ Eigen::Map<VectorXd> fromB(from->bData(), fromDim);
+
+ // ii block in the hessian
+#ifdef G2O_OPENMP
+ from->lockQuadraticForm();
+#endif
+ fromMap.noalias() += AtO * A;
+ fromB.noalias() += A.transpose() * weightedError;
+
+ // compute the off-diagonal blocks ij for all j
+ for (size_t j = i+1; j < _vertices.size(); ++j) {
+ OptimizableGraph::Vertex* to = static_cast<OptimizableGraph::Vertex*>(_vertices[j]);
+#ifdef G2O_OPENMP
+ to->lockQuadraticForm();
+#endif
+ bool jstatus = !(to->fixed());
+ if (jstatus) {
+ const MatrixXd& B = _jacobianOplus[j];
+ int idx = internal::computeUpperTriangleIndex(i, j);
+ assert(idx < (int)_hessian.size());
+ HessianHelper& hhelper = _hessian[idx];
+ if (hhelper.transposed) { // we have to write to the block as transposed
+ hhelper.matrix.noalias() += B.transpose() * AtO.transpose();
+ } else {
+ hhelper.matrix.noalias() += AtO * B;
+ }
+ }
+#ifdef G2O_OPENMP
+ to->unlockQuadraticForm();
+#endif
+ }
+
+#ifdef G2O_OPENMP
+ from->unlockQuadraticForm();
+#endif
+ }
+
+ }
+}
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.h
new file mode 100644
index 0000000000000000000000000000000000000000..28df8a564b0c874948bb7aa18e518ee23c4c9a21
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.h
@@ -0,0 +1,100 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_UNARY_EDGE_H
+#define G2O_BASE_UNARY_EDGE_H
+
+#include <iostream>
+#include <cassert>
+#include <limits>
+
+#include "base_edge.h"
+#include "robust_kernel.h"
+#include "../../config.h"
+
+namespace g2o {
+
+ using namespace Eigen;
+
+ template <int D, typename E, typename VertexXi>
+ class BaseUnaryEdge : public BaseEdge<D,E>
+ {
+ public:
+ static const int Dimension = BaseEdge<D, E>::Dimension;
+ typedef typename BaseEdge<D,E>::Measurement Measurement;
+ typedef VertexXi VertexXiType;
+ typedef typename Matrix<double, D, VertexXiType::Dimension>::AlignedMapType JacobianXiOplusType;
+ typedef typename BaseEdge<D,E>::ErrorVector ErrorVector;
+ typedef typename BaseEdge<D,E>::InformationType InformationType;
+
+ BaseUnaryEdge() : BaseEdge<D,E>(),
+ _jacobianOplusXi(0, D, VertexXiType::Dimension)
+ {
+ _vertices.resize(1);
+ }
+
+ virtual void resize(size_t size);
+
+ virtual bool allVerticesFixed() const;
+
+ virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace);
+
+ /**
+ * Linearizes the oplus operator in the vertex, and stores
+ * the result in temporary variables _jacobianOplusXi and _jacobianOplusXj
+ */
+ virtual void linearizeOplus();
+
+ //! returns the result of the linearization in the manifold space for the node xi
+ const JacobianXiOplusType& jacobianOplusXi() const { return _jacobianOplusXi;}
+
+ virtual void constructQuadraticForm();
+
+ virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to);
+
+ virtual void mapHessianMemory(double*, int, int, bool) {assert(0 && "BaseUnaryEdge does not map memory of the Hessian");}
+
+ using BaseEdge<D,E>::resize;
+ using BaseEdge<D,E>::computeError;
+
+ protected:
+ using BaseEdge<D,E>::_measurement;
+ using BaseEdge<D,E>::_information;
+ using BaseEdge<D,E>::_error;
+ using BaseEdge<D,E>::_vertices;
+ using BaseEdge<D,E>::_dimension;
+
+ JacobianXiOplusType _jacobianOplusXi;
+
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ };
+
+#include "base_unary_edge.hpp"
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..464900fb1ef66fa94c9a8c861033356df4bc1303
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_unary_edge.hpp
@@ -0,0 +1,129 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::resize(size_t size)
+{
+ if (size != 1) {
+ std::cerr << "WARNING, attempting to resize unary edge " << BaseEdge<D, E>::id() << " to " << size << std::endl;
+ }
+ BaseEdge<D, E>::resize(size);
+}
+
+template <int D, typename E, typename VertexXiType>
+bool BaseUnaryEdge<D, E, VertexXiType>::allVerticesFixed() const
+{
+ return static_cast<const VertexXiType*> (_vertices[0])->fixed();
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::constructQuadraticForm()
+{
+ VertexXiType* from=static_cast<VertexXiType*>(_vertices[0]);
+
+ // chain rule to get the Jacobian of the nodes in the manifold domain
+ const JacobianXiOplusType& A = jacobianOplusXi();
+ const InformationType& omega = _information;
+
+ bool istatus = !from->fixed();
+ if (istatus) {
+#ifdef G2O_OPENMP
+ from->lockQuadraticForm();
+#endif
+ if (this->robustKernel()) {
+ double error = this->chi2();
+ Eigen::Vector3d rho;
+ this->robustKernel()->robustify(error, rho);
+ InformationType weightedOmega = this->robustInformation(rho);
+
+ from->b().noalias() -= rho[1] * A.transpose() * omega * _error;
+ from->A().noalias() += A.transpose() * weightedOmega * A;
+ } else {
+ from->b().noalias() -= A.transpose() * omega * _error;
+ from->A().noalias() += A.transpose() * omega * A;
+ }
+#ifdef G2O_OPENMP
+ from->unlockQuadraticForm();
+#endif
+ }
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus(JacobianWorkspace& jacobianWorkspace)
+{
+ new (&_jacobianOplusXi) JacobianXiOplusType(jacobianWorkspace.workspaceForVertex(0), D, VertexXiType::Dimension);
+ linearizeOplus();
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::linearizeOplus()
+{
+ //Xi - estimate the jacobian numerically
+ VertexXiType* vi = static_cast<VertexXiType*>(_vertices[0]);
+
+ if (vi->fixed())
+ return;
+
+#ifdef G2O_OPENMP
+ vi->lockQuadraticForm();
+#endif
+
+ const double delta = 1e-9;
+ const double scalar = 1.0 / (2*delta);
+ ErrorVector error1;
+ ErrorVector errorBeforeNumeric = _error;
+
+ double add_vi[VertexXiType::Dimension];
+ std::fill(add_vi, add_vi + VertexXiType::Dimension, 0.0);
+ // add small step along the unit vector in each dimension
+ for (int d = 0; d < VertexXiType::Dimension; ++d) {
+ vi->push();
+ add_vi[d] = delta;
+ vi->oplus(add_vi);
+ computeError();
+ error1 = _error;
+ vi->pop();
+ vi->push();
+ add_vi[d] = -delta;
+ vi->oplus(add_vi);
+ computeError();
+ vi->pop();
+ add_vi[d] = 0.0;
+
+ _jacobianOplusXi.col(d) = scalar * (error1 - _error);
+ } // end dimension
+
+ _error = errorBeforeNumeric;
+#ifdef G2O_OPENMP
+ vi->unlockQuadraticForm();
+#endif
+}
+
+template <int D, typename E, typename VertexXiType>
+void BaseUnaryEdge<D, E, VertexXiType>::initialEstimate(const OptimizableGraph::VertexSet&, OptimizableGraph::Vertex*)
+{
+ std::cerr << __PRETTY_FUNCTION__ << " is not implemented, please give implementation in your derived class" << std::endl;
+}
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.h
new file mode 100644
index 0000000000000000000000000000000000000000..e375fdeef918b744e4e08a0686d5842f24da1b8a
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.h
@@ -0,0 +1,120 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BASE_VERTEX_H
+#define G2O_BASE_VERTEX_H
+
+#include "optimizable_graph.h"
+#include "creators.h"
+#include "../stuff/macros.h"
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <Eigen/Cholesky>
+#include <Eigen/StdVector>
+#include <stack>
+
+namespace g2o {
+
+ using namespace Eigen;
+
+
+/**
+ * \brief Templatized BaseVertex
+ *
+ * Templatized BaseVertex
+ * D : minimal dimension of the vertex, e.g., 3 for rotation in 3D
+ * T : internal type to represent the estimate, e.g., Quaternion for rotation in 3D
+ */
+ template <int D, typename T>
+ class BaseVertex : public OptimizableGraph::Vertex {
+ public:
+ typedef T EstimateType;
+ typedef std::stack<EstimateType,
+ std::vector<EstimateType, Eigen::aligned_allocator<EstimateType> > >
+ BackupStackType;
+
+ static const int Dimension = D; ///< dimension of the estimate (minimal) in the manifold space
+
+ typedef Eigen::Map<Matrix<double, D, D>, Matrix<double,D,D>::Flags & AlignedBit ? Aligned : Unaligned > HessianBlockType;
+
+ public:
+ BaseVertex();
+
+ virtual const double& hessian(int i, int j) const { assert(i<D && j<D); return _hessian(i,j);}
+ virtual double& hessian(int i, int j) { assert(i<D && j<D); return _hessian(i,j);}
+ virtual double hessianDeterminant() const {return _hessian.determinant();}
+ virtual double* hessianData() { return const_cast<double*>(_hessian.data());}
+
+ virtual void mapHessianMemory(double* d);
+
+ virtual int copyB(double* b_) const {
+ memcpy(b_, _b.data(), Dimension * sizeof(double));
+ return Dimension;
+ }
+
+ virtual const double& b(int i) const { assert(i < D); return _b(i);}
+ virtual double& b(int i) { assert(i < D); return _b(i);}
+ virtual double* bData() { return _b.data();}
+
+ virtual void clearQuadraticForm();
+
+ //! updates the current vertex with the direct solution x += H_ii\b_ii
+ //! @returns the determinant of the inverted hessian
+ virtual double solveDirect(double lambda=0);
+
+ //! return right hand side b of the constructed linear system
+ Matrix<double, D, 1>& b() { return _b;}
+ const Matrix<double, D, 1>& b() const { return _b;}
+ //! return the hessian block associated with the vertex
+ HessianBlockType& A() { return _hessian;}
+ const HessianBlockType& A() const { return _hessian;}
+
+ virtual void push() { _backup.push(_estimate);}
+ virtual void pop() { assert(!_backup.empty()); _estimate = _backup.top(); _backup.pop(); updateCache();}
+ virtual void discardTop() { assert(!_backup.empty()); _backup.pop();}
+ virtual int stackSize() const {return _backup.size();}
+
+ //! return the current estimate of the vertex
+ const EstimateType& estimate() const { return _estimate;}
+ //! set the estimate for the vertex also calls updateCache()
+ void setEstimate(const EstimateType& et) { _estimate = et; updateCache();}
+
+ protected:
+ HessianBlockType _hessian;
+ Matrix<double, D, 1> _b;
+ EstimateType _estimate;
+ BackupStackType _backup;
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+#include "base_vertex.hpp"
+
+} // end namespace g2o
+
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f21ff165c16e1bee57fd9ebfcef3d86d0f49104f
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/base_vertex.hpp
@@ -0,0 +1,55 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+template <int D, typename T>
+BaseVertex<D, T>::BaseVertex() :
+ OptimizableGraph::Vertex(),
+ _hessian(0, D, D)
+{
+ _dimension = D;
+}
+
+template <int D, typename T>
+double BaseVertex<D, T>::solveDirect(double lambda) {
+ Matrix <double, D, D> tempA=_hessian + Matrix <double, D, D>::Identity()*lambda;
+ double det=tempA.determinant();
+ if (g2o_isnan(det) || det < std::numeric_limits<double>::epsilon())
+ return det;
+ Matrix <double, D, 1> dx=tempA.llt().solve(_b);
+ oplus(&dx[0]);
+ return det;
+}
+
+template <int D, typename T>
+void BaseVertex<D, T>::clearQuadraticForm() {
+ _b.setZero();
+}
+
+template <int D, typename T>
+void BaseVertex<D, T>::mapHessianMemory(double* d)
+{
+ new (&_hessian) HessianBlockType(d, D, D);
+}
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a6beb69b23832691a381f6f1e8ad4e2369d30718
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.cpp
@@ -0,0 +1,90 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "batch_stats.h"
+#include <cstring>
+
+namespace g2o {
+ using namespace std;
+
+ G2OBatchStatistics* G2OBatchStatistics::_globalStats=0;
+
+ #ifndef PTHING
+ #define PTHING(s) \
+ #s << "= " << (st.s) << "\t "
+ #endif
+
+ G2OBatchStatistics::G2OBatchStatistics(){
+ // zero all.
+ memset (this, 0, sizeof(G2OBatchStatistics));
+
+ // set the iteration to -1 to show that it isn't valid
+ iteration = -1;
+ }
+
+ std::ostream& operator << (std::ostream& os , const G2OBatchStatistics& st)
+ {
+ os << PTHING(iteration);
+
+ os << PTHING( numVertices ); // how many vertices are involved
+ os << PTHING( numEdges ); // hoe many edges
+ os << PTHING( chi2 ); // total chi2
+
+ /** timings **/
+ // nonlinear part
+ os << PTHING( timeResiduals );
+ os << PTHING( timeLinearize ); // jacobians
+ os << PTHING( timeQuadraticForm ); // construct the quadratic form in the graph
+
+ // block_solver (constructs Ax=b, plus maybe schur);
+ os << PTHING( timeSchurComplement ); // compute schur complement (0 if not done);
+
+ // linear solver (computes Ax=b); );
+ os << PTHING( timeSymbolicDecomposition ); // symbolic decomposition (0 if not done);
+ os << PTHING( timeNumericDecomposition ); // numeric decomposition (0 if not done);
+ os << PTHING( timeLinearSolution ); // total time for solving Ax=b
+ os << PTHING( iterationsLinearSolver ); // iterations of PCG
+ os << PTHING( timeUpdate ); // oplus
+ os << PTHING( timeIteration ); // total time );
+
+ os << PTHING( levenbergIterations );
+ os << PTHING( timeLinearSolver);
+
+ os << PTHING(hessianDimension);
+ os << PTHING(hessianPoseDimension);
+ os << PTHING(hessianLandmarkDimension);
+ os << PTHING(choleskyNNZ);
+ os << PTHING(timeMarginals);
+
+ return os;
+ };
+
+ void G2OBatchStatistics::setGlobalStats(G2OBatchStatistics* b)
+ {
+ _globalStats = b;
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.h
new file mode 100644
index 0000000000000000000000000000000000000000..d039f656779752d1eb25a7f2727820433b4e7f83
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/batch_stats.h
@@ -0,0 +1,83 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BATCH_STATS_H_
+#define G2O_BATCH_STATS_H_
+
+#include <iostream>
+#include <vector>
+
+
+namespace g2o {
+
+ /**
+ * \brief statistics about the optimization
+ */
+ struct G2OBatchStatistics {
+ G2OBatchStatistics();
+ int iteration; ///< which iteration
+ int numVertices; ///< how many vertices are involved
+ int numEdges; ///< how many edges
+ double chi2; ///< total chi2
+
+ /** timings **/
+ // nonlinear part
+ double timeResiduals; ///< residuals
+ double timeLinearize; ///< jacobians
+ double timeQuadraticForm; ///< construct the quadratic form in the graph
+ int levenbergIterations; ///< number of iterations performed by LM
+ // block_solver (constructs Ax=b, plus maybe schur)
+ double timeSchurComplement; ///< compute schur complement (0 if not done)
+
+ // linear solver (computes Ax=b);
+ double timeSymbolicDecomposition; ///< symbolic decomposition (0 if not done)
+ double timeNumericDecomposition; ///< numeric decomposition (0 if not done)
+ double timeLinearSolution; ///< total time for solving Ax=b (including detup for schur)
+ double timeLinearSolver; ///< time for solving, excluding Schur setup
+ int iterationsLinearSolver; ///< iterations of PCG, (0 if not used, i.e., Cholesky)
+ double timeUpdate; ///< time to apply the update
+ double timeIteration; ///< total time;
+
+ double timeMarginals; ///< computing the inverse elements (solve blocks) and thus the marginal covariances
+
+ // information about the Hessian matrix
+ size_t hessianDimension; ///< rows / cols of the Hessian
+ size_t hessianPoseDimension; ///< dimension of the pose matrix in Schur
+ size_t hessianLandmarkDimension; ///< dimension of the landmark matrix in Schur
+ size_t choleskyNNZ; ///< number of non-zeros in the cholesky factor
+
+ static G2OBatchStatistics* globalStats() {return _globalStats;}
+ static void setGlobalStats(G2OBatchStatistics* b);
+ protected:
+ static G2OBatchStatistics* _globalStats;
+ };
+
+ std::ostream& operator<<(std::ostream&, const G2OBatchStatistics&);
+
+ typedef std::vector<G2OBatchStatistics> BatchStatisticsContainer;
+}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a39339896229b96781a887a1b92e0d9391fff541
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.h
@@ -0,0 +1,193 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_BLOCK_SOLVER_H
+#define G2O_BLOCK_SOLVER_H
+#include <Eigen/Core>
+#include "solver.h"
+#include "linear_solver.h"
+#include "sparse_block_matrix.h"
+#include "sparse_block_matrix_diagonal.h"
+#include "openmp_mutex.h"
+#include "../../config.h"
+
+namespace g2o {
+ using namespace Eigen;
+
+ /**
+ * \brief traits to summarize the properties of the fixed size optimization problem
+ */
+ template <int _PoseDim, int _LandmarkDim>
+ struct BlockSolverTraits
+ {
+ static const int PoseDim = _PoseDim;
+ static const int LandmarkDim = _LandmarkDim;
+ typedef Matrix<double, PoseDim, PoseDim> PoseMatrixType;
+ typedef Matrix<double, LandmarkDim, LandmarkDim> LandmarkMatrixType;
+ typedef Matrix<double, PoseDim, LandmarkDim> PoseLandmarkMatrixType;
+ typedef Matrix<double, PoseDim, 1> PoseVectorType;
+ typedef Matrix<double, LandmarkDim, 1> LandmarkVectorType;
+
+ typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
+ typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
+ typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
+ typedef LinearSolver<PoseMatrixType> LinearSolverType;
+ };
+
+ /**
+ * \brief traits to summarize the properties of the dynamic size optimization problem
+ */
+ template <>
+ struct BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic>
+ {
+ static const int PoseDim = Eigen::Dynamic;
+ static const int LandmarkDim = Eigen::Dynamic;
+ typedef MatrixXd PoseMatrixType;
+ typedef MatrixXd LandmarkMatrixType;
+ typedef MatrixXd PoseLandmarkMatrixType;
+ typedef VectorXd PoseVectorType;
+ typedef VectorXd LandmarkVectorType;
+
+ typedef SparseBlockMatrix<PoseMatrixType> PoseHessianType;
+ typedef SparseBlockMatrix<LandmarkMatrixType> LandmarkHessianType;
+ typedef SparseBlockMatrix<PoseLandmarkMatrixType> PoseLandmarkHessianType;
+ typedef LinearSolver<PoseMatrixType> LinearSolverType;
+ };
+
+ /**
+ * \brief base for the block solvers with some basic function interfaces
+ */
+ class BlockSolverBase : public Solver
+ {
+ public:
+ virtual ~BlockSolverBase() {}
+ /**
+ * compute dest = H * src
+ */
+ virtual void multiplyHessian(double* dest, const double* src) const = 0;
+ };
+
+ /**
+ * \brief Implementation of a solver operating on the blocks of the Hessian
+ */
+ template <typename Traits>
+ class BlockSolver: public BlockSolverBase {
+ public:
+
+ static const int PoseDim = Traits::PoseDim;
+ static const int LandmarkDim = Traits::LandmarkDim;
+ typedef typename Traits::PoseMatrixType PoseMatrixType;
+ typedef typename Traits::LandmarkMatrixType LandmarkMatrixType;
+ typedef typename Traits::PoseLandmarkMatrixType PoseLandmarkMatrixType;
+ typedef typename Traits::PoseVectorType PoseVectorType;
+ typedef typename Traits::LandmarkVectorType LandmarkVectorType;
+
+ typedef typename Traits::PoseHessianType PoseHessianType;
+ typedef typename Traits::LandmarkHessianType LandmarkHessianType;
+ typedef typename Traits::PoseLandmarkHessianType PoseLandmarkHessianType;
+ typedef typename Traits::LinearSolverType LinearSolverType;
+
+ public:
+
+ /**
+ * allocate a block solver ontop of the underlying linear solver.
+ * NOTE: The BlockSolver assumes exclusive access to the linear solver and will therefore free the pointer
+ * in its destructor.
+ */
+ BlockSolver(LinearSolverType* linearSolver);
+ ~BlockSolver();
+
+ virtual bool init(SparseOptimizer* optmizer, bool online = false);
+ virtual bool buildStructure(bool zeroBlocks = false);
+ virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
+ virtual bool buildSystem();
+ virtual bool solve();
+ virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+ virtual bool setLambda(double lambda, bool backup = false);
+ virtual void restoreDiagonal();
+ virtual bool supportsSchur() {return true;}
+ virtual bool schur() { return _doSchur;}
+ virtual void setSchur(bool s) { _doSchur = s;}
+
+ LinearSolver<PoseMatrixType>* linearSolver() const { return _linearSolver;}
+
+ virtual void setWriteDebug(bool writeDebug);
+ virtual bool writeDebug() const {return _linearSolver->writeDebug();}
+
+ virtual bool saveHessian(const std::string& fileName) const;
+
+ virtual void multiplyHessian(double* dest, const double* src) const { _Hpp->multiplySymmetricUpperTriangle(dest, src);}
+
+ protected:
+ void resize(int* blockPoseIndices, int numPoseBlocks,
+ int* blockLandmarkIndices, int numLandmarkBlocks, int totalDim);
+
+ void deallocate();
+
+ SparseBlockMatrix<PoseMatrixType>* _Hpp;
+ SparseBlockMatrix<LandmarkMatrixType>* _Hll;
+ SparseBlockMatrix<PoseLandmarkMatrixType>* _Hpl;
+
+ SparseBlockMatrix<PoseMatrixType>* _Hschur;
+ SparseBlockMatrixDiagonal<LandmarkMatrixType>* _DInvSchur;
+
+ SparseBlockMatrixCCS<PoseLandmarkMatrixType>* _HplCCS;
+ SparseBlockMatrixCCS<PoseMatrixType>* _HschurTransposedCCS;
+
+ LinearSolver<PoseMatrixType>* _linearSolver;
+
+ std::vector<PoseVectorType, Eigen::aligned_allocator<PoseVectorType> > _diagonalBackupPose;
+ std::vector<LandmarkVectorType, Eigen::aligned_allocator<LandmarkVectorType> > _diagonalBackupLandmark;
+
+# ifdef G2O_OPENMP
+ std::vector<OpenMPMutex> _coefficientsMutex;
+# endif
+
+ bool _doSchur;
+
+ double* _coefficients;
+ double* _bschur;
+
+ int _numPoses, _numLandmarks;
+ int _sizePoses, _sizeLandmarks;
+ };
+
+
+ //variable size solver
+ typedef BlockSolver< BlockSolverTraits<Eigen::Dynamic, Eigen::Dynamic> > BlockSolverX;
+ // solver for BA/3D SLAM
+ typedef BlockSolver< BlockSolverTraits<6, 3> > BlockSolver_6_3;
+ // solver fo BA with scale
+ typedef BlockSolver< BlockSolverTraits<7, 3> > BlockSolver_7_3;
+ // 2Dof landmarks 3Dof poses
+ typedef BlockSolver< BlockSolverTraits<3, 2> > BlockSolver_3_2;
+
+} // end namespace
+
+#include "block_solver.hpp"
+
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..f8b032eedf11fa941cf25bfddc0ea2eabf0d6e97
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/block_solver.hpp
@@ -0,0 +1,634 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_optimizer.h"
+#include <Eigen/LU>
+#include <fstream>
+#include <iomanip>
+
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+#include "../stuff/misc.h"
+
+namespace g2o {
+
+using namespace std;
+using namespace Eigen;
+
+template <typename Traits>
+BlockSolver<Traits>::BlockSolver(LinearSolverType* linearSolver) :
+ BlockSolverBase(),
+ _linearSolver(linearSolver)
+{
+ // workspace
+ _Hpp=0;
+ _Hll=0;
+ _Hpl=0;
+ _HplCCS = 0;
+ _HschurTransposedCCS = 0;
+ _Hschur=0;
+ _DInvSchur=0;
+ _coefficients=0;
+ _bschur = 0;
+ _xSize=0;
+ _numPoses=0;
+ _numLandmarks=0;
+ _sizePoses=0;
+ _sizeLandmarks=0;
+ _doSchur=true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::resize(int* blockPoseIndices, int numPoseBlocks,
+ int* blockLandmarkIndices, int numLandmarkBlocks,
+ int s)
+{
+ deallocate();
+
+ resizeVector(s);
+
+ if (_doSchur) {
+ // the following two are only used in schur
+ assert(_sizePoses > 0 && "allocating with wrong size");
+ _coefficients = new double [s];
+ _bschur = new double[_sizePoses];
+ }
+
+ _Hpp=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
+ if (_doSchur) {
+ _Hschur=new PoseHessianType(blockPoseIndices, blockPoseIndices, numPoseBlocks, numPoseBlocks);
+ _Hll=new LandmarkHessianType(blockLandmarkIndices, blockLandmarkIndices, numLandmarkBlocks, numLandmarkBlocks);
+ _DInvSchur = new SparseBlockMatrixDiagonal<LandmarkMatrixType>(_Hll->colBlockIndices());
+ _Hpl=new PoseLandmarkHessianType(blockPoseIndices, blockLandmarkIndices, numPoseBlocks, numLandmarkBlocks);
+ _HplCCS = new SparseBlockMatrixCCS<PoseLandmarkMatrixType>(_Hpl->rowBlockIndices(), _Hpl->colBlockIndices());
+ _HschurTransposedCCS = new SparseBlockMatrixCCS<PoseMatrixType>(_Hschur->colBlockIndices(), _Hschur->rowBlockIndices());
+#ifdef G2O_OPENMP
+ _coefficientsMutex.resize(numPoseBlocks);
+#endif
+ }
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::deallocate()
+{
+ if (_Hpp){
+ delete _Hpp;
+ _Hpp=0;
+ }
+ if (_Hll){
+ delete _Hll;
+ _Hll=0;
+ }
+ if (_Hpl){
+ delete _Hpl;
+ _Hpl = 0;
+ }
+ if (_Hschur){
+ delete _Hschur;
+ _Hschur=0;
+ }
+ if (_DInvSchur){
+ delete _DInvSchur;
+ _DInvSchur=0;
+ }
+ if (_coefficients) {
+ delete[] _coefficients;
+ _coefficients = 0;
+ }
+ if (_bschur) {
+ delete[] _bschur;
+ _bschur = 0;
+ }
+ if (_HplCCS) {
+ delete _HplCCS;
+ _HplCCS = 0;
+ }
+ if (_HschurTransposedCCS) {
+ delete _HschurTransposedCCS;
+ _HschurTransposedCCS = 0;
+ }
+}
+
+template <typename Traits>
+BlockSolver<Traits>::~BlockSolver()
+{
+ delete _linearSolver;
+ deallocate();
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::buildStructure(bool zeroBlocks)
+{
+ assert(_optimizer);
+
+ size_t sparseDim = 0;
+ _numPoses=0;
+ _numLandmarks=0;
+ _sizePoses=0;
+ _sizeLandmarks=0;
+ int* blockPoseIndices = new int[_optimizer->indexMapping().size()];
+ int* blockLandmarkIndices = new int[_optimizer->indexMapping().size()];
+
+ for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+ OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+ int dim = v->dimension();
+ if (! v->marginalized()){
+ v->setColInHessian(_sizePoses);
+ _sizePoses+=dim;
+ blockPoseIndices[_numPoses]=_sizePoses;
+ ++_numPoses;
+ } else {
+ v->setColInHessian(_sizeLandmarks);
+ _sizeLandmarks+=dim;
+ blockLandmarkIndices[_numLandmarks]=_sizeLandmarks;
+ ++_numLandmarks;
+ }
+ sparseDim += dim;
+ }
+ resize(blockPoseIndices, _numPoses, blockLandmarkIndices, _numLandmarks, sparseDim);
+ delete[] blockLandmarkIndices;
+ delete[] blockPoseIndices;
+
+ // allocate the diagonal on Hpp and Hll
+ int poseIdx = 0;
+ int landmarkIdx = 0;
+ for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+ OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+ if (! v->marginalized()){
+ //assert(poseIdx == v->hessianIndex());
+ PoseMatrixType* m = _Hpp->block(poseIdx, poseIdx, true);
+ if (zeroBlocks)
+ m->setZero();
+ v->mapHessianMemory(m->data());
+ ++poseIdx;
+ } else {
+ LandmarkMatrixType* m = _Hll->block(landmarkIdx, landmarkIdx, true);
+ if (zeroBlocks)
+ m->setZero();
+ v->mapHessianMemory(m->data());
+ ++landmarkIdx;
+ }
+ }
+ assert(poseIdx == _numPoses && landmarkIdx == _numLandmarks);
+
+ // temporary structures for building the pattern of the Schur complement
+ SparseBlockMatrixHashMap<PoseMatrixType>* schurMatrixLookup = 0;
+ if (_doSchur) {
+ schurMatrixLookup = new SparseBlockMatrixHashMap<PoseMatrixType>(_Hschur->rowBlockIndices(), _Hschur->colBlockIndices());
+ schurMatrixLookup->blockCols().resize(_Hschur->blockCols().size());
+ }
+
+ // here we assume that the landmark indices start after the pose ones
+ // create the structure in Hpp, Hll and in Hpl
+ for (SparseOptimizer::EdgeContainer::const_iterator it=_optimizer->activeEdges().begin(); it!=_optimizer->activeEdges().end(); ++it){
+ OptimizableGraph::Edge* e = *it;
+
+ for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
+ OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
+ int ind1 = v1->hessianIndex();
+ if (ind1 == -1)
+ continue;
+ int indexV1Bak = ind1;
+ for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
+ OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
+ int ind2 = v2->hessianIndex();
+ if (ind2 == -1)
+ continue;
+ ind1 = indexV1Bak;
+ bool transposedBlock = ind1 > ind2;
+ if (transposedBlock){ // make sure, we allocate the upper triangle block
+ swap(ind1, ind2);
+ }
+ if (! v1->marginalized() && !v2->marginalized()){
+ PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
+ if (zeroBlocks)
+ m->setZero();
+ e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
+ if (_Hschur) {// assume this is only needed in case we solve with the schur complement
+ schurMatrixLookup->addBlock(ind1, ind2);
+ }
+ } else if (v1->marginalized() && v2->marginalized()){
+ // RAINER hmm.... should we ever reach this here????
+ LandmarkMatrixType* m = _Hll->block(ind1-_numPoses, ind2-_numPoses, true);
+ if (zeroBlocks)
+ m->setZero();
+ e->mapHessianMemory(m->data(), viIdx, vjIdx, false);
+ } else {
+ if (v1->marginalized()){
+ PoseLandmarkMatrixType* m = _Hpl->block(v2->hessianIndex(),v1->hessianIndex()-_numPoses, true);
+ if (zeroBlocks)
+ m->setZero();
+ e->mapHessianMemory(m->data(), viIdx, vjIdx, true); // transpose the block before writing to it
+ } else {
+ PoseLandmarkMatrixType* m = _Hpl->block(v1->hessianIndex(),v2->hessianIndex()-_numPoses, true);
+ if (zeroBlocks)
+ m->setZero();
+ e->mapHessianMemory(m->data(), viIdx, vjIdx, false); // directly the block
+ }
+ }
+ }
+ }
+ }
+
+ if (! _doSchur)
+ return true;
+
+ _DInvSchur->diagonal().resize(landmarkIdx);
+ _Hpl->fillSparseBlockMatrixCCS(*_HplCCS);
+
+ for (size_t i = 0; i < _optimizer->indexMapping().size(); ++i) {
+ OptimizableGraph::Vertex* v = _optimizer->indexMapping()[i];
+ if (v->marginalized()){
+ const HyperGraph::EdgeSet& vedges=v->edges();
+ for (HyperGraph::EdgeSet::const_iterator it1=vedges.begin(); it1!=vedges.end(); ++it1){
+ for (size_t i=0; i<(*it1)->vertices().size(); ++i)
+ {
+ OptimizableGraph::Vertex* v1= (OptimizableGraph::Vertex*) (*it1)->vertex(i);
+ if (v1->hessianIndex()==-1 || v1==v)
+ continue;
+ for (HyperGraph::EdgeSet::const_iterator it2=vedges.begin(); it2!=vedges.end(); ++it2){
+ for (size_t j=0; j<(*it2)->vertices().size(); ++j)
+ {
+ OptimizableGraph::Vertex* v2= (OptimizableGraph::Vertex*) (*it2)->vertex(j);
+ if (v2->hessianIndex()==-1 || v2==v)
+ continue;
+ int i1=v1->hessianIndex();
+ int i2=v2->hessianIndex();
+ if (i1<=i2) {
+ schurMatrixLookup->addBlock(i1, i2);
+ }
+ }
+ }
+ }
+ }
+ }
+ }
+
+ _Hschur->takePatternFromHash(*schurMatrixLookup);
+ delete schurMatrixLookup;
+ _Hschur->fillSparseBlockMatrixCCSTransposed(*_HschurTransposedCCS);
+
+ return true;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
+{
+ for (std::vector<HyperGraph::Vertex*>::const_iterator vit = vset.begin(); vit != vset.end(); ++vit) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
+ int dim = v->dimension();
+ if (! v->marginalized()){
+ v->setColInHessian(_sizePoses);
+ _sizePoses+=dim;
+ _Hpp->rowBlockIndices().push_back(_sizePoses);
+ _Hpp->colBlockIndices().push_back(_sizePoses);
+ _Hpp->blockCols().push_back(typename SparseBlockMatrix<PoseMatrixType>::IntBlockMap());
+ ++_numPoses;
+ int ind = v->hessianIndex();
+ PoseMatrixType* m = _Hpp->block(ind, ind, true);
+ v->mapHessianMemory(m->data());
+ } else {
+ std::cerr << "updateStructure(): Schur not supported" << std::endl;
+ abort();
+ }
+ }
+ resizeVector(_sizePoses + _sizeLandmarks);
+
+ for (HyperGraph::EdgeSet::const_iterator it = edges.begin(); it != edges.end(); ++it) {
+ OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+
+ for (size_t viIdx = 0; viIdx < e->vertices().size(); ++viIdx) {
+ OptimizableGraph::Vertex* v1 = (OptimizableGraph::Vertex*) e->vertex(viIdx);
+ int ind1 = v1->hessianIndex();
+ int indexV1Bak = ind1;
+ if (ind1 == -1)
+ continue;
+ for (size_t vjIdx = viIdx + 1; vjIdx < e->vertices().size(); ++vjIdx) {
+ OptimizableGraph::Vertex* v2 = (OptimizableGraph::Vertex*) e->vertex(vjIdx);
+ int ind2 = v2->hessianIndex();
+ if (ind2 == -1)
+ continue;
+ ind1 = indexV1Bak;
+ bool transposedBlock = ind1 > ind2;
+ if (transposedBlock) // make sure, we allocate the upper triangular block
+ swap(ind1, ind2);
+
+ if (! v1->marginalized() && !v2->marginalized()) {
+ PoseMatrixType* m = _Hpp->block(ind1, ind2, true);
+ e->mapHessianMemory(m->data(), viIdx, vjIdx, transposedBlock);
+ } else {
+ std::cerr << __PRETTY_FUNCTION__ << ": not supported" << std::endl;
+ }
+ }
+ }
+
+ }
+
+ return true;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::solve(){
+ //cerr << __PRETTY_FUNCTION__ << endl;
+ if (! _doSchur){
+ double t=get_monotonic_time();
+ bool ok = _linearSolver->solve(*_Hpp, _x, _b);
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeLinearSolver = get_monotonic_time() - t;
+ globalStats->hessianDimension = globalStats->hessianPoseDimension = _Hpp->cols();
+ }
+ return ok;
+ }
+
+ // schur thing
+
+ // backup the coefficient matrix
+ double t=get_monotonic_time();
+
+ // _Hschur = _Hpp, but keeping the pattern of _Hschur
+ _Hschur->clear();
+ _Hpp->add(_Hschur);
+
+ //_DInvSchur->clear();
+ memset (_coefficients, 0, _sizePoses*sizeof(double));
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) schedule(dynamic, 10)
+# endif
+ for (int landmarkIndex = 0; landmarkIndex < static_cast<int>(_Hll->blockCols().size()); ++landmarkIndex) {
+ const typename SparseBlockMatrix<LandmarkMatrixType>::IntBlockMap& marginalizeColumn = _Hll->blockCols()[landmarkIndex];
+ assert(marginalizeColumn.size() == 1 && "more than one block in _Hll column");
+
+ // calculate inverse block for the landmark
+ const LandmarkMatrixType * D = marginalizeColumn.begin()->second;
+ assert (D && D->rows()==D->cols() && "Error in landmark matrix");
+ LandmarkMatrixType& Dinv = _DInvSchur->diagonal()[landmarkIndex];
+ Dinv = D->inverse();
+
+ LandmarkVectorType db(D->rows());
+ for (int j=0; j<D->rows(); ++j) {
+ db[j]=_b[_Hll->rowBaseOfBlock(landmarkIndex) + _sizePoses + j];
+ }
+ db=Dinv*db;
+
+ assert((size_t)landmarkIndex < _HplCCS->blockCols().size() && "Index out of bounds");
+ const typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn& landmarkColumn = _HplCCS->blockCols()[landmarkIndex];
+
+ for (typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_outer = landmarkColumn.begin();
+ it_outer != landmarkColumn.end(); ++it_outer) {
+ int i1 = it_outer->row;
+
+ const PoseLandmarkMatrixType* Bi = it_outer->block;
+ assert(Bi);
+
+ PoseLandmarkMatrixType BDinv = (*Bi)*(Dinv);
+ assert(_HplCCS->rowBaseOfBlock(i1) < _sizePoses && "Index out of bounds");
+ typename PoseVectorType::MapType Bb(&_coefficients[_HplCCS->rowBaseOfBlock(i1)], Bi->rows());
+# ifdef G2O_OPENMP
+ ScopedOpenMPMutex mutexLock(&_coefficientsMutex[i1]);
+# endif
+ Bb.noalias() += (*Bi)*db;
+
+ assert(i1 >= 0 && i1 < static_cast<int>(_HschurTransposedCCS->blockCols().size()) && "Index out of bounds");
+ typename SparseBlockMatrixCCS<PoseMatrixType>::SparseColumn::iterator targetColumnIt = _HschurTransposedCCS->blockCols()[i1].begin();
+
+ typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::RowBlock aux(i1, 0);
+ typename SparseBlockMatrixCCS<PoseLandmarkMatrixType>::SparseColumn::const_iterator it_inner = lower_bound(landmarkColumn.begin(), landmarkColumn.end(), aux);
+ for (; it_inner != landmarkColumn.end(); ++it_inner) {
+ int i2 = it_inner->row;
+ const PoseLandmarkMatrixType* Bj = it_inner->block;
+ assert(Bj);
+ while (targetColumnIt->row < i2 /*&& targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end()*/)
+ ++targetColumnIt;
+ assert(targetColumnIt != _HschurTransposedCCS->blockCols()[i1].end() && targetColumnIt->row == i2 && "invalid iterator, something wrong with the matrix structure");
+ PoseMatrixType* Hi1i2 = targetColumnIt->block;//_Hschur->block(i1,i2);
+ assert(Hi1i2);
+ (*Hi1i2).noalias() -= BDinv*Bj->transpose();
+ }
+ }
+ }
+ //cerr << "Solve [marginalize] = " << get_monotonic_time()-t << endl;
+
+ // _bschur = _b for calling solver, and not touching _b
+ memcpy(_bschur, _b, _sizePoses * sizeof(double));
+ for (int i=0; i<_sizePoses; ++i){
+ _bschur[i]-=_coefficients[i];
+ }
+
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats){
+ globalStats->timeSchurComplement = get_monotonic_time() - t;
+ }
+
+ t=get_monotonic_time();
+ bool solvedPoses = _linearSolver->solve(*_Hschur, _x, _bschur);
+ if (globalStats) {
+ globalStats->timeLinearSolver = get_monotonic_time() - t;
+ globalStats->hessianPoseDimension = _Hpp->cols();
+ globalStats->hessianLandmarkDimension = _Hll->cols();
+ globalStats->hessianDimension = globalStats->hessianPoseDimension + globalStats->hessianLandmarkDimension;
+ }
+ //cerr << "Solve [decompose and solve] = " << get_monotonic_time()-t << endl;
+
+ if (! solvedPoses)
+ return false;
+
+ // _x contains the solution for the poses, now applying it to the landmarks to get the new part of the
+ // solution;
+ double* xp = _x;
+ double* cp = _coefficients;
+
+ double* xl=_x+_sizePoses;
+ double* cl=_coefficients + _sizePoses;
+ double* bl=_b+_sizePoses;
+
+ // cp = -xp
+ for (int i=0; i<_sizePoses; ++i)
+ cp[i]=-xp[i];
+
+ // cl = bl
+ memcpy(cl,bl,_sizeLandmarks*sizeof(double));
+
+ // cl = bl - Bt * xp
+ //Bt->multiply(cl, cp);
+ _HplCCS->rightMultiply(cl, cp);
+
+ // xl = Dinv * cl
+ memset(xl,0, _sizeLandmarks*sizeof(double));
+ _DInvSchur->multiply(xl,cl);
+ //_DInvSchur->rightMultiply(xl,cl);
+ //cerr << "Solve [landmark delta] = " << get_monotonic_time()-t << endl;
+
+ return true;
+}
+
+
+template <typename Traits>
+bool BlockSolver<Traits>::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
+{
+ double t = get_monotonic_time();
+ bool ok = _linearSolver->solvePattern(spinv, blockIndices, *_Hpp);
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeMarginals = get_monotonic_time() - t;
+ }
+ return ok;
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::buildSystem()
+{
+ // clear b vector
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
+# endif
+ for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
+ OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
+ assert(v);
+ v->clearQuadraticForm();
+ }
+ _Hpp->clear();
+ if (_doSchur) {
+ _Hll->clear();
+ _Hpl->clear();
+ }
+
+ // resetting the terms for the pairwise constraints
+ // built up the current system by storing the Hessian blocks in the edges and vertices
+# ifndef G2O_OPENMP
+ // no threading, we do not need to copy the workspace
+ JacobianWorkspace& jacobianWorkspace = _optimizer->jacobianWorkspace();
+# else
+ // if running with threads need to produce copies of the workspace for each thread
+ JacobianWorkspace jacobianWorkspace = _optimizer->jacobianWorkspace();
+# pragma omp parallel for default (shared) firstprivate(jacobianWorkspace) if (_optimizer->activeEdges().size() > 100)
+# endif
+ for (int k = 0; k < static_cast<int>(_optimizer->activeEdges().size()); ++k) {
+ OptimizableGraph::Edge* e = _optimizer->activeEdges()[k];
+ e->linearizeOplus(jacobianWorkspace); // jacobian of the nodes' oplus (manifold)
+ e->constructQuadraticForm();
+# ifndef NDEBUG
+ for (size_t i = 0; i < e->vertices().size(); ++i) {
+ const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
+ if (! v->fixed()) {
+ bool hasANan = arrayHasNaN(jacobianWorkspace.workspaceForVertex(i), e->dimension() * v->dimension());
+ if (hasANan) {
+ cerr << "buildSystem(): NaN within Jacobian for edge " << e << " for vertex " << i << endl;
+ break;
+ }
+ }
+ }
+# endif
+ }
+
+ // flush the current system in a sparse block matrix
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_optimizer->indexMapping().size() > 1000)
+# endif
+ for (int i = 0; i < static_cast<int>(_optimizer->indexMapping().size()); ++i) {
+ OptimizableGraph::Vertex* v=_optimizer->indexMapping()[i];
+ int iBase = v->colInHessian();
+ if (v->marginalized())
+ iBase+=_sizePoses;
+ v->copyB(_b+iBase);
+ }
+
+ return 0;
+}
+
+
+template <typename Traits>
+bool BlockSolver<Traits>::setLambda(double lambda, bool backup)
+{
+ if (backup) {
+ _diagonalBackupPose.resize(_numPoses);
+ _diagonalBackupLandmark.resize(_numLandmarks);
+ }
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_numPoses > 100)
+# endif
+ for (int i = 0; i < _numPoses; ++i) {
+ PoseMatrixType *b=_Hpp->block(i,i);
+ if (backup)
+ _diagonalBackupPose[i] = b->diagonal();
+ b->diagonal().array() += lambda;
+ }
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_numLandmarks > 100)
+# endif
+ for (int i = 0; i < _numLandmarks; ++i) {
+ LandmarkMatrixType *b=_Hll->block(i,i);
+ if (backup)
+ _diagonalBackupLandmark[i] = b->diagonal();
+ b->diagonal().array() += lambda;
+ }
+ return true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::restoreDiagonal()
+{
+ assert((int) _diagonalBackupPose.size() == _numPoses && "Mismatch in dimensions");
+ assert((int) _diagonalBackupLandmark.size() == _numLandmarks && "Mismatch in dimensions");
+ for (int i = 0; i < _numPoses; ++i) {
+ PoseMatrixType *b=_Hpp->block(i,i);
+ b->diagonal() = _diagonalBackupPose[i];
+ }
+ for (int i = 0; i < _numLandmarks; ++i) {
+ LandmarkMatrixType *b=_Hll->block(i,i);
+ b->diagonal() = _diagonalBackupLandmark[i];
+ }
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::init(SparseOptimizer* optimizer, bool online)
+{
+ _optimizer = optimizer;
+ if (! online) {
+ if (_Hpp)
+ _Hpp->clear();
+ if (_Hpl)
+ _Hpl->clear();
+ if (_Hll)
+ _Hll->clear();
+ }
+ _linearSolver->init();
+ return true;
+}
+
+template <typename Traits>
+void BlockSolver<Traits>::setWriteDebug(bool writeDebug)
+{
+ _linearSolver->setWriteDebug(writeDebug);
+}
+
+template <typename Traits>
+bool BlockSolver<Traits>::saveHessian(const std::string& fileName) const
+{
+ return _Hpp->writeOctave(fileName.c_str(), true);
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..89f9c240d7b56924c6ad04ce06ec463cf3911673
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.cpp
@@ -0,0 +1,183 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 G. Grisetti, R. Kuemmerle, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "cache.h"
+#include "optimizable_graph.h"
+#include "factory.h"
+
+#include <iostream>
+
+namespace g2o {
+ using namespace std;
+
+ Cache::CacheKey::CacheKey() :
+ _type(), _parameters()
+ {
+ }
+
+ Cache::CacheKey::CacheKey(const std::string& type_, const ParameterVector& parameters_) :
+ _type(type_), _parameters(parameters_)
+ {
+ }
+
+ Cache::Cache(CacheContainer* container_, const ParameterVector& parameters_) :
+ _updateNeeded(true), _parameters(parameters_), _container(container_)
+ {
+ }
+
+ bool Cache::CacheKey::operator<(const Cache::CacheKey& c) const{
+ if (_type < c._type)
+ return true;
+ return std::lexicographical_compare (_parameters.begin( ), _parameters.end( ),
+ c._parameters.begin( ), c._parameters.end( ) );
+ }
+
+
+ OptimizableGraph::Vertex* Cache::vertex() {
+ if (container() )
+ return container()->vertex();
+ return 0;
+ }
+
+ OptimizableGraph* Cache::graph() {
+ if (container())
+ return container()->graph();
+ return 0;
+ }
+
+ CacheContainer* Cache::container() {
+ return _container;
+ }
+
+ ParameterVector& Cache::parameters() {
+ return _parameters;
+ }
+
+ Cache::CacheKey Cache::key() const {
+ Factory* factory=Factory::instance();
+ return CacheKey(factory->tag(this), _parameters);
+ };
+
+
+ void Cache::update(){
+ if (! _updateNeeded)
+ return;
+ for(std::vector<Cache*>::iterator it=_parentCaches.begin(); it!=_parentCaches.end(); it++){
+ (*it)->update();
+ }
+ updateImpl();
+ _updateNeeded=false;
+ }
+
+ Cache* Cache::installDependency(const std::string& type_, const std::vector<int>& parameterIndices){
+ ParameterVector pv(parameterIndices.size());
+ for (size_t i=0; i<parameterIndices.size(); i++){
+ if (parameterIndices[i]<0 || parameterIndices[i] >=(int)_parameters.size())
+ return 0;
+ pv[i]=_parameters[ parameterIndices[i] ];
+ }
+ CacheKey k(type_, pv);
+ if (!container())
+ return 0;
+ Cache* c=container()->findCache(k);
+ if (!c) {
+ c = container()->createCache(k);
+ }
+ if (c)
+ _parentCaches.push_back(c);
+ return c;
+ }
+
+ bool Cache::resolveDependancies(){
+ return true;
+ }
+
+ CacheContainer::CacheContainer(OptimizableGraph::Vertex* vertex_) {
+ _vertex = vertex_;
+ }
+
+ Cache* CacheContainer::findCache(const Cache::CacheKey& key) {
+ iterator it=find(key);
+ if (it==end())
+ return 0;
+ return it->second;
+ }
+
+ Cache* CacheContainer::createCache(const Cache::CacheKey& key){
+ Factory* f = Factory::instance();
+ HyperGraph::HyperGraphElement* e = f->construct(key.type());
+ if (!e) {
+ cerr << __PRETTY_FUNCTION__ << endl;
+ cerr << "fatal error in creating cache of type " << key.type() << endl;
+ return 0;
+ }
+ Cache* c = dynamic_cast<Cache*>(e);
+ if (! c){
+ cerr << __PRETTY_FUNCTION__ << endl;
+ cerr << "fatal error in creating cache of type " << key.type() << endl;
+ return 0;
+ }
+ c->_container = this;
+ c->_parameters = key._parameters;
+ if (c->resolveDependancies()){
+ insert(make_pair(key,c));
+ c->update();
+ return c;
+ }
+ return 0;
+ }
+
+ OptimizableGraph::Vertex* CacheContainer::vertex() {
+ return _vertex;
+ }
+
+ OptimizableGraph* CacheContainer::graph(){
+ if (_vertex)
+ return _vertex->graph();
+ return 0;
+ }
+
+ void CacheContainer::update() {
+ for (iterator it=begin(); it!=end(); it++){
+ (it->second)->update();
+ }
+ _updateNeeded=false;
+ }
+
+ void CacheContainer::setUpdateNeeded(bool needUpdate) {
+ _updateNeeded=needUpdate;
+ for (iterator it=begin(); it!=end(); ++it){
+ (it->second)->_updateNeeded = needUpdate;
+ }
+ }
+
+ CacheContainer::~CacheContainer(){
+ for (iterator it=begin(); it!=end(); ++it){
+ delete (it->second);
+ }
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.h
new file mode 100644
index 0000000000000000000000000000000000000000..c5b00a491eac67801276b25f41ab09ffab507841
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/cache.h
@@ -0,0 +1,140 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CACHE_HH_
+#define G2O_CACHE_HH_
+
+#include <map>
+
+#include "optimizable_graph.h"
+
+namespace g2o {
+
+ class CacheContainer;
+
+ class Cache: public HyperGraph::HyperGraphElement
+ {
+ public:
+ friend class CacheContainer;
+ class CacheKey
+ {
+ public:
+ friend class CacheContainer;
+ CacheKey();
+ CacheKey(const std::string& type_, const ParameterVector& parameters_);
+
+ bool operator<(const CacheKey& c) const;
+
+ const std::string& type() const { return _type;}
+ const ParameterVector& parameters() const { return _parameters;}
+
+ protected:
+ std::string _type;
+ ParameterVector _parameters;
+ };
+
+ Cache(CacheContainer* container_ = 0, const ParameterVector& parameters_ = ParameterVector());
+
+ CacheKey key() const;
+
+ OptimizableGraph::Vertex* vertex();
+ OptimizableGraph* graph();
+ CacheContainer* container();
+ ParameterVector& parameters();
+
+ void update();
+
+ virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_CACHE;}
+
+ protected:
+ //! redefine this to do the update
+ virtual void updateImpl() = 0;
+
+ /**
+ * this function installs and satisfies a cache
+ * @param type_: the typename of the dependency
+ * @param parameterIndices: a vector containing the indices if the parameters
+ * in _parameters that will be used to assemble the Key of the cache being created
+ * For example if I have a cache of type C2, having parameters "A, B, and C",
+ * and it depends on a cache of type C1 that depends on the parameters A and C,
+ * the parameterIndices should contain "0, 2", since they are the positions in the
+ * parameter vector of C2 of the parameters needed to construct C1.
+ * @returns the newly created cache
+ */
+ Cache* installDependency(const std::string& type_, const std::vector<int>& parameterIndices);
+
+ /**
+ * Function to be called from a cache that has dependencies. It just invokes a
+ * sequence of installDependency().
+ * Although the caches returned are stored in the _parentCache vector,
+ * it is better that you redefine your own cache member variables, for better readability
+ */
+ virtual bool resolveDependancies();
+
+ bool _updateNeeded;
+ ParameterVector _parameters;
+ std::vector<Cache*> _parentCaches;
+ CacheContainer* _container;
+ };
+
+ class CacheContainer: public std::map<Cache::CacheKey, Cache*>
+ {
+ public:
+ CacheContainer(OptimizableGraph::Vertex* vertex_);
+ virtual ~CacheContainer();
+ OptimizableGraph::Vertex* vertex();
+ OptimizableGraph* graph();
+ Cache* findCache(const Cache::CacheKey& key);
+ Cache* createCache(const Cache::CacheKey& key);
+ void setUpdateNeeded(bool needUpdate=true);
+ void update();
+ protected:
+ OptimizableGraph::Vertex* _vertex;
+ bool _updateNeeded;
+ };
+
+
+ template <typename CacheType>
+ void OptimizableGraph::Edge::resolveCache(CacheType*& cache,
+ OptimizableGraph::Vertex* v,
+ const std::string& type_,
+ const ParameterVector& parameters_)
+ {
+ cache = 0;
+ CacheContainer* container= v->cacheContainer();
+ Cache::CacheKey key(type_, parameters_);
+ Cache* c = container->findCache(key);
+ if (!c) {
+ c = container->createCache(key);
+ }
+ if (c) {
+ cache = dynamic_cast<CacheType*>(c);
+ }
+ }
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/creators.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/creators.h
new file mode 100644
index 0000000000000000000000000000000000000000..9ca9967cb3c92bc676e98c487100569583e819a3
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/creators.h
@@ -0,0 +1,75 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CREATORS_H
+#define G2O_CREATORS_H
+
+#include "hyper_graph.h"
+
+#include <string>
+#include <typeinfo>
+
+namespace g2o
+{
+
+ /**
+ * \brief Abstract interface for allocating HyperGraphElement
+ */
+ class AbstractHyperGraphElementCreator
+ {
+ public:
+ /**
+ * create a hyper graph element. Has to implemented in derived class.
+ */
+ virtual HyperGraph::HyperGraphElement* construct() = 0;
+ /**
+ * name of the class to be created. Has to implemented in derived class.
+ */
+ virtual const std::string& name() const = 0;
+
+ virtual ~AbstractHyperGraphElementCreator() { }
+ };
+
+ /**
+ * \brief templatized creator class which creates graph elements
+ */
+ template <typename T>
+ class HyperGraphElementCreator : public AbstractHyperGraphElementCreator
+ {
+ public:
+ HyperGraphElementCreator() : _name(typeid(T).name()) {}
+#if defined (WINDOWS) && defined(__GNUC__) // force stack alignment on Windows with GCC
+ __attribute__((force_align_arg_pointer))
+#endif
+ HyperGraph::HyperGraphElement* construct() { return new T;}
+ virtual const std::string& name() const { return _name;}
+ protected:
+ std::string _name;
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/eigen_types.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/eigen_types.h
new file mode 100644
index 0000000000000000000000000000000000000000..f2c685656285077b8bbba89043086095b07f1c3e
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/eigen_types.h
@@ -0,0 +1,73 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_EIGEN_TYPES_H
+#define G2O_EIGEN_TYPES_H
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace g2o {
+
+ typedef Eigen::Matrix<int,2,1,Eigen::ColMajor> Vector2I;
+ typedef Eigen::Matrix<int,3,1,Eigen::ColMajor> Vector3I;
+ typedef Eigen::Matrix<int,4,1,Eigen::ColMajor> Vector4I;
+ typedef Eigen::Matrix<int,Eigen::Dynamic,1,Eigen::ColMajor> VectorXI;
+
+ typedef Eigen::Matrix<float,2,1,Eigen::ColMajor> Vector2F;
+ typedef Eigen::Matrix<float,3,1,Eigen::ColMajor> Vector3F;
+ typedef Eigen::Matrix<float,4,1,Eigen::ColMajor> Vector4F;
+ typedef Eigen::Matrix<float,Eigen::Dynamic,1,Eigen::ColMajor> VectorXF;
+
+ typedef Eigen::Matrix<double,2,1,Eigen::ColMajor> Vector2D;
+ typedef Eigen::Matrix<double,3,1,Eigen::ColMajor> Vector3D;
+ typedef Eigen::Matrix<double,4,1,Eigen::ColMajor> Vector4D;
+ typedef Eigen::Matrix<double,Eigen::Dynamic,1,Eigen::ColMajor> VectorXD;
+
+ typedef Eigen::Matrix<int,2,2,Eigen::ColMajor> Matrix2I;
+ typedef Eigen::Matrix<int,3,3,Eigen::ColMajor> Matrix3I;
+ typedef Eigen::Matrix<int,4,4,Eigen::ColMajor> Matrix4I;
+ typedef Eigen::Matrix<int,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXI;
+
+ typedef Eigen::Matrix<float,2,2,Eigen::ColMajor> Matrix2F;
+ typedef Eigen::Matrix<float,3,3,Eigen::ColMajor> Matrix3F;
+ typedef Eigen::Matrix<float,4,4,Eigen::ColMajor> Matrix4F;
+ typedef Eigen::Matrix<float,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXF;
+
+ typedef Eigen::Matrix<double,2,2,Eigen::ColMajor> Matrix2D;
+ typedef Eigen::Matrix<double,3,3,Eigen::ColMajor> Matrix3D;
+ typedef Eigen::Matrix<double,4,4,Eigen::ColMajor> Matrix4D;
+ typedef Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::ColMajor> MatrixXD;
+
+ typedef Eigen::Transform<double,2,Eigen::Isometry,Eigen::ColMajor> Isometry2D;
+ typedef Eigen::Transform<double,3,Eigen::Isometry,Eigen::ColMajor> Isometry3D;
+
+ typedef Eigen::Transform<double,2,Eigen::Affine,Eigen::ColMajor> Affine2D;
+ typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Affine3D;
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..010dac1cf6c95668da61c68358c5825e833cc4e1
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.cpp
@@ -0,0 +1,267 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "estimate_propagator.h"
+
+#include <queue>
+#include <vector>
+#include <cassert>
+#include <iostream>
+#include <algorithm>
+#include <fstream>
+
+//#define DEBUG_ESTIMATE_PROPAGATOR
+
+using namespace std;
+
+namespace g2o {
+
+# ifdef DEBUG_ESTIMATE_PROPAGATOR
+ struct FrontierLevelCmp {
+ bool operator()(EstimatePropagator::AdjacencyMapEntry* e1, EstimatePropagator::AdjacencyMapEntry* e2) const
+ {
+ return e1->frontierLevel() < e2->frontierLevel();
+ }
+ };
+# endif
+
+ EstimatePropagator::AdjacencyMapEntry::AdjacencyMapEntry()
+ {
+ reset();
+ }
+
+ void EstimatePropagator::AdjacencyMapEntry::reset()
+ {
+ _child = 0;
+ _parent.clear();
+ _edge = 0;
+ _distance = numeric_limits<double>::max();
+ _frontierLevel = -1;
+ inQueue = false;
+ }
+
+ EstimatePropagator::EstimatePropagator(OptimizableGraph* g): _graph(g)
+ {
+ for (OptimizableGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); ++it){
+ AdjacencyMapEntry entry;
+ entry._child = static_cast<OptimizableGraph::Vertex*>(it->second);
+ _adjacencyMap.insert(make_pair(entry.child(), entry));
+ }
+ }
+
+ void EstimatePropagator::reset()
+ {
+ for (OptimizableGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); ++it){
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ AdjacencyMap::iterator at = _adjacencyMap.find(v);
+ assert(at != _adjacencyMap.end());
+ at->second.reset();
+ }
+ _visited.clear();
+ }
+
+ void EstimatePropagator::propagate(OptimizableGraph::Vertex* v,
+ const EstimatePropagator::PropagateCost& cost,
+ const EstimatePropagator::PropagateAction& action,
+ double maxDistance,
+ double maxEdgeCost)
+ {
+ OptimizableGraph::VertexSet vset;
+ vset.insert(v);
+ propagate(vset, cost, action, maxDistance, maxEdgeCost);
+ }
+
+ void EstimatePropagator::propagate(OptimizableGraph::VertexSet& vset,
+ const EstimatePropagator::PropagateCost& cost,
+ const EstimatePropagator::PropagateAction& action,
+ double maxDistance,
+ double maxEdgeCost)
+ {
+ reset();
+
+ PriorityQueue frontier;
+ for (OptimizableGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*vit);
+ AdjacencyMap::iterator it = _adjacencyMap.find(v);
+ assert(it != _adjacencyMap.end());
+ it->second._distance = 0.;
+ it->second._parent.clear();
+ it->second._frontierLevel = 0;
+ frontier.push(&it->second);
+ }
+
+ while(! frontier.empty()){
+ AdjacencyMapEntry* entry = frontier.pop();
+ OptimizableGraph::Vertex* u = entry->child();
+ double uDistance = entry->distance();
+ //cerr << "uDistance " << uDistance << endl;
+
+ // initialize the vertex
+ if (entry->_frontierLevel > 0) {
+ action(entry->edge(), entry->parent(), u);
+ }
+
+ /* std::pair< OptimizableGraph::VertexSet::iterator, bool> insertResult = */ _visited.insert(u);
+ OptimizableGraph::EdgeSet::iterator et = u->edges().begin();
+ while (et != u->edges().end()){
+ OptimizableGraph::Edge* edge = static_cast<OptimizableGraph::Edge*>(*et);
+ ++et;
+
+ int maxFrontier = -1;
+ OptimizableGraph::VertexSet initializedVertices;
+ for (size_t i = 0; i < edge->vertices().size(); ++i) {
+ OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
+ AdjacencyMap::iterator ot = _adjacencyMap.find(z);
+ if (ot->second._distance != numeric_limits<double>::max()) {
+ initializedVertices.insert(z);
+ maxFrontier = (max)(maxFrontier, ot->second._frontierLevel);
+ }
+ }
+ assert(maxFrontier >= 0);
+
+ for (size_t i = 0; i < edge->vertices().size(); ++i) {
+ OptimizableGraph::Vertex* z = static_cast<OptimizableGraph::Vertex*>(edge->vertex(i));
+ if (z == u)
+ continue;
+
+ size_t wasInitialized = initializedVertices.erase(z);
+
+ double edgeDistance = cost(edge, initializedVertices, z);
+ if (edgeDistance > 0. && edgeDistance != std::numeric_limits<double>::max() && edgeDistance < maxEdgeCost) {
+ double zDistance = uDistance + edgeDistance;
+ //cerr << z->id() << " " << zDistance << endl;
+
+ AdjacencyMap::iterator ot = _adjacencyMap.find(z);
+ assert(ot!=_adjacencyMap.end());
+
+ if (zDistance < ot->second.distance() && zDistance < maxDistance){
+ //if (ot->second.inQueue)
+ //cerr << "Updating" << endl;
+ ot->second._distance = zDistance;
+ ot->second._parent = initializedVertices;
+ ot->second._edge = edge;
+ ot->second._frontierLevel = maxFrontier + 1;
+ frontier.push(&ot->second);
+ }
+ }
+
+ if (wasInitialized > 0)
+ initializedVertices.insert(z);
+
+ }
+ }
+ }
+
+ // writing debug information like cost for reaching each vertex and the parent used to initialize
+#ifdef DEBUG_ESTIMATE_PROPAGATOR
+ cerr << "Writing cost.dat" << endl;
+ ofstream costStream("cost.dat");
+ for (AdjacencyMap::const_iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
+ HyperGraph::Vertex* u = it->second.child();
+ costStream << "vertex " << u->id() << " cost " << it->second._distance << endl;
+ }
+ cerr << "Writing init.dat" << endl;
+ ofstream initStream("init.dat");
+ vector<AdjacencyMapEntry*> frontierLevels;
+ for (AdjacencyMap::iterator it = _adjacencyMap.begin(); it != _adjacencyMap.end(); ++it) {
+ if (it->second._frontierLevel > 0)
+ frontierLevels.push_back(&it->second);
+ }
+ sort(frontierLevels.begin(), frontierLevels.end(), FrontierLevelCmp());
+ for (vector<AdjacencyMapEntry*>::const_iterator it = frontierLevels.begin(); it != frontierLevels.end(); ++it) {
+ AdjacencyMapEntry* entry = *it;
+ OptimizableGraph::Vertex* to = entry->child();
+
+ initStream << "calling init level = " << entry->_frontierLevel << "\t (";
+ for (OptimizableGraph::VertexSet::iterator pit = entry->parent().begin(); pit != entry->parent().end(); ++pit) {
+ initStream << " " << (*pit)->id();
+ }
+ initStream << " ) -> " << to->id() << endl;
+ }
+#endif
+
+ }
+
+ void EstimatePropagator::PriorityQueue::push(AdjacencyMapEntry* entry)
+ {
+ assert(entry != NULL);
+ if (entry->inQueue) {
+ assert(entry->queueIt->second == entry);
+ erase(entry->queueIt);
+ }
+
+ entry->queueIt = insert(std::make_pair(entry->distance(), entry));
+ assert(entry->queueIt != end());
+ entry->inQueue = true;
+ }
+
+ EstimatePropagator::AdjacencyMapEntry* EstimatePropagator::PriorityQueue::pop()
+ {
+ assert(!empty());
+ iterator it = begin();
+ AdjacencyMapEntry* entry = it->second;
+ erase(it);
+
+ assert(entry != NULL);
+ entry->queueIt = end();
+ entry->inQueue = false;
+ return entry;
+ }
+
+ EstimatePropagatorCost::EstimatePropagatorCost (SparseOptimizer* graph) :
+ _graph(graph)
+ {
+ }
+
+ double EstimatePropagatorCost::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const
+ {
+ OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
+ OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
+ SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
+ if (it == _graph->activeEdges().end()) // it has to be an active edge
+ return std::numeric_limits<double>::max();
+ return e->initialEstimatePossible(from, to);
+ }
+
+ EstimatePropagatorCostOdometry::EstimatePropagatorCostOdometry(SparseOptimizer* graph) :
+ EstimatePropagatorCost(graph)
+ {
+ }
+
+ double EstimatePropagatorCostOdometry::operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const
+ {
+ OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(edge);
+ OptimizableGraph::Vertex* from = dynamic_cast<OptimizableGraph::Vertex*>(*from_.begin());
+ OptimizableGraph::Vertex* to = dynamic_cast<OptimizableGraph::Vertex*>(to_);
+ if (std::abs(from->id() - to->id()) != 1) // simple method to identify odometry edges in a pose graph
+ return std::numeric_limits<double>::max();
+ SparseOptimizer::EdgeContainer::const_iterator it = _graph->findActiveEdge(e);
+ if (it == _graph->activeEdges().end()) // it has to be an active edge
+ return std::numeric_limits<double>::max();
+ return e->initialEstimatePossible(from_, to);
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a16d11d3a3c0320eedbe64270df5e4d0e62cf84
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/estimate_propagator.h
@@ -0,0 +1,175 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ESTIMATE_PROPAGATOR_H
+#define G2O_ESTIMATE_PROPAGATOR_H
+
+#include "optimizable_graph.h"
+#include "sparse_optimizer.h"
+
+#include <map>
+#include <set>
+#include <limits>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+namespace g2o {
+
+ /**
+ * \brief cost for traversing along active edges in the optimizer
+ *
+ * You may derive an own one, if necessary. The default is to return initialEstimatePossible(from, to) for the edge.
+ */
+ class EstimatePropagatorCost {
+ public:
+ EstimatePropagatorCost (SparseOptimizer* graph);
+ virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to_) const;
+ virtual const char* name() const { return "spanning tree";}
+ protected:
+ SparseOptimizer* _graph;
+ };
+
+ /**
+ * \brief cost for traversing only odometry edges.
+ *
+ * Initialize your graph along odometry edges. An odometry edge is assumed to connect vertices
+ * whose IDs only differs by one.
+ */
+ class EstimatePropagatorCostOdometry : public EstimatePropagatorCost {
+ public:
+ EstimatePropagatorCostOdometry(SparseOptimizer* graph);
+ virtual double operator()(OptimizableGraph::Edge* edge, const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* to_) const;
+ virtual const char* name() const { return "odometry";}
+ };
+
+ /**
+ * \brief propagation of an initial guess
+ */
+ class EstimatePropagator {
+ public:
+
+ /**
+ * \brief Applying the action for propagating.
+ *
+ * You may derive an own one, if necessary. The default is to call initialEstimate(from, to) for the edge.
+ */
+ struct PropagateAction {
+ virtual void operator()(OptimizableGraph::Edge* e, const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) const
+ {
+ if (! to->fixed())
+ e->initialEstimate(from, to);
+ }
+ };
+
+ typedef EstimatePropagatorCost PropagateCost;
+
+ class AdjacencyMapEntry;
+
+ /**
+ * \brief priority queue for AdjacencyMapEntry
+ */
+ class PriorityQueue : public std::multimap<double, AdjacencyMapEntry*> {
+ public:
+ void push(AdjacencyMapEntry* entry);
+ AdjacencyMapEntry* pop();
+ };
+
+ /**
+ * \brief data structure for loopuk during Dijkstra
+ */
+ class AdjacencyMapEntry {
+ public:
+ friend class EstimatePropagator;
+ friend class PriorityQueue;
+ AdjacencyMapEntry();
+ void reset();
+ OptimizableGraph::Vertex* child() const {return _child;}
+ const OptimizableGraph::VertexSet& parent() const {return _parent;}
+ OptimizableGraph::Edge* edge() const {return _edge;}
+ double distance() const {return _distance;}
+ int frontierLevel() const { return _frontierLevel;}
+
+ protected:
+ OptimizableGraph::Vertex* _child;
+ OptimizableGraph::VertexSet _parent;
+ OptimizableGraph::Edge* _edge;
+ double _distance;
+ int _frontierLevel;
+ private: // for PriorityQueue
+ bool inQueue;
+ PriorityQueue::iterator queueIt;
+ };
+
+ /**
+ * \brief hash function for a vertex
+ */
+ class VertexIDHashFunction {
+ public:
+ size_t operator ()(const OptimizableGraph::Vertex* v) const { return v->id();}
+ };
+
+ typedef std::tr1::unordered_map<OptimizableGraph::Vertex*, AdjacencyMapEntry, VertexIDHashFunction> AdjacencyMap;
+
+ public:
+ EstimatePropagator(OptimizableGraph* g);
+ OptimizableGraph::VertexSet& visited() {return _visited; }
+ AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
+ OptimizableGraph* graph() {return _graph;}
+
+ /**
+ * propagate an initial guess starting from v. The function computes a spanning tree
+ * whereas the cost for each edge is determined by calling cost() and the action applied to
+ * each vertex is action().
+ */
+ void propagate(OptimizableGraph::Vertex* v,
+ const EstimatePropagator::PropagateCost& cost,
+ const EstimatePropagator::PropagateAction& action = PropagateAction(),
+ double maxDistance=std::numeric_limits<double>::max(),
+ double maxEdgeCost=std::numeric_limits<double>::max());
+
+ /**
+ * same as above but starting to propagate from a set of vertices instead of just a single one.
+ */
+ void propagate(OptimizableGraph::VertexSet& vset,
+ const EstimatePropagator::PropagateCost& cost,
+ const EstimatePropagator::PropagateAction& action = PropagateAction(),
+ double maxDistance=std::numeric_limits<double>::max(),
+ double maxEdgeCost=std::numeric_limits<double>::max());
+
+ protected:
+ void reset();
+
+ AdjacencyMap _adjacencyMap;
+ OptimizableGraph::VertexSet _visited;
+ OptimizableGraph* _graph;
+ };
+
+}
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6263ae2d8f84a5c14a5478e35f0b4fe0d5ce9995
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.cpp
@@ -0,0 +1,217 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "factory.h"
+
+#include "creators.h"
+#include "parameter.h"
+#include "cache.h"
+#include "optimizable_graph.h"
+#include "../stuff/color_macros.h"
+
+#include <iostream>
+#include <typeinfo>
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+Factory* Factory::factoryInstance = 0;
+
+Factory::Factory()
+{
+}
+
+Factory::~Factory()
+{
+# ifdef G2O_DEBUG_FACTORY
+ cerr << "# Factory destroying " << (void*)this << endl;
+# endif
+ for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ delete it->second->creator;
+ }
+ _creator.clear();
+ _tagLookup.clear();
+}
+
+Factory* Factory::instance()
+{
+ if (factoryInstance == 0) {
+ factoryInstance = new Factory;
+# ifdef G2O_DEBUG_FACTORY
+ cerr << "# Factory allocated " << (void*)factoryInstance << endl;
+# endif
+ }
+
+ return factoryInstance;
+}
+
+void Factory::registerType(const std::string& tag, AbstractHyperGraphElementCreator* c)
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end()) {
+ cerr << "FACTORY WARNING: Overwriting Vertex tag " << tag << endl;
+ assert(0);
+ }
+ TagLookup::const_iterator tagIt = _tagLookup.find(c->name());
+ if (tagIt != _tagLookup.end()) {
+ cerr << "FACTORY WARNING: Registering same class for two tags " << c->name() << endl;
+ assert(0);
+ }
+
+ CreatorInformation* ci = new CreatorInformation();
+ ci->creator = c;
+
+#ifdef G2O_DEBUG_FACTORY
+ cerr << "# Factory " << (void*)this << " constructing type " << tag << " ";
+#endif
+ // construct an element once to figure out its type
+ HyperGraph::HyperGraphElement* element = c->construct();
+ ci->elementTypeBit = element->elementType();
+
+#ifdef G2O_DEBUG_FACTORY
+ cerr << "done." << endl;
+ cerr << "# Factory " << (void*)this << " registering " << tag;
+ cerr << " " << (void*) c << " ";
+ switch (element->elementType()) {
+ case HyperGraph::HGET_VERTEX:
+ cerr << " -> Vertex";
+ break;
+ case HyperGraph::HGET_EDGE:
+ cerr << " -> Edge";
+ break;
+ case HyperGraph::HGET_PARAMETER:
+ cerr << " -> Parameter";
+ break;
+ case HyperGraph::HGET_CACHE:
+ cerr << " -> Cache";
+ break;
+ case HyperGraph::HGET_DATA:
+ cerr << " -> Data";
+ break;
+ default:
+ assert(0 && "Unknown element type occured, fix elementTypes");
+ break;
+ }
+ cerr << endl;
+#endif
+
+ _creator[tag] = ci;
+ _tagLookup[c->name()] = tag;
+ delete element;
+}
+
+ void Factory::unregisterType(const std::string& tag)
+ {
+ // Look for the tag
+ CreatorMap::iterator tagPosition = _creator.find(tag);
+
+ if (tagPosition != _creator.end()) {
+
+ AbstractHyperGraphElementCreator* c = tagPosition->second->creator;
+
+ // If we found it, remove the creator from the tag lookup map
+ TagLookup::iterator classPosition = _tagLookup.find(c->name());
+ if (classPosition != _tagLookup.end())
+ {
+ _tagLookup.erase(classPosition);
+ }
+ _creator.erase(tagPosition);
+ }
+ }
+
+HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag) const
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end()) {
+ //cerr << "tag " << tag << " -> " << (void*) foundIt->second->creator << " " << foundIt->second->creator->name() << endl;
+ return foundIt->second->creator->construct();
+ }
+ return 0;
+}
+
+const std::string& Factory::tag(const HyperGraph::HyperGraphElement* e) const
+{
+ static std::string emptyStr("");
+ TagLookup::const_iterator foundIt = _tagLookup.find(typeid(*e).name());
+ if (foundIt != _tagLookup.end())
+ return foundIt->second;
+ return emptyStr;
+}
+
+void Factory::fillKnownTypes(std::vector<std::string>& types) const
+{
+ types.clear();
+ for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+ types.push_back(it->first);
+}
+
+bool Factory::knowsTag(const std::string& tag, int* elementType) const
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt == _creator.end()) {
+ if (elementType)
+ *elementType = -1;
+ return false;
+ }
+ if (elementType)
+ *elementType = foundIt->second->elementTypeBit;
+ return true;
+}
+
+void Factory::destroy()
+{
+ delete factoryInstance;
+ factoryInstance = 0;
+}
+
+void Factory::printRegisteredTypes(std::ostream& os, bool comment) const
+{
+ if (comment)
+ os << "# ";
+ os << "types:" << endl;
+ for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ if (comment)
+ os << "#";
+ cerr << "\t" << it->first << endl;
+ }
+}
+
+HyperGraph::HyperGraphElement* Factory::construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const
+{
+ if (elemsToConstruct.none()) {
+ return construct(tag);
+ }
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end() && foundIt->second->elementTypeBit >= 0 && elemsToConstruct.test(foundIt->second->elementTypeBit)) {
+ return foundIt->second->creator->construct();
+ }
+ return 0;
+}
+
+} // end namespace
+
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..0e189aedc12658c82b55bdffcee1447a6671e2cc
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/factory.h
@@ -0,0 +1,179 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_FACTORY_H
+#define G2O_FACTORY_H
+
+#include "../../config.h"
+#include "../stuff/misc.h"
+#include "hyper_graph.h"
+#include "creators.h"
+
+#include <string>
+#include <map>
+#include <iostream>
+
+// define to get some verbose output
+//#define G2O_DEBUG_FACTORY
+
+namespace g2o {
+
+ class AbstractHyperGraphElementCreator;
+
+ /**
+ * \brief create vertices and edges based on TAGs in, for example, a file
+ */
+ class Factory
+ {
+ public:
+
+ //! return the instance
+ static Factory* instance();
+
+ //! free the instance
+ static void destroy();
+
+ /**
+ * register a tag for a specific creator
+ */
+ void registerType(const std::string& tag, AbstractHyperGraphElementCreator* c);
+
+ /**
+ * unregister a tag for a specific creator
+ */
+ void unregisterType(const std::string& tag);
+
+ /**
+ * construct a graph element based on its tag
+ */
+ HyperGraph::HyperGraphElement* construct(const std::string& tag) const;
+
+ /**
+ * construct a graph element based on its tag, but only if it's type (a bitmask) matches. A bitmask without any
+ * bit set will construct any item. Otherwise a bit has to be set to allow construction of a graph element.
+ */
+ HyperGraph::HyperGraphElement* construct(const std::string& tag, const HyperGraph::GraphElemBitset& elemsToConstruct) const;
+
+ /**
+ * return whether the factory knows this tag or not
+ */
+ bool knowsTag(const std::string& tag, int* elementType = 0) const;
+
+ //! return the TAG given a vertex
+ const std::string& tag(const HyperGraph::HyperGraphElement* v) const;
+
+ /**
+ * get a list of all known types
+ */
+ void fillKnownTypes(std::vector<std::string>& types) const;
+
+ /**
+ * print a list of the known registered types to the given stream
+ */
+ void printRegisteredTypes(std::ostream& os, bool comment = false) const;
+
+ protected:
+ class CreatorInformation
+ {
+ public:
+ AbstractHyperGraphElementCreator* creator;
+ int elementTypeBit;
+ CreatorInformation()
+ {
+ creator = 0;
+ elementTypeBit = -1;
+ }
+
+ ~CreatorInformation()
+ {
+ std::cout << "Deleting " << (void*) creator << std::endl;
+
+ delete creator;
+ }
+ };
+
+ typedef std::map<std::string, CreatorInformation*> CreatorMap;
+ typedef std::map<std::string, std::string> TagLookup;
+ Factory();
+ ~Factory();
+
+ CreatorMap _creator; ///< look-up map for the existing creators
+ TagLookup _tagLookup; ///< reverse look-up, class name to tag
+
+ private:
+ static Factory* factoryInstance;
+ };
+
+ template<typename T>
+ class RegisterTypeProxy
+ {
+ public:
+ RegisterTypeProxy(const std::string& name) : _name(name)
+ {
+#ifdef G2O_DEBUG_FACTORY
+ std::cout << __FUNCTION__ << ": Registering " << _name << " of type " << typeid(T).name() << std::endl;
+#endif
+ Factory::instance()->registerType(_name, new HyperGraphElementCreator<T>());
+ }
+
+ ~RegisterTypeProxy()
+ {
+#ifdef G2O_DEBUG_FACTORY
+ std::cout << __FUNCTION__ << ": Unregistering " << _name << " of type " << typeid(T).name() << std::endl;
+#endif
+ Factory::instance()->unregisterType(_name);
+ }
+
+ private:
+ std::string _name;
+ };
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+# define G2O_FACTORY_EXPORT __declspec(dllexport)
+# define G2O_FACTORY_IMPORT __declspec(dllimport)
+#else
+# define G2O_FACTORY_EXPORT
+# define G2O_FACTORY_IMPORT
+#endif
+
+ // These macros are used to automate registering types and forcing linkage
+#define G2O_REGISTER_TYPE(name, classname) \
+ extern "C" void G2O_FACTORY_EXPORT g2o_type_##classname(void) {} \
+ static g2o::RegisterTypeProxy<classname> g_type_proxy_##classname(#name);
+
+#define G2O_USE_TYPE_BY_CLASS_NAME(classname) \
+ extern "C" void G2O_FACTORY_IMPORT g2o_type_##classname(void); \
+ static g2o::ForceLinker proxy_##classname(g2o_type_##classname);
+
+#define G2O_REGISTER_TYPE_GROUP(typeGroupName) \
+ extern "C" void G2O_FACTORY_EXPORT g2o_type_group_##typeGroupName(void) {}
+
+#define G2O_USE_TYPE_GROUP(typeGroupName) \
+ extern "C" void G2O_FACTORY_IMPORT g2o_type_group_##typeGroupName(void); \
+ static g2o::ForceLinker g2o_force_type_link_##typeGroupName(g2o_type_group_##typeGroupName);
+}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a54eca5232b5c4f54e8f9412d6b9eb37e3f094be
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.cpp
@@ -0,0 +1,261 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include <queue>
+#include <deque>
+#include <vector>
+#include <assert.h>
+#include <iostream>
+#include "hyper_dijkstra.h"
+#include "../stuff/macros.h"
+
+namespace g2o{
+
+ using namespace std;
+
+ double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e){
+ (void) v;
+ (void) vParent;
+ (void) e;
+ return std::numeric_limits<double>::max();
+ }
+
+ double HyperDijkstra::TreeAction::perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance){
+ if (distance==-1)
+ return perform (v,vParent,e);
+ return std::numeric_limits<double>::max();
+ }
+
+ HyperDijkstra::AdjacencyMapEntry::AdjacencyMapEntry(HyperGraph::Vertex* child_, HyperGraph::Vertex* parent_,
+ HyperGraph::Edge* edge_, double distance_)
+ {
+ _child=child_;
+ _parent=parent_;
+ _edge=edge_;
+ _distance=distance_;
+ }
+
+ HyperDijkstra::HyperDijkstra(HyperGraph* g): _graph(g)
+ {
+ for (HyperGraph::VertexIDMap::const_iterator it=_graph->vertices().begin(); it!=_graph->vertices().end(); it++){
+ AdjacencyMapEntry entry(it->second, 0,0,std::numeric_limits< double >::max());
+ _adjacencyMap.insert(make_pair(entry.child(), entry));
+ }
+ }
+
+ void HyperDijkstra::reset()
+ {
+ for (HyperGraph::VertexSet::iterator it=_visited.begin(); it!=_visited.end(); it++){
+ AdjacencyMap::iterator at=_adjacencyMap.find(*it);
+ assert(at!=_adjacencyMap.end());
+ at->second=AdjacencyMapEntry(at->first,0,0,std::numeric_limits< double >::max());
+ }
+ _visited.clear();
+ }
+
+
+ bool operator<(const HyperDijkstra::AdjacencyMapEntry& a, const HyperDijkstra::AdjacencyMapEntry& b)
+ {
+ return a.distance()>b.distance();
+ }
+
+
+ void HyperDijkstra::shortestPaths(HyperGraph::VertexSet& vset, HyperDijkstra::CostFunction* cost,
+ double maxDistance, double comparisonConditioner, bool directed, double maxEdgeCost)
+ {
+ reset();
+ std::priority_queue< AdjacencyMapEntry > frontier;
+ for (HyperGraph::VertexSet::iterator vit=vset.begin(); vit!=vset.end(); ++vit){
+ HyperGraph::Vertex* v=*vit;
+ assert(v!=0);
+ AdjacencyMap::iterator it=_adjacencyMap.find(v);
+ if (it == _adjacencyMap.end()) {
+ cerr << __PRETTY_FUNCTION__ << "Vertex " << v->id() << " is not in the adjacency map" << endl;
+ }
+ assert(it!=_adjacencyMap.end());
+ it->second._distance=0.;
+ it->second._parent=0;
+ frontier.push(it->second);
+ }
+
+ while(! frontier.empty()){
+ AdjacencyMapEntry entry=frontier.top();
+ frontier.pop();
+ HyperGraph::Vertex* u=entry.child();
+ AdjacencyMap::iterator ut=_adjacencyMap.find(u);
+ if (ut == _adjacencyMap.end()) {
+ cerr << __PRETTY_FUNCTION__ << "Vertex " << u->id() << " is not in the adjacency map" << endl;
+ }
+ assert(ut!=_adjacencyMap.end());
+ double uDistance=ut->second.distance();
+
+ std::pair< HyperGraph::VertexSet::iterator, bool> insertResult=_visited.insert(u); (void) insertResult;
+ HyperGraph::EdgeSet::iterator et=u->edges().begin();
+ while (et != u->edges().end()){
+ HyperGraph::Edge* edge=*et;
+ ++et;
+
+ if (directed && edge->vertex(0) != u)
+ continue;
+
+ for (size_t i = 0; i < edge->vertices().size(); ++i) {
+ HyperGraph::Vertex* z = edge->vertex(i);
+ if (z == u)
+ continue;
+
+ double edgeDistance=(*cost)(edge, u, z);
+ if (edgeDistance==std::numeric_limits< double >::max() || edgeDistance > maxEdgeCost)
+ continue;
+ double zDistance=uDistance+edgeDistance;
+ //cerr << z->id() << " " << zDistance << endl;
+
+ AdjacencyMap::iterator ot=_adjacencyMap.find(z);
+ assert(ot!=_adjacencyMap.end());
+
+ if (zDistance+comparisonConditioner<ot->second.distance() && zDistance<maxDistance){
+ ot->second._distance=zDistance;
+ ot->second._parent=u;
+ ot->second._edge=edge;
+ frontier.push(ot->second);
+ }
+ }
+ }
+ }
+ }
+
+ void HyperDijkstra::shortestPaths(HyperGraph::Vertex* v, HyperDijkstra::CostFunction* cost, double maxDistance,
+ double comparisonConditioner, bool directed, double maxEdgeCost)
+ {
+ HyperGraph::VertexSet vset;
+ vset.insert(v);
+ shortestPaths(vset, cost, maxDistance, comparisonConditioner, directed, maxEdgeCost);
+ }
+
+ void HyperDijkstra::computeTree(AdjacencyMap& amap)
+ {
+ for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+ AdjacencyMapEntry& entry(it->second);
+ entry._children.clear();
+ }
+ for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+ AdjacencyMapEntry& entry(it->second);
+ HyperGraph::Vertex* parent=entry.parent();
+ if (!parent){
+ continue;
+ }
+ HyperGraph::Vertex* v=entry.child();
+ assert (v==it->first);
+
+ AdjacencyMap::iterator pt=amap.find(parent);
+ assert(pt!=amap.end());
+ pt->second._children.insert(v);
+ }
+ }
+
+
+ void HyperDijkstra::visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance)
+ {
+
+ typedef std::deque<HyperGraph::Vertex*> Deque;
+ Deque q;
+ // scans for the vertices without the parent (whcih are the roots of the trees) and applies the action to them.
+ for (AdjacencyMap::iterator it=amap.begin(); it!=amap.end(); ++it){
+ AdjacencyMapEntry& entry(it->second);
+ if (! entry.parent()) {
+ action->perform(it->first,0,0);
+ q.push_back(it->first);
+ }
+ }
+
+ //std::cerr << "q.size()" << q.size() << endl;
+ int count=0;
+ while (! q.empty()){
+ HyperGraph::Vertex* parent=q.front();
+ q.pop_front();
+ ++count;
+ AdjacencyMap::iterator parentIt=amap.find(parent);
+ if (parentIt==amap.end()) {
+ continue;
+ }
+ //cerr << "parent= " << parent << " parent id= " << parent->id() << "\t children id =";
+ HyperGraph::VertexSet& childs(parentIt->second.children());
+ for (HyperGraph::VertexSet::iterator childsIt=childs.begin(); childsIt!=childs.end(); ++childsIt){
+ HyperGraph::Vertex* child=*childsIt;
+ //cerr << child->id();
+ AdjacencyMap::iterator adjacencyIt=amap.find(child);
+ assert (adjacencyIt!=amap.end());
+ HyperGraph::Edge* edge=adjacencyIt->second.edge();
+
+ assert(adjacencyIt->first==child);
+ assert(adjacencyIt->second.child()==child);
+ assert(adjacencyIt->second.parent()==parent);
+ if (! useDistance) {
+ action->perform(child, parent, edge);
+ } else {
+ action->perform(child, parent, edge, adjacencyIt->second.distance());
+ }
+ q.push_back(child);
+ }
+ //cerr << endl;
+ }
+
+ }
+
+ void HyperDijkstra::connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited,
+ HyperGraph::VertexSet& startingSet,
+ HyperGraph* g, HyperGraph::Vertex* v,
+ HyperDijkstra::CostFunction* cost, double distance,
+ double comparisonConditioner, double maxEdgeCost)
+ {
+ typedef std::queue<HyperGraph::Vertex*> VertexDeque;
+ visited.clear();
+ connected.clear();
+ VertexDeque frontier;
+ HyperDijkstra dv(g);
+ connected.insert(v);
+ frontier.push(v);
+ while (! frontier.empty()){
+ HyperGraph::Vertex* v0=frontier.front();
+ frontier.pop();
+ dv.shortestPaths(v0, cost, distance, comparisonConditioner, false, maxEdgeCost);
+ for (HyperGraph::VertexSet::iterator it=dv.visited().begin(); it!=dv.visited().end(); ++it){
+ visited.insert(*it);
+ if (startingSet.find(*it)==startingSet.end())
+ continue;
+ std::pair<HyperGraph::VertexSet::iterator, bool> insertOutcome=connected.insert(*it);
+ if (insertOutcome.second){ // the node was not in the connectedSet;
+ frontier.push(dynamic_cast<HyperGraph::Vertex*>(*it));
+ }
+ }
+ }
+ }
+
+ double UniformCostFunction::operator () (HyperGraph::Edge* /*edge*/, HyperGraph::Vertex* /*from*/, HyperGraph::Vertex* /*to*/)
+ {
+ return 1.;
+ }
+
+};
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe2c2f4bfa7f8553485bf5a3b7db679ae73b9f99
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_dijkstra.h
@@ -0,0 +1,112 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_GENERAL_DIJKSTRA_HH
+#define G2O_AIS_GENERAL_DIJKSTRA_HH
+
+#include <map>
+#include <set>
+#include <limits>
+
+#include "hyper_graph.h"
+
+namespace g2o{
+
+ struct HyperDijkstra{
+ struct CostFunction {
+ virtual double operator() (HyperGraph::Edge* e, HyperGraph::Vertex* from, HyperGraph::Vertex* to)=0;
+ virtual ~CostFunction() { }
+ };
+
+ struct TreeAction {
+ virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e);
+ virtual double perform(HyperGraph::Vertex* v, HyperGraph::Vertex* vParent, HyperGraph::Edge* e, double distance);
+ };
+
+
+ struct AdjacencyMapEntry{
+ friend struct HyperDijkstra;
+ AdjacencyMapEntry(HyperGraph::Vertex* _child=0,
+ HyperGraph::Vertex* _parent=0,
+ HyperGraph::Edge* _edge=0,
+ double _distance=std::numeric_limits<double>::max());
+ HyperGraph::Vertex* child() const {return _child;}
+ HyperGraph::Vertex* parent() const {return _parent;}
+ HyperGraph::Edge* edge() const {return _edge;}
+ double distance() const {return _distance;}
+ HyperGraph::VertexSet& children() {return _children;}
+ const HyperGraph::VertexSet& children() const {return _children;}
+ protected:
+ HyperGraph::Vertex* _child;
+ HyperGraph::Vertex* _parent;
+ HyperGraph::Edge* _edge;
+ double _distance;
+ HyperGraph::VertexSet _children;
+ };
+
+ typedef std::map<HyperGraph::Vertex*, AdjacencyMapEntry> AdjacencyMap;
+ HyperDijkstra(HyperGraph* g);
+ HyperGraph::VertexSet& visited() {return _visited; }
+ AdjacencyMap& adjacencyMap() {return _adjacencyMap; }
+ HyperGraph* graph() {return _graph;}
+
+ void shortestPaths(HyperGraph::Vertex* v,
+ HyperDijkstra::CostFunction* cost,
+ double maxDistance=std::numeric_limits< double >::max(),
+ double comparisonConditioner=1e-3,
+ bool directed=false,
+ double maxEdgeCost=std::numeric_limits< double >::max());
+
+ void shortestPaths(HyperGraph::VertexSet& vset,
+ HyperDijkstra::CostFunction* cost,
+ double maxDistance=std::numeric_limits< double >::max(),
+ double comparisonConditioner=1e-3,
+ bool directed=false,
+ double maxEdgeCost=std::numeric_limits< double >::max());
+
+
+ static void computeTree(AdjacencyMap& amap);
+ static void visitAdjacencyMap(AdjacencyMap& amap, TreeAction* action, bool useDistance=false);
+ static void connectedSubset(HyperGraph::VertexSet& connected, HyperGraph::VertexSet& visited,
+ HyperGraph::VertexSet& startingSet,
+ HyperGraph* g, HyperGraph::Vertex* v,
+ HyperDijkstra::CostFunction* cost, double distance, double comparisonConditioner,
+ double maxEdgeCost=std::numeric_limits< double >::max() );
+
+ protected:
+ void reset();
+
+ AdjacencyMap _adjacencyMap;
+ HyperGraph::VertexSet _visited;
+ HyperGraph* _graph;
+ };
+
+ struct UniformCostFunction: public HyperDijkstra::CostFunction {
+ virtual double operator ()(HyperGraph::Edge* edge, HyperGraph::Vertex* from, HyperGraph::Vertex* to);
+ };
+
+}
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e1ea76e9f2f99e3cbb6b2c0732e2a5bdb105780
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.cpp
@@ -0,0 +1,166 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "hyper_graph.h"
+
+#include <assert.h>
+#include <queue>
+
+namespace g2o {
+
+ HyperGraph::Vertex::Vertex(int id) : _id(id)
+ {
+ }
+
+ HyperGraph::Vertex::~Vertex()
+ {
+ }
+
+ HyperGraph::Edge::Edge(int id) : _id(id)
+ {
+ }
+
+ HyperGraph::Edge::~Edge()
+ {
+ }
+
+ void HyperGraph::Edge::resize(size_t size)
+ {
+ _vertices.resize(size, 0);
+ }
+
+ void HyperGraph::Edge::setId(int id)
+ {
+ _id = id;
+ }
+
+ HyperGraph::Vertex* HyperGraph::vertex(int id)
+ {
+ VertexIDMap::iterator it=_vertices.find(id);
+ if (it==_vertices.end())
+ return 0;
+ return it->second;
+ }
+
+ const HyperGraph::Vertex* HyperGraph::vertex(int id) const
+ {
+ VertexIDMap::const_iterator it=_vertices.find(id);
+ if (it==_vertices.end())
+ return 0;
+ return it->second;
+ }
+
+ bool HyperGraph::addVertex(Vertex* v)
+ {
+ Vertex* vn=vertex(v->id());
+ if (vn)
+ return false;
+ _vertices.insert( std::make_pair(v->id(),v) );
+ return true;
+ }
+
+ /**
+ * changes the id of a vertex already in the graph, and updates the bookkeeping
+ @ returns false if the vertex is not in the graph;
+ */
+ bool HyperGraph::changeId(Vertex* v, int newId){
+ Vertex* v2 = vertex(v->id());
+ if (v != v2)
+ return false;
+ _vertices.erase(v->id());
+ v->setId(newId);
+ _vertices.insert(std::make_pair(v->id(), v));
+ return true;
+ }
+
+ bool HyperGraph::addEdge(Edge* e)
+ {
+ std::pair<EdgeSet::iterator, bool> result = _edges.insert(e);
+ if (! result.second)
+ return false;
+ for (std::vector<Vertex*>::iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ Vertex* v = *it;
+ v->edges().insert(e);
+ }
+ return true;
+ }
+
+ bool HyperGraph::removeVertex(Vertex* v)
+ {
+ VertexIDMap::iterator it=_vertices.find(v->id());
+ if (it==_vertices.end())
+ return false;
+ assert(it->second==v);
+ //remove all edges which are entering or leaving v;
+ EdgeSet tmp(v->edges());
+ for (EdgeSet::iterator it=tmp.begin(); it!=tmp.end(); ++it){
+ if (!removeEdge(*it)){
+ assert(0);
+ }
+ }
+ _vertices.erase(it);
+ delete v;
+ return true;
+ }
+
+ bool HyperGraph::removeEdge(Edge* e)
+ {
+ EdgeSet::iterator it = _edges.find(e);
+ if (it == _edges.end())
+ return false;
+ _edges.erase(it);
+
+ for (std::vector<Vertex*>::iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+ Vertex* v = *vit;
+ it = v->edges().find(e);
+ assert(it!=v->edges().end());
+ v->edges().erase(it);
+ }
+
+ delete e;
+ return true;
+ }
+
+ HyperGraph::HyperGraph()
+ {
+ }
+
+ void HyperGraph::clear()
+ {
+ for (VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it)
+ delete (it->second);
+ for (EdgeSet::iterator it=_edges.begin(); it!=_edges.end(); ++it)
+ delete (*it);
+ _vertices.clear();
+ _edges.clear();
+ }
+
+ HyperGraph::~HyperGraph()
+ {
+ clear();
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.h
new file mode 100644
index 0000000000000000000000000000000000000000..da6bb3d361839a9de8cb54cb2e702be043409ef9
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph.h
@@ -0,0 +1,220 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_HYPER_GRAPH_HH
+#define G2O_AIS_HYPER_GRAPH_HH
+
+#include <map>
+#include <set>
+#include <bitset>
+#include <cassert>
+#include <vector>
+#include <limits>
+#include <cstddef>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+
+/** @addtogroup graph */
+//@{
+namespace g2o {
+
+ /**
+ Class that models a directed Hyper-Graph. An hyper graph is a graph where an edge
+ can connect one or more nodes. Both Vertices and Edges of an hyoper graph
+ derive from the same class HyperGraphElement, thus one can implement generic algorithms
+ that operate transparently on edges or vertices (see HyperGraphAction).
+
+ The vertices are uniquely identified by an int id, while the edges are
+ identfied by their pointers.
+ */
+ class HyperGraph
+ {
+ public:
+
+ /**
+ * \brief enum of all the types we have in our graphs
+ */
+ enum HyperGraphElementType {
+ HGET_VERTEX,
+ HGET_EDGE,
+ HGET_PARAMETER,
+ HGET_CACHE,
+ HGET_DATA,
+ HGET_NUM_ELEMS // keep as last elem
+ };
+
+ typedef std::bitset<HyperGraph::HGET_NUM_ELEMS> GraphElemBitset;
+
+ class Vertex;
+ class Edge;
+
+ /**
+ * base hyper graph element, specialized in vertex and edge
+ */
+ struct HyperGraphElement {
+ virtual ~HyperGraphElement() {}
+ /**
+ * returns the type of the graph element, see HyperGraphElementType
+ */
+ virtual HyperGraphElementType elementType() const = 0;
+ };
+
+ typedef std::set<Edge*> EdgeSet;
+ typedef std::set<Vertex*> VertexSet;
+
+ typedef std::tr1::unordered_map<int, Vertex*> VertexIDMap;
+ typedef std::vector<Vertex*> VertexContainer;
+
+ //! abstract Vertex, your types must derive from that one
+ class Vertex : public HyperGraphElement {
+ public:
+ //! creates a vertex having an ID specified by the argument
+ explicit Vertex(int id=-1);
+ virtual ~Vertex();
+ //! returns the id
+ int id() const {return _id;}
+ virtual void setId( int newId) { _id=newId; }
+ //! returns the set of hyper-edges that are leaving/entering in this vertex
+ const EdgeSet& edges() const {return _edges;}
+ //! returns the set of hyper-edges that are leaving/entering in this vertex
+ EdgeSet& edges() {return _edges;}
+ virtual HyperGraphElementType elementType() const { return HGET_VERTEX;}
+ protected:
+ int _id;
+ EdgeSet _edges;
+ };
+
+ /**
+ * Abstract Edge class. Your nice edge classes should inherit from that one.
+ * An hyper-edge has pointers to the vertices it connects and stores them in a vector.
+ */
+ class Edge : public HyperGraphElement {
+ public:
+ //! creates and empty edge with no vertices
+ explicit Edge(int id = -1);
+ virtual ~Edge();
+
+ /**
+ * resizes the number of vertices connected by this edge
+ */
+ virtual void resize(size_t size);
+ /**
+ returns the vector of pointers to the vertices connected by the hyper-edge.
+ */
+ const VertexContainer& vertices() const { return _vertices;}
+ /**
+ returns the vector of pointers to the vertices connected by the hyper-edge.
+ */
+ VertexContainer& vertices() { return _vertices;}
+ /**
+ returns the pointer to the ith vertex connected to the hyper-edge.
+ */
+ const Vertex* vertex(size_t i) const { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
+ /**
+ returns the pointer to the ith vertex connected to the hyper-edge.
+ */
+ Vertex* vertex(size_t i) { assert(i < _vertices.size() && "index out of bounds"); return _vertices[i];}
+ /**
+ set the ith vertex on the hyper-edge to the pointer supplied
+ */
+ void setVertex(size_t i, Vertex* v) { assert(i < _vertices.size() && "index out of bounds"); _vertices[i]=v;}
+
+ int id() const {return _id;}
+ void setId(int id);
+ virtual HyperGraphElementType elementType() const { return HGET_EDGE;}
+ protected:
+ VertexContainer _vertices;
+ int _id; ///< unique id
+ };
+
+ public:
+ //! constructs an empty hyper graph
+ HyperGraph();
+ //! destroys the hyper-graph and all the vertices of the graph
+ virtual ~HyperGraph();
+
+ //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
+ Vertex* vertex(int id);
+ //! returns a vertex <i>id</i> in the hyper-graph, or 0 if the vertex id is not present
+ const Vertex* vertex(int id) const;
+
+ //! removes a vertex from the graph. Returns true on success (vertex was present)
+ virtual bool removeVertex(Vertex* v);
+ //! removes a vertex from the graph. Returns true on success (edge was present)
+ virtual bool removeEdge(Edge* e);
+ //! clears the graph and empties all structures.
+ virtual void clear();
+
+ //! @returns the map <i>id -> vertex</i> where the vertices are stored
+ const VertexIDMap& vertices() const {return _vertices;}
+ //! @returns the map <i>id -> vertex</i> where the vertices are stored
+ VertexIDMap& vertices() {return _vertices;}
+
+ //! @returns the set of edges of the hyper graph
+ const EdgeSet& edges() const {return _edges;}
+ //! @returns the set of edges of the hyper graph
+ EdgeSet& edges() {return _edges;}
+
+ /**
+ * adds a vertex to the graph. The id of the vertex should be set before
+ * invoking this function. the function fails if another vertex
+ * with the same id is already in the graph.
+ * returns true, on success, or false on failure.
+ */
+ virtual bool addVertex(Vertex* v);
+
+ /**
+ * Adds an edge to the graph. If the edge is already in the graph, it
+ * does nothing and returns false. Otherwise it returns true.
+ */
+ virtual bool addEdge(Edge* e);
+
+ /**
+ * changes the id of a vertex already in the graph, and updates the bookkeeping
+ @ returns false if the vertex is not in the graph;
+ */
+ virtual bool changeId(Vertex* v, int newId);
+
+ protected:
+ VertexIDMap _vertices;
+ EdgeSet _edges;
+
+ private:
+ // Disable the copy constructor and assignment operator
+ HyperGraph(const HyperGraph&) { }
+ HyperGraph& operator= (const HyperGraph&) { return *this; }
+ };
+
+} // end namespace
+
+//@}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1fe2439cc23ffec15c97ea8cab465a953f582c2d
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.cpp
@@ -0,0 +1,268 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "hyper_graph_action.h"
+#include "optimizable_graph.h"
+#include "../stuff/macros.h"
+
+
+#include <iostream>
+
+namespace g2o {
+ using namespace std;
+
+ HyperGraphActionLibrary* HyperGraphActionLibrary::actionLibInstance = 0;
+
+ HyperGraphAction::Parameters::~Parameters()
+ {
+ }
+
+ HyperGraphAction::ParametersIteration::ParametersIteration(int iter) :
+ HyperGraphAction::Parameters(),
+ iteration(iter)
+ {
+ }
+
+ HyperGraphAction::~HyperGraphAction()
+ {
+ }
+
+ HyperGraphAction* HyperGraphAction::operator()(const HyperGraph*, Parameters*)
+ {
+ return 0;
+ }
+
+ HyperGraphElementAction::Parameters::~Parameters()
+ {
+ }
+
+ HyperGraphElementAction::HyperGraphElementAction(const std::string& typeName_)
+ {
+ _typeName = typeName_;
+ }
+
+ void HyperGraphElementAction::setTypeName(const std::string& typeName_)
+ {
+ _typeName = typeName_;
+ }
+
+
+ HyperGraphElementAction* HyperGraphElementAction::operator()(HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
+ {
+ return 0;
+ }
+
+ HyperGraphElementAction* HyperGraphElementAction::operator()(const HyperGraph::HyperGraphElement* , HyperGraphElementAction::Parameters* )
+ {
+ return 0;
+ }
+
+ HyperGraphElementAction::~HyperGraphElementAction()
+ {
+ }
+
+ HyperGraphElementActionCollection::HyperGraphElementActionCollection(const std::string& name_)
+ {
+ _name = name_;
+ }
+
+ HyperGraphElementActionCollection::~HyperGraphElementActionCollection()
+ {
+ for (ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
+ delete it->second;
+ }
+ }
+
+ HyperGraphElementAction* HyperGraphElementActionCollection::operator()(HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
+ {
+ ActionMap::iterator it=_actionMap.find(typeid(*element).name());
+ //cerr << typeid(*element).name() << endl;
+ if (it==_actionMap.end())
+ return 0;
+ HyperGraphElementAction* action=it->second;
+ return (*action)(element, params);
+ }
+
+ HyperGraphElementAction* HyperGraphElementActionCollection::operator()(const HyperGraph::HyperGraphElement* element, HyperGraphElementAction::Parameters* params)
+ {
+ ActionMap::iterator it=_actionMap.find(typeid(*element).name());
+ if (it==_actionMap.end())
+ return 0;
+ HyperGraphElementAction* action=it->second;
+ return (*action)(element, params);
+ }
+
+ bool HyperGraphElementActionCollection::registerAction(HyperGraphElementAction* action)
+ {
+# ifdef G2O_DEBUG_ACTIONLIB
+ cerr << __PRETTY_FUNCTION__ << " " << action->name() << " " << action->typeName() << endl;
+# endif
+ if (action->name()!=name()){
+ cerr << __PRETTY_FUNCTION__ << ": invalid attempt to register an action in a collection with a different name " << name() << " " << action->name() << endl;
+ }
+ _actionMap.insert(make_pair ( action->typeName(), action) );
+ return true;
+ }
+
+ bool HyperGraphElementActionCollection::unregisterAction(HyperGraphElementAction* action)
+ {
+ for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
+ if (it->second == action){
+ _actionMap.erase(it);
+ return true;
+ }
+ }
+ return false;
+ }
+
+ HyperGraphActionLibrary::HyperGraphActionLibrary()
+ {
+ }
+
+ HyperGraphActionLibrary* HyperGraphActionLibrary::instance()
+ {
+ if (actionLibInstance == 0) {
+ actionLibInstance = new HyperGraphActionLibrary;
+ }
+ return actionLibInstance;
+ }
+
+ void HyperGraphActionLibrary::destroy()
+ {
+ delete actionLibInstance;
+ actionLibInstance = 0;
+ }
+
+ HyperGraphActionLibrary::~HyperGraphActionLibrary()
+ {
+ for (HyperGraphElementAction::ActionMap::iterator it = _actionMap.begin(); it != _actionMap.end(); ++it) {
+ delete it->second;
+ }
+ }
+
+ HyperGraphElementAction* HyperGraphActionLibrary::actionByName(const std::string& name)
+ {
+ HyperGraphElementAction::ActionMap::iterator it=_actionMap.find(name);
+ if (it!=_actionMap.end())
+ return it->second;
+ return 0;
+ }
+
+ bool HyperGraphActionLibrary::registerAction(HyperGraphElementAction* action)
+ {
+ HyperGraphElementAction* oldAction = actionByName(action->name());
+ HyperGraphElementActionCollection* collection = 0;
+ if (oldAction) {
+ collection = dynamic_cast<HyperGraphElementActionCollection*>(oldAction);
+ if (! collection) {
+ cerr << __PRETTY_FUNCTION__ << ": fatal error, a collection is not at the first level in the library" << endl;
+ return 0;
+ }
+ }
+ if (! collection) {
+#ifdef G2O_DEBUG_ACTIONLIB
+ cerr << __PRETTY_FUNCTION__ << ": creating collection for \"" << action->name() << "\"" << endl;
+#endif
+ collection = new HyperGraphElementActionCollection(action->name());
+ _actionMap.insert(make_pair(action->name(), collection));
+ }
+ return collection->registerAction(action);
+ }
+
+ bool HyperGraphActionLibrary::unregisterAction(HyperGraphElementAction* action)
+ {
+ list<HyperGraphElementActionCollection*> collectionDeleteList;
+
+ // Search all the collections and delete the registered actions; if a collection becomes empty, schedule it for deletion; note that we can't delete the collections as we go because this will screw up the state of the iterators
+ for (HyperGraphElementAction::ActionMap::iterator it=_actionMap.begin(); it != _actionMap.end(); ++it) {
+ HyperGraphElementActionCollection* collection = dynamic_cast<HyperGraphElementActionCollection*> (it->second);
+ if (collection != 0) {
+ collection->unregisterAction(action);
+ if (collection->actionMap().size() == 0) {
+ collectionDeleteList.push_back(collection);
+ }
+ }
+ }
+
+ // Delete any empty action collections
+ for (list<HyperGraphElementActionCollection*>::iterator itc = collectionDeleteList.begin(); itc != collectionDeleteList.end(); ++itc) {
+ //cout << "Deleting collection " << (*itc)->name() << endl;
+ _actionMap.erase((*itc)->name());
+ }
+
+ return true;
+ }
+
+
+ WriteGnuplotAction::WriteGnuplotAction(const std::string& typeName_)
+ : HyperGraphElementAction(typeName_)
+ {
+ _name="writeGnuplot";
+ }
+
+ DrawAction::Parameters::Parameters(){
+ }
+
+ DrawAction::DrawAction(const std::string& typeName_)
+ : HyperGraphElementAction(typeName_)
+ {
+ _name="draw";
+ _previousParams = (Parameters*)0x42;
+ refreshPropertyPtrs(0);
+ }
+
+ bool DrawAction::refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_){
+ if (_previousParams == params_)
+ return false;
+ DrawAction::Parameters* p=dynamic_cast<DrawAction::Parameters*>(params_);
+ if (! p){
+ _previousParams = 0;
+ _show = 0;
+ _showId = 0;
+ } else {
+ _previousParams = p;
+ _show = p->makeProperty<BoolProperty>(_typeName+"::SHOW", true);
+ _showId = p->makeProperty<BoolProperty>(_typeName+"::SHOW_ID", false);
+ }
+ return true;
+ }
+
+ void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* params, const std::string& typeName)
+ {
+ for (HyperGraph::VertexIDMap::iterator it=graph->vertices().begin();
+ it!=graph->vertices().end(); ++it){
+ if ( typeName.empty() || typeid(*it->second).name()==typeName){
+ (*action)(it->second, params);
+ }
+ }
+ for (HyperGraph::EdgeSet::iterator it=graph->edges().begin();
+ it!=graph->edges().end(); ++it){
+ if ( typeName.empty() || typeid(**it).name()==typeName)
+ (*action)(*it, params);
+ }
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.h
new file mode 100644
index 0000000000000000000000000000000000000000..21417c9ba16258d72cff2a70db6e276297ad34d2
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/hyper_graph_action.h
@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_HYPER_GRAPH_ACTION_H
+#define G2O_HYPER_GRAPH_ACTION_H
+
+#include "hyper_graph.h"
+#include "../stuff/property.h"
+
+#include <typeinfo>
+#include <iosfwd>
+#include <set>
+#include <string>
+#include <iostream>
+
+
+// define to get verbose output
+//#define G2O_DEBUG_ACTIONLIB
+
+namespace g2o {
+
+ /**
+ * \brief Abstract action that operates on an entire graph
+ */
+ class HyperGraphAction {
+ public:
+ class Parameters {
+ public:
+ virtual ~Parameters();
+ };
+
+ class ParametersIteration : public Parameters {
+ public:
+ explicit ParametersIteration(int iter);
+ int iteration;
+ };
+
+ virtual ~HyperGraphAction();
+
+ /**
+ * re-implement to carry out an action given the graph
+ */
+ virtual HyperGraphAction* operator()(const HyperGraph* graph, Parameters* parameters = 0);
+ };
+
+ /**
+ * \brief Abstract action that operates on a graph entity
+ */
+ class HyperGraphElementAction{
+ public:
+ struct Parameters{
+ virtual ~Parameters();
+ };
+ typedef std::map<std::string, HyperGraphElementAction*> ActionMap;
+ //! an action should be instantiated with the typeid.name of the graph element
+ //! on which it operates
+ HyperGraphElementAction(const std::string& typeName_="");
+
+ //! redefine this to do the action stuff. If successful, the action returns a pointer to itself
+ virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
+
+ //! redefine this to do the action stuff. If successful, the action returns a pointer to itself
+ virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
+
+ //! destroyed actions release the memory
+ virtual ~HyperGraphElementAction();
+
+ //! returns the typeid name of the action
+ const std::string& typeName() const { return _typeName;}
+
+ //! returns the name of an action, e.g "draw"
+ const std::string& name() const{ return _name;}
+
+ //! sets the type on which an action has to operate
+ void setTypeName(const std::string& typeName_);
+
+ protected:
+ std::string _typeName;
+ std::string _name;
+ };
+
+ /**
+ * \brief collection of actions
+ *
+ * collection of actions calls contains homogeneous actions operating on different types
+ * all collected actions have the same name and should have the same functionality
+ */
+ class HyperGraphElementActionCollection: public HyperGraphElementAction{
+ public:
+ //! constructor. name_ is the name of the action e.g.draw).
+ HyperGraphElementActionCollection(const std::string& name_);
+ //! destructor: it deletes all actions in the pool.
+ virtual ~HyperGraphElementActionCollection();
+ //! calling functions, they return a pointer to the instance of action in actionMap
+ //! that was active on element
+ virtual HyperGraphElementAction* operator()(HyperGraph::HyperGraphElement* element, Parameters* parameters);
+ virtual HyperGraphElementAction* operator()(const HyperGraph::HyperGraphElement* element, Parameters* parameters);
+ ActionMap& actionMap() {return _actionMap;}
+ //! inserts an action in the pool. The action should have the same name of the container.
+ //! returns false on failure (the container has a different name than the action);
+ bool registerAction(HyperGraphElementAction* action);
+ bool unregisterAction(HyperGraphElementAction* action);
+ protected:
+ ActionMap _actionMap;
+ };
+
+ /**
+ * \brief library of actions, indexed by the action name;
+ *
+ * library of actions, indexed by the action name;
+ * one can use ti to register a collection of actions
+ */
+ class HyperGraphActionLibrary{
+ public:
+ //! return the single instance of the HyperGraphActionLibrary
+ static HyperGraphActionLibrary* instance();
+ //! free the instance
+ static void destroy();
+
+ // returns a pointer to a collection indexed by name
+ HyperGraphElementAction* actionByName(const std::string& name);
+ // registers a basic action in the pool. If necessary a container is created
+ bool registerAction(HyperGraphElementAction* action);
+ bool unregisterAction(HyperGraphElementAction* action);
+
+ inline HyperGraphElementAction::ActionMap& actionMap() {return _actionMap;}
+ protected:
+ HyperGraphActionLibrary();
+ ~HyperGraphActionLibrary();
+ HyperGraphElementAction::ActionMap _actionMap;
+ private:
+ static HyperGraphActionLibrary* actionLibInstance;
+ };
+
+ /**
+ * apply an action to all the elements of the graph.
+ */
+ void applyAction(HyperGraph* graph, HyperGraphElementAction* action, HyperGraphElementAction::Parameters* parameters=0, const std::string& typeName="");
+
+ /**
+ * brief write into gnuplot
+ */
+ class WriteGnuplotAction: public HyperGraphElementAction{
+ public:
+ struct Parameters: public HyperGraphElementAction::Parameters{
+ std::ostream* os;
+ };
+ WriteGnuplotAction(const std::string& typeName_);
+ };
+
+ /**
+ * \brief draw actions
+ */
+
+ class DrawAction : public HyperGraphElementAction{
+ public:
+ class Parameters: public HyperGraphElementAction::Parameters, public PropertyMap{
+ public:
+ Parameters();
+ };
+ DrawAction(const std::string& typeName_);
+ protected:
+ virtual bool refreshPropertyPtrs(HyperGraphElementAction::Parameters* params_);
+ Parameters* _previousParams;
+ BoolProperty* _show;
+ BoolProperty* _showId;
+ };
+
+ template<typename T> class RegisterActionProxy
+ {
+ public:
+ RegisterActionProxy()
+ {
+#ifdef G2O_DEBUG_ACTIONLIB
+ std::cout << __FUNCTION__ << ": Registering action of type " << typeid(T).name() << std::endl;
+#endif
+ _action = new T();
+ HyperGraphActionLibrary::instance()->registerAction(_action);
+ }
+
+ ~RegisterActionProxy()
+ {
+#ifdef G2O_DEBUG_ACTIONLIB
+ std::cout << __FUNCTION__ << ": Unregistering action of type " << typeid(T).name() << std::endl;
+#endif
+ HyperGraphActionLibrary::instance()->unregisterAction(_action);
+ delete _action;
+ }
+
+ private:
+ HyperGraphElementAction* _action;
+ };
+
+#define G2O_REGISTER_ACTION(classname) \
+ extern "C" void g2o_action_##classname(void) {} \
+ static g2o::RegisterActionProxy<classname> g_action_proxy_##classname;
+};
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9890a181d07903a1094be62b4bf42d9035682d31
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.cpp
@@ -0,0 +1,89 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "jacobian_workspace.h"
+
+#include <cmath>
+
+#include "optimizable_graph.h"
+
+using namespace std;
+
+namespace g2o {
+
+JacobianWorkspace::JacobianWorkspace() :
+ _maxNumVertices(-1), _maxDimension(-1)
+{
+}
+
+JacobianWorkspace::~JacobianWorkspace()
+{
+}
+
+bool JacobianWorkspace::allocate()
+{
+ //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
+ if (_maxNumVertices <=0 || _maxDimension <= 0)
+ return false;
+ _workspace.resize(_maxNumVertices);
+ for (WorkspaceVector::iterator it = _workspace.begin(); it != _workspace.end(); ++it) {
+ it->resize(_maxDimension);
+ it->setZero();
+ }
+ return true;
+}
+
+void JacobianWorkspace::updateSize(const HyperGraph::Edge* e_)
+{
+ const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(e_);
+ int errorDimension = e->dimension();
+ int numVertices = e->vertices().size();
+ int maxDimensionForEdge = -1;
+ for (int i = 0; i < numVertices; ++i) {
+ const OptimizableGraph::Vertex* v = static_cast<const OptimizableGraph::Vertex*>(e->vertex(i));
+ assert(v && "Edge has no vertex assigned");
+ maxDimensionForEdge = max(v->dimension() * errorDimension, maxDimensionForEdge);
+ }
+ _maxNumVertices = max(numVertices, _maxNumVertices);
+ _maxDimension = max(maxDimensionForEdge, _maxDimension);
+ //cerr << __PRETTY_FUNCTION__ << " " << PVAR(this) << " " << PVAR(_maxNumVertices) << " " << PVAR(_maxDimension) << endl;
+}
+
+void JacobianWorkspace::updateSize(const OptimizableGraph& graph)
+{
+ for (OptimizableGraph::EdgeSet::const_iterator it = graph.edges().begin(); it != graph.edges().end(); ++it) {
+ const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
+ updateSize(e);
+ }
+}
+
+void JacobianWorkspace::updateSize(int numVertices, int dimension)
+{
+ _maxNumVertices = max(numVertices, _maxNumVertices);
+ _maxDimension = max(dimension, _maxDimension);
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.h
new file mode 100644
index 0000000000000000000000000000000000000000..e1f160263338fd56610d06f4dde43a968a7ffdf4
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/jacobian_workspace.h
@@ -0,0 +1,96 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef JACOBIAN_WORKSPACE_H
+#define JACOBIAN_WORKSPACE_H
+
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+#include <vector>
+#include <cassert>
+
+#include "hyper_graph.h"
+
+namespace g2o {
+
+ struct OptimizableGraph;
+
+ /**
+ * \brief provide memory workspace for computing the Jacobians
+ *
+ * The workspace is used by an OptimizableGraph to provide temporary memory
+ * for computing the Jacobian of the error functions.
+ * Before calling linearizeOplus on an edge, the workspace needs to be allocated
+ * by calling allocate().
+ */
+ class JacobianWorkspace
+ {
+ public:
+ typedef std::vector<Eigen::VectorXd, Eigen::aligned_allocator<Eigen::VectorXd> > WorkspaceVector;
+
+ public:
+ JacobianWorkspace();
+ ~JacobianWorkspace();
+
+ /**
+ * allocate the workspace
+ */
+ bool allocate();
+
+ /**
+ * update the maximum required workspace needed by taking into account this edge
+ */
+ void updateSize(const HyperGraph::Edge* e);
+
+ /**
+ * update the required workspace by looking at a full graph
+ */
+ void updateSize(const OptimizableGraph& graph);
+
+ /**
+ * manually update with the given parameters
+ */
+ void updateSize(int numVertices, int dimension);
+
+ /**
+ * return the workspace for a vertex in an edge
+ */
+ double* workspaceForVertex(int vertexIndex)
+ {
+ assert(vertexIndex >= 0 && (size_t)vertexIndex < _workspace.size() && "Index out of bounds");
+ return _workspace[vertexIndex].data();
+ }
+
+ protected:
+ WorkspaceVector _workspace; ///< the memory pre-allocated for computing the Jacobians
+ int _maxNumVertices; ///< the maximum number of vertices connected by a hyper-edge
+ int _maxDimension; ///< the maximum dimension (number of elements) for a Jacobian
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/linear_solver.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/linear_solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..20213d439bff1a245a124345fedb70e47602701c
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/linear_solver.h
@@ -0,0 +1,109 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_H
+#define G2O_LINEAR_SOLVER_H
+#include "sparse_block_matrix.h"
+#include "sparse_block_matrix_ccs.h"
+
+namespace g2o {
+
+/**
+ * \brief basic solver for Ax = b
+ *
+ * basic solver for Ax = b which has to reimplemented for different linear algebra libraries.
+ * A is assumed to be symmetric (only upper triangular block is stored) and positive-semi-definit.
+ */
+template <typename MatrixType>
+class LinearSolver
+{
+ public:
+ LinearSolver() {};
+ virtual ~LinearSolver() {}
+
+ /**
+ * init for operating on matrices with a different non-zero pattern like before
+ */
+ virtual bool init() = 0;
+
+ /**
+ * Assumes that A is the same matrix for several calls.
+ * Among other assumptions, the non-zero pattern does not change!
+ * If the matrix changes call init() before.
+ * solve system Ax = b, x and b have to allocated beforehand!!
+ */
+ virtual bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b) = 0;
+
+ /**
+ * Inverts the diagonal blocks of A
+ * @returns false if not defined.
+ */
+ virtual bool solveBlocks(double**&blocks, const SparseBlockMatrix<MatrixType>& A) { (void)blocks; (void) A; return false; }
+
+
+ /**
+ * Inverts the a block pattern of A in spinv
+ * @returns false if not defined.
+ */
+ virtual bool solvePattern(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices, const SparseBlockMatrix<MatrixType>& A){
+ (void) spinv;
+ (void) blockIndices;
+ (void) A;
+ return false;
+ }
+
+ //! write a debug dump of the system matrix if it is not PSD in solve
+ virtual bool writeDebug() const { return false;}
+ virtual void setWriteDebug(bool) {}
+};
+
+/**
+ * \brief Solver with faster iterating structure for the linear matrix
+ */
+template <typename MatrixType>
+class LinearSolverCCS : public LinearSolver<MatrixType>
+{
+ public:
+ LinearSolverCCS() : LinearSolver<MatrixType>(), _ccsMatrix(0) {}
+ ~LinearSolverCCS()
+ {
+ delete _ccsMatrix;
+ }
+
+ protected:
+ SparseBlockMatrixCCS<MatrixType>* _ccsMatrix;
+
+ void initMatrixStructure(const SparseBlockMatrix<MatrixType>& A)
+ {
+ delete _ccsMatrix;
+ _ccsMatrix = new SparseBlockMatrixCCS<MatrixType>(A.rowBlockIndices(), A.colBlockIndices());
+ A.fillSparseBlockMatrixCCS(*_ccsMatrix);
+ }
+};
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bc11f5ccca1fc45703f8d21ad38e715eb504fd91
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.cpp
@@ -0,0 +1,222 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "marginal_covariance_cholesky.h"
+
+#include <algorithm>
+#include <cassert>
+using namespace std;
+
+namespace g2o {
+
+struct MatrixElem
+{
+ int r, c;
+ MatrixElem(int r_, int c_) : r(r_), c(c_) {}
+ bool operator<(const MatrixElem& other) const
+ {
+ return c > other.c || (c == other.c && r > other.r);
+ }
+};
+
+MarginalCovarianceCholesky::MarginalCovarianceCholesky() :
+ _n(0), _Ap(0), _Ai(0), _Ax(0), _perm(0)
+{
+}
+
+MarginalCovarianceCholesky::~MarginalCovarianceCholesky()
+{
+}
+
+void MarginalCovarianceCholesky::setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv)
+{
+ _n = n;
+ _Ap = Lp;
+ _Ai = Li;
+ _Ax = Lx;
+ _perm = permInv;
+
+ // pre-compute reciprocal values of the diagonal of L
+ _diag.resize(n);
+ for (int r = 0; r < n; ++r) {
+ const int& sc = _Ap[r]; // L is lower triangular, thus the first elem in the column is the diagonal entry
+ assert(r == _Ai[sc] && "Error in CCS storage of L");
+ _diag[r] = 1.0 / _Ax[sc];
+ }
+}
+
+double MarginalCovarianceCholesky::computeEntry(int r, int c)
+{
+ assert(r <= c);
+ int idx = computeIndex(r, c);
+
+ LookupMap::const_iterator foundIt = _map.find(idx);
+ if (foundIt != _map.end()) {
+ return foundIt->second;
+ }
+
+ // compute the summation over column r
+ double s = 0.;
+ const int& sc = _Ap[r];
+ const int& ec = _Ap[r+1];
+ for (int j = sc+1; j < ec; ++j) { // sum over row r while skipping the element on the diagonal
+ const int& rr = _Ai[j];
+ double val = rr < c ? computeEntry(rr, c) : computeEntry(c, rr);
+ s += val * _Ax[j];
+ }
+
+ double result;
+ if (r == c) {
+ const double& diagElem = _diag[r];
+ result = diagElem * (diagElem - s);
+ } else {
+ result = -s * _diag[r];
+ }
+ _map[idx] = result;
+ return result;
+}
+
+void MarginalCovarianceCholesky::computeCovariance(double** covBlocks, const std::vector<int>& blockIndices)
+{
+ _map.clear();
+ int base = 0;
+ vector<MatrixElem> elemsToCompute;
+ for (size_t i = 0; i < blockIndices.size(); ++i) {
+ int nbase = blockIndices[i];
+ int vdim = nbase - base;
+ for (int rr = 0; rr < vdim; ++rr)
+ for (int cc = rr; cc < vdim; ++cc) {
+ int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
+ int c = _perm ? _perm[cc + base] : cc + base;
+ if (r > c) // make sure it's still upper triangular after applying the permutation
+ swap(r, c);
+ elemsToCompute.push_back(MatrixElem(r, c));
+ }
+ base = nbase;
+ }
+
+ // sort the elems to reduce the recursive calls
+ sort(elemsToCompute.begin(), elemsToCompute.end());
+
+ // compute the inverse elements we need
+ for (size_t i = 0; i < elemsToCompute.size(); ++i) {
+ const MatrixElem& me = elemsToCompute[i];
+ computeEntry(me.r, me.c);
+ }
+
+ // set the marginal covariance for the vertices, by writing to the blocks memory
+ base = 0;
+ for (size_t i = 0; i < blockIndices.size(); ++i) {
+ int nbase = blockIndices[i];
+ int vdim = nbase - base;
+ double* cov = covBlocks[i];
+ for (int rr = 0; rr < vdim; ++rr)
+ for (int cc = rr; cc < vdim; ++cc) {
+ int r = _perm ? _perm[rr + base] : rr + base; // apply permutation
+ int c = _perm ? _perm[cc + base] : cc + base;
+ if (r > c) // upper triangle
+ swap(r, c);
+ int idx = computeIndex(r, c);
+ LookupMap::const_iterator foundIt = _map.find(idx);
+ assert(foundIt != _map.end());
+ cov[rr*vdim + cc] = foundIt->second;
+ if (rr != cc)
+ cov[cc*vdim + rr] = foundIt->second;
+ }
+ base = nbase;
+ }
+}
+
+
+void MarginalCovarianceCholesky::computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices)
+{
+ // allocate the sparse
+ spinv = SparseBlockMatrix<MatrixXd>(&rowBlockIndices[0],
+ &rowBlockIndices[0],
+ rowBlockIndices.size(),
+ rowBlockIndices.size(), true);
+ _map.clear();
+ vector<MatrixElem> elemsToCompute;
+ for (size_t i = 0; i < blockIndices.size(); ++i) {
+ int blockRow=blockIndices[i].first;
+ int blockCol=blockIndices[i].second;
+ assert(blockRow>=0);
+ assert(blockRow < (int)rowBlockIndices.size());
+ assert(blockCol>=0);
+ assert(blockCol < (int)rowBlockIndices.size());
+
+ int rowBase=spinv.rowBaseOfBlock(blockRow);
+ int colBase=spinv.colBaseOfBlock(blockCol);
+
+ MatrixXd *block=spinv.block(blockRow, blockCol, true);
+ assert(block);
+ for (int iRow=0; iRow<block->rows(); ++iRow)
+ for (int iCol=0; iCol<block->cols(); ++iCol){
+ int rr=rowBase+iRow;
+ int cc=colBase+iCol;
+ int r = _perm ? _perm[rr] : rr; // apply permutation
+ int c = _perm ? _perm[cc] : cc;
+ if (r > c)
+ swap(r, c);
+ elemsToCompute.push_back(MatrixElem(r, c));
+ }
+ }
+
+ // sort the elems to reduce the number of recursive calls
+ sort(elemsToCompute.begin(), elemsToCompute.end());
+
+ // compute the inverse elements we need
+ for (size_t i = 0; i < elemsToCompute.size(); ++i) {
+ const MatrixElem& me = elemsToCompute[i];
+ computeEntry(me.r, me.c);
+ }
+
+ // set the marginal covariance
+ for (size_t i = 0; i < blockIndices.size(); ++i) {
+ int blockRow=blockIndices[i].first;
+ int blockCol=blockIndices[i].second;
+ int rowBase=spinv.rowBaseOfBlock(blockRow);
+ int colBase=spinv.colBaseOfBlock(blockCol);
+
+ MatrixXd *block=spinv.block(blockRow, blockCol);
+ assert(block);
+ for (int iRow=0; iRow<block->rows(); ++iRow)
+ for (int iCol=0; iCol<block->cols(); ++iCol){
+ int rr=rowBase+iRow;
+ int cc=colBase+iCol;
+ int r = _perm ? _perm[rr] : rr; // apply permutation
+ int c = _perm ? _perm[cc] : cc;
+ if (r > c)
+ swap(r, c);
+ int idx = computeIndex(r, c);
+ LookupMap::const_iterator foundIt = _map.find(idx);
+ assert(foundIt != _map.end());
+ (*block)(iRow, iCol) = foundIt->second;
+ }
+ }
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h
new file mode 100644
index 0000000000000000000000000000000000000000..e7dfce88f88d6f2ea1055a442b4dbf82439ed2cf
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/marginal_covariance_cholesky.h
@@ -0,0 +1,103 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MARGINAL_COVARIANCE_CHOLESKY_H
+#define G2O_MARGINAL_COVARIANCE_CHOLESKY_H
+
+#include "optimizable_graph.h"
+#include "sparse_block_matrix.h"
+
+#include <cassert>
+#include <vector>
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+
+namespace g2o {
+
+ /**
+ * \brief computing the marginal covariance given a cholesky factor (lower triangle of the factor)
+ */
+ class MarginalCovarianceCholesky {
+ protected:
+ /**
+ * hash struct for storing the matrix elements needed to compute the covariance
+ */
+ typedef std::tr1::unordered_map<int, double> LookupMap;
+
+ public:
+ MarginalCovarianceCholesky();
+ ~MarginalCovarianceCholesky();
+
+ /**
+ * compute the marginal cov for the given block indices, write the result to the covBlocks memory (which has to
+ * be provided by the caller).
+ */
+ void computeCovariance(double** covBlocks, const std::vector<int>& blockIndices);
+
+
+ /**
+ * compute the marginal cov for the given block indices, write the result in spinv).
+ */
+ void computeCovariance(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<int>& rowBlockIndices, const std::vector< std::pair<int, int> >& blockIndices);
+
+
+ /**
+ * set the CCS representation of the cholesky factor along with the inverse permutation used to reduce the fill-in.
+ * permInv might be 0, will then not permute the entries.
+ *
+ * The pointers provided by the user need to be still valid when calling computeCovariance(). The pointers
+ * are owned by the caller, MarginalCovarianceCholesky does not free the pointers.
+ */
+ void setCholeskyFactor(int n, int* Lp, int* Li, double* Lx, int* permInv);
+
+ protected:
+ // information about the cholesky factor (lower triangle)
+ int _n; ///< L is an n X n matrix
+ int* _Ap; ///< column pointer of the CCS storage
+ int* _Ai; ///< row indices of the CCS storage
+ double* _Ax; ///< values of the cholesky factor
+ int* _perm; ///< permutation of the cholesky factor. Variable re-ordering for better fill-in
+
+ LookupMap _map; ///< hash look up table for the already computed entries
+ std::vector<double> _diag; ///< cache 1 / H_ii to avoid recalculations
+
+ //! compute the index used for hashing
+ int computeIndex(int r, int c) const { /*assert(r <= c);*/ return r*_n + c;}
+ /**
+ * compute one entry in the covariance, r and c are values after applying the permutation, and upper triangular.
+ * May issue recursive calls to itself to compute the missing values.
+ */
+ double computeEntry(int r, int c);
+ };
+
+}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_operations.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_operations.h
new file mode 100644
index 0000000000000000000000000000000000000000..28e6fbefc7edfaf3cd1cc4d7becdf6aca24b4e58
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_operations.h
@@ -0,0 +1,74 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_CORE_MATRIX_OPERATIONS_H
+#define G2O_CORE_MATRIX_OPERATIONS_H
+
+#include <Eigen/Core>
+
+namespace g2o {
+ namespace internal {
+
+ template<typename MatrixType>
+ inline void axpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment<MatrixType::RowsAtCompileTime>(yoff) += A * x.segment<MatrixType::ColsAtCompileTime>(xoff);
+ }
+
+ template<int t>
+ inline void axpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment(yoff, A.rows()) += A * x.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(xoff);
+ }
+
+ template<>
+ inline void axpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment(yoff, A.rows()) += A * x.segment(xoff, A.cols());
+ }
+
+ template<typename MatrixType>
+ inline void atxpy(const MatrixType& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment<MatrixType::ColsAtCompileTime>(yoff) += A.transpose() * x.segment<MatrixType::RowsAtCompileTime>(xoff);
+ }
+
+ template<int t>
+ inline void atxpy(const Eigen::Matrix<double, Eigen::Dynamic, t>& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment<Eigen::Matrix<double, Eigen::Dynamic, t>::ColsAtCompileTime>(yoff) += A.transpose() * x.segment(xoff, A.rows());
+ }
+
+ template<>
+ inline void atxpy(const Eigen::MatrixXd& A, const Eigen::Map<const Eigen::VectorXd>& x, int xoff, Eigen::Map<Eigen::VectorXd>& y, int yoff)
+ {
+ y.segment(yoff, A.cols()) += A.transpose() * x.segment(xoff, A.rows());
+ }
+
+ } // end namespace internal
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3152195a33b74fa4d9e1b513d8a4bb11e298c5f7
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.cpp
@@ -0,0 +1,125 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "matrix_structure.h"
+
+#include <string>
+#include <vector>
+#include <fstream>
+#include <algorithm>
+using namespace std;
+
+namespace g2o {
+
+struct ColSort
+{
+ bool operator()(const pair<int, int>& e1, const pair<int, int>& e2) const
+ {
+ return e1.second < e2.second || (e1.second == e2.second && e1.first < e2.first);
+ }
+};
+
+MatrixStructure::MatrixStructure() :
+ n(0), m(0), Ap(0), Aii(0), maxN(0), maxNz(0)
+{
+}
+
+MatrixStructure::~MatrixStructure()
+{
+ free();
+}
+
+void MatrixStructure::alloc(int n_, int nz)
+{
+ if (n == 0) {
+ maxN = n = n_;
+ maxNz = nz;
+ Ap = new int[maxN + 1];
+ Aii = new int[maxNz];
+ }
+ else {
+ n = n_;
+ if (maxNz < nz) {
+ maxNz = 2 * nz;
+ delete[] Aii;
+ Aii = new int[maxNz];
+ }
+ if (maxN < n) {
+ maxN = 2 * n;
+ delete[] Ap;
+ Ap = new int[maxN + 1];
+ }
+ }
+}
+
+void MatrixStructure::free()
+{
+ n = 0;
+ m = 0;
+ maxN = 0;
+ maxNz = 0;
+ delete[] Aii; Aii = 0;
+ delete[] Ap; Ap = 0;
+}
+
+bool MatrixStructure::write(const char* filename) const
+{
+ const int& cols = n;
+ const int& rows = m;
+
+ string name = filename;
+ std::string::size_type lastDot = name.find_last_of('.');
+ if (lastDot != std::string::npos)
+ name = name.substr(0, lastDot);
+
+ vector<pair<int, int> > entries;
+ for (int i=0; i < cols; ++i) {
+ const int& rbeg = Ap[i];
+ const int& rend = Ap[i+1];
+ for (int j = rbeg; j < rend; ++j) {
+ entries.push_back(make_pair(Aii[j], i));
+ if (Aii[j] != i)
+ entries.push_back(make_pair(i, Aii[j]));
+ }
+ }
+
+ sort(entries.begin(), entries.end(), ColSort());
+
+ std::ofstream fout(filename);
+ fout << "# name: " << name << std::endl;
+ fout << "# type: sparse matrix" << std::endl;
+ fout << "# nnz: " << entries.size() << std::endl;
+ fout << "# rows: " << rows << std::endl;
+ fout << "# columns: " << cols << std::endl;
+ for (vector<pair<int, int> >::const_iterator it = entries.begin(); it != entries.end(); ++it) {
+ const pair<int, int>& entry = *it;
+ fout << entry.first << " " << entry.second << " 0" << std::endl; // write a constant value of 0
+ }
+
+ return fout.good();
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd70e531112bb652d1f90388f7848b6da83fe22d
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/matrix_structure.h
@@ -0,0 +1,69 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MATRIX_STRUCTURE_H
+#define G2O_MATRIX_STRUCTURE_H
+
+
+namespace g2o {
+
+/**
+ * \brief representing the structure of a matrix in column compressed structure (only the upper triangular part of the matrix)
+ */
+class MatrixStructure
+{
+ public:
+ MatrixStructure();
+ ~MatrixStructure();
+ /**
+ * allocate space for the Matrix Structure. You may call this on an already allocated struct, it will
+ * then reallocate the memory + additional space (double the required space).
+ */
+ void alloc(int n_, int nz);
+
+ void free();
+
+ /**
+ * Write the matrix pattern to a file. File is also loadable by octave, e.g., then use spy(matrix)
+ */
+ bool write(const char* filename) const;
+
+ int n; ///< A is m-by-n. n must be >= 0.
+ int m; ///< A is m-by-n. m must be >= 0.
+ int* Ap; ///< column pointers for A, of size n+1
+ int* Aii; ///< row indices of A, of size nz = Ap [n]
+
+ //! max number of non-zeros blocks
+ int nzMax() const { return maxNz;}
+
+ protected:
+ int maxN; ///< size of the allocated memory
+ int maxNz; ///< size of the allocated memory
+};
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/openmp_mutex.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/openmp_mutex.h
new file mode 100644
index 0000000000000000000000000000000000000000..6a9b8f4a66acdc537819af0741e18b9c00409bca
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/openmp_mutex.h
@@ -0,0 +1,97 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPENMP_MUTEX
+#define G2O_OPENMP_MUTEX
+
+#include "../../config.h"
+
+#ifdef G2O_OPENMP
+#include <omp.h>
+#else
+#include <cassert>
+#endif
+
+namespace g2o {
+
+#ifdef G2O_OPENMP
+
+ /**
+ * \brief Mutex realized via OpenMP
+ */
+ class OpenMPMutex
+ {
+ public:
+ OpenMPMutex() { omp_init_lock(&_lock); }
+ ~OpenMPMutex() { omp_destroy_lock(&_lock); }
+ void lock() { omp_set_lock(&_lock); }
+ void unlock() { omp_unset_lock(&_lock); }
+ protected:
+ omp_lock_t _lock;
+ };
+
+#else
+
+ /*
+ * dummy which does nothing in case we don't have OpenMP support.
+ * In debug mode, the mutex allows to verify the correct lock and unlock behavior
+ */
+ class OpenMPMutex
+ {
+ public:
+#ifdef NDEBUG
+ OpenMPMutex() {}
+#else
+ OpenMPMutex() : _cnt(0) {}
+#endif
+ ~OpenMPMutex() { assert(_cnt == 0 && "Freeing locked mutex");}
+ void lock() { assert(++_cnt == 1 && "Locking already locked mutex");}
+ void unlock() { assert(--_cnt == 0 && "Trying to unlock a mutex which is not locked");}
+ protected:
+#ifndef NDEBUG
+ char _cnt;
+#endif
+ };
+
+#endif
+
+ /**
+ * \brief lock a mutex within a scope
+ */
+ class ScopedOpenMPMutex
+ {
+ public:
+ explicit ScopedOpenMPMutex(OpenMPMutex* mutex) : _mutex(mutex) { _mutex->lock(); }
+ ~ScopedOpenMPMutex() { _mutex->unlock(); }
+ private:
+ OpenMPMutex* const _mutex;
+ ScopedOpenMPMutex(const ScopedOpenMPMutex&);
+ void operator=(const ScopedOpenMPMutex&);
+ };
+
+}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bb6b54bd6a6e9311c9f2c0b1a3a74b51ce7ddae2
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.cpp
@@ -0,0 +1,910 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimizable_graph.h"
+
+#include <cassert>
+#include <iostream>
+#include <iomanip>
+#include <fstream>
+#include <algorithm>
+
+#include <Eigen/Dense>
+
+#include "estimate_propagator.h"
+#include "factory.h"
+#include "optimization_algorithm_property.h"
+#include "hyper_graph_action.h"
+#include "cache.h"
+#include "robust_kernel.h"
+
+#include "../stuff/macros.h"
+#include "../stuff/color_macros.h"
+#include "../stuff/string_tools.h"
+#include "../stuff/misc.h"
+
+namespace g2o {
+
+ using namespace std;
+
+ OptimizableGraph::Data::Data(){
+ _next = 0;
+ }
+
+ OptimizableGraph::Data::~Data(){
+ if (_next)
+ delete _next;
+ }
+
+
+ OptimizableGraph::Vertex::Vertex() :
+ HyperGraph::Vertex(),
+ _graph(0), _userData(0), _hessianIndex(-1), _fixed(false), _marginalized(false),
+ _colInHessian(-1), _cacheContainer(0)
+ {
+ }
+
+ CacheContainer* OptimizableGraph::Vertex::cacheContainer(){
+ if (! _cacheContainer)
+ _cacheContainer = new CacheContainer(this);
+ return _cacheContainer;
+ }
+
+
+ void OptimizableGraph::Vertex::updateCache(){
+ if (_cacheContainer){
+ _cacheContainer->setUpdateNeeded();
+ _cacheContainer->update();
+ }
+ }
+
+ OptimizableGraph::Vertex::~Vertex()
+ {
+ if (_cacheContainer)
+ delete (_cacheContainer);
+ if (_userData)
+ delete _userData;
+ }
+
+ OptimizableGraph::Vertex* OptimizableGraph::Vertex::clone() const
+ {
+ return 0;
+ }
+
+ bool OptimizableGraph::Vertex::setEstimateData(const double* v)
+ {
+ bool ret = setEstimateDataImpl(v);
+ updateCache();
+ return ret;
+ }
+
+ bool OptimizableGraph::Vertex::getEstimateData(double *) const
+ {
+ return false;
+ }
+
+ int OptimizableGraph::Vertex::estimateDimension() const
+ {
+ return -1;
+ }
+
+ bool OptimizableGraph::Vertex::setMinimalEstimateData(const double* v)
+ {
+ bool ret = setMinimalEstimateDataImpl(v);
+ updateCache();
+ return ret;
+ }
+
+ bool OptimizableGraph::Vertex::getMinimalEstimateData(double *) const
+ {
+ return false;
+ }
+
+ int OptimizableGraph::Vertex::minimalEstimateDimension() const
+ {
+ return -1;
+ }
+
+
+ OptimizableGraph::Edge::Edge() :
+ HyperGraph::Edge(),
+ _dimension(-1), _level(0), _robustKernel(0)
+ {
+ }
+
+ OptimizableGraph::Edge::~Edge()
+ {
+ delete _robustKernel;
+ }
+
+ OptimizableGraph* OptimizableGraph::Edge::graph(){
+ if (! _vertices.size())
+ return 0;
+ OptimizableGraph::Vertex* v=(OptimizableGraph::Vertex*)_vertices[0];
+ if (!v)
+ return 0;
+ return v->graph();
+ }
+
+ const OptimizableGraph* OptimizableGraph::Edge::graph() const{
+ if (! _vertices.size())
+ return 0;
+ const OptimizableGraph::Vertex* v=(const OptimizableGraph::Vertex*) _vertices[0];
+ if (!v)
+ return 0;
+ return v->graph();
+ }
+
+ bool OptimizableGraph::Edge::setParameterId(int argNum, int paramId){
+ if ((int)_parameters.size()<=argNum)
+ return false;
+ if (argNum<0)
+ return false;
+ *_parameters[argNum] = 0;
+ _parameterIds[argNum] = paramId;
+ return true;
+ }
+
+ bool OptimizableGraph::Edge::resolveParameters() {
+ if (!graph()) {
+ cerr << __PRETTY_FUNCTION__ << ": edge not registered with a graph" << endl;
+ return false;
+ }
+
+ assert (_parameters.size() == _parameterIds.size());
+ //cerr << __PRETTY_FUNCTION__ << ": encountered " << _parameters.size() << " parameters" << endl;
+ for (size_t i=0; i<_parameters.size(); i++){
+ int index = _parameterIds[i];
+ *_parameters[i] = graph()->parameter(index);
+ if (typeid(**_parameters[i]).name()!=_parameterTypes[i]){
+ cerr << __PRETTY_FUNCTION__ << ": FATAL, parameter type mismatch - encountered " << typeid(**_parameters[i]).name() << "; should be " << _parameterTypes[i] << endl;
+ }
+ if (!*_parameters[i]) {
+ cerr << __PRETTY_FUNCTION__ << ": FATAL, *_parameters[i] == 0" << endl;
+ return false;
+ }
+ }
+ return true;
+ }
+
+ void OptimizableGraph::Edge::setRobustKernel(RobustKernel* ptr)
+ {
+ if (_robustKernel)
+ delete _robustKernel;
+ _robustKernel = ptr;
+ }
+
+ bool OptimizableGraph::Edge::resolveCaches() {
+ return true;
+ }
+
+ bool OptimizableGraph::Edge::setMeasurementData(const double *)
+ {
+ return false;
+ }
+
+ bool OptimizableGraph::Edge::getMeasurementData(double *) const
+ {
+ return false;
+ }
+
+ int OptimizableGraph::Edge::measurementDimension() const
+ {
+ return -1;
+ }
+
+ bool OptimizableGraph::Edge::setMeasurementFromState(){
+ return false;
+ }
+
+
+ OptimizableGraph::Edge* OptimizableGraph::Edge::clone() const
+ {
+ // TODO
+ return 0;
+ }
+
+
+ OptimizableGraph::OptimizableGraph()
+ {
+ _nextEdgeId = 0; _edge_has_id = false;
+ _graphActions.resize(AT_NUM_ELEMENTS);
+ }
+
+ OptimizableGraph::~OptimizableGraph()
+ {
+ clear();
+ clearParameters();
+ }
+
+ bool OptimizableGraph::addVertex(HyperGraph::Vertex* v, Data* userData)
+ {
+ Vertex* inserted = vertex(v->id());
+ if (inserted) {
+ cerr << __FUNCTION__ << ": FATAL, a vertex with ID " << v->id() << " has already been registered with this graph" << endl;
+ assert(0 && "Vertex with this ID already contained in the graph");
+ return false;
+ }
+ OptimizableGraph::Vertex* ov=dynamic_cast<OptimizableGraph::Vertex*>(v);
+ assert(ov && "Vertex does not inherit from OptimizableGraph::Vertex");
+ if (ov->_graph != 0 && ov->_graph != this) {
+ cerr << __FUNCTION__ << ": FATAL, vertex with ID " << v->id() << " has already registered with another graph " << ov->_graph << endl;
+ assert(0 && "Vertex already registered with another graph");
+ return false;
+ }
+ if (userData)
+ ov->setUserData(userData);
+ ov->_graph=this;
+ return HyperGraph::addVertex(v);
+ }
+
+ bool OptimizableGraph::addEdge(HyperGraph::Edge* e_)
+ {
+ OptimizableGraph::Edge* e = dynamic_cast<OptimizableGraph::Edge*>(e_);
+ assert(e && "Edge does not inherit from OptimizableGraph::Edge");
+ if (! e)
+ return false;
+ bool eresult = HyperGraph::addEdge(e);
+ if (! eresult)
+ return false;
+ e->_internalId = _nextEdgeId++;
+ if (! e->resolveParameters()){
+ cerr << __FUNCTION__ << ": FATAL, cannot resolve parameters for edge " << e << endl;
+ return false;
+ }
+ if (! e->resolveCaches()){
+ cerr << __FUNCTION__ << ": FATAL, cannot resolve caches for edge " << e << endl;
+ return false;
+ }
+ _jacobianWorkspace.updateSize(e);
+
+ return true;
+ }
+
+ int OptimizableGraph::optimize(int /*iterations*/, bool /*online*/) {return 0;}
+
+double OptimizableGraph::chi2() const
+{
+ double chi = 0.0;
+ for (OptimizableGraph::EdgeSet::const_iterator it = this->edges().begin(); it != this->edges().end(); ++it) {
+ const OptimizableGraph::Edge* e = static_cast<const OptimizableGraph::Edge*>(*it);
+ chi += e->chi2();
+ }
+ return chi;
+}
+
+void OptimizableGraph::push()
+{
+ for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+ v->push();
+ }
+}
+
+void OptimizableGraph::pop()
+{
+ for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+ OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
+ v->pop();
+ }
+}
+
+void OptimizableGraph::discardTop()
+{
+ for (OptimizableGraph::VertexIDMap::iterator it=_vertices.begin(); it!=_vertices.end(); ++it) {
+ OptimizableGraph::Vertex* v= static_cast<OptimizableGraph::Vertex*>(it->second);
+ v->discardTop();
+ }
+}
+
+void OptimizableGraph::push(HyperGraph::VertexSet& vset)
+{
+ for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ v->push();
+ }
+}
+
+void OptimizableGraph::pop(HyperGraph::VertexSet& vset)
+{
+ for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ v->pop();
+ }
+}
+
+void OptimizableGraph::discardTop(HyperGraph::VertexSet& vset)
+{
+ for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ v->discardTop();
+ }
+}
+
+ void OptimizableGraph::setFixed(HyperGraph::VertexSet& vset, bool fixed)
+{
+ for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ v->setFixed(fixed);
+ }
+}
+
+
+bool OptimizableGraph::load(istream& is, bool createEdges)
+{
+ // scna for the paramers in the whole file
+ if (!_parameters.read(is,&_renamedTypesLookup))
+ return false;
+#ifndef NDEBUG
+ cerr << "Loaded " << _parameters.size() << " parameters" << endl;
+#endif
+ is.clear();
+ is.seekg(ios_base::beg);
+ set<string> warnedUnknownTypes;
+ stringstream currentLine;
+ string token;
+
+ Factory* factory = Factory::instance();
+ HyperGraph::GraphElemBitset elemBitset;
+ elemBitset[HyperGraph::HGET_PARAMETER] = 1;
+ elemBitset.flip();
+
+ Vertex* previousVertex = 0;
+ Data* previousData = 0;
+
+ while (1) {
+ int bytesRead = readLine(is, currentLine);
+ if (bytesRead == -1)
+ break;
+ currentLine >> token;
+ //cerr << "Token=" << token << endl;
+ if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
+ continue;
+
+ // handle commands encoded in the file
+ bool handledCommand = false;
+
+ if (token == "FIX") {
+ handledCommand = true;
+ int id;
+ while (currentLine >> id) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(vertex(id));
+ if (v) {
+# ifndef NDEBUG
+ cerr << "Fixing vertex " << v->id() << endl;
+# endif
+ v->setFixed(true);
+ } else {
+ cerr << "Warning: Unable to fix vertex with id " << id << ". Not found in the graph." << endl;
+ }
+ }
+ }
+
+ if (handledCommand)
+ continue;
+
+ // do the mapping to an internal type if it matches
+ if (_renamedTypesLookup.size() > 0) {
+ map<string, string>::const_iterator foundIt = _renamedTypesLookup.find(token);
+ if (foundIt != _renamedTypesLookup.end()) {
+ token = foundIt->second;
+ }
+ }
+
+ if (! factory->knowsTag(token)) {
+ if (warnedUnknownTypes.count(token) != 1) {
+ warnedUnknownTypes.insert(token);
+ cerr << CL_RED(__PRETTY_FUNCTION__ << " unknown type: " << token) << endl;
+ }
+ continue;
+ }
+
+ HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
+ if (dynamic_cast<Vertex*>(element)) { // it's a vertex type
+ //cerr << "it is a vertex" << endl;
+ previousData = 0;
+ Vertex* v = static_cast<Vertex*>(element);
+ int id;
+ currentLine >> id;
+ bool r = v->read(currentLine);
+ if (! r)
+ cerr << __PRETTY_FUNCTION__ << ": Error reading vertex " << token << " " << id << endl;
+ v->setId(id);
+ if (!addVertex(v)) {
+ cerr << __PRETTY_FUNCTION__ << ": Failure adding Vertex, " << token << " " << id << endl;
+ delete v;
+ } else {
+ previousVertex = v;
+ }
+ }
+ else if (dynamic_cast<Edge*>(element)) {
+ //cerr << "it is an edge" << endl;
+ previousData = 0;
+ Edge* e = static_cast<Edge*>(element);
+ int numV = e->vertices().size();
+ if (_edge_has_id){
+ int id;
+ currentLine >> id;
+ e->setId(id);
+ }
+ //cerr << PVAR(token) << " " << PVAR(numV) << endl;
+ if (numV == 2) { // it's a pairwise / binary edge type which we handle in a special way
+ int id1, id2;
+ currentLine >> id1 >> id2;
+ Vertex* from = vertex(id1);
+ Vertex* to = vertex(id2);
+ int doInit=0;
+ if ((!from || !to) ) {
+ if (! createEdges) {
+ cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token << " " << id1 << " <-> " << id2 << endl;
+ delete e;
+ } else {
+ if (! from) {
+ from=e->createFrom();
+ from->setId(id1);
+ addVertex(from);
+ doInit=2;
+ }
+ if (! to) {
+ to=e->createTo();
+ to->setId(id2);
+ addVertex(to);
+ doInit=1;
+ }
+ }
+ }
+ if (from && to) {
+ e->setVertex(0, from);
+ e->setVertex(1, to);
+ e->read(currentLine);
+ if (!addEdge(e)) {
+ cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token << " " << id1 << " <-> " << id2 << endl;
+ delete e;
+ } else {
+ switch (doInit){
+ case 1:
+ {
+ HyperGraph::VertexSet fromSet;
+ fromSet.insert(from);
+ e->initialEstimate(fromSet, to);
+ break;
+ }
+ case 2:
+ {
+ HyperGraph::VertexSet toSet;
+ toSet.insert(to);
+ e->initialEstimate(toSet, from);
+ break;
+ }
+ default:;
+ }
+ }
+ }
+ }
+ else {
+ vector<int> ids;
+ ids.resize(numV);
+ for (int l = 0; l < numV; ++l)
+ currentLine >> ids[l];
+ bool vertsOkay = true;
+ for (int l = 0; l < numV; ++l) {
+ e->setVertex(l, vertex(ids[l]));
+ if (e->vertex(l) == 0) {
+ vertsOkay = false;
+ break;
+ }
+ }
+ if (! vertsOkay) {
+ cerr << __PRETTY_FUNCTION__ << ": Unable to find vertices for edge " << token;
+ for (int l = 0; l < numV; ++l) {
+ if (l > 0)
+ cerr << " <->";
+ cerr << " " << ids[l];
+ }
+ delete e;
+ } else {
+ bool r = e->read(currentLine);
+ if (!r || !addEdge(e)) {
+ cerr << __PRETTY_FUNCTION__ << ": Unable to add edge " << token;
+ for (int l = 0; l < numV; ++l) {
+ if (l > 0)
+ cerr << " <->";
+ cerr << " " << ids[l];
+ }
+ delete e;
+ }
+ }
+ }
+ } else if (dynamic_cast<Data*>(element)) { // reading in the data packet for the vertex
+ //cerr << "read data packet " << token << " vertex " << previousVertex->id() << endl;
+ Data* d = static_cast<Data*>(element);
+ bool r = d->read(currentLine);
+ if (! r) {
+ cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for vertex " << previousVertex->id() << endl;
+ delete d;
+ previousData = 0;
+ } else if (previousData){
+ //cerr << "chaining" << endl;
+ previousData->setNext(d);
+ previousData = d;
+ //cerr << "done" << endl;
+ } else if (previousVertex){
+ //cerr << "embedding in vertex" << endl;
+ previousVertex->setUserData(d);
+ previousData = d;
+ previousVertex = 0;
+ //cerr << "done" << endl;
+ } else {
+ cerr << __PRETTY_FUNCTION__ << ": got data element, but no vertex available" << endl;
+ delete d;
+ previousData = 0;
+ }
+ }
+ } // while read line
+
+ return true;
+}
+
+bool OptimizableGraph::load(const char* filename, bool createEdges)
+{
+ ifstream ifs(filename);
+ if (!ifs) {
+ cerr << __PRETTY_FUNCTION__ << " unable to open file " << filename << endl;
+ return false;
+ }
+ return load(ifs, createEdges);
+}
+
+bool OptimizableGraph::save(const char* filename, int level) const
+{
+ ofstream ofs(filename);
+ if (!ofs)
+ return false;
+ return save(ofs, level);
+}
+
+bool OptimizableGraph::save(ostream& os, int level) const
+{
+ if (! _parameters.write(os))
+ return false;
+ set<Vertex*, VertexIDCompare> verticesToSave;
+ for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+ OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+ if (e->level() == level) {
+ for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ verticesToSave.insert(static_cast<OptimizableGraph::Vertex*>(*it));
+ }
+ }
+ }
+
+ for (set<Vertex*, VertexIDCompare>::const_iterator it = verticesToSave.begin(); it != verticesToSave.end(); ++it){
+ OptimizableGraph::Vertex* v = *it;
+ saveVertex(os, v);
+ }
+
+ EdgeContainer edgesToSave;
+ for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+ const OptimizableGraph::Edge* e = dynamic_cast<const OptimizableGraph::Edge*>(*it);
+ if (e->level() == level)
+ edgesToSave.push_back(const_cast<Edge*>(e));
+ }
+ sort(edgesToSave.begin(), edgesToSave.end(), EdgeIDCompare());
+
+ for (EdgeContainer::const_iterator it = edgesToSave.begin(); it != edgesToSave.end(); ++it) {
+ OptimizableGraph::Edge* e = *it;
+ saveEdge(os, e);
+ }
+
+ return os.good();
+}
+
+
+bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::VertexSet& vset, int level)
+{
+ if (! _parameters.write(os))
+ return false;
+
+ for (HyperGraph::VertexSet::const_iterator it=vset.begin(); it!=vset.end(); it++){
+ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+ saveVertex(os, v);
+ }
+ for (HyperGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+ OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
+ if (e->level() != level)
+ continue;
+
+ bool verticesInEdge = true;
+ for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ if (vset.find(*it) == vset.end()) {
+ verticesInEdge = false;
+ break;
+ }
+ }
+ if (! verticesInEdge)
+ continue;
+
+ saveEdge(os, e);
+ }
+
+ return os.good();
+}
+
+bool OptimizableGraph::saveSubset(ostream& os, HyperGraph::EdgeSet& eset)
+{
+ if (!_parameters.write(os))
+ return false;
+ std::set<OptimizableGraph::Vertex*> vset;
+ for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
+ HyperGraph::Edge* e = *it;
+ for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ vset.insert(v);
+ }
+ }
+
+ for (std::set<OptimizableGraph::Vertex*>::const_iterator it=vset.begin(); it!=vset.end(); ++it){
+ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+ saveVertex(os, v);
+ }
+
+ for (HyperGraph::EdgeSet::const_iterator it = eset.begin(); it != eset.end(); ++it) {
+ OptimizableGraph::Edge* e = dynamic_cast< OptimizableGraph::Edge*>(*it);
+ saveEdge(os, e);
+ }
+
+ return os.good();
+}
+
+void OptimizableGraph::addGraph(OptimizableGraph* g){
+ for (HyperGraph::VertexIDMap::iterator it=g->vertices().begin(); it!=g->vertices().end(); ++it){
+ OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*)(it->second);
+ if (vertex(v->id()))
+ continue;
+ OptimizableGraph::Vertex* v2=v->clone();
+ v2->edges().clear();
+ v2->setHessianIndex(-1);
+ addVertex(v2);
+ }
+ for (HyperGraph::EdgeSet::iterator it=g->edges().begin(); it!=g->edges().end(); ++it){
+ OptimizableGraph::Edge* e = (OptimizableGraph::Edge*)(*it);
+ OptimizableGraph::Edge* en = e->clone();
+ en->resize(e->vertices().size());
+ int cnt = 0;
+ for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ OptimizableGraph::Vertex* v = (OptimizableGraph::Vertex*) vertex((*it)->id());
+ assert(v);
+ en->setVertex(cnt++, v);
+ }
+ addEdge(en);
+ }
+}
+
+int OptimizableGraph::maxDimension() const{
+ int maxDim=0;
+ for (HyperGraph::VertexIDMap::const_iterator it=vertices().begin(); it!=vertices().end(); ++it){
+ const OptimizableGraph::Vertex* v= static_cast< const OptimizableGraph::Vertex*>(it->second);
+ maxDim = (std::max)(maxDim, v->dimension());
+ }
+ return maxDim;
+}
+
+void OptimizableGraph::setRenamedTypesFromString(const std::string& types)
+{
+ Factory* factory = Factory::instance();
+ vector<string> typesMap = strSplit(types, ",");
+ for (size_t i = 0; i < typesMap.size(); ++i) {
+ vector<string> m = strSplit(typesMap[i], "=");
+ if (m.size() != 2) {
+ cerr << __PRETTY_FUNCTION__ << ": unable to extract type map from " << typesMap[i] << endl;
+ continue;
+ }
+ string typeInFile = trim(m[0]);
+ string loadedType = trim(m[1]);
+ if (! factory->knowsTag(loadedType)) {
+ cerr << __PRETTY_FUNCTION__ << ": unknown type " << loadedType << endl;
+ continue;
+ }
+
+ _renamedTypesLookup[typeInFile] = loadedType;
+ }
+
+ cerr << "# load look up table" << endl;
+ for (std::map<std::string, std::string>::const_iterator it = _renamedTypesLookup.begin(); it != _renamedTypesLookup.end(); ++it) {
+ cerr << "#\t" << it->first << " -> " << it->second << endl;
+ }
+}
+
+bool OptimizableGraph::isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims_) const
+{
+ std::set<int> auxDims;
+ if (vertDims_.size() == 0) {
+ auxDims = dimensions();
+ }
+ const set<int>& vertDims = vertDims_.size() == 0 ? auxDims : vertDims_;
+ bool suitableSolver = true;
+ if (vertDims.size() == 2) {
+ if (solverProperty.requiresMarginalize) {
+ suitableSolver = vertDims.count(solverProperty.poseDim) == 1 && vertDims.count(solverProperty.landmarkDim) == 1;
+ }
+ else {
+ suitableSolver = solverProperty.poseDim == -1;
+ }
+ } else if (vertDims.size() == 1) {
+ suitableSolver = vertDims.count(solverProperty.poseDim) == 1 || solverProperty.poseDim == -1;
+ } else {
+ suitableSolver = solverProperty.poseDim == -1 && !solverProperty.requiresMarginalize;
+ }
+ return suitableSolver;
+}
+
+std::set<int> OptimizableGraph::dimensions() const
+{
+ std::set<int> auxDims;
+ for (VertexIDMap::const_iterator it = vertices().begin(); it != vertices().end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+ auxDims.insert(v->dimension());
+ }
+ return auxDims;
+}
+
+void OptimizableGraph::preIteration(int iter)
+{
+ HyperGraphActionSet& actions = _graphActions[AT_PREITERATION];
+ if (actions.size() > 0) {
+ HyperGraphAction::ParametersIteration params(iter);
+ for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
+ (*(*it))(this, ¶ms);
+ }
+ }
+}
+
+void OptimizableGraph::postIteration(int iter)
+{
+ HyperGraphActionSet& actions = _graphActions[AT_POSTITERATION];
+ if (actions.size() > 0) {
+ HyperGraphAction::ParametersIteration params(iter);
+ for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it) {
+ (*(*it))(this, ¶ms);
+ }
+ }
+}
+
+bool OptimizableGraph::addPostIterationAction(HyperGraphAction* action)
+{
+ std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_POSTITERATION].insert(action);
+ return insertResult.second;
+}
+
+bool OptimizableGraph::addPreIterationAction(HyperGraphAction* action)
+{
+ std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_PREITERATION].insert(action);
+ return insertResult.second;
+}
+
+bool OptimizableGraph::removePreIterationAction(HyperGraphAction* action)
+{
+ return _graphActions[AT_PREITERATION].erase(action) > 0;
+}
+
+bool OptimizableGraph::removePostIterationAction(HyperGraphAction* action)
+{
+ return _graphActions[AT_POSTITERATION].erase(action) > 0;
+}
+
+bool OptimizableGraph::saveVertex(std::ostream& os, OptimizableGraph::Vertex* v) const
+{
+ Factory* factory = Factory::instance();
+ string tag = factory->tag(v);
+ if (tag.size() > 0) {
+ os << tag << " " << v->id() << " ";
+ v->write(os);
+ os << endl;
+ Data* d=v->userData();
+ while (d) { // write the data packet for the vertex
+ tag = factory->tag(d);
+ if (tag.size() > 0) {
+ os << tag << " ";
+ d->write(os);
+ os << endl;
+ }
+ d=d->next();
+ }
+ if (v->fixed()) {
+ os << "FIX " << v->id() << endl;
+ }
+ return os.good();
+ }
+ return false;
+}
+
+bool OptimizableGraph::saveEdge(std::ostream& os, OptimizableGraph::Edge* e) const
+{
+ Factory* factory = Factory::instance();
+ string tag = factory->tag(e);
+ if (tag.size() > 0) {
+ os << tag << " ";
+ if (_edge_has_id)
+ os << e->id() << " ";
+ for (vector<HyperGraph::Vertex*>::const_iterator it = e->vertices().begin(); it != e->vertices().end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(*it);
+ os << v->id() << " ";
+ }
+ e->write(os);
+ os << endl;
+ return os.good();
+ }
+ return false;
+}
+
+void OptimizableGraph::clearParameters()
+{
+ _parameters.clear();
+}
+
+bool OptimizableGraph::verifyInformationMatrices(bool verbose) const
+{
+ bool allEdgeOk = true;
+ SelfAdjointEigenSolver<MatrixXd> eigenSolver;
+ for (OptimizableGraph::EdgeSet::const_iterator it = edges().begin(); it != edges().end(); ++it) {
+ OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+ Eigen::MatrixXd::MapType information(e->informationData(), e->dimension(), e->dimension());
+ // test on symmetry
+ bool isSymmetric = information.transpose() == information;
+ bool okay = isSymmetric;
+ if (isSymmetric) {
+ // compute the eigenvalues
+ eigenSolver.compute(information, Eigen::EigenvaluesOnly);
+ bool isSPD = eigenSolver.eigenvalues()(0) >= 0.;
+ okay = okay && isSPD;
+ }
+ allEdgeOk = allEdgeOk && okay;
+ if (! okay) {
+ if (verbose) {
+ if (! isSymmetric)
+ cerr << "Information Matrix for an edge is not symmetric:";
+ else
+ cerr << "Information Matrix for an edge is not SPD:";
+ for (size_t i = 0; i < e->vertices().size(); ++i)
+ cerr << " " << e->vertex(i)->id();
+ if (isSymmetric)
+ cerr << "\teigenvalues: " << eigenSolver.eigenvalues().transpose();
+ cerr << endl;
+ }
+ }
+ }
+ return allEdgeOk;
+}
+
+bool OptimizableGraph::initMultiThreading()
+{
+# if (defined G2O_OPENMP) && EIGEN_VERSION_AT_LEAST(3,1,0)
+ Eigen::initParallel();
+# endif
+ return true;
+}
+
+} // end namespace
+
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d9b561cc2372748105a03ef25e8d47cbfc0c64e
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimizable_graph.h
@@ -0,0 +1,688 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, H. Strasdat, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_AIS_OPTIMIZABLE_GRAPH_HH_
+#define G2O_AIS_OPTIMIZABLE_GRAPH_HH_
+
+#include <set>
+#include <iostream>
+#include <list>
+#include <limits>
+#include <cmath>
+#include <typeinfo>
+
+#include "openmp_mutex.h"
+#include "hyper_graph.h"
+#include "parameter.h"
+#include "parameter_container.h"
+#include "jacobian_workspace.h"
+
+#include "../stuff/macros.h"
+
+namespace g2o {
+
+ class HyperGraphAction;
+ struct OptimizationAlgorithmProperty;
+ class Cache;
+ class CacheContainer;
+ class RobustKernel;
+
+ /**
+ @addtogroup g2o
+ */
+ /**
+ This is an abstract class that represents one optimization
+ problem. It specializes the general graph to contain special
+ vertices and edges. The vertices represent parameters that can
+ be optimized, while the edges represent constraints. This class
+ also provides basic functionalities to handle the backup/restore
+ of portions of the vertices.
+ */
+ struct OptimizableGraph : public HyperGraph {
+
+ enum ActionType {
+ AT_PREITERATION, AT_POSTITERATION,
+ AT_NUM_ELEMENTS, // keep as last element
+ };
+
+ typedef std::set<HyperGraphAction*> HyperGraphActionSet;
+
+ // forward declarations
+ class Vertex;
+ class Edge;
+
+ /**
+ * \brief data packet for a vertex. Extend this class to store in the vertices
+ * the potential additional information you need (e.g. images, laser scans, ...).
+ */
+ class Data : public HyperGraph::HyperGraphElement
+ {
+ friend struct OptimizableGraph;
+ public:
+ virtual ~Data();
+ Data();
+ //! read the data from a stream
+ virtual bool read(std::istream& is) = 0;
+ //! write the data to a stream
+ virtual bool write(std::ostream& os) const = 0;
+ virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_DATA;}
+ const Data* next() const {return _next;}
+ Data* next() {return _next;}
+ void setNext(Data* next_) { _next = next_; }
+ protected:
+ Data* _next; // linked list of multiple data;
+ };
+
+ /**
+ * \brief order vertices based on their ID
+ */
+ struct VertexIDCompare {
+ bool operator() (const Vertex* v1, const Vertex* v2) const
+ {
+ return v1->id() < v2->id();
+ }
+ };
+
+ /**
+ * \brief order edges based on the internal ID, which is assigned to the edge in addEdge()
+ */
+ struct EdgeIDCompare {
+ bool operator() (const Edge* e1, const Edge* e2) const
+ {
+ return e1->internalId() < e2->internalId();
+ }
+ };
+
+ //! vector container for vertices
+ typedef std::vector<OptimizableGraph::Vertex*> VertexContainer;
+ //! vector container for edges
+ typedef std::vector<OptimizableGraph::Edge*> EdgeContainer;
+
+ /**
+ * \brief A general case Vertex for optimization
+ */
+ class Vertex : public HyperGraph::Vertex {
+ private:
+ friend struct OptimizableGraph;
+ public:
+ Vertex();
+
+ //! returns a deep copy of the current vertex
+ virtual Vertex* clone() const ;
+
+ //! the user data associated with this vertex
+ const Data* userData() const { return _userData; }
+ Data* userData() { return _userData; }
+
+ void setUserData(Data* obs) { _userData = obs;}
+ void addUserData(Data* obs) {
+ if (obs) {
+ obs->setNext(_userData);
+ _userData=obs;
+ }
+ }
+
+ virtual ~Vertex();
+
+ //! sets the node to the origin (used in the multilevel stuff)
+ void setToOrigin() { setToOriginImpl(); updateCache();}
+
+ //! get the element from the hessian matrix
+ virtual const double& hessian(int i, int j) const = 0;
+ virtual double& hessian(int i, int j) = 0;
+ virtual double hessianDeterminant() const = 0;
+ virtual double* hessianData() = 0;
+
+ /** maps the internal matrix to some external memory location */
+ virtual void mapHessianMemory(double* d) = 0;
+
+ /**
+ * copies the b vector in the array b_
+ * @return the number of elements copied
+ */
+ virtual int copyB(double* b_) const = 0;
+
+ //! get the b vector element
+ virtual const double& b(int i) const = 0;
+ virtual double& b(int i) = 0;
+ //! return a pointer to the b vector associated with this vertex
+ virtual double* bData() = 0;
+
+ /**
+ * set the b vector part of this vertex to zero
+ */
+ virtual void clearQuadraticForm() = 0;
+
+ /**
+ * updates the current vertex with the direct solution x += H_ii\b_ii
+ * @return the determinant of the inverted hessian
+ */
+ virtual double solveDirect(double lambda=0) = 0;
+
+ /**
+ * sets the initial estimate from an array of double
+ * Implement setEstimateDataImpl()
+ * @return true on success
+ */
+ bool setEstimateData(const double* estimate);
+
+ /**
+ * sets the initial estimate from an array of double
+ * Implement setEstimateDataImpl()
+ * @return true on success
+ */
+ bool setEstimateData(const std::vector<double>& estimate) {
+#ifndef NDEBUG
+ int dim = estimateDimension();
+ assert((dim == -1) || (estimate.size() == std::size_t(dim)));
+#endif
+ return setEstimateData(&estimate[0]);
+ };
+
+ /**
+ * writes the estimater to an array of double
+ * @returns true on success
+ */
+ virtual bool getEstimateData(double* estimate) const;
+
+ /**
+ * writes the estimater to an array of double
+ * @returns true on success
+ */
+ virtual bool getEstimateData(std::vector<double>& estimate) const {
+ int dim = estimateDimension();
+ if (dim < 0)
+ return false;
+ estimate.resize(dim);
+ return getEstimateData(&estimate[0]);
+ };
+
+ /**
+ * returns the dimension of the extended representation used by get/setEstimate(double*)
+ * -1 if it is not supported
+ */
+ virtual int estimateDimension() const;
+
+ /**
+ * sets the initial estimate from an array of double.
+ * Implement setMinimalEstimateDataImpl()
+ * @return true on success
+ */
+ bool setMinimalEstimateData(const double* estimate);
+
+ /**
+ * sets the initial estimate from an array of double.
+ * Implement setMinimalEstimateDataImpl()
+ * @return true on success
+ */
+ bool setMinimalEstimateData(const std::vector<double>& estimate) {
+#ifndef NDEBUG
+ int dim = minimalEstimateDimension();
+ assert((dim == -1) || (estimate.size() == std::size_t(dim)));
+#endif
+ return setMinimalEstimateData(&estimate[0]);
+ };
+
+ /**
+ * writes the estimate to an array of double
+ * @returns true on success
+ */
+ virtual bool getMinimalEstimateData(double* estimate) const ;
+
+ /**
+ * writes the estimate to an array of double
+ * @returns true on success
+ */
+ virtual bool getMinimalEstimateData(std::vector<double>& estimate) const {
+ int dim = minimalEstimateDimension();
+ if (dim < 0)
+ return false;
+ estimate.resize(dim);
+ return getMinimalEstimateData(&estimate[0]);
+ };
+
+ /**
+ * returns the dimension of the extended representation used by get/setEstimate(double*)
+ * -1 if it is not supported
+ */
+ virtual int minimalEstimateDimension() const;
+
+ //! backup the position of the vertex to a stack
+ virtual void push() = 0;
+
+ //! restore the position of the vertex by retrieving the position from the stack
+ virtual void pop() = 0;
+
+ //! pop the last element from the stack, without restoring the current estimate
+ virtual void discardTop() = 0;
+
+ //! return the stack size
+ virtual int stackSize() const = 0;
+
+ /**
+ * Update the position of the node from the parameters in v.
+ * Depends on the implementation of oplusImpl in derived classes to actually carry
+ * out the update.
+ * Will also call updateCache() to update the caches of depending on the vertex.
+ */
+ void oplus(const double* v)
+ {
+ oplusImpl(v);
+ updateCache();
+ }
+
+ //! temporary index of this node in the parameter vector obtained from linearization
+ int hessianIndex() const { return _hessianIndex;}
+ int G2O_ATTRIBUTE_DEPRECATED(tempIndex() const) { return hessianIndex();}
+ //! set the temporary index of the vertex in the parameter blocks
+ void setHessianIndex(int ti) { _hessianIndex = ti;}
+ void G2O_ATTRIBUTE_DEPRECATED(setTempIndex(int ti)) { setHessianIndex(ti);}
+
+ //! true => this node is fixed during the optimization
+ bool fixed() const {return _fixed;}
+ //! true => this node should be considered fixed during the optimization
+ void setFixed(bool fixed) { _fixed = fixed;}
+
+ //! true => this node is marginalized out during the optimization
+ bool marginalized() const {return _marginalized;}
+ //! true => this node should be marginalized out during the optimization
+ void setMarginalized(bool marginalized) { _marginalized = marginalized;}
+
+ //! dimension of the estimated state belonging to this node
+ int dimension() const { return _dimension;}
+
+ //! sets the id of the node in the graph be sure that the graph keeps consistent after changing the id
+ virtual void setId(int id) {_id = id;}
+
+ //! set the row of this vertex in the Hessian
+ void setColInHessian(int c) { _colInHessian = c;}
+ //! get the row of this vertex in the Hessian
+ int colInHessian() const {return _colInHessian;}
+
+ const OptimizableGraph* graph() const {return _graph;}
+
+ OptimizableGraph* graph() {return _graph;}
+
+ /**
+ * lock for the block of the hessian and the b vector associated with this vertex, to avoid
+ * race-conditions if multi-threaded.
+ */
+ void lockQuadraticForm() { _quadraticFormMutex.lock();}
+ /**
+ * unlock the block of the hessian and the b vector associated with this vertex
+ */
+ void unlockQuadraticForm() { _quadraticFormMutex.unlock();}
+
+ //! read the vertex from a stream, i.e., the internal state of the vertex
+ virtual bool read(std::istream& is) = 0;
+ //! write the vertex to a stream
+ virtual bool write(std::ostream& os) const = 0;
+
+ virtual void updateCache();
+
+ CacheContainer* cacheContainer();
+ protected:
+ OptimizableGraph* _graph;
+ Data* _userData;
+ int _hessianIndex;
+ bool _fixed;
+ bool _marginalized;
+ int _dimension;
+ int _colInHessian;
+ OpenMPMutex _quadraticFormMutex;
+
+ CacheContainer* _cacheContainer;
+
+ /**
+ * update the position of the node from the parameters in v.
+ * Implement in your class!
+ */
+ virtual void oplusImpl(const double* v) = 0;
+
+ //! sets the node to the origin (used in the multilevel stuff)
+ virtual void setToOriginImpl() = 0;
+
+ /**
+ * writes the estimater to an array of double
+ * @returns true on success
+ */
+ virtual bool setEstimateDataImpl(const double* ) { return false;}
+
+ /**
+ * sets the initial estimate from an array of double
+ * @return true on success
+ */
+ virtual bool setMinimalEstimateDataImpl(const double* ) { return false;}
+
+ };
+
+ class Edge: public HyperGraph::Edge {
+ private:
+ friend struct OptimizableGraph;
+ public:
+ Edge();
+ virtual ~Edge();
+ virtual Edge* clone() const;
+
+ // indicates if all vertices are fixed
+ virtual bool allVerticesFixed() const = 0;
+
+ // computes the error of the edge and stores it in an internal structure
+ virtual void computeError() = 0;
+
+ //! sets the measurement from an array of double
+ //! @returns true on success
+ virtual bool setMeasurementData(const double* m);
+
+ //! writes the measurement to an array of double
+ //! @returns true on success
+ virtual bool getMeasurementData(double* m) const;
+
+ //! returns the dimension of the measurement in the extended representation which is used
+ //! by get/setMeasurement;
+ virtual int measurementDimension() const;
+
+ /**
+ * sets the estimate to have a zero error, based on the current value of the state variables
+ * returns false if not supported.
+ */
+ virtual bool setMeasurementFromState();
+
+ //! if NOT NULL, error of this edge will be robustifed with the kernel
+ RobustKernel* robustKernel() const { return _robustKernel;}
+ /**
+ * specify the robust kernel to be used in this edge
+ */
+ void setRobustKernel(RobustKernel* ptr);
+
+ //! returns the error vector cached after calling the computeError;
+ virtual const double* errorData() const = 0;
+ virtual double* errorData() = 0;
+
+ //! returns the memory of the information matrix, usable for example with a Eigen::Map<MatrixXd>
+ virtual const double* informationData() const = 0;
+ virtual double* informationData() = 0;
+
+ //! computes the chi2 based on the cached error value, only valid after computeError has been called.
+ virtual double chi2() const = 0;
+
+ /**
+ * Linearizes the constraint in the edge.
+ * Makes side effect on the vertices of the graph by changing
+ * the parameter vector b and the hessian blocks ii and jj.
+ * The off diagoinal block is accesed via _hessian.
+ */
+ virtual void constructQuadraticForm() = 0;
+
+ /**
+ * maps the internal matrix to some external memory location,
+ * you need to provide the memory before calling constructQuadraticForm
+ * @param d the memory location to which we map
+ * @param i index of the vertex i
+ * @param j index of the vertex j (j > i, upper triangular fashion)
+ * @param rowMajor if true, will write in rowMajor order to the block. Since EIGEN is columnMajor by default, this results in writing the transposed
+ */
+ virtual void mapHessianMemory(double* d, int i, int j, bool rowMajor) = 0;
+
+ /**
+ * Linearizes the constraint in the edge in the manifold space, and store
+ * the result in the given workspace
+ */
+ virtual void linearizeOplus(JacobianWorkspace& jacobianWorkspace) = 0;
+
+ /** set the estimate of the to vertex, based on the estimate of the from vertices in the edge. */
+ virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) = 0;
+
+ /**
+ * override in your class if it's possible to initialize the vertices in certain combinations.
+ * The return value may correspond to the cost for initiliaizng the vertex but should be positive if
+ * the initialization is possible and negative if not possible.
+ */
+ virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* to) { (void) from; (void) to; return -1.;}
+
+ //! returns the level of the edge
+ int level() const { return _level;}
+ //! sets the level of the edge
+ void setLevel(int l) { _level=l;}
+
+ //! returns the dimensions of the error function
+ int dimension() const { return _dimension;}
+
+ virtual Vertex* createFrom() {return 0;}
+ virtual Vertex* createTo() {return 0;}
+
+ //! read the vertex from a stream, i.e., the internal state of the vertex
+ virtual bool read(std::istream& is) = 0;
+ //! write the vertex to a stream
+ virtual bool write(std::ostream& os) const = 0;
+
+ //! the internal ID of the edge
+ long long internalId() const { return _internalId;}
+
+ OptimizableGraph* graph();
+ const OptimizableGraph* graph() const;
+
+ bool setParameterId(int argNum, int paramId);
+ inline const Parameter* parameter(int argNo) const {return *_parameters.at(argNo);}
+ inline size_t numParameters() const {return _parameters.size();}
+ inline void resizeParameters(size_t newSize) {
+ _parameters.resize(newSize, 0);
+ _parameterIds.resize(newSize, -1);
+ _parameterTypes.resize(newSize, typeid(void*).name());
+ }
+ protected:
+ int _dimension;
+ int _level;
+ RobustKernel* _robustKernel;
+ long long _internalId;
+
+ std::vector<int> _cacheIds;
+
+ template <typename ParameterType>
+ bool installParameter(ParameterType*& p, size_t argNo, int paramId=-1){
+ if (argNo>=_parameters.size())
+ return false;
+ _parameterIds[argNo] = paramId;
+ _parameters[argNo] = (Parameter**)&p;
+ _parameterTypes[argNo] = typeid(ParameterType).name();
+ return true;
+ }
+
+ template <typename CacheType>
+ void resolveCache(CacheType*& cache, OptimizableGraph::Vertex*,
+ const std::string& _type,
+ const ParameterVector& parameters);
+
+ bool resolveParameters();
+ virtual bool resolveCaches();
+
+ std::vector<std::string> _parameterTypes;
+ std::vector<Parameter**> _parameters;
+ std::vector<int> _parameterIds;
+ };
+
+ //! returns the vertex number <i>id</i> appropriately casted
+ inline Vertex* vertex(int id) { return reinterpret_cast<Vertex*>(HyperGraph::vertex(id));}
+
+ //! returns the vertex number <i>id</i> appropriately casted
+ inline const Vertex* vertex (int id) const{ return reinterpret_cast<const Vertex*>(HyperGraph::vertex(id));}
+
+ //! empty constructor
+ OptimizableGraph();
+ virtual ~OptimizableGraph();
+
+ //! adds all edges and vertices of the graph <i>g</i> to this graph.
+ void addGraph(OptimizableGraph* g);
+
+ /**
+ * adds a new vertex. The new vertex is then "taken".
+ * @return false if a vertex with the same id as v is already in the graph, true otherwise.
+ */
+ virtual bool addVertex(HyperGraph::Vertex* v, Data* userData);
+ virtual bool addVertex(HyperGraph::Vertex* v) { return addVertex(v, 0);}
+
+ /**
+ * adds a new edge.
+ * The edge should point to the vertices that it is connecting (setFrom/setTo).
+ * @return false if the insertion does not work (incompatible types of the vertices/missing vertex). true otherwise.
+ */
+ virtual bool addEdge(HyperGraph::Edge* e);
+
+ //! returns the chi2 of the current configuration
+ double chi2() const;
+
+ //! return the maximum dimension of all vertices in the graph
+ int maxDimension() const;
+
+ /**
+ * iterates over all vertices and returns a set of all the vertex dimensions in the graph
+ */
+ std::set<int> dimensions() const;
+
+ /**
+ * carry out n iterations
+ * @return the number of performed iterations
+ */
+ virtual int optimize(int iterations, bool online=false);
+
+ //! called at the beginning of an iteration (argument is the number of the iteration)
+ virtual void preIteration(int);
+ //! called at the end of an iteration (argument is the number of the iteration)
+ virtual void postIteration(int);
+
+ //! add an action to be executed before each iteration
+ bool addPreIterationAction(HyperGraphAction* action);
+ //! add an action to be executed after each iteration
+ bool addPostIterationAction(HyperGraphAction* action);
+
+ //! remove an action that should no longer be execured before each iteration
+ bool removePreIterationAction(HyperGraphAction* action);
+ //! remove an action that should no longer be execured after each iteration
+ bool removePostIterationAction(HyperGraphAction* action);
+
+ //! push the estimate of all variables onto a stack
+ virtual void push();
+ //! pop (restore) the estimate of all variables from the stack
+ virtual void pop();
+ //! discard the last backup of the estimate for all variables by removing it from the stack
+ virtual void discardTop();
+
+ //! load the graph from a stream. Uses the Factory singleton for creating the vertices and edges.
+ virtual bool load(std::istream& is, bool createEdges=true);
+ bool load(const char* filename, bool createEdges=true);
+ //! save the graph to a stream. Again uses the Factory system.
+ virtual bool save(std::ostream& os, int level = 0) const;
+ //! function provided for convenience, see save() above
+ bool save(const char* filename, int level = 0) const;
+
+
+ //! save a subgraph to a stream. Again uses the Factory system.
+ bool saveSubset(std::ostream& os, HyperGraph::VertexSet& vset, int level = 0);
+
+ //! save a subgraph to a stream. Again uses the Factory system.
+ bool saveSubset(std::ostream& os, HyperGraph::EdgeSet& eset);
+
+ //! push the estimate of a subset of the variables onto a stack
+ virtual void push(HyperGraph::VertexSet& vset);
+ //! pop (restore) the estimate a subset of the variables from the stack
+ virtual void pop(HyperGraph::VertexSet& vset);
+ //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
+ virtual void discardTop(HyperGraph::VertexSet& vset);
+
+ //! fixes/releases a set of vertices
+ virtual void setFixed(HyperGraph::VertexSet& vset, bool fixed);
+
+ /**
+ * set the renamed types lookup from a string, format is for example:
+ * VERTEX_CAM=VERTEX_SE3:EXPMAP,EDGE_PROJECT_P2MC=EDGE_PROJECT_XYZ:EXPMAP
+ * This will change the occurance of VERTEX_CAM in the file to VERTEX_SE3:EXPMAP
+ */
+ void setRenamedTypesFromString(const std::string& types);
+
+ /**
+ * test whether a solver is suitable for optimizing this graph.
+ * @param solverProperty the solver property to evaluate.
+ * @param vertDims should equal to the set returned by dimensions() to avoid re-evaluating.
+ */
+ bool isSolverSuitable(const OptimizationAlgorithmProperty& solverProperty, const std::set<int>& vertDims = std::set<int>()) const;
+
+ /**
+ * remove the parameters of the graph
+ */
+ virtual void clearParameters();
+
+ bool addParameter(Parameter* p) {
+ return _parameters.addParameter(p);
+ }
+
+ Parameter* parameter(int id) {
+ return _parameters.getParameter(id);
+ }
+
+ /**
+ * verify that all the information of the edges are semi positive definite, i.e.,
+ * all Eigenvalues are >= 0.
+ * @param verbose output edges with not PSD information matrix on cerr
+ * @return true if all edges have PSD information matrix
+ */
+ bool verifyInformationMatrices(bool verbose = false) const;
+
+ // helper functions to save an individual vertex
+ bool saveVertex(std::ostream& os, Vertex* v) const;
+
+ // helper functions to save an individual edge
+ bool saveEdge(std::ostream& os, Edge* e) const;
+ //! the workspace for storing the Jacobians of the graph
+ JacobianWorkspace& jacobianWorkspace() { return _jacobianWorkspace;}
+ const JacobianWorkspace& jacobianWorkspace() const { return _jacobianWorkspace;}
+
+ /**
+ * Eigen starting from version 3.1 requires that we call an initialize
+ * function, if we perform calls to Eigen from several threads.
+ * Currently, this function calls Eigen::initParallel if g2o is compiled
+ * with OpenMP support and Eigen's version is at least 3.1
+ */
+ static bool initMultiThreading();
+
+ protected:
+ std::map<std::string, std::string> _renamedTypesLookup;
+ long long _nextEdgeId;
+ std::vector<HyperGraphActionSet> _graphActions;
+
+ // do not watch this. To be removed soon, or integrated in a nice way
+ bool _edge_has_id;
+
+ ParameterContainer _parameters;
+ JacobianWorkspace _jacobianWorkspace;
+ };
+
+ /**
+ @}
+ */
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0888176ff552b1ea9e6c586fd275c3ca4e702dff
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.cpp
@@ -0,0 +1,62 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm.h"
+
+using namespace std;
+
+namespace g2o {
+
+OptimizationAlgorithm::OptimizationAlgorithm() :
+ _optimizer(0)
+{
+}
+
+OptimizationAlgorithm::~OptimizationAlgorithm()
+{
+}
+
+void OptimizationAlgorithm::printProperties(std::ostream& os) const
+{
+ os << "------------- Algorithm Properties -------------" << endl;
+ for (PropertyMap::const_iterator it = _properties.begin(); it != _properties.end(); ++it) {
+ BaseProperty* p = it->second;
+ os << it->first << "\t" << p->toString() << endl;
+ }
+ os << "------------------------------------------------" << endl;
+}
+
+bool OptimizationAlgorithm::updatePropertiesFromString(const std::string& propString)
+{
+ return _properties.updateMapFromString(propString);
+}
+
+void OptimizationAlgorithm::setOptimizer(SparseOptimizer* optimizer)
+{
+ _optimizer = optimizer;
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.h
new file mode 100644
index 0000000000000000000000000000000000000000..470a4501aac7171ba80ee64fcf5dc8b6b90be843
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm.h
@@ -0,0 +1,117 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_H
+#define G2O_OPTIMIZATION_ALGORITHM_H
+
+#include <vector>
+#include <utility>
+#include <iosfwd>
+
+#include "../stuff/property.h"
+
+#include "hyper_graph.h"
+#include "sparse_block_matrix.h"
+
+namespace g2o {
+
+ class SparseOptimizer;
+
+ /**
+ * \brief Generic interface for a non-linear solver operating on a graph
+ */
+ class OptimizationAlgorithm
+ {
+ public:
+ enum SolverResult {Terminate=2, OK=1, Fail=-1};
+ OptimizationAlgorithm();
+ virtual ~OptimizationAlgorithm();
+
+ /**
+ * initialize the solver, called once before the first call to solve()
+ */
+ virtual bool init(bool online = false) = 0;
+
+ /**
+ * Solve one iteration. The SparseOptimizer running on-top will call this
+ * for the given number of iterations.
+ * @param iteration indicates the current iteration
+ */
+ virtual SolverResult solve(int iteration, bool online = false) = 0;
+
+ /**
+ * computes the block diagonal elements of the pattern specified in the input
+ * and stores them in given SparseBlockMatrix.
+ * If your solver does not support computing the marginals, return false.
+ */
+ virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
+
+ /**
+ * update the structures for online processing
+ */
+ virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
+
+ /**
+ * called by the optimizer if verbose. re-implement, if you want to print something
+ */
+ virtual void printVerbose(std::ostream& os) const {(void) os;};
+
+ public:
+ //! return the optimizer operating on
+ const SparseOptimizer* optimizer() const { return _optimizer;}
+ SparseOptimizer* optimizer() { return _optimizer;}
+
+ /**
+ * specify on which optimizer the solver should work on
+ */
+ void setOptimizer(SparseOptimizer* optimizer);
+
+ //! return the properties of the solver
+ const PropertyMap& properties() const { return _properties;}
+
+ /**
+ * update the properties from a string, see PropertyMap::updateMapFromString()
+ */
+ bool updatePropertiesFromString(const std::string& propString);
+
+ /**
+ * print the properties to a stream in a human readable fashion
+ */
+ void printProperties(std::ostream& os) const;
+
+ protected:
+ SparseOptimizer* _optimizer; ///< the optimizer the solver is working on
+ PropertyMap _properties; ///< the properties of your solver, use this to store the parameters of your solver
+
+ private:
+ // Disable the copy constructor and assignment operator
+ OptimizationAlgorithm(const OptimizationAlgorithm&) { }
+ OptimizationAlgorithm& operator= (const OptimizationAlgorithm&) { return *this; }
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f63e0718a82202942ec5f01571644b3da5a02b8a
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.cpp
@@ -0,0 +1,229 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_dogleg.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+
+#include "block_solver.h"
+#include "sparse_optimizer.h"
+#include "solver.h"
+#include "batch_stats.h"
+using namespace std;
+
+namespace g2o {
+
+ OptimizationAlgorithmDogleg::OptimizationAlgorithmDogleg(BlockSolverBase* solver) :
+ OptimizationAlgorithmWithHessian(solver)
+ {
+ _userDeltaInit = _properties.makeProperty<Property<double> >("initialDelta", 1e4);
+ _maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 100);
+ _initialLambda = _properties.makeProperty<Property<double> >("initialLambda", 1e-7);
+ _lamdbaFactor = _properties.makeProperty<Property<double> >("lambdaFactor", 10.);
+ _delta = _userDeltaInit->value();
+ _lastStep = STEP_UNDEFINED;
+ _wasPDInAllIterations = true;
+ }
+
+ OptimizationAlgorithmDogleg::~OptimizationAlgorithmDogleg()
+ {
+ }
+
+ OptimizationAlgorithm::SolverResult OptimizationAlgorithmDogleg::solve(int iteration, bool online)
+ {
+ assert(_optimizer && "_optimizer not set");
+ assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+ assert(dynamic_cast<BlockSolverBase*>(_solver) && "underlying linear solver is not a block solver");
+
+ BlockSolverBase* blockSolver = static_cast<BlockSolverBase*>(_solver);
+
+ if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+ bool ok = _solver->buildStructure();
+ if (! ok) {
+ cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+ return OptimizationAlgorithm::Fail;
+ }
+
+ // init some members to the current size of the problem
+ _hsd.resize(_solver->vectorSize());
+ _hdl.resize(_solver->vectorSize());
+ _auxVector.resize(_solver->vectorSize());
+ _delta = _userDeltaInit->value();
+ _currentLambda = _initialLambda->value();
+ _wasPDInAllIterations = true;
+ }
+
+ double t=get_monotonic_time();
+ _optimizer->computeActiveErrors();
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeResiduals = get_monotonic_time()-t;
+ t=get_monotonic_time();
+ }
+
+ double currentChi = _optimizer->activeRobustChi2();
+
+ _solver->buildSystem();
+ if (globalStats) {
+ globalStats->timeQuadraticForm = get_monotonic_time()-t;
+ }
+
+ Eigen::VectorXd::ConstMapType b(_solver->b(), _solver->vectorSize());
+
+ // compute alpha
+ _auxVector.setZero();
+ blockSolver->multiplyHessian(_auxVector.data(), _solver->b());
+ double bNormSquared = b.squaredNorm();
+ double alpha = bNormSquared / _auxVector.dot(b);
+
+ _hsd = alpha * b;
+ double hsdNorm = _hsd.norm();
+ double hgnNorm = -1.;
+
+ bool solvedGaussNewton = false;
+ bool goodStep = false;
+ int& numTries = _lastNumTries;
+ numTries = 0;
+ do {
+ ++numTries;
+
+ if (! solvedGaussNewton) {
+ const double minLambda = 1e-12;
+ const double maxLambda = 1e3;
+ solvedGaussNewton = true;
+ // apply a damping factor to enforce positive definite Hessian, if the matrix appeared
+ // to be not positive definite in at least one iteration before.
+ // We apply a damping factor to obtain a PD matrix.
+ bool solverOk = false;
+ while(!solverOk) {
+ if (! _wasPDInAllIterations)
+ _solver->setLambda(_currentLambda, true); // add _currentLambda to the diagonal
+ solverOk = _solver->solve();
+ if (! _wasPDInAllIterations)
+ _solver->restoreDiagonal();
+ _wasPDInAllIterations = _wasPDInAllIterations && solverOk;
+ if (! _wasPDInAllIterations) {
+ // simple strategy to control the damping factor
+ if (solverOk) {
+ _currentLambda = std::max(minLambda, _currentLambda / (0.5 * _lamdbaFactor->value()));
+ } else {
+ _currentLambda *= _lamdbaFactor->value();
+ if (_currentLambda > maxLambda) {
+ _currentLambda = maxLambda;
+ return Fail;
+ }
+ }
+ }
+ }
+ if (!solverOk) {
+ return Fail;
+ }
+ hgnNorm = Eigen::VectorXd::ConstMapType(_solver->x(), _solver->vectorSize()).norm();
+ }
+
+ Eigen::VectorXd::ConstMapType hgn(_solver->x(), _solver->vectorSize());
+ assert(hgnNorm >= 0. && "Norm of the GN step is not computed");
+
+ if (hgnNorm < _delta) {
+ _hdl = hgn;
+ _lastStep = STEP_GN;
+ }
+ else if (hsdNorm > _delta) {
+ _hdl = _delta / hsdNorm * _hsd;
+ _lastStep = STEP_SD;
+ } else {
+ _auxVector = hgn - _hsd; // b - a
+ double c = _hsd.dot(_auxVector);
+ double bmaSquaredNorm = _auxVector.squaredNorm();
+ double beta;
+ if (c <= 0.)
+ beta = (-c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - _hsd.squaredNorm()))) / bmaSquaredNorm;
+ else {
+ double hsdSqrNorm = _hsd.squaredNorm();
+ beta = (_delta*_delta - hsdSqrNorm) / (c + sqrt(c*c + bmaSquaredNorm * (_delta*_delta - hsdSqrNorm)));
+ }
+ assert(beta > 0. && beta < 1 && "Error while computing beta");
+ _hdl = _hsd + beta * (hgn - _hsd);
+ _lastStep = STEP_DL;
+ assert(_hdl.norm() < _delta + 1e-5 && "Computed step does not correspond to the trust region");
+ }
+
+ // compute the linear gain
+ _auxVector.setZero();
+ blockSolver->multiplyHessian(_auxVector.data(), _hdl.data());
+ double linearGain = -1 * (_auxVector.dot(_hdl)) + 2 * (b.dot(_hdl));
+
+ // apply the update and see what happens
+ _optimizer->push();
+ _optimizer->update(_hdl.data());
+ _optimizer->computeActiveErrors();
+ double newChi = _optimizer-> activeRobustChi2();
+ double nonLinearGain = currentChi - newChi;
+ if (fabs(linearGain) < 1e-12)
+ linearGain = 1e-12;
+ double rho = nonLinearGain / linearGain;
+ //cerr << PVAR(nonLinearGain) << " " << PVAR(linearGain) << " " << PVAR(rho) << endl;
+ if (rho > 0) { // step is good and will be accepted
+ _optimizer->discardTop();
+ goodStep = true;
+ } else { // recover previous state
+ _optimizer->pop();
+ }
+
+ // update trust region based on the step quality
+ if (rho > 0.75)
+ _delta = std::max(_delta, 3. * _hdl.norm());
+ else if (rho < 0.25)
+ _delta *= 0.5;
+ } while (!goodStep && numTries < _maxTrialsAfterFailure->value());
+ if (numTries == _maxTrialsAfterFailure->value() || !goodStep)
+ return Terminate;
+ return OK;
+ }
+
+ void OptimizationAlgorithmDogleg::printVerbose(std::ostream& os) const
+ {
+ os
+ << "\t Delta= " << _delta
+ << "\t step= " << stepType2Str(_lastStep)
+ << "\t tries= " << _lastNumTries;
+ if (! _wasPDInAllIterations)
+ os << "\t lambda= " << _currentLambda;
+ }
+
+ const char* OptimizationAlgorithmDogleg::stepType2Str(int stepType)
+ {
+ switch (stepType) {
+ case STEP_SD: return "Descent";
+ case STEP_GN: return "GN";
+ case STEP_DL: return "Dogleg";
+ default: return "Undefined";
+ }
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h
new file mode 100644
index 0000000000000000000000000000000000000000..e49761ca05cab90fe082069a4103d68b796a6b60
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_dogleg.h
@@ -0,0 +1,89 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
+#define G2O_OPTIMIZATION_ALGORITHM_DOGLEG_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+ class BlockSolverBase;
+
+ /**
+ * \brief Implementation of Powell's Dogleg Algorithm
+ */
+ class OptimizationAlgorithmDogleg : public OptimizationAlgorithmWithHessian
+ {
+ public:
+ /** \brief type of the step to take */
+ enum {
+ STEP_UNDEFINED,
+ STEP_SD, STEP_GN, STEP_DL
+ };
+
+ public:
+ /**
+ * construct the Dogleg algorithm, which will use the given Solver for solving the
+ * linearized system.
+ */
+ explicit OptimizationAlgorithmDogleg(BlockSolverBase* solver);
+ virtual ~OptimizationAlgorithmDogleg();
+
+ virtual SolverResult solve(int iteration, bool online = false);
+
+ virtual void printVerbose(std::ostream& os) const;
+
+ //! return the type of the last step taken by the algorithm
+ int lastStep() const { return _lastStep;}
+ //! return the diameter of the trust region
+ double trustRegion() const { return _delta;}
+
+ //! convert the type into an integer
+ static const char* stepType2Str(int stepType);
+
+ protected:
+ // parameters
+ Property<int>* _maxTrialsAfterFailure;
+ Property<double>* _userDeltaInit;
+ // damping to enforce positive definite matrix
+ Property<double>* _initialLambda;
+ Property<double>* _lamdbaFactor;
+
+ Eigen::VectorXd _hsd; ///< steepest decent step
+ Eigen::VectorXd _hdl; ///< final dogleg step
+ Eigen::VectorXd _auxVector; ///< auxilary vector used to perform multiplications or other stuff
+
+ double _currentLambda; ///< the damping factor to force positive definite matrix
+ double _delta; ///< trust region
+ int _lastStep; ///< type of the step taken by the algorithm
+ bool _wasPDInAllIterations; ///< the matrix we solve was positive definite in all iterations -> if not apply damping
+ int _lastNumTries;
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a9be96e834fd67cf8b410386e7304ebd4080aeff
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.cpp
@@ -0,0 +1,137 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_factory.h"
+
+#include <iostream>
+#include <typeinfo>
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+ AbstractOptimizationAlgorithmCreator::AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p) :
+ _property(p)
+ {
+ }
+
+ OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::factoryInstance = 0;
+
+ OptimizationAlgorithmFactory::OptimizationAlgorithmFactory()
+ {
+ }
+
+ OptimizationAlgorithmFactory::~OptimizationAlgorithmFactory()
+ {
+ for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it)
+ delete *it;
+ }
+
+ OptimizationAlgorithmFactory* OptimizationAlgorithmFactory::instance()
+ {
+ if (factoryInstance == 0) {
+ factoryInstance = new OptimizationAlgorithmFactory;
+ }
+ return factoryInstance;
+ }
+
+ void OptimizationAlgorithmFactory::registerSolver(AbstractOptimizationAlgorithmCreator* c)
+ {
+ const string& name = c->property().name;
+ CreatorList::iterator foundIt = findSolver(name);
+ if (foundIt != _creator.end()) {
+ _creator.erase(foundIt);
+ cerr << "SOLVER FACTORY WARNING: Overwriting Solver creator " << name << endl;
+ assert(0);
+ }
+ _creator.push_back(c);
+ }
+
+ void OptimizationAlgorithmFactory::unregisterSolver(AbstractOptimizationAlgorithmCreator* c)
+ {
+ const string& name = c->property().name;
+ CreatorList::iterator foundIt = findSolver(name);
+ if (foundIt != _creator.end()) {
+ delete *foundIt;
+ _creator.erase(foundIt);
+ }
+ }
+
+ OptimizationAlgorithm* OptimizationAlgorithmFactory::construct(const std::string& name, OptimizationAlgorithmProperty& solverProperty) const
+ {
+ CreatorList::const_iterator foundIt = findSolver(name);
+ if (foundIt != _creator.end()) {
+ solverProperty = (*foundIt)->property();
+ return (*foundIt)->construct();
+ }
+ cerr << "SOLVER FACTORY WARNING: Unable to create solver " << name << endl;
+ return 0;
+ }
+
+ void OptimizationAlgorithmFactory::destroy()
+ {
+ delete factoryInstance;
+ factoryInstance = 0;
+ }
+
+ void OptimizationAlgorithmFactory::listSolvers(std::ostream& os) const
+ {
+ size_t solverNameColumnLength = 0;
+ for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+ solverNameColumnLength = std::max(solverNameColumnLength, (*it)->property().name.size());
+ solverNameColumnLength += 4;
+
+ for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ const OptimizationAlgorithmProperty& sp = (*it)->property();
+ os << sp.name;
+ for (size_t i = sp.name.size(); i < solverNameColumnLength; ++i)
+ os << ' ';
+ os << sp.desc << endl;
+ }
+ }
+
+ OptimizationAlgorithmFactory::CreatorList::const_iterator OptimizationAlgorithmFactory::findSolver(const std::string& name) const
+ {
+ for (CreatorList::const_iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ const OptimizationAlgorithmProperty& sp = (*it)->property();
+ if (sp.name == name)
+ return it;
+ }
+ return _creator.end();
+ }
+
+ OptimizationAlgorithmFactory::CreatorList::iterator OptimizationAlgorithmFactory::findSolver(const std::string& name)
+ {
+ for (CreatorList::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ const OptimizationAlgorithmProperty& sp = (*it)->property();
+ if (sp.name == name)
+ return it;
+ }
+ return _creator.end();
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8bf26e78ebb292513e9c8dd4dfd474af3a0c819
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_factory.h
@@ -0,0 +1,167 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
+#define G2O_OPTMIZATION_ALGORITHM_PROPERTY_H
+
+#include "../../config.h"
+#include "../stuff/misc.h"
+#include "optimization_algorithm_property.h"
+
+#include <list>
+#include <iostream>
+#include <typeinfo>
+
+
+// define to get some verbose output
+//#define G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+
+namespace g2o {
+
+ // forward decl
+ class OptimizationAlgorithm;
+ class SparseOptimizer;
+
+ /**
+ * \brief base for allocating an optimization algorithm
+ *
+ * Allocating a solver for a given optimizer. The method construct() has to be
+ * implemented in your derived class to allocate the desired solver.
+ */
+ class AbstractOptimizationAlgorithmCreator
+ {
+ public:
+ AbstractOptimizationAlgorithmCreator(const OptimizationAlgorithmProperty& p);
+ virtual ~AbstractOptimizationAlgorithmCreator() { }
+ //! allocate a solver operating on optimizer, re-implement for your creator
+ virtual OptimizationAlgorithm* construct() = 0;
+ //! return the properties of the solver
+ const OptimizationAlgorithmProperty& property() const { return _property;}
+ protected:
+ OptimizationAlgorithmProperty _property;
+ };
+
+ /**
+ * \brief create solvers based on their short name
+ *
+ * Factory to allocate solvers based on their short name.
+ * The Factory is implemented as a sigleton and the single
+ * instance can be accessed via the instance() function.
+ */
+ class OptimizationAlgorithmFactory
+ {
+ public:
+ typedef std::list<AbstractOptimizationAlgorithmCreator*> CreatorList;
+
+ //! return the instance
+ static OptimizationAlgorithmFactory* instance();
+
+ //! free the instance
+ static void destroy();
+
+ /**
+ * register a specific creator for allocating a solver
+ */
+ void registerSolver(AbstractOptimizationAlgorithmCreator* c);
+
+ /**
+ * unregister a specific creator for allocating a solver
+ */
+ void unregisterSolver(AbstractOptimizationAlgorithmCreator* c);
+
+ /**
+ * construct a solver based on its name, e.g., var, fix3_2_cholmod
+ */
+ OptimizationAlgorithm* construct(const std::string& tag, OptimizationAlgorithmProperty& solverProperty) const;
+
+ //! list the known solvers into a stream
+ void listSolvers(std::ostream& os) const;
+
+ //! return the underlying list of creators
+ const CreatorList& creatorList() const { return _creator;}
+
+ protected:
+ OptimizationAlgorithmFactory();
+ ~OptimizationAlgorithmFactory();
+
+ CreatorList _creator;
+
+ CreatorList::const_iterator findSolver(const std::string& name) const;
+ CreatorList::iterator findSolver(const std::string& name);
+
+ private:
+ static OptimizationAlgorithmFactory* factoryInstance;
+ };
+
+ class RegisterOptimizationAlgorithmProxy
+ {
+ public:
+ RegisterOptimizationAlgorithmProxy(AbstractOptimizationAlgorithmCreator* c)
+ {
+ _creator = c;
+#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+ std::cout << __FUNCTION__ << ": Registering " << _creator->property().name << " of type " << typeid(*_creator).name() << std::endl;
+#endif
+ OptimizationAlgorithmFactory::instance()->registerSolver(c);
+ }
+
+ ~RegisterOptimizationAlgorithmProxy()
+ {
+#ifdef G2O_DEBUG_OPTIMIZATION_ALGORITHM_FACTORY
+ std::cout << __FUNCTION__ << ": Unregistering " << _creator->property().name << std::endl;
+#endif
+ OptimizationAlgorithmFactory::instance()->unregisterSolver(_creator);
+ }
+ private:
+ AbstractOptimizationAlgorithmCreator* _creator;
+ };
+
+}
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+# define G2O_OAF_EXPORT __declspec(dllexport)
+# define G2O_OAF_IMPORT __declspec(dllimport)
+#else
+# define G2O_OAF_EXPORT
+# define G2O_OAF_IMPORT
+#endif
+
+#define G2O_REGISTER_OPTIMIZATION_LIBRARY(libraryname) \
+ extern "C" void G2O_OAF_EXPORT g2o_optimization_library_##libraryname(void) {}
+
+#define G2O_USE_OPTIMIZATION_LIBRARY(libraryname) \
+ extern "C" void G2O_OAF_IMPORT g2o_optimization_library_##libraryname(void); \
+ static g2o::ForceLinker g2o_force_optimization_algorithm_library_##libraryname(g2o_optimization_library_##libraryname);
+
+#define G2O_REGISTER_OPTIMIZATION_ALGORITHM(optimizername, instance) \
+ extern "C" void G2O_OAF_EXPORT g2o_optimization_algorithm_##optimizername(void) {} \
+ static g2o::RegisterOptimizationAlgorithmProxy g_optimization_algorithm_proxy_##optimizername(instance);
+
+#define G2O_USE_OPTIMIZATION_ALGORITHM(optimizername) \
+ extern "C" void G2O_OAF_IMPORT g2o_optimization_algorithm_##optimizername(void); \
+ static g2o::ForceLinker g2o_force_optimization_algorithm_link_##optimizername(g2o_optimization_algorithm_##optimizername);
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..04ca44540b7499470add2440e201e7b3e7fb56eb
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.cpp
@@ -0,0 +1,101 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_gauss_newton.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+
+#include "solver.h"
+#include "batch_stats.h"
+#include "sparse_optimizer.h"
+using namespace std;
+
+namespace g2o {
+
+ OptimizationAlgorithmGaussNewton::OptimizationAlgorithmGaussNewton(Solver* solver) :
+ OptimizationAlgorithmWithHessian(solver)
+ {
+ }
+
+ OptimizationAlgorithmGaussNewton::~OptimizationAlgorithmGaussNewton()
+ {
+ }
+
+ OptimizationAlgorithm::SolverResult OptimizationAlgorithmGaussNewton::solve(int iteration, bool online)
+ {
+ assert(_optimizer && "_optimizer not set");
+ assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+ bool ok = true;
+
+ //here so that correct component for max-mixtures can be computed before the build structure
+ double t=get_monotonic_time();
+ _optimizer->computeActiveErrors();
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeResiduals = get_monotonic_time()-t;
+ }
+
+ if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+ ok = _solver->buildStructure();
+ if (! ok) {
+ cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+ return OptimizationAlgorithm::Fail;
+ }
+ }
+
+ t=get_monotonic_time();
+ _solver->buildSystem();
+ if (globalStats) {
+ globalStats->timeQuadraticForm = get_monotonic_time()-t;
+ t=get_monotonic_time();
+ }
+
+ ok = _solver->solve();
+ if (globalStats) {
+ globalStats->timeLinearSolution = get_monotonic_time()-t;
+ t=get_monotonic_time();
+ }
+
+ _optimizer->update(_solver->x());
+ if (globalStats) {
+ globalStats->timeUpdate = get_monotonic_time()-t;
+ }
+ if (ok)
+ return OK;
+ else
+ return Fail;
+ }
+
+ void OptimizationAlgorithmGaussNewton::printVerbose(std::ostream& os) const
+ {
+ os
+ << "\t schur= " << _solver->schur();
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h
new file mode 100644
index 0000000000000000000000000000000000000000..409aa27c7a67549f15345fc4b5f70f550e20d1cb
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_gauss_newton.h
@@ -0,0 +1,54 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
+#define G2O_OPTIMIZATION_ALGORITHM_GAUSS_NEWTON_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+ /**
+ * \brief Implementation of the Gauss Newton Algorithm
+ */
+ class OptimizationAlgorithmGaussNewton : public OptimizationAlgorithmWithHessian
+ {
+ public:
+ /**
+ * construct the Gauss Newton algorithm, which use the given Solver for solving the
+ * linearized system.
+ */
+ explicit OptimizationAlgorithmGaussNewton(Solver* solver);
+ virtual ~OptimizationAlgorithmGaussNewton();
+
+ virtual SolverResult solve(int iteration, bool online = false);
+
+ virtual void printVerbose(std::ostream& os) const;
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b24718f5494706e27191c1242a9ba7218116143f
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.cpp
@@ -0,0 +1,209 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Modified Raul Mur Artal (2014)
+// - Stop criterium (solve function)
+
+#include "optimization_algorithm_levenberg.h"
+
+#include <iostream>
+
+#include "../stuff/timeutil.h"
+
+#include "sparse_optimizer.h"
+#include "solver.h"
+#include "batch_stats.h"
+using namespace std;
+
+namespace g2o {
+
+ OptimizationAlgorithmLevenberg::OptimizationAlgorithmLevenberg(Solver* solver) :
+ OptimizationAlgorithmWithHessian(solver)
+ {
+ _currentLambda = -1.;
+ _tau = 1e-5;
+ _goodStepUpperScale = 2./3.;
+ _goodStepLowerScale = 1./3.;
+ _userLambdaInit = _properties.makeProperty<Property<double> >("initialLambda", 0.);
+ _maxTrialsAfterFailure = _properties.makeProperty<Property<int> >("maxTrialsAfterFailure", 10);
+ _ni=2.;
+ _levenbergIterations = 0;
+ _nBad = 0;
+ }
+
+ OptimizationAlgorithmLevenberg::~OptimizationAlgorithmLevenberg()
+ {
+ }
+
+ OptimizationAlgorithm::SolverResult OptimizationAlgorithmLevenberg::solve(int iteration, bool online)
+ {
+ assert(_optimizer && "_optimizer not set");
+ assert(_solver->optimizer() == _optimizer && "underlying linear solver operates on different graph");
+
+ if (iteration == 0 && !online) { // built up the CCS structure, here due to easy time measure
+ bool ok = _solver->buildStructure();
+ if (! ok) {
+ cerr << __PRETTY_FUNCTION__ << ": Failure while building CCS structure" << endl;
+ return OptimizationAlgorithm::Fail;
+ }
+ }
+
+ double t=get_monotonic_time();
+ _optimizer->computeActiveErrors();
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeResiduals = get_monotonic_time()-t;
+ t=get_monotonic_time();
+ }
+
+ double currentChi = _optimizer->activeRobustChi2();
+ double tempChi=currentChi;
+
+ double iniChi = currentChi;
+
+ _solver->buildSystem();
+ if (globalStats) {
+ globalStats->timeQuadraticForm = get_monotonic_time()-t;
+ }
+
+ // core part of the Levenbarg algorithm
+ if (iteration == 0) {
+ _currentLambda = computeLambdaInit();
+ _ni = 2;
+ _nBad = 0;
+ }
+
+ double rho=0;
+ int& qmax = _levenbergIterations;
+ qmax = 0;
+ do {
+ _optimizer->push();
+ if (globalStats) {
+ globalStats->levenbergIterations++;
+ t=get_monotonic_time();
+ }
+ // update the diagonal of the system matrix
+ _solver->setLambda(_currentLambda, true);
+ bool ok2 = _solver->solve();
+ if (globalStats) {
+ globalStats->timeLinearSolution+=get_monotonic_time()-t;
+ t=get_monotonic_time();
+ }
+ _optimizer->update(_solver->x());
+ if (globalStats) {
+ globalStats->timeUpdate = get_monotonic_time()-t;
+ }
+
+ // restore the diagonal
+ _solver->restoreDiagonal();
+
+ _optimizer->computeActiveErrors();
+ tempChi = _optimizer->activeRobustChi2();
+
+ if (! ok2)
+ tempChi=std::numeric_limits<double>::max();
+
+ rho = (currentChi-tempChi);
+ double scale = computeScale();
+ scale += 1e-3; // make sure it's non-zero :)
+ rho /= scale;
+
+ if (rho>0 && g2o_isfinite(tempChi)){ // last step was good
+ double alpha = 1.-pow((2*rho-1),3);
+ // crop lambda between minimum and maximum factors
+ alpha = (std::min)(alpha, _goodStepUpperScale);
+ double scaleFactor = (std::max)(_goodStepLowerScale, alpha);
+ _currentLambda *= scaleFactor;
+ _ni = 2;
+ currentChi=tempChi;
+ _optimizer->discardTop();
+ } else {
+ _currentLambda*=_ni;
+ _ni*=2;
+ _optimizer->pop(); // restore the last state before trying to optimize
+ }
+ qmax++;
+ } while (rho<0 && qmax < _maxTrialsAfterFailure->value() && ! _optimizer->terminate());
+
+ if (qmax == _maxTrialsAfterFailure->value() || rho==0)
+ return Terminate;
+
+ //Stop criterium (Raul)
+ if((iniChi-currentChi)*1e3<iniChi)
+ _nBad++;
+ else
+ _nBad=0;
+
+ if(_nBad>=3)
+ return Terminate;
+
+ return OK;
+ }
+
+ double OptimizationAlgorithmLevenberg::computeLambdaInit() const
+ {
+ if (_userLambdaInit->value() > 0)
+ return _userLambdaInit->value();
+ double maxDiagonal=0.;
+ for (size_t k = 0; k < _optimizer->indexMapping().size(); k++) {
+ OptimizableGraph::Vertex* v = _optimizer->indexMapping()[k];
+ assert(v);
+ int dim = v->dimension();
+ for (int j = 0; j < dim; ++j){
+ maxDiagonal = std::max(fabs(v->hessian(j,j)),maxDiagonal);
+ }
+ }
+ return _tau*maxDiagonal;
+ }
+
+ double OptimizationAlgorithmLevenberg::computeScale() const
+ {
+ double scale = 0.;
+ for (size_t j=0; j < _solver->vectorSize(); j++){
+ scale += _solver->x()[j] * (_currentLambda * _solver->x()[j] + _solver->b()[j]);
+ }
+ return scale;
+ }
+
+ void OptimizationAlgorithmLevenberg::setMaxTrialsAfterFailure(int max_trials)
+ {
+ _maxTrialsAfterFailure->setValue(max_trials);
+ }
+
+ void OptimizationAlgorithmLevenberg::setUserLambdaInit(double lambda)
+ {
+ _userLambdaInit->setValue(lambda);
+ }
+
+ void OptimizationAlgorithmLevenberg::printVerbose(std::ostream& os) const
+ {
+ os
+ << "\t schur= " << _solver->schur()
+ << "\t lambda= " << FIXED(_currentLambda)
+ << "\t levenbergIter= " << _levenbergIterations;
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc4a4a066087acab18cfffaba9d8be8ad596a9b1
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h
@@ -0,0 +1,92 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SOLVER_LEVENBERG_H
+#define G2O_SOLVER_LEVENBERG_H
+
+#include "optimization_algorithm_with_hessian.h"
+
+namespace g2o {
+
+ /**
+ * \brief Implementation of the Levenberg Algorithm
+ */
+ class OptimizationAlgorithmLevenberg : public OptimizationAlgorithmWithHessian
+ {
+ public:
+ /**
+ * construct the Levenberg algorithm, which will use the given Solver for solving the
+ * linearized system.
+ */
+ explicit OptimizationAlgorithmLevenberg(Solver* solver);
+ virtual ~OptimizationAlgorithmLevenberg();
+
+ virtual SolverResult solve(int iteration, bool online = false);
+
+ virtual void printVerbose(std::ostream& os) const;
+
+ //! return the currently used damping factor
+ double currentLambda() const { return _currentLambda;}
+
+ //! the number of internal iteration if an update step increases chi^2 within Levenberg-Marquardt
+ void setMaxTrialsAfterFailure(int max_trials);
+
+ //! get the number of inner iterations for Levenberg-Marquardt
+ int maxTrialsAfterFailure() const { return _maxTrialsAfterFailure->value();}
+
+ //! return the lambda set by the user, if < 0 the SparseOptimizer will compute the initial lambda
+ double userLambdaInit() {return _userLambdaInit->value();}
+ //! specify the initial lambda used for the first iteraion, if not given the SparseOptimizer tries to compute a suitable value
+ void setUserLambdaInit(double lambda);
+
+ //! return the number of levenberg iterations performed in the last round
+ int levenbergIteration() { return _levenbergIterations;}
+
+ protected:
+ // Levenberg parameters
+ Property<int>* _maxTrialsAfterFailure;
+ Property<double>* _userLambdaInit;
+ double _currentLambda;
+ double _tau;
+ double _goodStepLowerScale; ///< lower bound for lambda decrease if a good LM step
+ double _goodStepUpperScale; ///< upper bound for lambda decrease if a good LM step
+ double _ni;
+ int _levenbergIterations; ///< the numer of levenberg iterations performed to accept the last step
+ //RAUL
+ int _nBad;
+
+ /**
+ * helper for Levenberg, this function computes the initial damping factor, if the user did not
+ * specify an own value, see setUserLambdaInit()
+ */
+ double computeLambdaInit() const;
+ double computeScale() const;
+
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h
new file mode 100644
index 0000000000000000000000000000000000000000..62abb0eccf05038e4e49a6a8c900080465522000
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_property.h
@@ -0,0 +1,57 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
+#define G2O_OPTIMIZATION_ALGORITHM_PROPERTY_H
+
+#include <string>
+
+namespace g2o {
+
+/**
+ * \brief describe the properties of a solver
+ */
+struct OptimizationAlgorithmProperty
+{
+ std::string name; ///< name of the solver, e.g., var
+ std::string desc; ///< short description of the solver
+ std::string type; ///< type of solver, e.g., "CSparse Cholesky", "PCG"
+ bool requiresMarginalize; ///< whether the solver requires marginalization of landmarks
+ int poseDim; ///< dimension of the pose vertices (-1 if variable)
+ int landmarkDim; ///< dimension of the landmar vertices (-1 if variable)
+ OptimizationAlgorithmProperty() :
+ name(), desc(), type(), requiresMarginalize(false), poseDim(-1), landmarkDim(-1)
+ {
+ }
+ OptimizationAlgorithmProperty(const std::string& name_, const std::string& desc_, const std::string& type_, bool requiresMarginalize_, int poseDim_, int landmarkDim_) :
+ name(name_), desc(desc_), type(type_), requiresMarginalize(requiresMarginalize_), poseDim(poseDim_), landmarkDim(landmarkDim_)
+ {
+ }
+};
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5c23a78b604d256ec71d3e313a04b1f276e6d748
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.cpp
@@ -0,0 +1,101 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "optimization_algorithm_with_hessian.h"
+
+#include "solver.h"
+#include "optimizable_graph.h"
+#include "sparse_optimizer.h"
+
+#include <iostream>
+using namespace std;
+
+namespace g2o {
+
+ OptimizationAlgorithmWithHessian::OptimizationAlgorithmWithHessian(Solver* solver) :
+ OptimizationAlgorithm(),
+ _solver(solver)
+ {
+ _writeDebug = _properties.makeProperty<Property<bool> >("writeDebug", true);
+ }
+
+ OptimizationAlgorithmWithHessian::~OptimizationAlgorithmWithHessian()
+ {
+ delete _solver;
+ }
+
+ bool OptimizationAlgorithmWithHessian::init(bool online)
+ {
+ assert(_optimizer && "_optimizer not set");
+ assert(_solver && "Solver not set");
+ _solver->setWriteDebug(_writeDebug->value());
+ bool useSchur=false;
+ for (OptimizableGraph::VertexContainer::const_iterator it=_optimizer->activeVertices().begin(); it!=_optimizer->activeVertices().end(); ++it) {
+ OptimizableGraph::Vertex* v= *it;
+ if (v->marginalized()){
+ useSchur=true;
+ break;
+ }
+ }
+ if (useSchur){
+ if (_solver->supportsSchur())
+ _solver->setSchur(true);
+ } else {
+ if (_solver->supportsSchur())
+ _solver->setSchur(false);
+ }
+
+ bool initState = _solver->init(_optimizer, online);
+ return initState;
+ }
+
+ bool OptimizationAlgorithmWithHessian::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices)
+ {
+ return _solver ? _solver->computeMarginals(spinv, blockIndices) : false;
+ }
+
+ bool OptimizationAlgorithmWithHessian::buildLinearStructure()
+ {
+ return _solver ? _solver->buildStructure() : false;
+ }
+
+ void OptimizationAlgorithmWithHessian::updateLinearSystem()
+ {
+ if (_solver)
+ _solver->buildSystem();
+ }
+
+ bool OptimizationAlgorithmWithHessian::updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges)
+ {
+ return _solver ? _solver->updateStructure(vset, edges) : false;
+ }
+
+ void OptimizationAlgorithmWithHessian::setWriteDebug(bool writeDebug)
+ {
+ _writeDebug->setValue(writeDebug);
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf5da58ccf575e5aaabda2777691157e1f49e90d
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/optimization_algorithm_with_hessian.h
@@ -0,0 +1,72 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
+#define G2O_OPTIMIZATION_ALGORITHM_WITH_HESSIAN_H
+
+#include "optimization_algorithm.h"
+
+namespace g2o {
+
+ class Solver;
+
+ /**
+ * \brief Base for solvers operating on the approximated Hessian, e.g., Gauss-Newton, Levenberg
+ */
+ class OptimizationAlgorithmWithHessian : public OptimizationAlgorithm
+ {
+ public:
+ explicit OptimizationAlgorithmWithHessian(Solver* solver);
+ virtual ~OptimizationAlgorithmWithHessian();
+
+ virtual bool init(bool online = false);
+
+ virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+
+ virtual bool buildLinearStructure();
+
+ virtual void updateLinearSystem();
+
+ virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges);
+
+ //! return the underlying solver used to solve the linear system
+ Solver* solver() { return _solver;}
+
+ /**
+ * write debug output of the Hessian if system is not positive definite
+ */
+ virtual void setWriteDebug(bool writeDebug);
+ virtual bool writeDebug() const { return _writeDebug->value();}
+
+ protected:
+ Solver* _solver;
+ Property<bool>* _writeDebug;
+
+ };
+
+}// end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..345f9c026ba18b6549d1da50ef152232ab6d1be7
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.cpp
@@ -0,0 +1,40 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "parameter.h"
+
+namespace g2o {
+
+ Parameter::Parameter() : _id(-1)
+ {
+ }
+
+ void Parameter::setId(int id_)
+ {
+ _id = id_;
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.h
new file mode 100644
index 0000000000000000000000000000000000000000..6def66446c1c1feadd1114beebf922efc0c28722
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter.h
@@ -0,0 +1,56 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_PARAMETER_HH_
+#define G2O_GRAPH_PARAMETER_HH_
+
+#include <iosfwd>
+
+#include "hyper_graph.h"
+
+namespace g2o {
+
+ class Parameter : public HyperGraph::HyperGraphElement
+ {
+ public:
+ Parameter();
+ virtual ~Parameter() {};
+ //! read the data from a stream
+ virtual bool read(std::istream& is) = 0;
+ //! write the data to a stream
+ virtual bool write(std::ostream& os) const = 0;
+ int id() const {return _id;}
+ void setId(int id_);
+ virtual HyperGraph::HyperGraphElementType elementType() const { return HyperGraph::HGET_PARAMETER;}
+ protected:
+ int _id;
+ };
+
+ typedef std::vector<Parameter*> ParameterVector;
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d0123310dec0e3e039c829e663655193910180e3
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.cpp
@@ -0,0 +1,142 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "parameter_container.h"
+
+#include <iostream>
+
+#include "factory.h"
+#include "parameter.h"
+
+#include "../stuff/macros.h"
+#include "../stuff/color_macros.h"
+#include "../stuff/string_tools.h"
+
+namespace g2o {
+
+ using namespace std;
+
+ ParameterContainer::ParameterContainer(bool isMainStorage_) :
+ _isMainStorage(isMainStorage_)
+ {
+ }
+
+ void ParameterContainer::clear() {
+ if (!_isMainStorage)
+ return;
+ for (iterator it = begin(); it!=end(); it++){
+ delete it->second;
+ }
+ BaseClass::clear();
+ }
+
+ ParameterContainer::~ParameterContainer(){
+ clear();
+ }
+
+ bool ParameterContainer::addParameter(Parameter* p){
+ if (p->id()<0)
+ return false;
+ iterator it=find(p->id());
+ if (it!=end())
+ return false;
+ insert(make_pair(p->id(), p));
+ return true;
+ }
+
+ Parameter* ParameterContainer::getParameter(int id) {
+ iterator it=find(id);
+ if (it==end())
+ return 0;
+ return it->second;
+ }
+
+ Parameter* ParameterContainer::detachParameter(int id){
+ iterator it=find(id);
+ if (it==end())
+ return 0;
+ Parameter* p=it->second;
+ erase(it);
+ return p;
+ }
+
+ bool ParameterContainer::write(std::ostream& os) const{
+ Factory* factory = Factory::instance();
+ for (const_iterator it=begin(); it!=end(); it++){
+ os << factory->tag(it->second) << " ";
+ os << it->second->id() << " ";
+ it->second->write(os);
+ os << endl;
+ }
+ return true;
+ }
+
+ bool ParameterContainer::read(std::istream& is, const std::map<std::string, std::string>* _renamedTypesLookup){
+ stringstream currentLine;
+ string token;
+
+ Factory* factory = Factory::instance();
+ HyperGraph::GraphElemBitset elemBitset;
+ elemBitset[HyperGraph::HGET_PARAMETER] = 1;
+
+ while (1) {
+ int bytesRead = readLine(is, currentLine);
+ if (bytesRead == -1)
+ break;
+ currentLine >> token;
+ if (bytesRead == 0 || token.size() == 0 || token[0] == '#')
+ continue;
+ if (_renamedTypesLookup && _renamedTypesLookup->size()>0){
+ map<string, string>::const_iterator foundIt = _renamedTypesLookup->find(token);
+ if (foundIt != _renamedTypesLookup->end()) {
+ token = foundIt->second;
+ }
+ }
+
+ HyperGraph::HyperGraphElement* element = factory->construct(token, elemBitset);
+ if (! element) // not a parameter or otherwise unknown tag
+ continue;
+ assert(element->elementType() == HyperGraph::HGET_PARAMETER && "Should be a param");
+
+ Parameter* p = static_cast<Parameter*>(element);
+ int pid;
+ currentLine >> pid;
+ p->setId(pid);
+ bool r = p->read(currentLine);
+ if (! r) {
+ cerr << __PRETTY_FUNCTION__ << ": Error reading data " << token << " for parameter " << pid << endl;
+ delete p;
+ } else {
+ if (! addParameter(p) ){
+ cerr << __PRETTY_FUNCTION__ << ": Parameter of type:" << token << " id:" << pid << " already defined" << endl;
+ }
+ }
+ } // while read line
+
+ return true;
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.h
new file mode 100644
index 0000000000000000000000000000000000000000..eef6c2ff6831dd0a899a7ff8d634c0426d17994a
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/parameter_container.h
@@ -0,0 +1,74 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_PARAMETER_CONTAINER_HH_
+#define G2O_GRAPH_PARAMETER_CONTAINER_HH_
+
+#include <iosfwd>
+#include <map>
+#include <string>
+
+namespace g2o {
+
+ class Parameter;
+
+ /**
+ * \brief map id to parameters
+ */
+ class ParameterContainer : protected std::map<int, Parameter*>
+ {
+ public:
+ typedef std::map<int, Parameter*> BaseClass;
+
+ /**
+ * create a container for the parameters.
+ * @param isMainStorage_ pointers to the parameters are owned by this container, i.e., freed in its constructor
+ */
+ ParameterContainer(bool isMainStorage_=true);
+ virtual ~ParameterContainer();
+ //! add parameter to the container
+ bool addParameter(Parameter* p);
+ //! return a parameter based on its ID
+ Parameter* getParameter(int id);
+ //! remove a parameter from the container, i.e., the user now owns the pointer
+ Parameter* detachParameter(int id);
+ //! read parameters from a stream
+ virtual bool read(std::istream& is, const std::map<std::string, std::string>* renamedMap =0);
+ //! write the data to a stream
+ virtual bool write(std::ostream& os) const;
+ bool isMainStorage() const {return _isMainStorage;}
+ void clear();
+
+ // stuff of the base class that should re-appear
+ using BaseClass::size;
+
+ protected:
+ bool _isMainStorage;
+ };
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac6776d8a1b316244f579f8f27fbd197011feebd
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.cpp
@@ -0,0 +1,46 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel.h"
+
+namespace g2o {
+
+RobustKernel::RobustKernel() :
+ _delta(1.)
+{
+}
+
+RobustKernel::RobustKernel(double delta) :
+ _delta(delta)
+{
+}
+
+void RobustKernel::setDelta(double delta)
+{
+ _delta = delta;
+}
+
+} // end namespace g2o
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.h
new file mode 100644
index 0000000000000000000000000000000000000000..29e1394a0d4cbabe6425a3a98af0adc57027f566
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel.h
@@ -0,0 +1,81 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_H
+#define G2O_ROBUST_KERNEL_H
+
+#ifdef _MSC_VER
+#include <memory>
+#else
+#include <tr1/memory>
+#endif
+#include <Eigen/Core>
+
+
+namespace g2o {
+
+ /**
+ * \brief base for all robust cost functions
+ *
+ * Note in all the implementations for the other cost functions the e in the
+ * funtions corresponds to the sqaured errors, i.e., the robustification
+ * functions gets passed the squared error.
+ *
+ * e.g. the robustified least squares function is
+ *
+ * chi^2 = sum_{e} rho( e^T Omega e )
+ */
+ class RobustKernel
+ {
+ public:
+ RobustKernel();
+ explicit RobustKernel(double delta);
+ virtual ~RobustKernel() {}
+ /**
+ * compute the scaling factor for a error:
+ * The error is e^T Omega e
+ * The output rho is
+ * rho[0]: The actual scaled error value
+ * rho[1]: First derivative of the scaling function
+ * rho[2]: Second derivative of the scaling function
+ */
+ virtual void robustify(double squaredError, Eigen::Vector3d& rho) const = 0;
+
+ /**
+ * set the window size of the error. A squared error above delta^2 is considered
+ * as outlier in the data.
+ */
+ virtual void setDelta(double delta);
+ double delta() const { return _delta;}
+
+ protected:
+ double _delta;
+ };
+ typedef std::tr1::shared_ptr<RobustKernel> RobustKernelPtr;
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1d028cf3c331898afc8413d0f1bab05b11980e51
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.cpp
@@ -0,0 +1,111 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel_factory.h"
+#include "robust_kernel.h"
+
+#include <cassert>
+
+using namespace std;
+
+namespace g2o {
+
+RobustKernelFactory* RobustKernelFactory::factoryInstance = 0;
+
+RobustKernelFactory::RobustKernelFactory()
+{
+}
+
+RobustKernelFactory::~RobustKernelFactory()
+{
+ for (CreatorMap::iterator it = _creator.begin(); it != _creator.end(); ++it) {
+ delete it->second;
+ }
+ _creator.clear();
+}
+
+RobustKernelFactory* RobustKernelFactory::instance()
+{
+ if (factoryInstance == 0) {
+ factoryInstance = new RobustKernelFactory;
+ }
+
+ return factoryInstance;
+}
+
+void RobustKernelFactory::registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c)
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end()) {
+ cerr << "RobustKernelFactory WARNING: Overwriting robust kernel tag " << tag << endl;
+ assert(0);
+ }
+
+ _creator[tag] = c;
+}
+
+void RobustKernelFactory::unregisterType(const std::string& tag)
+{
+ CreatorMap::iterator tagPosition = _creator.find(tag);
+ if (tagPosition != _creator.end()) {
+ AbstractRobustKernelCreator* c = tagPosition->second;
+ delete c;
+ _creator.erase(tagPosition);
+ }
+}
+
+RobustKernel* RobustKernelFactory::construct(const std::string& tag) const
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end()) {
+ return foundIt->second->construct();
+ }
+ return 0;
+}
+
+AbstractRobustKernelCreator* RobustKernelFactory::creator(const std::string& tag) const
+{
+ CreatorMap::const_iterator foundIt = _creator.find(tag);
+ if (foundIt != _creator.end()) {
+ return foundIt->second;
+ }
+ return 0;
+}
+
+void RobustKernelFactory::fillKnownKernels(std::vector<std::string>& types) const
+{
+ types.clear();
+ for (CreatorMap::const_iterator it = _creator.begin(); it != _creator.end(); ++it)
+ types.push_back(it->first);
+}
+
+void RobustKernelFactory::destroy()
+{
+ delete factoryInstance;
+ factoryInstance = 0;
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..bcdec07f47cd598a35d220ee6c8c4301ff0424a8
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_factory.h
@@ -0,0 +1,151 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_FACTORY_H
+#define G2O_ROBUST_KERNEL_FACTORY_H
+
+#include "../stuff/misc.h"
+
+#include <string>
+#include <map>
+#include <vector>
+#include <iostream>
+
+namespace g2o {
+
+ class RobustKernel;
+
+ /**
+ * \brief Abstract interface for allocating a robust kernel
+ */
+ class AbstractRobustKernelCreator
+ {
+ public:
+ /**
+ * create a hyper graph element. Has to implemented in derived class.
+ */
+ virtual RobustKernel* construct() = 0;
+ virtual ~AbstractRobustKernelCreator() { }
+ };
+
+ /**
+ * \brief templatized creator class which creates graph elements
+ */
+ template <typename T>
+ class RobustKernelCreator : public AbstractRobustKernelCreator
+ {
+ public:
+ RobustKernel* construct() { return new T;}
+ };
+
+ /**
+ * \brief create robust kernels based on their human readable name
+ */
+ class RobustKernelFactory
+ {
+ public:
+
+ //! return the instance
+ static RobustKernelFactory* instance();
+
+ //! free the instance
+ static void destroy();
+
+ /**
+ * register a tag for a specific creator
+ */
+ void registerRobustKernel(const std::string& tag, AbstractRobustKernelCreator* c);
+
+ /**
+ * unregister a tag for a specific creator
+ */
+ void unregisterType(const std::string& tag);
+
+ /**
+ * construct a robust kernel based on its tag
+ */
+ RobustKernel* construct(const std::string& tag) const;
+
+ /**
+ * return the creator for a specific tag
+ */
+ AbstractRobustKernelCreator* creator(const std::string& tag) const;
+
+ /**
+ * get a list of all known robust kernels
+ */
+ void fillKnownKernels(std::vector<std::string>& types) const;
+
+ protected:
+
+ typedef std::map<std::string, AbstractRobustKernelCreator*> CreatorMap;
+ RobustKernelFactory();
+ ~RobustKernelFactory();
+
+ CreatorMap _creator; ///< look-up map for the existing creators
+
+ private:
+ static RobustKernelFactory* factoryInstance;
+ };
+
+ template<typename T>
+ class RegisterRobustKernelProxy
+ {
+ public:
+ RegisterRobustKernelProxy(const std::string& name) : _name(name)
+ {
+ RobustKernelFactory::instance()->registerRobustKernel(_name, new RobustKernelCreator<T>());
+ }
+
+ ~RegisterRobustKernelProxy()
+ {
+ RobustKernelFactory::instance()->unregisterType(_name);
+ }
+
+ private:
+ std::string _name;
+ };
+
+#if defined _MSC_VER && defined G2O_SHARED_LIBS
+# define G2O_ROBUST_KERNEL_FACTORY_EXPORT __declspec(dllexport)
+# define G2O_ROBUST_KERNEL_FACTORY_IMPORT __declspec(dllimport)
+#else
+# define G2O_ROBUST_KERNEL_FACTORY_EXPORT
+# define G2O_ROBUST_KERNEL_FACTORY_IMPORT
+#endif
+
+ // These macros are used to automate registering of robust kernels and forcing linkage
+#define G2O_REGISTER_ROBUST_KERNEL(name, classname) \
+ extern "C" void G2O_ROBUST_KERNEL_FACTORY_EXPORT g2o_robust_kernel_##classname(void) {} \
+ static g2o::RegisterRobustKernelProxy<classname> g_robust_kernel_proxy_##classname(#name);
+
+#define G2O_USE_ROBUST_KERNEL(classname) \
+ extern "C" void G2O_ROBUST_KERNEL_FACTORY_IMPORT g2o_robust_kernel_##classname(void); \
+ static g2o::TypeFunctionProxy proxy_##classname(g2o_robust_kernel_##classname);
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e446efcd87698f525e9080b7090e1a468de6c25b
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.cpp
@@ -0,0 +1,173 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "robust_kernel_impl.h"
+#include "robust_kernel_factory.h"
+
+#include <cmath>
+
+namespace g2o {
+
+RobustKernelScaleDelta::RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta) :
+ RobustKernel(delta),
+ _kernel(kernel)
+{
+}
+
+RobustKernelScaleDelta::RobustKernelScaleDelta(double delta) :
+ RobustKernel(delta)
+{
+}
+
+void RobustKernelScaleDelta::setKernel(const RobustKernelPtr& ptr)
+{
+ _kernel = ptr;
+}
+
+void RobustKernelScaleDelta::robustify(double error, Eigen::Vector3d& rho) const
+{
+ if (_kernel.get()) {
+ double dsqr = _delta * _delta;
+ double dsqrReci = 1. / dsqr;
+ _kernel->robustify(dsqrReci * error, rho);
+ rho[0] *= dsqr;
+ rho[2] *= dsqrReci;
+ } else { // no robustification
+ rho[0] = error;
+ rho[1] = 1.;
+ rho[2] = 0.;
+ }
+}
+
+void RobustKernelHuber::setDelta(double delta)
+{
+ dsqr = delta*delta;
+ _delta = delta;
+}
+
+
+void RobustKernelHuber::setDeltaSqr(const double &delta, const double &deltaSqr)
+{
+ dsqr = deltaSqr;
+ _delta = delta;
+}
+
+void RobustKernelHuber::robustify(double e, Eigen::Vector3d& rho) const
+{
+ //dsqr = _delta * _delta;
+ if (e <= dsqr) { // inlier
+ rho[0] = e;
+ rho[1] = 1.;
+ rho[2] = 0.;
+ } else { // outlier
+ double sqrte = sqrt(e); // absolut value of the error
+ rho[0] = 2*sqrte*_delta - dsqr; // rho(e) = 2 * delta * e^(1/2) - delta^2
+ rho[1] = _delta / sqrte; // rho'(e) = delta / sqrt(e)
+ rho[2] = - 0.5 * rho[1] / e; // rho''(e) = -1 / (2*e^(3/2)) = -1/2 * (delta/e) / e
+ }
+}
+
+void RobustKernelTukey::setDeltaSqr(const double &deltaSqr, const double &inv)
+{
+ _deltaSqr = deltaSqr;
+ _invDeltaSqr = inv;
+
+}
+
+void RobustKernelTukey::robustify(double e, Eigen::Vector3d& rho) const
+{
+ if (e <= _deltaSqr) { // inlier
+ double factor = e*_invDeltaSqr;
+ double d = 1-factor;
+ double dd = d*d;
+ rho[0] = _deltaSqr*(1-dd*d);
+ rho[1] = 3*dd;
+ rho[2] = -6*_invDeltaSqr*d;
+ } else { // outlier
+ rho[0] = _deltaSqr; // rho(e) = delta^2
+ rho[1] = 0.;
+ rho[2] = 0.;
+ }
+}
+
+void RobustKernelPseudoHuber::robustify(double e2, Eigen::Vector3d& rho) const
+{
+ double dsqr = _delta * _delta;
+ double dsqrReci = 1. / dsqr;
+ double aux1 = dsqrReci * e2 + 1.0;
+ double aux2 = sqrt(aux1);
+ rho[0] = 2 * dsqr * (aux2 - 1);
+ rho[1] = 1. / aux2;
+ rho[2] = -0.5 * dsqrReci * rho[1] / aux1;
+}
+
+void RobustKernelCauchy::robustify(double e2, Eigen::Vector3d& rho) const
+{
+ double dsqr = _delta * _delta;
+ double dsqrReci = 1. / dsqr;
+ double aux = dsqrReci * e2 + 1.0;
+ rho[0] = dsqr * log(aux);
+ rho[1] = 1. / aux;
+ rho[2] = -dsqrReci * std::pow(rho[1], 2);
+}
+
+void RobustKernelSaturated::robustify(double e2, Eigen::Vector3d& rho) const
+{
+ double dsqr = _delta * _delta;
+ if (e2 <= dsqr) { // inlier
+ rho[0] = e2;
+ rho[1] = 1.;
+ rho[2] = 0.;
+ } else { // outlier
+ rho[0] = dsqr;
+ rho[1] = 0.;
+ rho[2] = 0.;
+ }
+}
+
+//delta is used as $phi$
+void RobustKernelDCS::robustify(double e2, Eigen::Vector3d& rho) const
+{
+ const double& phi = _delta;
+ double scale = (2.0*phi)/(phi+e2);
+ if(scale>=1.0)
+ scale = 1.0;
+
+ rho[0] = scale*e2*scale;
+ rho[1] = (scale*scale);
+ rho[2] = 0;
+}
+
+
+// register the kernel to their factory
+G2O_REGISTER_ROBUST_KERNEL(Huber, RobustKernelHuber)
+G2O_REGISTER_ROBUST_KERNEL(Tukey, RobustKernelTukey)
+G2O_REGISTER_ROBUST_KERNEL(PseudoHuber, RobustKernelPseudoHuber)
+G2O_REGISTER_ROBUST_KERNEL(Cauchy, RobustKernelCauchy)
+G2O_REGISTER_ROBUST_KERNEL(Saturated, RobustKernelSaturated)
+G2O_REGISTER_ROBUST_KERNEL(DCS, RobustKernelDCS)
+
+} // end namespace g2o
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.h
new file mode 100644
index 0000000000000000000000000000000000000000..066cc37f387e52f5524c12d770410461990c52ee
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/robust_kernel_impl.h
@@ -0,0 +1,167 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_ROBUST_KERNEL_IMPL_H
+#define G2O_ROBUST_KERNEL_IMPL_H
+
+#include "robust_kernel.h"
+
+namespace g2o {
+
+ /**
+ * \brief scale a robust kernel to another delta (window size)
+ *
+ * Scales a robust kernel to another window size. Useful, in case if
+ * one implements a kernel which only is designed for a fixed window
+ * size.
+ */
+ class RobustKernelScaleDelta : public RobustKernel
+ {
+ public:
+ /**
+ * construct the scaled kernel ontop of another kernel which might be shared accross
+ * several scaled kernels
+ */
+ explicit RobustKernelScaleDelta(const RobustKernelPtr& kernel, double delta = 1.);
+ explicit RobustKernelScaleDelta(double delta = 1.);
+
+ //! return the underlying kernel
+ const RobustKernelPtr kernel() const { return _kernel;}
+ //! use another kernel for the underlying operation
+ void setKernel(const RobustKernelPtr& ptr);
+
+ void robustify(double error, Eigen::Vector3d& rho) const;
+
+ protected:
+ RobustKernelPtr _kernel;
+ };
+
+ /**
+ * \brief Huber Cost Function
+ *
+ * Loss function as described by Huber
+ * See http://en.wikipedia.org/wiki/Huber_loss_function
+ *
+ * If e^(1/2) < d
+ * rho(e) = e
+ *
+ * else
+ *
+ * 1/2 2
+ * rho(e) = 2 d e - d
+ */
+ class RobustKernelHuber : public RobustKernel
+ {
+ public:
+ virtual void setDelta(double delta);
+ virtual void setDeltaSqr(const double &delta, const double &deltaSqr);
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+
+ private:
+ float dsqr;
+ };
+
+ /**
+ * \brief Tukey Cost Function
+ *
+ *
+ * If e^(1/2) < d
+ * rho(e) = delta2(1-(1-e/delta2)^3)
+ *
+ * else
+ *
+ * rho(e) = delta2
+ */
+ class RobustKernelTukey : public RobustKernel
+ {
+ public:
+
+ virtual void setDeltaSqr(const double &deltaSqr, const double &inv);
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+ private:
+ float _deltaSqr;
+ float _invDeltaSqr;
+ };
+
+
+ /**
+ * \brief Pseudo Huber Cost Function
+ *
+ * The smooth pseudo huber cost function:
+ * See http://en.wikipedia.org/wiki/Huber_loss_function
+ *
+ * 2 e
+ * 2 d (sqrt(-- + 1) - 1)
+ * 2
+ * d
+ */
+ class RobustKernelPseudoHuber : public RobustKernel
+ {
+ public:
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+ };
+
+ /**
+ * \brief Cauchy cost function
+ *
+ * 2 e
+ * d log(-- + 1)
+ * 2
+ * d
+ */
+ class RobustKernelCauchy : public RobustKernel
+ {
+ public:
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+ };
+
+ /**
+ * \brief Saturated cost function.
+ *
+ * The error is at most delta^2
+ */
+ class RobustKernelSaturated : public RobustKernel
+ {
+ public:
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+ };
+
+ /**
+ * \brief Dynamic covariance scaling - DCS
+ *
+ * See paper Robust Map Optimization from Agarwal et al. ICRA 2013
+ *
+ * delta is used as $phi$
+ */
+ class RobustKernelDCS : public RobustKernel
+ {
+ public:
+ virtual void robustify(double e2, Eigen::Vector3d& rho) const;
+ };
+
+} // end namespace g2o
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdddfac0e586364b3938aaa887e695d6a1309115
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.cpp
@@ -0,0 +1,87 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "solver.h"
+
+#include <cstring>
+#include <algorithm>
+
+namespace g2o {
+
+Solver::Solver() :
+ _optimizer(0), _x(0), _b(0), _xSize(0), _maxXSize(0),
+ _isLevenberg(false), _additionalVectorSpace(0)
+{
+}
+
+Solver::~Solver()
+{
+ delete[] _x;
+ delete[] _b;
+}
+
+void Solver::resizeVector(size_t sx)
+{
+ size_t oldSize = _xSize;
+ _xSize = sx;
+ sx += _additionalVectorSpace; // allocate some additional space if requested
+ if (_maxXSize < sx) {
+ _maxXSize = 2*sx;
+ delete[] _x;
+ _x = new double[_maxXSize];
+#ifndef NDEBUG
+ memset(_x, 0, _maxXSize * sizeof(double));
+#endif
+ if (_b) { // backup the former b, might still be needed for online processing
+ memcpy(_x, _b, oldSize * sizeof(double));
+ delete[] _b;
+ _b = new double[_maxXSize];
+ std::swap(_b, _x);
+ } else {
+ _b = new double[_maxXSize];
+#ifndef NDEBUG
+ memset(_b, 0, _maxXSize * sizeof(double));
+#endif
+ }
+ }
+}
+
+void Solver::setOptimizer(SparseOptimizer* optimizer)
+{
+ _optimizer = optimizer;
+}
+
+void Solver::setLevenberg(bool levenberg)
+{
+ _isLevenberg = levenberg;
+}
+
+void Solver::setAdditionalVectorSpace(size_t s)
+{
+ _additionalVectorSpace = s;
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a801d319c446f5a4fd132d208561ff66e20e0a56
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/solver.h
@@ -0,0 +1,151 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SOLVER_H
+#define G2O_SOLVER_H
+
+#include "hyper_graph.h"
+#include "batch_stats.h"
+#include "sparse_block_matrix.h"
+#include <cstddef>
+
+namespace g2o {
+
+
+ class SparseOptimizer;
+
+ /**
+ * \brief Generic interface for a sparse solver operating on a graph which solves one iteration of the linearized objective function
+ */
+ class Solver
+ {
+ public:
+ Solver();
+ virtual ~Solver();
+
+ public:
+ /**
+ * initialize the solver, called once before the first iteration
+ */
+ virtual bool init(SparseOptimizer* optimizer, bool online = false) = 0;
+
+ /**
+ * build the structure of the system
+ */
+ virtual bool buildStructure(bool zeroBlocks = false) = 0;
+ /**
+ * update the structures for online processing
+ */
+ virtual bool updateStructure(const std::vector<HyperGraph::Vertex*>& vset, const HyperGraph::EdgeSet& edges) = 0;
+ /**
+ * build the current system
+ */
+ virtual bool buildSystem() = 0;
+
+ /**
+ * solve Ax = b
+ */
+ virtual bool solve() = 0;
+
+ /**
+ * computes the block diagonal elements of the pattern specified in the input
+ * and stores them in given SparseBlockMatrix
+ */
+ virtual bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices) = 0;
+
+ /**
+ * update the system while performing Levenberg, i.e., modifying the diagonal
+ * components of A by doing += lambda along the main diagonal of the Matrix.
+ * Note that this function may be called with a positive and a negative lambda.
+ * The latter is used to undo a former modification.
+ * If backup is true, then the solver should store a backup of the diagonal, which
+ * can be restored by restoreDiagonal()
+ */
+ virtual bool setLambda(double lambda, bool backup = false) = 0;
+
+ /**
+ * restore a previosly made backup of the diagonal
+ */
+ virtual void restoreDiagonal() = 0;
+
+ //! return x, the solution vector
+ double* x() { return _x;}
+ const double* x() const { return _x;}
+ //! return b, the right hand side of the system
+ double* b() { return _b;}
+ const double* b() const { return _b;}
+
+ //! return the size of the solution vector (x) and b
+ size_t vectorSize() const { return _xSize;}
+
+ //! the optimizer (graph) on which the solver works
+ SparseOptimizer* optimizer() const { return _optimizer;}
+ void setOptimizer(SparseOptimizer* optimizer);
+
+ //! the system is Levenberg-Marquardt
+ bool levenberg() const { return _isLevenberg;}
+ void setLevenberg(bool levenberg);
+
+ /**
+ * does this solver support the Schur complement for solving a system consisting of poses and
+ * landmarks. Re-implemement in a derived solver, if your solver supports it.
+ */
+ virtual bool supportsSchur() {return false;}
+
+ //! should the solver perform the schur complement or not
+ virtual bool schur()=0;
+ virtual void setSchur(bool s)=0;
+
+ size_t additionalVectorSpace() const { return _additionalVectorSpace;}
+ void setAdditionalVectorSpace(size_t s);
+
+ /**
+ * write debug output of the Hessian if system is not positive definite
+ */
+ virtual void setWriteDebug(bool) = 0;
+ virtual bool writeDebug() const = 0;
+
+ //! write the hessian to disk using the specified file name
+ virtual bool saveHessian(const std::string& /*fileName*/) const = 0;
+
+ protected:
+ SparseOptimizer* _optimizer;
+ double* _x;
+ double* _b;
+ size_t _xSize, _maxXSize;
+ bool _isLevenberg; ///< the system we gonna solve is a Levenberg-Marquardt system
+ size_t _additionalVectorSpace;
+
+ void resizeVector(size_t sx);
+
+ private:
+ // Disable the copy constructor and assignment operator
+ Solver(const Solver&) { }
+ Solver& operator= (const Solver&) { return *this; }
+ };
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.h
new file mode 100644
index 0000000000000000000000000000000000000000..8e9b5efdf4460377b3657910e6fcebd42dcdff40
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.h
@@ -0,0 +1,231 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_
+#define G2O_SPARSE_BLOCK_MATRIX_
+
+#include <map>
+#include <vector>
+#include <fstream>
+#include <iostream>
+#include <iomanip>
+#include <cassert>
+#include <Eigen/Core>
+
+#include "sparse_block_matrix_ccs.h"
+#include "matrix_structure.h"
+#include "matrix_operations.h"
+#include "../../config.h"
+
+namespace g2o {
+ using namespace Eigen;
+/**
+ * \brief Sparse matrix which uses blocks
+ *
+ * Template class that specifies a sparse block matrix. A block
+ * matrix is a sparse matrix made of dense blocks. These blocks
+ * cannot have a random pattern, but follow a (variable) grid
+ * structure. This structure is specified by a partition of the rows
+ * and the columns of the matrix. The blocks are represented by the
+ * Eigen::Matrix structure, thus they can be statically or dynamically
+ * allocated. For efficiency reasons it is convenient to allocate them
+ * statically, when possible. A static block matrix has all blocks of
+ * the same size, and the size of the block is specified by the
+ * template argument. If this is not the case, and you have different
+ * block sizes than you have to use a dynamic-block matrix (default
+ * template argument).
+ */
+template <class MatrixType = MatrixXd >
+class SparseBlockMatrix {
+
+ public:
+ //! this is the type of the elementary block, it is an Eigen::Matrix.
+ typedef MatrixType SparseMatrixBlock;
+
+ //! columns of the matrix
+ inline int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+ //! rows of the matrix
+ inline int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+ typedef std::map<int, SparseMatrixBlock*> IntBlockMap;
+
+ /**
+ * constructs a sparse block matrix having a specific layout
+ * @param rbi: array of int containing the row layout of the blocks.
+ * the component i of the array should contain the index of the first row of the block i+1.
+ * @param rbi: array of int containing the column layout of the blocks.
+ * the component i of the array should contain the index of the first col of the block i+1.
+ * @param rb: number of row blocks
+ * @param cb: number of col blocks
+ * @param hasStorage: set it to true if the matrix "owns" the blocks, thus it deletes it on destruction.
+ * if false the matrix is only a "view" over an existing structure.
+ */
+ SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage=true);
+
+ SparseBlockMatrix();
+
+ ~SparseBlockMatrix();
+
+
+ //! this zeroes all the blocks. If dealloc=true the blocks are removed from memory
+ void clear(bool dealloc=false) ;
+
+ //! returns the block at location r,c. if alloc=true he block is created if it does not exist
+ SparseMatrixBlock* block(int r, int c, bool alloc=false);
+ //! returns the block at location r,c
+ const SparseMatrixBlock* block(int r, int c) const;
+
+ //! how many rows does the block at block-row r has?
+ inline int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+ //! how many cols does the block at block-col c has?
+ inline int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+ //! where does the row at block-row r starts?
+ inline int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+ //! where does the col at block-col r starts?
+ inline int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+ //! number of non-zero elements
+ size_t nonZeros() const;
+ //! number of allocated blocks
+ size_t nonZeroBlocks() const;
+
+ //! deep copy of a sparse-block-matrix;
+ SparseBlockMatrix* clone() const ;
+
+ /**
+ * returns a view or a copy of the block matrix
+ * @param rmin: starting block row
+ * @param rmax: ending block row
+ * @param cmin: starting block col
+ * @param cmax: ending block col
+ * @param alloc: if true it makes a deep copy, if false it creates a view.
+ */
+ SparseBlockMatrix* slice(int rmin, int rmax, int cmin, int cmax, bool alloc=true) const;
+
+ //! transposes a block matrix, The transposed type should match the argument false on failure
+ template <class MatrixTransposedType>
+ bool transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const;
+
+ //! adds the current matrix to the destination
+ bool add(SparseBlockMatrix<MatrixType>*& dest) const ;
+
+ //! dest = (*this) * M
+ template <class MatrixResultType, class MatrixFactorType>
+ bool multiply(SparseBlockMatrix<MatrixResultType> *& dest, const SparseBlockMatrix<MatrixFactorType>* M) const;
+
+ //! dest = (*this) * src
+ void multiply(double*& dest, const double* src) const;
+
+ /**
+ * compute dest = (*this) * src
+ * However, assuming that this is a symmetric matrix where only the upper triangle is stored
+ */
+ void multiplySymmetricUpperTriangle(double*& dest, const double* src) const;
+
+ //! dest = M * (*this)
+ void rightMultiply(double*& dest, const double* src) const;
+
+ //! *this *= a
+ void scale( double a);
+
+ /**
+ * writes in dest a block permutaton specified by pinv.
+ * @param pinv: array such that new_block[i] = old_block[pinv[i]]
+ */
+ bool symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool onlyUpper=false) const;
+
+ /**
+ * fill the CCS arrays of a matrix, arrays have to be allocated beforehand
+ */
+ int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const;
+
+ /**
+ * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
+ * the values and assumes that column and row structures have already been written.
+ */
+ int fillCCS(double* Cx, bool upperTriangle = false) const;
+
+ //! exports the non zero blocks in the structure matrix ms
+ void fillBlockStructure(MatrixStructure& ms) const;
+
+ //! the block matrices per block-column
+ const std::vector<IntBlockMap>& blockCols() const { return _blockCols;}
+ std::vector<IntBlockMap>& blockCols() { return _blockCols;}
+
+ //! indices of the row blocks
+ const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+ std::vector<int>& rowBlockIndices() { return _rowBlockIndices;}
+
+ //! indices of the column blocks
+ const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+ std::vector<int>& colBlockIndices() { return _colBlockIndices;}
+
+ /**
+ * write the content of this matrix to a stream loadable by Octave
+ * @param upperTriangle does this matrix store only the upper triangular blocks
+ */
+ bool writeOctave(const char* filename, bool upperTriangle = true) const;
+
+ /**
+ * copy into CCS structure
+ * @return number of processed blocks, -1 on error
+ */
+ int fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
+
+ /**
+ * copy as transposed into a CCS structure
+ * @return number of processed blocks, -1 on error
+ */
+ int fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const;
+
+ /**
+ * take over the memory and matrix pattern from a hash matrix.
+ * The structure of the hash matrix will be cleared.
+ */
+ void takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix);
+
+ protected:
+ std::vector<int> _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+ std::vector<int> _colBlockIndices; ///< vector of the indices of the blocks along the cols
+ //! array of maps of blocks. The index of the array represent a block column of the matrix
+ //! and the block column is stored as a map row_block -> matrix_block_ptr.
+ std::vector <IntBlockMap> _blockCols;
+ bool _hasStorage;
+};
+
+template < class MatrixType >
+std::ostream& operator << (std::ostream&, const SparseBlockMatrix<MatrixType>& m);
+
+ typedef SparseBlockMatrix<MatrixXd> SparseBlockMatrixXd;
+
+} //end namespace
+
+#include "sparse_block_matrix.hpp"
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..8dfa99c1b753d37db3a8e70a64d5e0747022db8e
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix.hpp
@@ -0,0 +1,657 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+namespace g2o {
+ using namespace Eigen;
+
+ namespace {
+ struct TripletEntry
+ {
+ int r, c;
+ double x;
+ TripletEntry(int r_, int c_, double x_) : r(r_), c(c_), x(x_) {}
+ };
+ struct TripletColSort
+ {
+ bool operator()(const TripletEntry& e1, const TripletEntry& e2) const
+ {
+ return e1.c < e2.c || (e1.c == e2.c && e1.r < e2.r);
+ }
+ };
+ /** Helper class to sort pair based on first elem */
+ template<class T1, class T2, class Pred = std::less<T1> >
+ struct CmpPairFirst {
+ bool operator()(const std::pair<T1,T2>& left, const std::pair<T1,T2>& right) {
+ return Pred()(left.first, right.first);
+ }
+ };
+ }
+
+ template <class MatrixType>
+ SparseBlockMatrix<MatrixType>::SparseBlockMatrix( const int * rbi, const int* cbi, int rb, int cb, bool hasStorage):
+ _rowBlockIndices(rbi,rbi+rb),
+ _colBlockIndices(cbi,cbi+cb),
+ _blockCols(cb), _hasStorage(hasStorage)
+ {
+ }
+
+ template <class MatrixType>
+ SparseBlockMatrix<MatrixType>::SparseBlockMatrix( ):
+ _blockCols(0), _hasStorage(true)
+ {
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::clear(bool dealloc) {
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_blockCols.size() > 100)
+# endif
+ for (int i=0; i < static_cast<int>(_blockCols.size()); ++i) {
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+ if (_hasStorage && dealloc)
+ delete b;
+ else
+ b->setZero();
+ }
+ if (_hasStorage && dealloc)
+ _blockCols[i].clear();
+ }
+ }
+
+ template <class MatrixType>
+ SparseBlockMatrix<MatrixType>::~SparseBlockMatrix(){
+ if (_hasStorage)
+ clear(true);
+ }
+
+ template <class MatrixType>
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c, bool alloc) {
+ typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator it =_blockCols[c].find(r);
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* _block=0;
+ if (it==_blockCols[c].end()){
+ if (!_hasStorage && ! alloc )
+ return 0;
+ else {
+ int rb=rowsOfBlock(r);
+ int cb=colsOfBlock(c);
+ _block=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(rb,cb);
+ _block->setZero();
+ std::pair < typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator, bool> result
+ =_blockCols[c].insert(std::make_pair(r,_block)); (void) result;
+ assert (result.second);
+ }
+ } else {
+ _block=it->second;
+ }
+ return _block;
+ }
+
+ template <class MatrixType>
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* SparseBlockMatrix<MatrixType>::block(int r, int c) const {
+ typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it =_blockCols[c].find(r);
+ if (it==_blockCols[c].end())
+ return 0;
+ return it->second;
+ }
+
+
+ template <class MatrixType>
+ SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::clone() const {
+ SparseBlockMatrix* ret= new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock(*it->second);
+ ret->_blockCols[i].insert(std::make_pair(it->first, b));
+ }
+ }
+ ret->_hasStorage=true;
+ return ret;
+ }
+
+
+ template <class MatrixType>
+ template <class MatrixTransposedType>
+ bool SparseBlockMatrix<MatrixType>::transpose(SparseBlockMatrix<MatrixTransposedType>*& dest) const {
+ if (! dest){
+ dest=new SparseBlockMatrix<MatrixTransposedType>(&_colBlockIndices[0], &_rowBlockIndices[0], _colBlockIndices.size(), _rowBlockIndices.size());
+ } else {
+ if (! dest->_hasStorage)
+ return false;
+ if(_rowBlockIndices.size()!=dest->_colBlockIndices.size())
+ return false;
+ if (_colBlockIndices.size()!=dest->_rowBlockIndices.size())
+ return false;
+ for (size_t i=0; i<_rowBlockIndices.size(); ++i){
+ if(_rowBlockIndices[i]!=dest->_colBlockIndices[i])
+ return false;
+ }
+ for (size_t i=0; i<_colBlockIndices.size(); ++i){
+ if(_colBlockIndices[i]!=dest->_rowBlockIndices[i])
+ return false;
+ }
+ }
+
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+ typename SparseBlockMatrix<MatrixTransposedType>::SparseMatrixBlock* d=dest->block(i,it->first,true);
+ *d = s->transpose();
+ }
+ }
+ return true;
+ }
+
+ template <class MatrixType>
+ bool SparseBlockMatrix<MatrixType>::add(SparseBlockMatrix*& dest) const {
+ if (! dest){
+ dest=new SparseBlockMatrix(&_rowBlockIndices[0], &_colBlockIndices[0], _rowBlockIndices.size(), _colBlockIndices.size());
+ } else {
+ if (! dest->_hasStorage)
+ return false;
+ if(_rowBlockIndices.size()!=dest->_rowBlockIndices.size())
+ return false;
+ if (_colBlockIndices.size()!=dest->_colBlockIndices.size())
+ return false;
+ for (size_t i=0; i<_rowBlockIndices.size(); ++i){
+ if(_rowBlockIndices[i]!=dest->_rowBlockIndices[i])
+ return false;
+ }
+ for (size_t i=0; i<_colBlockIndices.size(); ++i){
+ if(_colBlockIndices[i]!=dest->_colBlockIndices[i])
+ return false;
+ }
+ }
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* d=dest->block(it->first,i,true);
+ (*d)+=*s;
+ }
+ }
+ return true;
+ }
+
+ template <class MatrixType>
+ template < class MatrixResultType, class MatrixFactorType >
+ bool SparseBlockMatrix<MatrixType>::multiply(SparseBlockMatrix<MatrixResultType>*& dest, const SparseBlockMatrix<MatrixFactorType> * M) const {
+ // sanity check
+ if (_colBlockIndices.size()!=M->_rowBlockIndices.size())
+ return false;
+ for (size_t i=0; i<_colBlockIndices.size(); ++i){
+ if (_colBlockIndices[i]!=M->_rowBlockIndices[i])
+ return false;
+ }
+ if (! dest) {
+ dest=new SparseBlockMatrix<MatrixResultType>(&_rowBlockIndices[0],&(M->_colBlockIndices[0]), _rowBlockIndices.size(), M->_colBlockIndices.size() );
+ }
+ if (! dest->_hasStorage)
+ return false;
+ for (size_t i=0; i<M->_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixFactorType>::IntBlockMap::const_iterator it=M->_blockCols[i].begin(); it!=M->_blockCols[i].end(); ++it){
+ // look for a non-zero block in a row of column it
+ int colM=i;
+ const typename SparseBlockMatrix<MatrixFactorType>::SparseMatrixBlock *b=it->second;
+ typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator rbt=_blockCols[it->first].begin();
+ while(rbt!=_blockCols[it->first].end()){
+ //int colA=it->first;
+ int rowA=rbt->first;
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock *a=rbt->second;
+ typename SparseBlockMatrix<MatrixResultType>::SparseMatrixBlock *c=dest->block(rowA,colM,true);
+ assert (c->rows()==a->rows());
+ assert (c->cols()==b->cols());
+ ++rbt;
+ (*c)+=(*a)*(*b);
+ }
+ }
+ }
+ return false;
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::multiply(double*& dest, const double* src) const {
+ if (! dest){
+ dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
+ memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
+ }
+
+ // map the memory by Eigen
+ Eigen::Map<VectorXd> destVec(dest, rows());
+ const Eigen::Map<const VectorXd> srcVec(src, cols());
+
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int srcOffset = i ? _colBlockIndices[i-1] : 0;
+
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+ int destOffset = it->first ? _rowBlockIndices[it->first - 1] : 0;
+ // destVec += *a * srcVec (according to the sub-vector parts)
+ internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+ }
+ }
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::multiplySymmetricUpperTriangle(double*& dest, const double* src) const
+ {
+ if (! dest){
+ dest=new double [_rowBlockIndices[_rowBlockIndices.size()-1] ];
+ memset(dest,0, _rowBlockIndices[_rowBlockIndices.size()-1]*sizeof(double));
+ }
+
+ // map the memory by Eigen
+ Eigen::Map<VectorXd> destVec(dest, rows());
+ const Eigen::Map<const VectorXd> srcVec(src, cols());
+
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int srcOffset = colBaseOfBlock(i);
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+ int destOffset = rowBaseOfBlock(it->first);
+ if (destOffset > srcOffset) // only upper triangle
+ break;
+ // destVec += *a * srcVec (according to the sub-vector parts)
+ internal::axpy(*a, srcVec, srcOffset, destVec, destOffset);
+ if (destOffset < srcOffset)
+ internal::atxpy(*a, srcVec, destOffset, destVec, srcOffset);
+ }
+ }
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::rightMultiply(double*& dest, const double* src) const {
+ int destSize=cols();
+
+ if (! dest){
+ dest=new double [ destSize ];
+ memset(dest,0, destSize*sizeof(double));
+ }
+
+ // map the memory by Eigen
+ Eigen::Map<VectorXd> destVec(dest, destSize);
+ Eigen::Map<const VectorXd> srcVec(src, rows());
+
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) schedule(dynamic, 10)
+# endif
+ for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+ int destOffset = colBaseOfBlock(i);
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin();
+ it!=_blockCols[i].end();
+ ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+ int srcOffset = rowBaseOfBlock(it->first);
+ // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
+ internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+ }
+ }
+
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::scale(double a_) {
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+ *a *= a_;
+ }
+ }
+ }
+
+ template <class MatrixType>
+ SparseBlockMatrix<MatrixType>* SparseBlockMatrix<MatrixType>::slice(int rmin, int rmax, int cmin, int cmax, bool alloc) const {
+ int m=rmax-rmin;
+ int n=cmax-cmin;
+ int rowIdx [m];
+ rowIdx[0] = rowsOfBlock(rmin);
+ for (int i=1; i<m; ++i){
+ rowIdx[i]=rowIdx[i-1]+rowsOfBlock(rmin+i);
+ }
+
+ int colIdx [n];
+ colIdx[0] = colsOfBlock(cmin);
+ for (int i=1; i<n; ++i){
+ colIdx[i]=colIdx[i-1]+colsOfBlock(cmin+i);
+ }
+ typename SparseBlockMatrix<MatrixType>::SparseBlockMatrix* s=new SparseBlockMatrix(rowIdx, colIdx, m, n, true);
+ for (int i=0; i<n; ++i){
+ int mc=cmin+i;
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[mc].begin(); it!=_blockCols[mc].end(); ++it){
+ if (it->first >= rmin && it->first < rmax){
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b = alloc ? new typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock (* (it->second) ) : it->second;
+ s->_blockCols[i].insert(std::make_pair(it->first-rmin, b));
+ }
+ }
+ }
+ s->_hasStorage=alloc;
+ return s;
+ }
+
+ template <class MatrixType>
+ size_t SparseBlockMatrix<MatrixType>::nonZeroBlocks() const {
+ size_t count=0;
+ for (size_t i=0; i<_blockCols.size(); ++i)
+ count+=_blockCols[i].size();
+ return count;
+ }
+
+ template <class MatrixType>
+ size_t SparseBlockMatrix<MatrixType>::nonZeros() const{
+ if (MatrixType::SizeAtCompileTime != Eigen::Dynamic) {
+ size_t nnz = nonZeroBlocks() * MatrixType::SizeAtCompileTime;
+ return nnz;
+ } else {
+ size_t count=0;
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* a=it->second;
+ count += a->cols()*a->rows();
+ }
+ }
+ return count;
+ }
+ }
+
+ template <class MatrixType>
+ std::ostream& operator << (std::ostream& os, const SparseBlockMatrix<MatrixType>& m){
+ os << "RBI: " << m.rowBlockIndices().size();
+ for (size_t i=0; i<m.rowBlockIndices().size(); ++i)
+ os << " " << m.rowBlockIndices()[i];
+ os << std::endl;
+ os << "CBI: " << m.colBlockIndices().size();
+ for (size_t i=0; i<m.colBlockIndices().size(); ++i)
+ os << " " << m.colBlockIndices()[i];
+ os << std::endl;
+
+ for (size_t i=0; i<m.blockCols().size(); ++i){
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=m.blockCols()[i].begin(); it!=m.blockCols()[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+ os << "BLOCK: " << it->first << " " << i << std::endl;
+ os << *b << std::endl;
+ }
+ }
+ return os;
+ }
+
+ template <class MatrixType>
+ bool SparseBlockMatrix<MatrixType>::symmPermutation(SparseBlockMatrix<MatrixType>*& dest, const int* pinv, bool upperTriangle) const{
+ // compute the permuted version of the new row/column layout
+ size_t n=_rowBlockIndices.size();
+ // computed the block sizes
+ int blockSizes[_rowBlockIndices.size()];
+ blockSizes[0]=_rowBlockIndices[0];
+ for (size_t i=1; i<n; ++i){
+ blockSizes[i]=_rowBlockIndices[i]-_rowBlockIndices[i-1];
+ }
+ // permute them
+ int pBlockIndices[_rowBlockIndices.size()];
+ for (size_t i=0; i<n; ++i){
+ pBlockIndices[pinv[i]]=blockSizes[i];
+ }
+ for (size_t i=1; i<n; ++i){
+ pBlockIndices[i]+=pBlockIndices[i-1];
+ }
+ // allocate C, or check the structure;
+ if (! dest){
+ dest=new SparseBlockMatrix(pBlockIndices, pBlockIndices, n, n);
+ } else {
+ if (dest->_rowBlockIndices.size()!=n)
+ return false;
+ if (dest->_colBlockIndices.size()!=n)
+ return false;
+ for (size_t i=0; i<n; ++i){
+ if (dest->_rowBlockIndices[i]!=pBlockIndices[i])
+ return false;
+ if (dest->_colBlockIndices[i]!=pBlockIndices[i])
+ return false;
+ }
+ dest->clear();
+ }
+ // now ready to permute the columns
+ for (size_t i=0; i<n; ++i){
+ //cerr << PVAR(i) << " ";
+ int pi=pinv[i];
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin();
+ it!=_blockCols[i].end(); ++it){
+ int pj=pinv[it->first];
+
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* s=it->second;
+
+ typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=0;
+ if (! upperTriangle || pj<=pi) {
+ b=dest->block(pj,pi,true);
+ assert(b->cols()==s->cols());
+ assert(b->rows()==s->rows());
+ *b=*s;
+ } else {
+ b=dest->block(pi,pj,true);
+ assert(b);
+ assert(b->rows()==s->cols());
+ assert(b->cols()==s->rows());
+ *b=s->transpose();
+ }
+ }
+ //cerr << endl;
+ // within each row,
+ }
+ return true;
+
+ }
+
+ template <class MatrixType>
+ int SparseBlockMatrix<MatrixType>::fillCCS(double* Cx, bool upperTriangle) const
+ {
+ assert(Cx && "Target destination is NULL");
+ double* CxStart = Cx;
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int cstart=i ? _colBlockIndices[i-1] : 0;
+ int csize=colsOfBlock(i);
+ for (int c=0; c<csize; ++c) {
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+ int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
+
+ int elemsToCopy = b->rows();
+ if (upperTriangle && rstart == cstart)
+ elemsToCopy = c + 1;
+ memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
+ Cx += elemsToCopy;
+
+ }
+ }
+ }
+ return Cx - CxStart;
+ }
+
+ template <class MatrixType>
+ int SparseBlockMatrix<MatrixType>::fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle) const
+ {
+ assert(Cp && Ci && Cx && "Target destination is NULL");
+ int nz=0;
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int cstart=i ? _colBlockIndices[i-1] : 0;
+ int csize=colsOfBlock(i);
+ for (int c=0; c<csize; ++c) {
+ *Cp=nz;
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it){
+ const typename SparseBlockMatrix<MatrixType>::SparseMatrixBlock* b=it->second;
+ int rstart=it->first ? _rowBlockIndices[it->first-1] : 0;
+
+ int elemsToCopy = b->rows();
+ if (upperTriangle && rstart == cstart)
+ elemsToCopy = c + 1;
+ for (int r=0; r<elemsToCopy; ++r){
+ *Cx++ = (*b)(r,c);
+ *Ci++ = rstart++;
+ ++nz;
+ }
+ }
+ ++Cp;
+ }
+ }
+ *Cp=nz;
+ return nz;
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::fillBlockStructure(MatrixStructure& ms) const
+ {
+ int n = _colBlockIndices.size();
+ int nzMax = (int)nonZeroBlocks();
+
+ ms.alloc(n, nzMax);
+ ms.m = _rowBlockIndices.size();
+
+ int nz = 0;
+ int* Cp = ms.Ap;
+ int* Ci = ms.Aii;
+ for (int i = 0; i < static_cast<int>(_blockCols.size()); ++i){
+ *Cp = nz;
+ const int& c = i;
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+ const int& r = it->first;
+ if (r <= c) {
+ *Ci++ = r;
+ ++nz;
+ }
+ }
+ Cp++;
+ }
+ *Cp=nz;
+ assert(nz <= nzMax);
+ }
+
+ template <class MatrixType>
+ bool SparseBlockMatrix<MatrixType>::writeOctave(const char* filename, bool upperTriangle) const
+ {
+ std::string name = filename;
+ std::string::size_type lastDot = name.find_last_of('.');
+ if (lastDot != std::string::npos)
+ name = name.substr(0, lastDot);
+
+ std::vector<TripletEntry> entries;
+ for (size_t i = 0; i<_blockCols.size(); ++i){
+ const int& c = i;
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it=_blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+ const int& r = it->first;
+ const MatrixType& m = *(it->second);
+ for (int cc = 0; cc < m.cols(); ++cc)
+ for (int rr = 0; rr < m.rows(); ++rr) {
+ int aux_r = rowBaseOfBlock(r) + rr;
+ int aux_c = colBaseOfBlock(c) + cc;
+ entries.push_back(TripletEntry(aux_r, aux_c, m(rr, cc)));
+ if (upperTriangle && r != c) {
+ entries.push_back(TripletEntry(aux_c, aux_r, m(rr, cc)));
+ }
+ }
+ }
+ }
+
+ int nz = entries.size();
+ std::sort(entries.begin(), entries.end(), TripletColSort());
+
+ std::ofstream fout(filename);
+ fout << "# name: " << name << std::endl;
+ fout << "# type: sparse matrix" << std::endl;
+ fout << "# nnz: " << nz << std::endl;
+ fout << "# rows: " << rows() << std::endl;
+ fout << "# columns: " << cols() << std::endl;
+ fout << std::setprecision(9) << std::fixed << std::endl;
+
+ for (std::vector<TripletEntry>::const_iterator it = entries.begin(); it != entries.end(); ++it) {
+ const TripletEntry& entry = *it;
+ fout << entry.r+1 << " " << entry.c+1 << " " << entry.x << std::endl;
+ }
+ return fout.good();
+ }
+
+ template <class MatrixType>
+ int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCS(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
+ {
+ blockCCS.blockCols().resize(blockCols().size());
+ int numblocks = 0;
+ for (size_t i = 0; i < blockCols().size(); ++i) {
+ const IntBlockMap& row = blockCols()[i];
+ typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[i];
+ dest.clear();
+ dest.reserve(row.size());
+ for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
+ dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(it->first, it->second));
+ ++numblocks;
+ }
+ }
+ return numblocks;
+ }
+
+ template <class MatrixType>
+ int SparseBlockMatrix<MatrixType>::fillSparseBlockMatrixCCSTransposed(SparseBlockMatrixCCS<MatrixType>& blockCCS) const
+ {
+ blockCCS.blockCols().clear();
+ blockCCS.blockCols().resize(_rowBlockIndices.size());
+ int numblocks = 0;
+ for (size_t i = 0; i < blockCols().size(); ++i) {
+ const IntBlockMap& row = blockCols()[i];
+ for (typename IntBlockMap::const_iterator it = row.begin(); it != row.end(); ++it) {
+ typename SparseBlockMatrixCCS<MatrixType>::SparseColumn& dest = blockCCS.blockCols()[it->first];
+ dest.push_back(typename SparseBlockMatrixCCS<MatrixType>::RowBlock(i, it->second));
+ ++numblocks;
+ }
+ }
+ return numblocks;
+ }
+
+ template <class MatrixType>
+ void SparseBlockMatrix<MatrixType>::takePatternFromHash(SparseBlockMatrixHashMap<MatrixType>& hashMatrix)
+ {
+ // sort the sparse columns and add them to the map structures by
+ // exploiting that we are inserting a sorted structure
+ typedef std::pair<int, MatrixType*> SparseColumnPair;
+ typedef typename SparseBlockMatrixHashMap<MatrixType>::SparseColumn HashSparseColumn;
+ for (size_t i = 0; i < hashMatrix.blockCols().size(); ++i) {
+ // prepare a temporary vector for sorting
+ HashSparseColumn& column = hashMatrix.blockCols()[i];
+ if (column.size() == 0)
+ continue;
+ std::vector<SparseColumnPair> sparseRowSorted; // temporary structure
+ sparseRowSorted.reserve(column.size());
+ for (typename HashSparseColumn::const_iterator it = column.begin(); it != column.end(); ++it)
+ sparseRowSorted.push_back(*it);
+ std::sort(sparseRowSorted.begin(), sparseRowSorted.end(), CmpPairFirst<int, MatrixType*>());
+ // try to free some memory early
+ HashSparseColumn aux;
+ swap(aux, column);
+ // now insert sorted vector to the std::map structure
+ IntBlockMap& destColumnMap = blockCols()[i];
+ destColumnMap.insert(sparseRowSorted[0]);
+ for (size_t j = 1; j < sparseRowSorted.size(); ++j) {
+ typename SparseBlockMatrix<MatrixType>::IntBlockMap::iterator hint = destColumnMap.end();
+ --hint; // cppreference says the element goes after the hint (until C++11)
+ destColumnMap.insert(hint, sparseRowSorted[j]);
+ }
+ }
+ }
+
+}// end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
new file mode 100644
index 0000000000000000000000000000000000000000..eb9042c218c925c49a28a2eff36cee981737dc89
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_ccs.h
@@ -0,0 +1,282 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_CCS_H
+#define G2O_SPARSE_BLOCK_MATRIX_CCS_H
+
+#include <vector>
+#include <cassert>
+#include <Eigen/Core>
+
+#include "../../config.h"
+#include "matrix_operations.h"
+
+#ifdef _MSC_VER
+#include <unordered_map>
+#else
+#include <tr1/unordered_map>
+#endif
+
+namespace g2o {
+
+ /**
+ * \brief Sparse matrix which uses blocks
+ *
+ * This class is used as a const view on a SparseBlockMatrix
+ * which allows a faster iteration over the elements of the
+ * matrix.
+ */
+ template <class MatrixType>
+ class SparseBlockMatrixCCS
+ {
+ public:
+ //! this is the type of the elementary block, it is an Eigen::Matrix.
+ typedef MatrixType SparseMatrixBlock;
+
+ //! columns of the matrix
+ int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+ //! rows of the matrix
+ int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+ /**
+ * \brief A block within a column
+ */
+ struct RowBlock
+ {
+ int row; ///< row of the block
+ MatrixType* block; ///< matrix pointer for the block
+ RowBlock() : row(-1), block(0) {}
+ RowBlock(int r, MatrixType* b) : row(r), block(b) {}
+ bool operator<(const RowBlock& other) const { return row < other.row;}
+ };
+ typedef std::vector<RowBlock> SparseColumn;
+
+ SparseBlockMatrixCCS(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
+ _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
+ {}
+
+ //! how many rows does the block at block-row r has?
+ int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+ //! how many cols does the block at block-col c has?
+ int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+ //! where does the row at block-row r start?
+ int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+ //! where does the col at block-col r start?
+ int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+ //! the block matrices per block-column
+ const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
+ std::vector<SparseColumn>& blockCols() { return _blockCols;}
+
+ //! indices of the row blocks
+ const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+
+ //! indices of the column blocks
+ const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+
+ void rightMultiply(double*& dest, const double* src) const
+ {
+ int destSize=cols();
+
+ if (! dest){
+ dest=new double [ destSize ];
+ memset(dest,0, destSize*sizeof(double));
+ }
+
+ // map the memory by Eigen
+ Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
+ Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
+
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) schedule(dynamic, 10)
+# endif
+ for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+ int destOffset = colBaseOfBlock(i);
+ for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+ const SparseMatrixBlock* a = it->block;
+ int srcOffset = rowBaseOfBlock(it->row);
+ // destVec += *a.transpose() * srcVec (according to the sub-vector parts)
+ internal::atxpy(*a, srcVec, srcOffset, destVec, destOffset);
+ }
+ }
+ }
+
+ /**
+ * sort the blocks in each column
+ */
+ void sortColumns()
+ {
+ for (int i=0; i < static_cast<int>(_blockCols.size()); ++i){
+ std::sort(_blockCols[i].begin(), _blockCols[i].end());
+ }
+ }
+
+ /**
+ * fill the CCS arrays of a matrix, arrays have to be allocated beforehand
+ */
+ int fillCCS(int* Cp, int* Ci, double* Cx, bool upperTriangle = false) const
+ {
+ assert(Cp && Ci && Cx && "Target destination is NULL");
+ int nz=0;
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int cstart=i ? _colBlockIndices[i-1] : 0;
+ int csize=colsOfBlock(i);
+ for (int c=0; c<csize; ++c) {
+ *Cp=nz;
+ for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+ const SparseMatrixBlock* b=it->block;
+ int rstart=it->row ? _rowBlockIndices[it->row-1] : 0;
+
+ int elemsToCopy = b->rows();
+ if (upperTriangle && rstart == cstart)
+ elemsToCopy = c + 1;
+ for (int r=0; r<elemsToCopy; ++r){
+ *Cx++ = (*b)(r,c);
+ *Ci++ = rstart++;
+ ++nz;
+ }
+ }
+ ++Cp;
+ }
+ }
+ *Cp=nz;
+ return nz;
+ }
+
+ /**
+ * fill the CCS arrays of a matrix, arrays have to be allocated beforehand. This function only writes
+ * the values and assumes that column and row structures have already been written.
+ */
+ int fillCCS(double* Cx, bool upperTriangle = false) const
+ {
+ assert(Cx && "Target destination is NULL");
+ double* CxStart = Cx;
+ int cstart = 0;
+ for (size_t i=0; i<_blockCols.size(); ++i){
+ int csize = _colBlockIndices[i] - cstart;
+ for (int c=0; c<csize; ++c) {
+ for (typename SparseColumn::const_iterator it = _blockCols[i].begin(); it!=_blockCols[i].end(); ++it) {
+ const SparseMatrixBlock* b = it->block;
+ int rstart = it->row ? _rowBlockIndices[it->row-1] : 0;
+
+ int elemsToCopy = b->rows();
+ if (upperTriangle && rstart == cstart)
+ elemsToCopy = c + 1;
+ memcpy(Cx, b->data() + c*b->rows(), elemsToCopy * sizeof(double));
+ Cx += elemsToCopy;
+
+ }
+ }
+ cstart = _colBlockIndices[i];
+ }
+ return Cx - CxStart;
+ }
+
+ protected:
+ const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+ const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
+ std::vector<SparseColumn> _blockCols; ///< the matrices stored in CCS order
+ };
+
+
+
+ /**
+ * \brief Sparse matrix which uses blocks based on hash structures
+ *
+ * This class is used to construct the pattern of a sparse block matrix
+ */
+ template <class MatrixType>
+ class SparseBlockMatrixHashMap
+ {
+ public:
+ //! this is the type of the elementary block, it is an Eigen::Matrix.
+ typedef MatrixType SparseMatrixBlock;
+
+ //! columns of the matrix
+ int cols() const {return _colBlockIndices.size() ? _colBlockIndices.back() : 0;}
+ //! rows of the matrix
+ int rows() const {return _rowBlockIndices.size() ? _rowBlockIndices.back() : 0;}
+
+ typedef std::tr1::unordered_map<int, MatrixType*> SparseColumn;
+
+ SparseBlockMatrixHashMap(const std::vector<int>& rowIndices, const std::vector<int>& colIndices) :
+ _rowBlockIndices(rowIndices), _colBlockIndices(colIndices)
+ {}
+
+ //! how many rows does the block at block-row r has?
+ int rowsOfBlock(int r) const { return r ? _rowBlockIndices[r] - _rowBlockIndices[r-1] : _rowBlockIndices[0] ; }
+
+ //! how many cols does the block at block-col c has?
+ int colsOfBlock(int c) const { return c ? _colBlockIndices[c] - _colBlockIndices[c-1] : _colBlockIndices[0]; }
+
+ //! where does the row at block-row r start?
+ int rowBaseOfBlock(int r) const { return r ? _rowBlockIndices[r-1] : 0 ; }
+
+ //! where does the col at block-col r start?
+ int colBaseOfBlock(int c) const { return c ? _colBlockIndices[c-1] : 0 ; }
+
+ //! the block matrices per block-column
+ const std::vector<SparseColumn>& blockCols() const { return _blockCols;}
+ std::vector<SparseColumn>& blockCols() { return _blockCols;}
+
+ //! indices of the row blocks
+ const std::vector<int>& rowBlockIndices() const { return _rowBlockIndices;}
+
+ //! indices of the column blocks
+ const std::vector<int>& colBlockIndices() const { return _colBlockIndices;}
+
+ /**
+ * add a block to the pattern, return a pointer to the added block
+ */
+ MatrixType* addBlock(int r, int c, bool zeroBlock = false)
+ {
+ assert(c <(int)_blockCols.size() && "accessing column which is not available");
+ SparseColumn& sparseColumn = _blockCols[c];
+ typename SparseColumn::iterator foundIt = sparseColumn.find(r);
+ if (foundIt == sparseColumn.end()) {
+ int rb = rowsOfBlock(r);
+ int cb = colsOfBlock(c);
+ MatrixType* m = new MatrixType(rb, cb);
+ if (zeroBlock)
+ m->setZero();
+ sparseColumn[r] = m;
+ return m;
+ }
+ return foundIt->second;
+ }
+
+ protected:
+ const std::vector<int>& _rowBlockIndices; ///< vector of the indices of the blocks along the rows.
+ const std::vector<int>& _colBlockIndices; ///< vector of the indices of the blocks along the cols
+ std::vector<SparseColumn> _blockCols; ///< the matrices stored in CCS order
+ };
+
+} //end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b13b9f2f37b60df395d8eef2c07153135e4fb10
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_diagonal.h
@@ -0,0 +1,108 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
+#define G2O_SPARSE_BLOCK_MATRIX_DIAGONAL_H
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/StdVector>
+
+#include "../../config.h"
+#include "matrix_operations.h"
+
+namespace g2o {
+
+ /**
+ * \brief Sparse matrix which uses blocks on the diagonal
+ *
+ * This class is used as a const view on a SparseBlockMatrix
+ * which allows a faster iteration over the elements of the
+ * matrix.
+ */
+ template <class MatrixType>
+ class SparseBlockMatrixDiagonal
+ {
+ public:
+ //! this is the type of the elementary block, it is an Eigen::Matrix.
+ typedef MatrixType SparseMatrixBlock;
+
+ //! columns of the matrix
+ int cols() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
+ //! rows of the matrix
+ int rows() const {return _blockIndices.size() ? _blockIndices.back() : 0;}
+
+ typedef std::vector<MatrixType, Eigen::aligned_allocator<MatrixType> > DiagonalVector;
+
+ SparseBlockMatrixDiagonal(const std::vector<int>& blockIndices) :
+ _blockIndices(blockIndices)
+ {}
+
+ //! how many rows/cols does the block at block-row / block-column r has?
+ inline int dimOfBlock(int r) const { return r ? _blockIndices[r] - _blockIndices[r-1] : _blockIndices[0] ; }
+
+ //! where does the row /col at block-row / block-column r starts?
+ inline int baseOfBlock(int r) const { return r ? _blockIndices[r-1] : 0 ; }
+
+ //! the block matrices per block-column
+ const DiagonalVector& diagonal() const { return _diagonal;}
+ DiagonalVector& diagonal() { return _diagonal;}
+
+ //! indices of the row blocks
+ const std::vector<int>& blockIndices() const { return _blockIndices;}
+
+ void multiply(double*& dest, const double* src) const
+ {
+ int destSize=cols();
+ if (! dest) {
+ dest=new double[destSize];
+ memset(dest,0, destSize*sizeof(double));
+ }
+
+ // map the memory by Eigen
+ Eigen::Map<Eigen::VectorXd> destVec(dest, destSize);
+ Eigen::Map<const Eigen::VectorXd> srcVec(src, rows());
+
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) schedule(dynamic, 10)
+# endif
+ for (int i=0; i < static_cast<int>(_diagonal.size()); ++i){
+ int destOffset = baseOfBlock(i);
+ int srcOffset = destOffset;
+ const SparseMatrixBlock& A = _diagonal[i];
+ // destVec += *A.transpose() * srcVec (according to the sub-vector parts)
+ internal::axpy(A, srcVec, srcOffset, destVec, destOffset);
+ }
+ }
+
+ protected:
+ const std::vector<int>& _blockIndices; ///< vector of the indices of the blocks along the diagonal
+ DiagonalVector _diagonal;
+ };
+
+} //end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42a4560824a490094450ea2b57fd1dcf86d7c764
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_block_matrix_test.cpp
@@ -0,0 +1,107 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_block_matrix.h"
+#include <iostream>
+
+using namespace std;
+using namespace g2o;
+using namespace Eigen;
+
+typedef SparseBlockMatrix< MatrixXd >
+SparseBlockMatrixX;
+
+std::ostream& operator << (std::ostream& os, const SparseBlockMatrixX::SparseMatrixBlock& m) {
+ for (int i=0; i<m.rows(); ++i){
+ for (int j=0; j<m.cols(); ++j)
+ cerr << m(i,j) << " ";
+ cerr << endl;
+ }
+ return os;
+}
+
+int main (int argc, char** argv){
+ int rcol[] = {3,6,8,12};
+ int ccol[] = {2,4,13};
+ cerr << "creation" << endl;
+ SparseBlockMatrixX* M=new SparseBlockMatrixX(rcol, ccol, 4,3);
+
+ cerr << "block access" << endl;
+
+ SparseBlockMatrixX::SparseMatrixBlock* b=M->block(0,0, true);
+ cerr << b->rows() << " " << b->cols() << endl;
+ for (int i=0; i<b->rows(); ++i)
+ for (int j=0; j<b->cols(); ++j){
+ (*b)(i,j)=i*b->cols()+j;
+ }
+
+
+ cerr << "block access 2" << endl;
+ b=M->block(0,2, true);
+ cerr << b->rows() << " " << b->cols() << endl;
+ for (int i=0; i<b->rows(); ++i)
+ for (int j=0; j<b->cols(); ++j){
+ (*b)(i,j)=i*b->cols()+j;
+ }
+
+ b=M->block(3,2, true);
+ cerr << b->rows() << " " << b->cols() << endl;
+ for (int i=0; i<b->rows(); ++i)
+ for (int j=0; j<b->cols(); ++j){
+ (*b)(i,j)=i*b->cols()+j;
+ }
+
+ cerr << *M << endl;
+
+ cerr << "SUM" << endl;
+
+ SparseBlockMatrixX* Ms=0;
+ M->add(Ms);
+ M->add(Ms);
+ cerr << *Ms;
+
+ SparseBlockMatrixX* Mt=0;
+ M->transpose(Mt);
+ cerr << *Mt << endl;
+
+ SparseBlockMatrixX* Mp=0;
+ M->multiply(Mp, Mt);
+ cerr << *Mp << endl;
+
+ int iperm[]={3,2,1,0};
+ SparseBlockMatrixX* PMp=0;
+
+ Mp->symmPermutation(PMp,iperm, false);
+ cerr << *PMp << endl;
+
+ PMp->clear(true);
+ Mp->block(3,0)->fill(0.);
+ Mp->symmPermutation(PMp,iperm, true);
+ cerr << *PMp << endl;
+
+
+
+}
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c316e656a523f008256239276915df3dd841bf8
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.cpp
@@ -0,0 +1,615 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "sparse_optimizer.h"
+
+#include <iostream>
+#include <iomanip>
+#include <algorithm>
+#include <iterator>
+#include <cassert>
+#include <algorithm>
+
+#include "estimate_propagator.h"
+#include "optimization_algorithm.h"
+#include "batch_stats.h"
+#include "hyper_graph_action.h"
+#include "robust_kernel.h"
+#include "../stuff/timeutil.h"
+#include "../stuff/macros.h"
+#include "../stuff/misc.h"
+#include "../../config.h"
+
+namespace g2o{
+ using namespace std;
+
+
+ SparseOptimizer::SparseOptimizer() :
+ _forceStopFlag(0), _verbose(false), _algorithm(0), _computeBatchStatistics(false)
+ {
+ _graphActions.resize(AT_NUM_ELEMENTS);
+ }
+
+ SparseOptimizer::~SparseOptimizer(){
+ delete _algorithm;
+ G2OBatchStatistics::setGlobalStats(0);
+ }
+
+ void SparseOptimizer::computeActiveErrors()
+ {
+ // call the callbacks in case there is something registered
+ HyperGraphActionSet& actions = _graphActions[AT_COMPUTEACTIVERROR];
+ if (actions.size() > 0) {
+ for (HyperGraphActionSet::iterator it = actions.begin(); it != actions.end(); ++it)
+ (*(*it))(this);
+ }
+
+# ifdef G2O_OPENMP
+# pragma omp parallel for default (shared) if (_activeEdges.size() > 50)
+# endif
+ for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
+ OptimizableGraph::Edge* e = _activeEdges[k];
+ e->computeError();
+ }
+
+# ifndef NDEBUG
+ for (int k = 0; k < static_cast<int>(_activeEdges.size()); ++k) {
+ OptimizableGraph::Edge* e = _activeEdges[k];
+ bool hasNan = arrayHasNaN(e->errorData(), e->dimension());
+ if (hasNan) {
+ cerr << "computeActiveErrors(): found NaN in error for edge " << e << endl;
+ }
+ }
+# endif
+
+ }
+
+ double SparseOptimizer::activeChi2( ) const
+ {
+ double chi = 0.0;
+ for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+ const OptimizableGraph::Edge* e = *it;
+ chi += e->chi2();
+ }
+ return chi;
+ }
+
+ double SparseOptimizer::activeRobustChi2() const
+ {
+ Eigen::Vector3d rho;
+ double chi = 0.0;
+ for (EdgeContainer::const_iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+ const OptimizableGraph::Edge* e = *it;
+ if (e->robustKernel()) {
+ e->robustKernel()->robustify(e->chi2(), rho);
+ chi += rho[0];
+ }
+ else
+ chi += e->chi2();
+ }
+ return chi;
+ }
+
+ OptimizableGraph::Vertex* SparseOptimizer::findGauge(){
+ if (vertices().empty())
+ return 0;
+
+ int maxDim=0;
+ for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+ maxDim=std::max(maxDim,v->dimension());
+ }
+
+ OptimizableGraph::Vertex* rut=0;
+ for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+ if (v->dimension()==maxDim){
+ rut=v;
+ break;
+ }
+ }
+ return rut;
+ }
+
+ bool SparseOptimizer::gaugeFreedom()
+ {
+ if (vertices().empty())
+ return false;
+
+ int maxDim=0;
+ for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+ maxDim = std::max(maxDim,v->dimension());
+ }
+
+ for (HyperGraph::VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it){
+ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(it->second);
+ if (v->dimension() == maxDim) {
+ // test for fixed vertex
+ if (v->fixed()) {
+ return false;
+ }
+ // test for full dimension prior
+ for (HyperGraph::EdgeSet::const_iterator eit = v->edges().begin(); eit != v->edges().end(); ++eit) {
+ OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*eit);
+ if (e->vertices().size() == 1 && e->dimension() == maxDim)
+ return false;
+ }
+ }
+ }
+ return true;
+ }
+
+ bool SparseOptimizer::buildIndexMapping(SparseOptimizer::VertexContainer& vlist){
+ if (! vlist.size()){
+ _ivMap.clear();
+ return false;
+ }
+
+ _ivMap.resize(vlist.size());
+ size_t i = 0;
+ for (int k=0; k<2; k++)
+ for (VertexContainer::iterator it=vlist.begin(); it!=vlist.end(); ++it){
+ OptimizableGraph::Vertex* v = *it;
+ if (! v->fixed()){
+ if (static_cast<int>(v->marginalized()) == k){
+ v->setHessianIndex(i);
+ _ivMap[i]=v;
+ i++;
+ }
+ }
+ else {
+ v->setHessianIndex(-1);
+ }
+ }
+ _ivMap.resize(i);
+ return true;
+ }
+
+ void SparseOptimizer::clearIndexMapping(){
+ for (size_t i=0; i<_ivMap.size(); ++i){
+ _ivMap[i]->setHessianIndex(-1);
+ _ivMap[i]=0;
+ }
+ }
+
+ bool SparseOptimizer::initializeOptimization(int level){
+ HyperGraph::VertexSet vset;
+ for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it)
+ vset.insert(it->second);
+ return initializeOptimization(vset,level);
+ }
+
+ bool SparseOptimizer::initializeOptimization(HyperGraph::VertexSet& vset, int level){
+ if (edges().size() == 0) {
+ cerr << __PRETTY_FUNCTION__ << ": Attempt to initialize an empty graph" << endl;
+ return false;
+ }
+ bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
+ assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
+ clearIndexMapping();
+ _activeVertices.clear();
+ _activeVertices.reserve(vset.size());
+ _activeEdges.clear();
+ set<Edge*> auxEdgeSet; // temporary structure to avoid duplicates
+ for (HyperGraph::VertexSet::iterator it=vset.begin(); it!=vset.end(); ++it){
+ OptimizableGraph::Vertex* v= (OptimizableGraph::Vertex*) *it;
+ const OptimizableGraph::EdgeSet& vEdges=v->edges();
+ // count if there are edges in that level. If not remove from the pool
+ int levelEdges=0;
+ for (OptimizableGraph::EdgeSet::const_iterator it=vEdges.begin(); it!=vEdges.end(); ++it){
+ OptimizableGraph::Edge* e=reinterpret_cast<OptimizableGraph::Edge*>(*it);
+ if (level < 0 || e->level() == level) {
+
+ bool allVerticesOK = true;
+ for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+ if (vset.find(*vit) == vset.end()) {
+ allVerticesOK = false;
+ break;
+ }
+ }
+ if (allVerticesOK && !e->allVerticesFixed()) {
+ auxEdgeSet.insert(e);
+ levelEdges++;
+ }
+
+ }
+ }
+ if (levelEdges){
+ _activeVertices.push_back(v);
+
+ // test for NANs in the current estimate if we are debugging
+# ifndef NDEBUG
+ int estimateDim = v->estimateDimension();
+ if (estimateDim > 0) {
+ Eigen::VectorXd estimateData(estimateDim);
+ if (v->getEstimateData(estimateData.data()) == true) {
+ int k;
+ bool hasNan = arrayHasNaN(estimateData.data(), estimateDim, &k);
+ if (hasNan)
+ cerr << __PRETTY_FUNCTION__ << ": Vertex " << v->id() << " contains a nan entry at index " << k << endl;
+ }
+ }
+# endif
+
+ }
+ }
+
+ _activeEdges.reserve(auxEdgeSet.size());
+ for (set<Edge*>::iterator it = auxEdgeSet.begin(); it != auxEdgeSet.end(); ++it)
+ _activeEdges.push_back(*it);
+
+ sortVectorContainers();
+ return buildIndexMapping(_activeVertices);
+ }
+
+ bool SparseOptimizer::initializeOptimization(HyperGraph::EdgeSet& eset){
+ bool workspaceAllocated = _jacobianWorkspace.allocate(); (void) workspaceAllocated;
+ assert(workspaceAllocated && "Error while allocating memory for the Jacobians");
+ clearIndexMapping();
+ _activeVertices.clear();
+ _activeEdges.clear();
+ _activeEdges.reserve(eset.size());
+ set<Vertex*> auxVertexSet; // temporary structure to avoid duplicates
+ for (HyperGraph::EdgeSet::iterator it=eset.begin(); it!=eset.end(); ++it){
+ OptimizableGraph::Edge* e=(OptimizableGraph::Edge*)(*it);
+ for (vector<HyperGraph::Vertex*>::const_iterator vit = e->vertices().begin(); vit != e->vertices().end(); ++vit) {
+ auxVertexSet.insert(static_cast<OptimizableGraph::Vertex*>(*vit));
+ }
+ _activeEdges.push_back(reinterpret_cast<OptimizableGraph::Edge*>(*it));
+ }
+
+ _activeVertices.reserve(auxVertexSet.size());
+ for (set<Vertex*>::iterator it = auxVertexSet.begin(); it != auxVertexSet.end(); ++it)
+ _activeVertices.push_back(*it);
+
+ sortVectorContainers();
+ return buildIndexMapping(_activeVertices);
+ }
+
+ void SparseOptimizer::setToOrigin(){
+ for (VertexIDMap::iterator it=vertices().begin(); it!=vertices().end(); ++it) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(it->second);
+ v->setToOrigin();
+ }
+ }
+
+ void SparseOptimizer::computeInitialGuess()
+ {
+ EstimatePropagator::PropagateCost costFunction(this);
+ computeInitialGuess(costFunction);
+ }
+
+ void SparseOptimizer::computeInitialGuess(EstimatePropagatorCost& costFunction)
+ {
+ OptimizableGraph::VertexSet emptySet;
+ std::set<Vertex*> backupVertices;
+ HyperGraph::VertexSet fixedVertices; // these are the root nodes where to start the initialization
+ for (EdgeContainer::iterator it = _activeEdges.begin(); it != _activeEdges.end(); ++it) {
+ OptimizableGraph::Edge* e = *it;
+ for (size_t i = 0; i < e->vertices().size(); ++i) {
+ OptimizableGraph::Vertex* v = static_cast<OptimizableGraph::Vertex*>(e->vertex(i));
+ if (v->fixed())
+ fixedVertices.insert(v);
+ else { // check for having a prior which is able to fully initialize a vertex
+ for (EdgeSet::const_iterator vedgeIt = v->edges().begin(); vedgeIt != v->edges().end(); ++vedgeIt) {
+ OptimizableGraph::Edge* vedge = static_cast<OptimizableGraph::Edge*>(*vedgeIt);
+ if (vedge->vertices().size() == 1 && vedge->initialEstimatePossible(emptySet, v) > 0.) {
+ //cerr << "Initialize with prior for " << v->id() << endl;
+ vedge->initialEstimate(emptySet, v);
+ fixedVertices.insert(v);
+ }
+ }
+ }
+ if (v->hessianIndex() == -1) {
+ std::set<Vertex*>::const_iterator foundIt = backupVertices.find(v);
+ if (foundIt == backupVertices.end()) {
+ v->push();
+ backupVertices.insert(v);
+ }
+ }
+ }
+ }
+
+ EstimatePropagator estimatePropagator(this);
+ estimatePropagator.propagate(fixedVertices, costFunction);
+
+ // restoring the vertices that should not be initialized
+ for (std::set<Vertex*>::iterator it = backupVertices.begin(); it != backupVertices.end(); ++it) {
+ Vertex* v = *it;
+ v->pop();
+ }
+ if (verbose()) {
+ computeActiveErrors();
+ cerr << "iteration= -1\t chi2= " << activeChi2()
+ << "\t time= 0.0"
+ << "\t cumTime= 0.0"
+ << "\t (using initial guess from " << costFunction.name() << ")" << endl;
+ }
+ }
+
+ int SparseOptimizer::optimize(int iterations, bool online)
+ {
+ if (_ivMap.size() == 0) {
+ cerr << __PRETTY_FUNCTION__ << ": 0 vertices to optimize, maybe forgot to call initializeOptimization()" << endl;
+ return -1;
+ }
+
+ int cjIterations=0;
+ double cumTime=0;
+ bool ok=true;
+
+ ok = _algorithm->init(online);
+ if (! ok) {
+ cerr << __PRETTY_FUNCTION__ << " Error while initializing" << endl;
+ return -1;
+ }
+
+ _batchStatistics.clear();
+ if (_computeBatchStatistics)
+ _batchStatistics.resize(iterations);
+
+ OptimizationAlgorithm::SolverResult result = OptimizationAlgorithm::OK;
+ for (int i=0; i<iterations && ! terminate() && ok; i++){
+ preIteration(i);
+
+ if (_computeBatchStatistics) {
+ G2OBatchStatistics& cstat = _batchStatistics[i];
+ G2OBatchStatistics::setGlobalStats(&cstat);
+ cstat.iteration = i;
+ cstat.numEdges = _activeEdges.size();
+ cstat.numVertices = _activeVertices.size();
+ }
+
+ double ts = get_monotonic_time();
+ result = _algorithm->solve(i, online);
+ ok = ( result == OptimizationAlgorithm::OK );
+
+ bool errorComputed = false;
+ if (_computeBatchStatistics) {
+ computeActiveErrors();
+ errorComputed = true;
+ _batchStatistics[i].chi2 = activeRobustChi2();
+ _batchStatistics[i].timeIteration = get_monotonic_time()-ts;
+ }
+
+ if (verbose()){
+ double dts = get_monotonic_time()-ts;
+ cumTime += dts;
+ if (! errorComputed)
+ computeActiveErrors();
+ cerr << "iteration= " << i
+ << "\t chi2= " << FIXED(activeRobustChi2())
+ << "\t time= " << dts
+ << "\t cumTime= " << cumTime
+ << "\t edges= " << _activeEdges.size();
+ _algorithm->printVerbose(cerr);
+ cerr << endl;
+ }
+ ++cjIterations;
+ postIteration(i);
+ }
+ if (result == OptimizationAlgorithm::Fail) {
+ return 0;
+ }
+ return cjIterations;
+ }
+
+
+ void SparseOptimizer::update(const double* update)
+ {
+ // update the graph by calling oplus on the vertices
+ for (size_t i=0; i < _ivMap.size(); ++i) {
+ OptimizableGraph::Vertex* v= _ivMap[i];
+#ifndef NDEBUG
+ bool hasNan = arrayHasNaN(update, v->dimension());
+ if (hasNan)
+ cerr << __PRETTY_FUNCTION__ << ": Update contains a nan for vertex " << v->id() << endl;
+#endif
+ v->oplus(update);
+ update += v->dimension();
+ }
+ }
+
+ void SparseOptimizer::setComputeBatchStatistics(bool computeBatchStatistics)
+ {
+ if ((_computeBatchStatistics == true) && (computeBatchStatistics == false)) {
+ G2OBatchStatistics::setGlobalStats(0);
+ _batchStatistics.clear();
+ }
+ _computeBatchStatistics = computeBatchStatistics;
+ }
+
+ bool SparseOptimizer::updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset)
+ {
+ std::vector<HyperGraph::Vertex*> newVertices;
+ newVertices.reserve(vset.size());
+ _activeVertices.reserve(_activeVertices.size() + vset.size());
+ _activeEdges.reserve(_activeEdges.size() + eset.size());
+ for (HyperGraph::EdgeSet::iterator it = eset.begin(); it != eset.end(); ++it) {
+ OptimizableGraph::Edge* e = static_cast<OptimizableGraph::Edge*>(*it);
+ if (!e->allVerticesFixed()) _activeEdges.push_back(e);
+ }
+
+ // update the index mapping
+ size_t next = _ivMap.size();
+ for (HyperGraph::VertexSet::iterator it = vset.begin(); it != vset.end(); ++it) {
+ OptimizableGraph::Vertex* v=static_cast<OptimizableGraph::Vertex*>(*it);
+ if (! v->fixed()){
+ if (! v->marginalized()){
+ v->setHessianIndex(next);
+ _ivMap.push_back(v);
+ newVertices.push_back(v);
+ _activeVertices.push_back(v);
+ next++;
+ }
+ else // not supported right now
+ abort();
+ }
+ else {
+ v->setHessianIndex(-1);
+ }
+ }
+
+ //if (newVertices.size() != vset.size())
+ //cerr << __PRETTY_FUNCTION__ << ": something went wrong " << PVAR(vset.size()) << " " << PVAR(newVertices.size()) << endl;
+ return _algorithm->updateStructure(newVertices, eset);
+ }
+
+ void SparseOptimizer::sortVectorContainers()
+ {
+ // sort vector structures to get deterministic ordering based on IDs
+ sort(_activeVertices.begin(), _activeVertices.end(), VertexIDCompare());
+ sort(_activeEdges.begin(), _activeEdges.end(), EdgeIDCompare());
+ }
+
+ void SparseOptimizer::clear() {
+ _ivMap.clear();
+ _activeVertices.clear();
+ _activeEdges.clear();
+ OptimizableGraph::clear();
+ }
+
+ SparseOptimizer::VertexContainer::const_iterator SparseOptimizer::findActiveVertex(const OptimizableGraph::Vertex* v) const
+ {
+ VertexContainer::const_iterator lower = lower_bound(_activeVertices.begin(), _activeVertices.end(), v, VertexIDCompare());
+ if (lower == _activeVertices.end())
+ return _activeVertices.end();
+ if ((*lower) == v)
+ return lower;
+ return _activeVertices.end();
+ }
+
+ SparseOptimizer::EdgeContainer::const_iterator SparseOptimizer::findActiveEdge(const OptimizableGraph::Edge* e) const
+ {
+ EdgeContainer::const_iterator lower = lower_bound(_activeEdges.begin(), _activeEdges.end(), e, EdgeIDCompare());
+ if (lower == _activeEdges.end())
+ return _activeEdges.end();
+ if ((*lower) == e)
+ return lower;
+ return _activeEdges.end();
+ }
+
+ void SparseOptimizer::push(SparseOptimizer::VertexContainer& vlist)
+ {
+ for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+ (*it)->push();
+ }
+
+ void SparseOptimizer::pop(SparseOptimizer::VertexContainer& vlist)
+ {
+ for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+ (*it)->pop();
+ }
+
+ void SparseOptimizer::push(HyperGraph::VertexSet& vlist)
+ {
+ for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it) {
+ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*>(*it);
+ if (v)
+ v->push();
+ else
+ cerr << __FUNCTION__ << ": FATAL PUSH SET" << endl;
+ }
+ }
+
+ void SparseOptimizer::pop(HyperGraph::VertexSet& vlist)
+ {
+ for (HyperGraph::VertexSet::iterator it = vlist.begin(); it != vlist.end(); ++it){
+ OptimizableGraph::Vertex* v = dynamic_cast<OptimizableGraph::Vertex*> (*it);
+ if (v)
+ v->pop();
+ else
+ cerr << __FUNCTION__ << ": FATAL POP SET" << endl;
+ }
+ }
+
+ void SparseOptimizer::discardTop(SparseOptimizer::VertexContainer& vlist)
+ {
+ for (VertexContainer::iterator it = vlist.begin(); it != vlist.end(); ++it)
+ (*it)->discardTop();
+ }
+
+ void SparseOptimizer::setVerbose(bool verbose)
+ {
+ _verbose = verbose;
+ }
+
+ void SparseOptimizer::setAlgorithm(OptimizationAlgorithm* algorithm)
+ {
+ if (_algorithm) // reset the optimizer for the formerly used solver
+ _algorithm->setOptimizer(0);
+ _algorithm = algorithm;
+ if (_algorithm)
+ _algorithm->setOptimizer(this);
+ }
+
+ bool SparseOptimizer::computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices){
+ return _algorithm->computeMarginals(spinv, blockIndices);
+ }
+
+ void SparseOptimizer::setForceStopFlag(bool* flag)
+ {
+ _forceStopFlag=flag;
+ }
+
+ bool SparseOptimizer::removeVertex(HyperGraph::Vertex* v)
+ {
+ OptimizableGraph::Vertex* vv = static_cast<OptimizableGraph::Vertex*>(v);
+ if (vv->hessianIndex() >= 0) {
+ clearIndexMapping();
+ _ivMap.clear();
+ }
+ return HyperGraph::removeVertex(v);
+ }
+
+ bool SparseOptimizer::addComputeErrorAction(HyperGraphAction* action)
+ {
+ std::pair<HyperGraphActionSet::iterator, bool> insertResult = _graphActions[AT_COMPUTEACTIVERROR].insert(action);
+ return insertResult.second;
+ }
+
+ bool SparseOptimizer::removeComputeErrorAction(HyperGraphAction* action)
+ {
+ return _graphActions[AT_COMPUTEACTIVERROR].erase(action) > 0;
+ }
+
+ void SparseOptimizer::push()
+ {
+ push(_activeVertices);
+ }
+
+ void SparseOptimizer::pop()
+ {
+ pop(_activeVertices);
+ }
+
+ void SparseOptimizer::discardTop()
+ {
+ discardTop(_activeVertices);
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.h
new file mode 100644
index 0000000000000000000000000000000000000000..3b09f5a3d5bf723a0669c5cc40d4b7d262e71bbb
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/core/sparse_optimizer.h
@@ -0,0 +1,312 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_GRAPH_OPTIMIZER_CHOL_H_
+#define G2O_GRAPH_OPTIMIZER_CHOL_H_
+
+#include "../stuff/macros.h"
+
+#include "optimizable_graph.h"
+#include "sparse_block_matrix.h"
+#include "batch_stats.h"
+
+#include <map>
+
+namespace g2o {
+
+ // forward declaration
+ class ActivePathCostFunction;
+ class OptimizationAlgorithm;
+ class EstimatePropagatorCost;
+
+ class SparseOptimizer : public OptimizableGraph {
+
+ public:
+ enum {
+ AT_COMPUTEACTIVERROR = OptimizableGraph::AT_NUM_ELEMENTS,
+ AT_NUM_ELEMENTS, // keep as last element
+ };
+
+ friend class ActivePathCostFunction;
+
+ // Attention: _solver & _statistics is own by SparseOptimizer and will be
+ // deleted in its destructor.
+ SparseOptimizer();
+ virtual ~SparseOptimizer();
+
+ // new interface for the optimizer
+ // the old functions will be dropped
+ /**
+ * Initializes the structures for optimizing a portion of the graph specified by a subset of edges.
+ * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
+ * schur complement or to set as fixed during the optimization.
+ * @param eset: the subgraph to be optimized.
+ * @returns false if somethings goes wrong
+ */
+ virtual bool initializeOptimization(HyperGraph::EdgeSet& eset);
+
+ /**
+ * Initializes the structures for optimizing a portion of the graph specified by a subset of vertices.
+ * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
+ * schur complement or to set as fixed during the optimization.
+ * @param vset: the subgraph to be optimized.
+ * @param level: is the level (in multilevel optimization)
+ * @returns false if somethings goes wrong
+ */
+ virtual bool initializeOptimization(HyperGraph::VertexSet& vset, int level=0);
+
+ /**
+ * Initializes the structures for optimizing the whole graph.
+ * Before calling it be sure to invoke marginalized() and fixed() to the vertices you want to include in the
+ * schur complement or to set as fixed during the optimization.
+ * @param level: is the level (in multilevel optimization)
+ * @returns false if somethings goes wrong
+ */
+ virtual bool initializeOptimization(int level=0);
+
+ /**
+ * HACK updating the internal structures for online processing
+ */
+ virtual bool updateInitialization(HyperGraph::VertexSet& vset, HyperGraph::EdgeSet& eset);
+
+ /**
+ * Propagates an initial guess from the vertex specified as origin.
+ * It should be called after initializeOptimization(...), as it relies on the _activeVertices/_edges structures.
+ * It constructs a set of trees starting from the nodes in the graph which are fixed and eligible for preforming optimization.
+ * The trees are constructed by utlizing a cost-function specified.
+ * @param costFunction: the cost function used
+ * @patam maxDistance: the distance where to stop the search
+ */
+ virtual void computeInitialGuess();
+
+ /**
+ * Same as above but using a specific propagator
+ */
+ virtual void computeInitialGuess(EstimatePropagatorCost& propagator);
+
+ /**
+ * sets all vertices to their origin.
+ */
+ virtual void setToOrigin();
+
+
+ /**
+ * starts one optimization run given the current configuration of the graph,
+ * and the current settings stored in the class instance.
+ * It can be called only after initializeOptimization
+ */
+ int optimize(int iterations, bool online = false);
+
+ /**
+ * computes the blocks of the inverse of the specified pattern.
+ * the pattern is given via pairs <row, col> of the blocks in the hessian
+ * @param blockIndices: the pattern
+ * @param spinv: the sparse block matrix with the result
+ * @returns false if the operation is not supported by the solver
+ */
+ bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const std::vector<std::pair<int, int> >& blockIndices);
+
+ /**
+ * computes the inverse of the specified vertex.
+ * @param vertex: the vertex whose state is to be marginalised
+ * @param spinv: the sparse block matrix with the result
+ * @returns false if the operation is not supported by the solver
+ */
+ bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const Vertex* vertex) {
+ if (vertex->hessianIndex() < 0) {
+ return false;
+ }
+ std::vector<std::pair<int, int> > index;
+ index.push_back(std::pair<int, int>(vertex->hessianIndex(), vertex->hessianIndex()));
+ return computeMarginals(spinv, index);
+ }
+
+ /**
+ * computes the inverse of the set specified vertices, assembled into a single covariance matrix.
+ * @param vertex: the pattern
+ * @param spinv: the sparse block matrix with the result
+ * @returns false if the operation is not supported by the solver
+ */
+ bool computeMarginals(SparseBlockMatrix<MatrixXd>& spinv, const VertexContainer& vertices) {
+ std::vector<std::pair<int, int> > indices;
+ for (VertexContainer::const_iterator it = vertices.begin(); it != vertices.end(); ++it) {
+ indices.push_back(std::pair<int, int>((*it)->hessianIndex(),(*it)->hessianIndex()));
+ }
+ return computeMarginals(spinv, indices);
+ }
+
+ //! finds a gauge in the graph to remove the undefined dof.
+ // The gauge should be fixed() and then the optimization can work (if no additional dof are in
+ // the system. The default implementation returns a node with maximum dimension.
+ virtual Vertex* findGauge();
+
+ bool gaugeFreedom();
+
+ /**returns the cached chi2 of the active portion of the graph*/
+ double activeChi2() const;
+ /**
+ * returns the cached chi2 of the active portion of the graph.
+ * In contrast to activeChi2() this functions considers the weighting
+ * of the error according to the robustification of the error functions.
+ */
+ double activeRobustChi2() const;
+
+ //! verbose information during optimization
+ bool verbose() const {return _verbose;}
+ void setVerbose(bool verbose);
+
+ /**
+ * sets a variable checked at every iteration to force a user stop. The iteration exits when the variable is true;
+ */
+ void setForceStopFlag(bool* flag);
+ bool* forceStopFlag() const { return _forceStopFlag;};
+
+ //! if external stop flag is given, return its state. False otherwise
+ bool terminate() {return _forceStopFlag ? (*_forceStopFlag) : false; }
+
+ //! the index mapping of the vertices
+ const VertexContainer& indexMapping() const {return _ivMap;}
+ //! the vertices active in the current optimization
+ const VertexContainer& activeVertices() const { return _activeVertices;}
+ //! the edges active in the current optimization
+ const EdgeContainer& activeEdges() const { return _activeEdges;}
+
+ /**
+ * Remove a vertex. If the vertex is contained in the currently active set
+ * of vertices, then the internal temporary structures are cleaned, e.g., the index
+ * mapping is erased. In case you need the index mapping for manipulating the
+ * graph, you have to store it in your own copy.
+ */
+ virtual bool removeVertex(HyperGraph::Vertex* v);
+
+ /**
+ * search for an edge in _activeVertices and return the iterator pointing to it
+ * getActiveVertices().end() if not found
+ */
+ VertexContainer::const_iterator findActiveVertex(const OptimizableGraph::Vertex* v) const;
+ /**
+ * search for an edge in _activeEdges and return the iterator pointing to it
+ * getActiveEdges().end() if not found
+ */
+ EdgeContainer::const_iterator findActiveEdge(const OptimizableGraph::Edge* e) const;
+
+ //! the solver used by the optimizer
+ const OptimizationAlgorithm* algorithm() const { return _algorithm;}
+ OptimizationAlgorithm* solver() { return _algorithm;}
+ void setAlgorithm(OptimizationAlgorithm* algorithm);
+
+ //! push the estimate of a subset of the variables onto a stack
+ void push(SparseOptimizer::VertexContainer& vlist);
+ //! push the estimate of a subset of the variables onto a stack
+ void push(HyperGraph::VertexSet& vlist);
+ //! push all the active vertices onto a stack
+ void push();
+ //! pop (restore) the estimate a subset of the variables from the stack
+ void pop(SparseOptimizer::VertexContainer& vlist);
+ //! pop (restore) the estimate a subset of the variables from the stack
+ void pop(HyperGraph::VertexSet& vlist);
+ //! pop (restore) the estimate of the active vertices from the stack
+ void pop();
+
+ //! ignore the latest stored element on the stack, remove it from the stack but do not restore the estimate
+ void discardTop(SparseOptimizer::VertexContainer& vlist);
+ //! same as above, but for the active vertices
+ void discardTop();
+ using OptimizableGraph::discardTop;
+
+ /**
+ * clears the graph, and polishes some intermediate structures
+ * Note that this only removes nodes / edges. Parameters can be removed
+ * with clearParameters().
+ */
+ virtual void clear();
+
+ /**
+ * computes the error vectors of all edges in the activeSet, and caches them
+ */
+ void computeActiveErrors();
+
+ /**
+ * Linearizes the system by computing the Jacobians for the nodes
+ * and edges in the graph
+ */
+ G2O_ATTRIBUTE_DEPRECATED(void linearizeSystem())
+ {
+ // nothing needed, linearization is now done inside the solver
+ }
+
+ /**
+ * update the estimate of the active vertices
+ * @param update: the double vector containing the stacked
+ * elements of the increments on the vertices.
+ */
+ void update(const double* update);
+
+ /**
+ returns the set of batch statistics about the optimisation
+ */
+ const BatchStatisticsContainer& batchStatistics() const { return _batchStatistics;}
+ /**
+ returns the set of batch statistics about the optimisation
+ */
+ BatchStatisticsContainer& batchStatistics() { return _batchStatistics;}
+
+ void setComputeBatchStatistics(bool computeBatchStatistics);
+
+ bool computeBatchStatistics() const { return _computeBatchStatistics;}
+
+ /**** callbacks ****/
+ //! add an action to be executed before the error vectors are computed
+ bool addComputeErrorAction(HyperGraphAction* action);
+ //! remove an action that should no longer be execured before computing the error vectors
+ bool removeComputeErrorAction(HyperGraphAction* action);
+
+
+
+ protected:
+ bool* _forceStopFlag;
+ bool _verbose;
+
+ VertexContainer _ivMap;
+ VertexContainer _activeVertices; ///< sorted according to VertexIDCompare
+ EdgeContainer _activeEdges; ///< sorted according to EdgeIDCompare
+
+ void sortVectorContainers();
+
+ OptimizationAlgorithm* _algorithm;
+
+ /**
+ * builds the mapping of the active vertices to the (block) row / column in the Hessian
+ */
+ bool buildIndexMapping(SparseOptimizer::VertexContainer& vlist);
+ void clearIndexMapping();
+
+ BatchStatisticsContainer _batchStatistics; ///< global statistics of the optimizer, e.g., timing, num-non-zeros
+ bool _computeBatchStatistics;
+ };
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h
new file mode 100644
index 0000000000000000000000000000000000000000..b34674c89c4df55a698c8b19dcfa1eb7bae3eee0
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_dense.h
@@ -0,0 +1,125 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// Copyright (C) 2012 R. Kümmerle
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_DENSE_H
+#define G2O_LINEAR_SOLVER_DENSE_H
+
+#include "../core/linear_solver.h"
+#include "../core/batch_stats.h"
+
+#include <vector>
+#include <utility>
+#include<Eigen/Core>
+#include<Eigen/Cholesky>
+
+
+namespace g2o {
+
+ /**
+ * \brief linear solver using dense cholesky decomposition
+ */
+ template <typename MatrixType>
+ class LinearSolverDense : public LinearSolver<MatrixType>
+ {
+ public:
+ LinearSolverDense() :
+ LinearSolver<MatrixType>(),
+ _reset(true)
+ {
+ }
+
+ virtual ~LinearSolverDense()
+ {
+ }
+
+ virtual bool init()
+ {
+ _reset = true;
+ return true;
+ }
+
+ bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
+ {
+ int n = A.cols();
+ int m = A.cols();
+
+ Eigen::MatrixXd& H = _H;
+ if (H.cols() != n) {
+ H.resize(n, m);
+ _reset = true;
+ }
+ if (_reset) {
+ _reset = false;
+ H.setZero();
+ }
+
+ // copy the sparse block matrix into a dense matrix
+ int c_idx = 0;
+ for (size_t i = 0; i < A.blockCols().size(); ++i) {
+ int c_size = A.colsOfBlock(i);
+ assert(c_idx == A.colBaseOfBlock(i) && "mismatch in block indices");
+
+ const typename SparseBlockMatrix<MatrixType>::IntBlockMap& col = A.blockCols()[i];
+ if (col.size() > 0) {
+ typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it;
+ for (it = col.begin(); it != col.end(); ++it) {
+ int r_idx = A.rowBaseOfBlock(it->first);
+ // only the upper triangular block is processed
+ if (it->first <= (int)i) {
+ int r_size = A.rowsOfBlock(it->first);
+ H.block(r_idx, c_idx, r_size, c_size) = *(it->second);
+ if (r_idx != c_idx) // write the lower triangular block
+ H.block(c_idx, r_idx, c_size, r_size) = it->second->transpose();
+ }
+ }
+ }
+
+ c_idx += c_size;
+ }
+
+ // solving via Cholesky decomposition
+ Eigen::VectorXd::MapType xvec(x, m);
+ Eigen::VectorXd::ConstMapType bvec(b, n);
+ _cholesky.compute(H);
+ if (_cholesky.isPositive()) {
+ xvec = _cholesky.solve(bvec);
+ return true;
+ }
+ return false;
+ }
+
+ protected:
+ bool _reset;
+ Eigen::MatrixXd _H;
+ Eigen::LDLT<Eigen::MatrixXd> _cholesky;
+
+ };
+
+
+}// end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c2789afee7a5eb964f74cf2bc4318be4dafab06
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h
@@ -0,0 +1,237 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_LINEAR_SOLVER_EIGEN_H
+#define G2O_LINEAR_SOLVER_EIGEN_H
+
+#include <Eigen/Sparse>
+#include <Eigen/SparseCholesky>
+
+#include "../core/linear_solver.h"
+#include "../core/batch_stats.h"
+#include "../stuff/timeutil.h"
+
+#include "../core/eigen_types.h"
+
+#include <iostream>
+#include <vector>
+
+namespace g2o {
+
+/**
+ * \brief linear solver which uses the sparse Cholesky solver from Eigen
+ *
+ * Has no dependencies except Eigen. Hence, should compile almost everywhere
+ * without to much issues. Performance should be similar to CSparse, I guess.
+ */
+template <typename MatrixType>
+class LinearSolverEigen: public LinearSolver<MatrixType>
+{
+ public:
+ typedef Eigen::SparseMatrix<double, Eigen::ColMajor> SparseMatrix;
+ typedef Eigen::Triplet<double> Triplet;
+ typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic> PermutationMatrix;
+ /**
+ * \brief Sub-classing Eigen's SimplicialLDLT to perform ordering with a given ordering
+ */
+ class CholeskyDecomposition : public Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>
+ {
+ public:
+ CholeskyDecomposition() : Eigen::SimplicialLDLT<SparseMatrix, Eigen::Upper>() {}
+ using Eigen::SimplicialLDLT< SparseMatrix, Eigen::Upper>::analyzePattern_preordered;
+
+ void analyzePatternWithPermutation(SparseMatrix& a, const PermutationMatrix& permutation)
+ {
+ m_Pinv = permutation;
+ m_P = permutation.inverse();
+ int size = a.cols();
+ SparseMatrix ap(size, size);
+ ap.selfadjointView<Eigen::Upper>() = a.selfadjointView<UpLo>().twistedBy(m_P);
+ analyzePattern_preordered(ap, true);
+ }
+ };
+
+ public:
+ LinearSolverEigen() :
+ LinearSolver<MatrixType>(),
+ _init(true), _blockOrdering(false), _writeDebug(false)
+ {
+ }
+
+ virtual ~LinearSolverEigen()
+ {
+ }
+
+ virtual bool init()
+ {
+ _init = true;
+ return true;
+ }
+
+ bool solve(const SparseBlockMatrix<MatrixType>& A, double* x, double* b)
+ {
+ if (_init)
+ _sparseMatrix.resize(A.rows(), A.cols());
+ fillSparseMatrix(A, !_init);
+ if (_init) // compute the symbolic composition once
+ computeSymbolicDecomposition(A);
+ _init = false;
+
+ double t=get_monotonic_time();
+ _cholesky.factorize(_sparseMatrix);
+ if (_cholesky.info() != Eigen::Success) { // the matrix is not positive definite
+ if (_writeDebug) {
+ std::cerr << "Cholesky failure, writing debug.txt (Hessian loadable by Octave)" << std::endl;
+ A.writeOctave("debug.txt");
+ }
+ return false;
+ }
+
+ // Solving the system
+ VectorXD::MapType xx(x, _sparseMatrix.cols());
+ VectorXD::ConstMapType bb(b, _sparseMatrix.cols());
+ xx = _cholesky.solve(bb);
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats) {
+ globalStats->timeNumericDecomposition = get_monotonic_time() - t;
+ globalStats->choleskyNNZ = _cholesky.matrixL().nestedExpression().nonZeros() + _sparseMatrix.cols(); // the elements of D
+ }
+
+ return true;
+ }
+
+ //! do the AMD ordering on the blocks or on the scalar matrix
+ bool blockOrdering() const { return _blockOrdering;}
+ void setBlockOrdering(bool blockOrdering) { _blockOrdering = blockOrdering;}
+
+ //! write a debug dump of the system matrix if it is not SPD in solve
+ virtual bool writeDebug() const { return _writeDebug;}
+ virtual void setWriteDebug(bool b) { _writeDebug = b;}
+
+ protected:
+ bool _init;
+ bool _blockOrdering;
+ bool _writeDebug;
+ SparseMatrix _sparseMatrix;
+ CholeskyDecomposition _cholesky;
+
+ /**
+ * compute the symbolic decompostion of the matrix only once.
+ * Since A has the same pattern in all the iterations, we only
+ * compute the fill-in reducing ordering once and re-use for all
+ * the following iterations.
+ */
+ void computeSymbolicDecomposition(const SparseBlockMatrix<MatrixType>& A)
+ {
+ double t=get_monotonic_time();
+ if (! _blockOrdering) {
+ _cholesky.analyzePattern(_sparseMatrix);
+ } else {
+ // block ordering with the Eigen Interface
+ // This is really ugly currently, as it calls internal functions from Eigen
+ // and modifies the SparseMatrix class
+ Eigen::PermutationMatrix<Eigen::Dynamic,Eigen::Dynamic> blockP;
+ {
+ // prepare a block structure matrix for calling AMD
+ std::vector<Triplet> triplets;
+ for (size_t c = 0; c < A.blockCols().size(); ++c){
+ const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
+ const int& r = it->first;
+ if (r > static_cast<int>(c)) // only upper triangle
+ break;
+ triplets.push_back(Triplet(r, c, 0.));
+ }
+ }
+
+ // call the AMD ordering on the block matrix.
+ // Relies on Eigen's internal stuff, probably bad idea
+ SparseMatrix auxBlockMatrix(A.blockCols().size(), A.blockCols().size());
+ auxBlockMatrix.setFromTriplets(triplets.begin(), triplets.end());
+ typename CholeskyDecomposition::CholMatrixType C;
+ C = auxBlockMatrix.selfadjointView<Eigen::Upper>();
+ Eigen::internal::minimum_degree_ordering(C, blockP);
+ }
+
+ int rows = A.rows();
+ assert(rows == A.cols() && "Matrix A is not square");
+
+ // Adapt the block permutation to the scalar matrix
+ PermutationMatrix scalarP;
+ scalarP.resize(rows);
+ int scalarIdx = 0;
+ for (int i = 0; i < blockP.size(); ++i) {
+ const int& p = blockP.indices()(i);
+ int base = A.colBaseOfBlock(p);
+ int nCols = A.colsOfBlock(p);
+ for (int j = 0; j < nCols; ++j)
+ scalarP.indices()(scalarIdx++) = base++;
+ }
+ assert(scalarIdx == rows && "did not completely fill the permutation matrix");
+ // analyze with the scalar permutation
+ _cholesky.analyzePatternWithPermutation(_sparseMatrix, scalarP);
+
+ }
+ G2OBatchStatistics* globalStats = G2OBatchStatistics::globalStats();
+ if (globalStats)
+ globalStats->timeSymbolicDecomposition = get_monotonic_time() - t;
+ }
+
+ void fillSparseMatrix(const SparseBlockMatrix<MatrixType>& A, bool onlyValues)
+ {
+ if (onlyValues) {
+ A.fillCCS(_sparseMatrix.valuePtr(), true);
+ } else {
+
+ // create from triplet structure
+ std::vector<Triplet> triplets;
+ triplets.reserve(A.nonZeros());
+ for (size_t c = 0; c < A.blockCols().size(); ++c) {
+ int colBaseOfBlock = A.colBaseOfBlock(c);
+ const typename SparseBlockMatrix<MatrixType>::IntBlockMap& column = A.blockCols()[c];
+ for (typename SparseBlockMatrix<MatrixType>::IntBlockMap::const_iterator it = column.begin(); it != column.end(); ++it) {
+ int rowBaseOfBlock = A.rowBaseOfBlock(it->first);
+ const MatrixType& m = *(it->second);
+ for (int cc = 0; cc < m.cols(); ++cc) {
+ int aux_c = colBaseOfBlock + cc;
+ for (int rr = 0; rr < m.rows(); ++rr) {
+ int aux_r = rowBaseOfBlock + rr;
+ if (aux_r > aux_c)
+ break;
+ triplets.push_back(Triplet(aux_r, aux_c, m(rr, cc)));
+ }
+ }
+ }
+ }
+ _sparseMatrix.setFromTriplets(triplets.begin(), triplets.end());
+
+ }
+ }
+};
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/color_macros.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/color_macros.h
new file mode 100644
index 0000000000000000000000000000000000000000..7115bbeacc03988e54d363e44b8ff76bfcc3bda1
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/color_macros.h
@@ -0,0 +1,65 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_COLOR_MACROS_H
+#define G2O_COLOR_MACROS_H
+
+// font attributes
+#define FT_BOLD "\033[1m"
+#define FT_UNDERLINE "\033[4m"
+
+//background color
+#define BG_BLACK "\033[40m"
+#define BG_RED "\033[41m"
+#define BG_GREEN "\033[42m"
+#define BG_YELLOW "\033[43m"
+#define BG_LIGHTBLUE "\033[44m"
+#define BG_MAGENTA "\033[45m"
+#define BG_BLUE "\033[46m"
+#define BG_WHITE "\033[47m"
+
+// font color
+#define CL_BLACK(s) "\033[30m" << s << "\033[0m"
+#define CL_RED(s) "\033[31m" << s << "\033[0m"
+#define CL_GREEN(s) "\033[32m" << s << "\033[0m"
+#define CL_YELLOW(s) "\033[33m" << s << "\033[0m"
+#define CL_LIGHTBLUE(s) "\033[34m" << s << "\033[0m"
+#define CL_MAGENTA(s) "\033[35m" << s << "\033[0m"
+#define CL_BLUE(s) "\033[36m" << s << "\033[0m"
+#define CL_WHITE(s) "\033[37m" << s << "\033[0m"
+
+#define FG_BLACK "\033[30m"
+#define FG_RED "\033[31m"
+#define FG_GREEN "\033[32m"
+#define FG_YELLOW "\033[33m"
+#define FG_LIGHTBLUE "\033[34m"
+#define FG_MAGENTA "\033[35m"
+#define FG_BLUE "\033[36m"
+#define FG_WHITE "\033[37m"
+
+#define FG_NORM "\033[0m"
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/macros.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/macros.h
new file mode 100644
index 0000000000000000000000000000000000000000..be4a24518fa06d8aaf6ff6ab2939b4bd7c2245e6
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/macros.h
@@ -0,0 +1,134 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MACROS_H
+#define G2O_MACROS_H
+
+#ifndef DEG2RAD
+#define DEG2RAD(x) ((x) * 0.01745329251994329575)
+#endif
+
+#ifndef RAD2DEG
+#define RAD2DEG(x) ((x) * 57.29577951308232087721)
+#endif
+
+// GCC the one and only
+#if defined(__GNUC__)
+# define G2O_ATTRIBUTE_CONSTRUCTOR(func) \
+ static void func(void)__attribute__ ((constructor)); \
+ static void func(void)
+
+# define G2O_ATTRIBUTE_UNUSED __attribute__((unused))
+# define G2O_ATTRIBUTE_FORMAT12 __attribute__ ((format (printf, 1, 2)))
+# define G2O_ATTRIBUTE_FORMAT23 __attribute__ ((format (printf, 2, 3)))
+# define G2O_ATTRIBUTE_WARNING(func) func __attribute__((warning))
+# define G2O_ATTRIBUTE_DEPRECATED(func) func __attribute__((deprecated))
+
+#ifdef ANDROID
+# define g2o_isnan(x) isnan(x)
+# define g2o_isinf(x) isinf(x)
+# define g2o_isfinite(x) isfinite(x)
+#else
+# define g2o_isnan(x) std::isnan(x)
+# define g2o_isinf(x) std::isinf(x)
+# define g2o_isfinite(x) std::isfinite(x)
+#endif
+
+// MSVC on Windows
+#elif defined _MSC_VER
+# define __PRETTY_FUNCTION__ __FUNCTION__
+
+/**
+Modified by Mark Pupilli from:
+
+ "Initializer/finalizer sample for MSVC and GCC.
+ 2010 Joe Lowe. Released into the public domain."
+
+ "For MSVC, places a ptr to the function in the user initializer section (.CRT$XCU), basically the same thing the compiler does for the constructor calls for static C++ objects. For GCC, uses a constructor attribute."
+
+ (As posted on Stack OVerflow)
+*/
+# define G2O_ATTRIBUTE_CONSTRUCTOR(f) \
+ __pragma(section(".CRT$XCU",read)) \
+ static void __cdecl f(void); \
+ __declspec(allocate(".CRT$XCU")) void (__cdecl*f##_)(void) = f; \
+ static void __cdecl f(void)
+
+# define G2O_ATTRIBUTE_UNUSED
+# define G2O_ATTRIBUTE_FORMAT12
+# define G2O_ATTRIBUTE_FORMAT23
+# define G2O_ATTRIBUTE_WARNING(func) func
+# define G2O_ATTRIBUTE_DEPRECATED(func) func
+
+#include <float.h>
+
+# define g2o_isnan(x) _isnan(x)
+# define g2o_isinf(x) (_finite(x) == 0)
+# define g2o_isfinite(x) (_finite(x) != 0)
+
+// unknown compiler
+#else
+# ifndef __PRETTY_FUNCTION__
+# define __PRETTY_FUNCTION__ ""
+# endif
+# define G2O_ATTRIBUTE_CONSTRUCTOR(func) func
+# define G2O_ATTRIBUTE_UNUSED
+# define G2O_ATTRIBUTE_FORMAT12
+# define G2O_ATTRIBUTE_FORMAT23
+# define G2O_ATTRIBUTE_WARNING(func) func
+# define G2O_ATTRIBUTE_DEPRECATED(func) func
+
+#include <math.h>
+#define g2o_isnan(x) isnan(x)
+#define g2o_isinf(x) isinf(x)
+#define g2o_isfinite(x) isfinite(x)
+
+#endif
+
+// some macros that are only useful for c++
+#ifdef __cplusplus
+
+#define G2O_FSKIP_LINE(f) \
+ {char c=' ';while(c != '\n' && f.good() && !(f).eof()) (f).get(c);}
+
+#ifndef PVAR
+ #define PVAR(s) \
+ #s << " = " << (s) << std::flush
+#endif
+
+#ifndef PVARA
+#define PVARA(s) \
+ #s << " = " << RAD2DEG(s) << "deg" << std::flush
+#endif
+
+#ifndef FIXED
+#define FIXED(s) \
+ std::fixed << s << std::resetiosflags(std::ios_base::fixed)
+#endif
+
+#endif // __cplusplus
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/misc.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/misc.h
new file mode 100644
index 0000000000000000000000000000000000000000..4fa4ff331f3f0001cfef8364bd268125c9c79d01
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/misc.h
@@ -0,0 +1,206 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_STUFF_MISC_H
+#define G2O_STUFF_MISC_H
+
+#include "macros.h"
+#include <cmath>
+
+#ifndef M_PI
+#define M_PI 3.14159265358979323846
+#endif
+
+/** @addtogroup utils **/
+// @{
+
+/** \file misc.h
+ * \brief some general case utility functions
+ *
+ * This file specifies some general case utility functions
+ **/
+
+namespace g2o {
+
+/**
+ * return the square value
+ */
+template <typename T>
+inline T square(T x)
+{
+ return x*x;
+}
+
+/**
+ * return the hypot of x and y
+ */
+template <typename T>
+inline T hypot(T x, T y)
+{
+ return (T) (sqrt(x*x + y*y));
+}
+
+/**
+ * return the squared hypot of x and y
+ */
+template <typename T>
+inline T hypot_sqr(T x, T y)
+{
+ return x*x + y*y;
+}
+
+/**
+ * convert from degree to radian
+ */
+inline double deg2rad(double degree)
+{
+ return degree * 0.01745329251994329576;
+}
+
+/**
+ * convert from radian to degree
+ */
+inline double rad2deg(double rad)
+{
+ return rad * 57.29577951308232087721;
+}
+
+/**
+ * normalize the angle
+ */
+inline double normalize_theta(double theta)
+{
+ if (theta >= -M_PI && theta < M_PI)
+ return theta;
+
+ double multiplier = floor(theta / (2*M_PI));
+ theta = theta - multiplier*2*M_PI;
+ if (theta >= M_PI)
+ theta -= 2*M_PI;
+ if (theta < -M_PI)
+ theta += 2*M_PI;
+
+ return theta;
+}
+
+/**
+ * inverse of an angle, i.e., +180 degree
+ */
+inline double inverse_theta(double th)
+{
+ return normalize_theta(th + M_PI);
+}
+
+/**
+ * average two angles
+ */
+inline double average_angle(double theta1, double theta2)
+{
+ double x, y;
+
+ x = cos(theta1) + cos(theta2);
+ y = sin(theta1) + sin(theta2);
+ if(x == 0 && y == 0)
+ return 0;
+ else
+ return std::atan2(y, x);
+}
+
+/**
+ * sign function.
+ * @return the sign of x. +1 for x > 0, -1 for x < 0, 0 for x == 0
+ */
+template <typename T>
+inline int sign(T x)
+{
+ if (x > 0)
+ return 1;
+ else if (x < 0)
+ return -1;
+ else
+ return 0;
+}
+
+/**
+ * clamp x to the interval [l, u]
+ */
+template <typename T>
+inline T clamp(T l, T x, T u)
+{
+ if (x < l)
+ return l;
+ if (x > u)
+ return u;
+ return x;
+}
+
+/**
+ * wrap x to be in the interval [l, u]
+ */
+template <typename T>
+inline T wrap(T l, T x, T u)
+{
+ T intervalWidth = u - l;
+ while (x < l)
+ x += intervalWidth;
+ while (x > u)
+ x -= intervalWidth;
+ return x;
+}
+
+/**
+ * tests whether there is a NaN in the array
+ */
+inline bool arrayHasNaN(const double* array, int size, int* nanIndex = 0)
+{
+ for (int i = 0; i < size; ++i)
+ if (g2o_isnan(array[i])) {
+ if (nanIndex)
+ *nanIndex = i;
+ return true;
+ }
+ return false;
+}
+
+/**
+ * The following two functions are used to force linkage with static libraries.
+ */
+extern "C"
+{
+ typedef void (* ForceLinkFunction) (void);
+}
+
+struct ForceLinker
+{
+ ForceLinker(ForceLinkFunction function) { (function)(); }
+};
+
+
+} // end namespace
+
+// @}
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.c b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.c
new file mode 100644
index 0000000000000000000000000000000000000000..85f936897e67232ca6ef4894d7586d63d15d5491
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.c
@@ -0,0 +1,64 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "os_specific.h"
+
+#ifdef WINDOWS
+
+int vasprintf(char** strp, const char* fmt, va_list ap)
+{
+ int n;
+ int size = 100;
+ char* p;
+ char* np;
+
+ if ((p = (char*)malloc(size * sizeof(char))) == NULL)
+ return -1;
+
+ while (1) {
+#ifdef _MSC_VER
+ n = vsnprintf_s(p, size, size - 1, fmt, ap);
+#else
+ n = vsnprintf(p, size, fmt, ap);
+#endif
+ if (n > -1 && n < size) {
+ *strp = p;
+ return n;
+ }
+ if (n > -1)
+ size = n+1;
+ else
+ size *= 2;
+ if ((np = (char*)realloc (p, size * sizeof(char))) == NULL) {
+ free(p);
+ return -1;
+ } else
+ p = np;
+ }
+}
+
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.h
new file mode 100644
index 0000000000000000000000000000000000000000..89dd0cccf93f1930a3d253088851128e5772bdcd
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/os_specific.h
@@ -0,0 +1,56 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_OS_SPECIFIC_HH_
+#define G2O_OS_SPECIFIC_HH_
+
+#ifdef WINDOWS
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#ifndef _WINDOWS
+#include <sys/time.h>
+#endif
+#define drand48() ((double) rand()/(double)RAND_MAX)
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int vasprintf(char** strp, const char* fmt, va_list ap);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+#ifdef UNIX
+#include <sys/time.h>
+// nothing to do on real operating systems
+#endif
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f13016e1a93a8eb9a844bb655dadb623f3decfcb
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.cpp
@@ -0,0 +1,105 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "property.h"
+
+#include <vector>
+#include <iostream>
+
+#include "macros.h"
+
+#include "string_tools.h"
+using namespace std;
+
+namespace g2o {
+
+ BaseProperty::BaseProperty(const std::string name_) :_name(name_){
+ }
+
+ BaseProperty::~BaseProperty(){}
+
+ bool PropertyMap::addProperty(BaseProperty* p) {
+ std::pair<PropertyMapIterator,bool> result = insert(make_pair(p->name(), p));
+ return result.second;
+ }
+
+ bool PropertyMap::eraseProperty(const std::string& name) {
+ PropertyMapIterator it=find(name);
+ if (it==end())
+ return false;
+ delete it->second;
+ erase(it);
+ return true;
+ }
+
+ PropertyMap::~PropertyMap() {
+ for (PropertyMapIterator it=begin(); it!=end(); it++){
+ if (it->second)
+ delete it->second;
+ }
+ }
+
+ bool PropertyMap::updatePropertyFromString(const std::string& name, const std::string& value)
+ {
+ PropertyMapIterator it = find(name);
+ if (it == end())
+ return false;
+ it->second->fromString(value);
+ return true;
+ }
+
+ void PropertyMap::writeToCSV(std::ostream& os) const
+ {
+ for (PropertyMapConstIterator it=begin(); it!=end(); it++){
+ BaseProperty* p =it->second;
+ os << p->name() << ", ";
+ }
+ os << std::endl;
+ for (PropertyMapConstIterator it=begin(); it!=end(); it++){
+ BaseProperty* p =it->second;
+ os << p->toString() << ", ";
+ }
+ os << std::endl;
+ }
+
+ bool PropertyMap::updateMapFromString(const std::string& values)
+ {
+ bool status = true;
+ vector<string> valuesMap = strSplit(values, ",");
+ for (size_t i = 0; i < valuesMap.size(); ++i) {
+ vector<string> m = strSplit(valuesMap[i], "=");
+ if (m.size() != 2) {
+ cerr << __PRETTY_FUNCTION__ << ": unable to extract name=value pair from " << valuesMap[i] << endl;
+ continue;
+ }
+ string name = trim(m[0]);
+ string value = trim(m[1]);
+ status = status && updatePropertyFromString(name, value);
+ }
+ return status;
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b638780d5993d9126a0e1b057f515ac4d81ddd1
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/property.h
@@ -0,0 +1,158 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_PROPERTY_H_
+#define G2O_PROPERTY_H_
+
+#include <string>
+#include <map>
+#include <sstream>
+
+#include "string_tools.h"
+
+namespace g2o {
+
+ class BaseProperty {
+ public:
+ BaseProperty(const std::string name_);
+ virtual ~BaseProperty();
+ const std::string& name() {return _name;}
+ virtual std::string toString() const = 0;
+ virtual bool fromString(const std::string& s) = 0;
+ protected:
+ std::string _name;
+ };
+
+ template <typename T>
+ class Property: public BaseProperty {
+ public:
+ typedef T ValueType;
+ Property(const std::string& name_): BaseProperty(name_){}
+ Property(const std::string& name_, const T& v): BaseProperty(name_), _value(v){}
+ void setValue(const T& v) {_value = v; }
+ const T& value() const {return _value;}
+ virtual std::string toString() const
+ {
+ std::stringstream sstr;
+ sstr << _value;
+ return sstr.str();
+ }
+ virtual bool fromString(const std::string& s)
+ {
+ bool status = convertString(s, _value);
+ return status;
+ }
+ protected:
+ T _value;
+ };
+
+ /**
+ * \brief a collection of properties mapping from name to the property itself
+ */
+ class PropertyMap : protected std::map<std::string, BaseProperty*>
+ {
+ public:
+ typedef std::map<std::string, BaseProperty*> BaseClass;
+ typedef BaseClass::iterator PropertyMapIterator;
+ typedef BaseClass::const_iterator PropertyMapConstIterator;
+
+ ~PropertyMap();
+
+ /**
+ * add a property to the map
+ */
+ bool addProperty(BaseProperty* p);
+
+ /**
+ * remove a property from the map
+ */
+ bool eraseProperty(const std::string& name_);
+
+ /**
+ * return a property by its name
+ */
+ template <typename P>
+ P* getProperty(const std::string& name_)
+ {
+ PropertyMapIterator it=find(name_);
+ if (it==end())
+ return 0;
+ return dynamic_cast<P*>(it->second);
+ }
+ template <typename P>
+ const P* getProperty(const std::string& name_) const
+ {
+ PropertyMapConstIterator it=find(name_);
+ if (it==end())
+ return 0;
+ return dynamic_cast<P*>(it->second);
+ }
+
+ /**
+ * create a property and insert it
+ */
+ template <typename P>
+ P* makeProperty(const std::string& name_, const typename P::ValueType& v)
+ {
+ PropertyMapIterator it=find(name_);
+ if (it==end()){
+ P* p=new P(name_, v);
+ addProperty(p);
+ return p;
+ } else
+ return dynamic_cast<P*>(it->second);
+ }
+
+ /**
+ * update a specfic property with a new value
+ * @return true if the params is stored and update was carried out
+ */
+ bool updatePropertyFromString(const std::string& name, const std::string& value);
+
+ /**
+ * update the map based on a name=value string, e.g., name1=val1,name2=val2
+ * @return true, if it was possible to update all parameters
+ */
+ bool updateMapFromString(const std::string& values);
+
+ void writeToCSV(std::ostream& os) const;
+
+ using BaseClass::size;
+ using BaseClass::begin;
+ using BaseClass::end;
+ using BaseClass::iterator;
+ using BaseClass::const_iterator;
+
+ };
+
+ typedef Property<int> IntProperty;
+ typedef Property<bool> BoolProperty;
+ typedef Property<float> FloatProperty;
+ typedef Property<double> DoubleProperty;
+ typedef Property<std::string> StringProperty;
+
+} // end namespace
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a4f60a277676786a1a363543dfa2814aa070386
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.cpp
@@ -0,0 +1,185 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "string_tools.h"
+#include "os_specific.h"
+#include "macros.h"
+
+#include <cctype>
+#include <string>
+#include <cstdarg>
+#include <cstring>
+#include <algorithm>
+#include <cstdio>
+#include <iostream>
+#include <iterator>
+
+#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)
+#include <wordexp.h>
+#endif
+
+namespace g2o {
+
+using namespace std;
+
+std::string trim(const std::string& s)
+{
+ if(s.length() == 0)
+ return s;
+ string::size_type b = s.find_first_not_of(" \t\n");
+ string::size_type e = s.find_last_not_of(" \t\n");
+ if(b == string::npos)
+ return "";
+ return std::string(s, b, e - b + 1);
+}
+
+std::string trimLeft(const std::string& s)
+{
+ if(s.length() == 0)
+ return s;
+ string::size_type b = s.find_first_not_of(" \t\n");
+ string::size_type e = s.length() - 1;
+ if(b == string::npos)
+ return "";
+ return std::string(s, b, e - b + 1);
+}
+
+std::string trimRight(const std::string& s)
+{
+ if(s.length() == 0)
+ return s;
+ string::size_type b = 0;
+ string::size_type e = s.find_last_not_of(" \t\n");
+ if(b == string::npos)
+ return "";
+ return std::string(s, b, e - b + 1);
+}
+
+std::string strToLower(const std::string& s)
+{
+ string ret;
+ std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::tolower);
+ return ret;
+}
+
+std::string strToUpper(const std::string& s)
+{
+ string ret;
+ std::transform(s.begin(), s.end(), back_inserter(ret), (int(*)(int)) std::toupper);
+ return ret;
+}
+
+std::string formatString(const char* fmt, ...)
+{
+ char* auxPtr = NULL;
+ va_list arg_list;
+ va_start(arg_list, fmt);
+ int numChar = vasprintf(&auxPtr, fmt, arg_list);
+ va_end(arg_list);
+ string retString;
+ if (numChar != -1)
+ retString = auxPtr;
+ else {
+ cerr << __PRETTY_FUNCTION__ << ": Error while allocating memory" << endl;
+ }
+ free(auxPtr);
+ return retString;
+}
+
+int strPrintf(std::string& str, const char* fmt, ...)
+{
+ char* auxPtr = NULL;
+ va_list arg_list;
+ va_start(arg_list, fmt);
+ int numChars = vasprintf(&auxPtr, fmt, arg_list);
+ va_end(arg_list);
+ str = auxPtr;
+ free(auxPtr);
+ return numChars;
+}
+
+std::string strExpandFilename(const std::string& filename)
+{
+#if (defined (UNIX) || defined(CYGWIN)) && !defined(ANDROID)
+ string result = filename;
+ wordexp_t p;
+
+ wordexp(filename.c_str(), &p, 0);
+ if(p.we_wordc > 0) {
+ result = p.we_wordv[0];
+ }
+ wordfree(&p);
+ return result;
+#else
+ (void) filename;
+ std::cerr << "WARNING: " << __PRETTY_FUNCTION__ << " not implemented" << std::endl;
+ return std::string();
+#endif
+}
+
+std::vector<std::string> strSplit(const std::string& str, const std::string& delimiters)
+{
+ std::vector<std::string> tokens;
+ string::size_type lastPos = 0;
+ string::size_type pos = 0;
+
+ do {
+ pos = str.find_first_of(delimiters, lastPos);
+ tokens.push_back(str.substr(lastPos, pos - lastPos));
+ lastPos = pos + 1;
+ } while (string::npos != pos);
+
+ return tokens;
+}
+
+bool strStartsWith(const std::string& s, const std::string& start)
+{
+ if (s.size() < start.size())
+ return false;
+ return equal(start.begin(), start.end(), s.begin());
+}
+
+bool strEndsWith(const std::string& s, const std::string& end)
+{
+ if (s.size() < end.size())
+ return false;
+ return equal(end.rbegin(), end.rend(), s.rbegin());
+}
+
+int readLine(std::istream& is, std::stringstream& currentLine)
+{
+ if (is.eof())
+ return -1;
+ currentLine.str("");
+ currentLine.clear();
+ is.get(*currentLine.rdbuf());
+ if (is.fail()) // fail is set on empty lines
+ is.clear();
+ G2O_FSKIP_LINE(is); // read \n not read by get()
+ return static_cast<int>(currentLine.str().size());
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.h
new file mode 100644
index 0000000000000000000000000000000000000000..226208ff302626656482d6a3ca1cd15620dabfb2
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/string_tools.h
@@ -0,0 +1,176 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_STRING_TOOLS_H
+#define G2O_STRING_TOOLS_H
+
+#include <string>
+#include <sstream>
+#include <cstdlib>
+#include <vector>
+
+#include "macros.h"
+
+namespace g2o {
+
+/** @addtogroup utils **/
+// @{
+
+/** \file stringTools.h
+ * \brief utility functions for handling strings
+ */
+
+/**
+ * remove whitespaces from the start/end of a string
+ */
+ std::string trim(const std::string& s);
+
+/**
+ * remove whitespaces from the left side of the string
+ */
+ std::string trimLeft(const std::string& s);
+
+/**
+ * remove whitespaced from the right side of the string
+ */
+ std::string trimRight(const std::string& s);
+
+/**
+ * convert the string to lower case
+ */
+ std::string strToLower(const std::string& s);
+
+/**
+ * convert a string to upper case
+ */
+ std::string strToUpper(const std::string& s);
+
+/**
+ * read integer values (seperated by spaces) from a string and store
+ * them in the given OutputIterator.
+ */
+template <typename OutputIterator>
+OutputIterator readInts(const char* str, OutputIterator out)
+{
+ char* cl = (char*)str;
+ char* cle = cl;
+ while (1) {
+ int id = strtol(cl, &cle, 10);
+ if (cl == cle)
+ break;
+ *out++ = id;
+ cl = cle;
+ }
+ return out;
+}
+
+/**
+ * read float values (seperated by spaces) from a string and store
+ * them in the given OutputIterator.
+ */
+template <typename OutputIterator>
+OutputIterator readFloats(const char* str, OutputIterator out)
+{
+ char* cl = (char*)str;
+ char* cle = cl;
+ while (1) {
+ double val = strtod(cl, &cle);
+ if (cl == cle)
+ break;
+ *out++ = val;
+ cl = cle;
+ }
+ return out;
+}
+
+/**
+ * format a string and return a std::string.
+ * Format is just like printf, see man 3 printf
+ */
+ std::string formatString(const char* fmt, ...) G2O_ATTRIBUTE_FORMAT12;
+
+/**
+ * replacement function for sprintf which fills a std::string instead of a char*
+ */
+ int strPrintf(std::string& str, const char* fmt, ...) G2O_ATTRIBUTE_FORMAT23;
+
+/**
+ * convert a string into an other type.
+ */
+template<typename T>
+bool convertString(const std::string& s, T& x, bool failIfLeftoverChars = true)
+{
+ std::istringstream i(s);
+ char c;
+ if (!(i >> x) || (failIfLeftoverChars && i.get(c)))
+ return false;
+ return true;
+}
+
+/**
+ * convert a string into an other type.
+ * Return the converted value. Throw error if parsing is wrong.
+ */
+template<typename T>
+T stringToType(const std::string& s, bool failIfLeftoverChars = true)
+{
+ T x;
+ convertString(s, x, failIfLeftoverChars);
+ return x;
+}
+
+/**
+ * return true, if str starts with substr
+ */
+ bool strStartsWith(const std::string & str, const std::string& substr);
+
+/**
+ * return true, if str ends with substr
+ */
+ bool strEndsWith(const std::string & str, const std::string& substr);
+
+/**
+ * expand the given filename like a posix shell, e.g., ~ $CARMEN_HOME and other will get expanded.
+ * Also command substitution, e.g. `pwd` will give the current directory.
+ */
+ std::string strExpandFilename(const std::string& filename);
+
+/**
+ * split a string into token based on the characters given in delim
+ */
+ std::vector<std::string> strSplit(const std::string& s, const std::string& delim);
+
+/**
+ * read a line from is into currentLine.
+ * @return the number of characters read into currentLine (excluding newline), -1 on eof()
+ */
+ int readLine(std::istream& is, std::stringstream& currentLine);
+
+// @}
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ec190519e6ef0856e80c7fa444deffa61327245c
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.cpp
@@ -0,0 +1,124 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "timeutil.h"
+#include <iostream>
+
+#ifdef _WINDOWS
+#include <time.h>
+#include <windows.h>
+#endif
+
+#ifdef UNIX
+#include <unistd.h>
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+#if defined(_MSC_VER) || defined(_MSC_EXTENSIONS)
+ #define DELTA_EPOCH_IN_MICROSECS 11644473600000000Ui64
+#else
+ #define DELTA_EPOCH_IN_MICROSECS 11644473600000000ULL
+#endif
+
+struct timezone
+{
+ int tz_minuteswest; /* minutes W of Greenwich */
+ int tz_dsttime; /* type of dst correction */
+};
+
+int gettimeofday(struct timeval *tv, struct timezone *tz)
+{
+// Define a structure to receive the current Windows filetime
+ FILETIME ft;
+
+// Initialize the present time to 0 and the timezone to UTC
+ unsigned __int64 tmpres = 0;
+ static int tzflag = 0;
+
+ if (NULL != tv)
+ {
+ GetSystemTimeAsFileTime(&ft);
+
+// The GetSystemTimeAsFileTime returns the number of 100 nanosecond
+// intervals since Jan 1, 1601 in a structure. Copy the high bits to
+// the 64 bit tmpres, shift it left by 32 then or in the low 32 bits.
+ tmpres |= ft.dwHighDateTime;
+ tmpres <<= 32;
+ tmpres |= ft.dwLowDateTime;
+
+// Convert to microseconds by dividing by 10
+ tmpres /= 10;
+
+// The Unix epoch starts on Jan 1 1970. Need to subtract the difference
+// in seconds from Jan 1 1601.
+ tmpres -= DELTA_EPOCH_IN_MICROSECS;
+
+// Finally change microseconds to seconds and place in the seconds value.
+// The modulus picks up the microseconds.
+ tv->tv_sec = (long)(tmpres / 1000000UL);
+ tv->tv_usec = (long)(tmpres % 1000000UL);
+ }
+
+ if (NULL != tz) {
+ if (!tzflag) {
+ _tzset();
+ tzflag++;
+ }
+
+ long sec;
+ int hours;
+ _get_timezone(&sec);
+ _get_daylight(&hours);
+
+// Adjust for the timezone west of Greenwich
+ tz->tz_minuteswest = sec / 60;
+ tz->tz_dsttime = hours;
+ }
+
+ return 0;
+}
+#endif
+
+ScopeTime::ScopeTime(const char* title) : _title(title), _startTime(get_monotonic_time()) {}
+
+ScopeTime::~ScopeTime() {
+ std::cerr << _title<<" took "<<1000*(get_monotonic_time()-_startTime)<<"ms.\n";
+}
+
+double get_monotonic_time()
+{
+#if (defined(_POSIX_TIMERS) && (_POSIX_TIMERS+0 >= 0) && defined(_POSIX_MONOTONIC_CLOCK))
+ struct timespec ts;
+ clock_gettime(CLOCK_MONOTONIC, &ts);
+ return ts.tv_sec + ts.tv_nsec*1e-9;
+#else
+ return get_time();
+#endif
+}
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.h
new file mode 100644
index 0000000000000000000000000000000000000000..bde8e318ae752ae7d9275fc106f7ffe5c8066bcc
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/stuff/timeutil.h
@@ -0,0 +1,132 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_TIMEUTIL_H
+#define G2O_TIMEUTIL_H
+
+#ifdef _WINDOWS
+#include <time.h>
+#else
+#include <sys/time.h>
+#endif
+
+#include <string>
+
+
+/** @addtogroup utils **/
+// @{
+
+/** \file timeutil.h
+ * \brief utility functions for handling time related stuff
+ */
+
+/// Executes code, only if secs are gone since last exec.
+/// extended version, in which the current time is given, e.g., timestamp of IPC message
+#ifndef DO_EVERY_TS
+#define DO_EVERY_TS(secs, currentTime, code) \
+if (1) {\
+ static double s_lastDone_ = (currentTime); \
+ double s_now_ = (currentTime); \
+ if (s_lastDone_ > s_now_) \
+ s_lastDone_ = s_now_; \
+ if (s_now_ - s_lastDone_ > (secs)) { \
+ code; \
+ s_lastDone_ = s_now_; \
+ }\
+} else \
+ (void)0
+#endif
+
+/// Executes code, only if secs are gone since last exec.
+#ifndef DO_EVERY
+#define DO_EVERY(secs, code) DO_EVERY_TS(secs, g2o::get_time(), code)
+#endif
+
+#ifndef MEASURE_TIME
+#define MEASURE_TIME(text, code) \
+ if(1) { \
+ double _start_time_ = g2o::get_time(); \
+ code; \
+ fprintf(stderr, "%s took %f sec\n", text, g2o::get_time() - _start_time_); \
+ } else \
+ (void) 0
+#endif
+
+namespace g2o {
+
+#ifdef _WINDOWS
+typedef struct timeval {
+ long tv_sec;
+ long tv_usec;
+} timeval;
+ int gettimeofday(struct timeval *tv, struct timezone *tz);
+#endif
+
+/**
+ * return the current time in seconds since 1. Jan 1970
+ */
+inline double get_time()
+{
+ struct timeval ts;
+ gettimeofday(&ts,0);
+ return ts.tv_sec + ts.tv_usec*1e-6;
+}
+
+/**
+ * return a monotonic increasing time which basically does not need to
+ * have a reference point. Consider this for measuring how long some
+ * code fragments required to execute.
+ *
+ * On Linux we call clock_gettime() on other systems we currently
+ * call get_time().
+ */
+ double get_monotonic_time();
+
+/**
+ * \brief Class to measure the time spent in a scope
+ *
+ * To use this class, e.g. to measure the time spent in a function,
+ * just create and instance at the beginning of the function.
+ */
+class ScopeTime {
+ public:
+ ScopeTime(const char* title);
+ ~ScopeTime();
+ private:
+ std::string _title;
+ double _startTime;
+};
+
+} // end namespace
+
+#ifndef MEASURE_FUNCTION_TIME
+#define MEASURE_FUNCTION_TIME \
+ g2o::ScopeTime scopeTime(__PRETTY_FUNCTION__)
+#endif
+
+
+// @}
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5c59d3ec02012b693e276a21130b734832221da
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.h
@@ -0,0 +1,47 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_MATH_STUFF
+#define G2O_MATH_STUFF
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace g2o {
+ using namespace Eigen;
+
+ inline Matrix3d skew(const Vector3d&v);
+ inline Vector3d deltaR(const Matrix3d& R);
+ inline Vector2d project(const Vector3d&);
+ inline Vector3d project(const Vector4d&);
+ inline Vector3d unproject(const Vector2d&);
+ inline Vector4d unproject(const Vector3d&);
+
+#include "se3_ops.hpp"
+
+}
+
+#endif //MATH_STUFF
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.hpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..c28860d49cef7d9a131ffd8cbfa243c580da0993
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3_ops.hpp
@@ -0,0 +1,85 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ Matrix3d skew(const Vector3d&v)
+ {
+ Matrix3d m;
+ m.fill(0.);
+ m(0,1) = -v(2);
+ m(0,2) = v(1);
+ m(1,2) = -v(0);
+ m(1,0) = v(2);
+ m(2,0) = -v(1);
+ m(2,1) = v(0);
+ return m;
+ }
+
+ Vector3d deltaR(const Matrix3d& R)
+ {
+ Vector3d v;
+ v(0)=R(2,1)-R(1,2);
+ v(1)=R(0,2)-R(2,0);
+ v(2)=R(1,0)-R(0,1);
+ return v;
+ }
+
+ Vector2d project(const Vector3d& v)
+ {
+ Vector2d res;
+ res(0) = v(0)/v(2);
+ res(1) = v(1)/v(2);
+ return res;
+ }
+
+ Vector3d project(const Vector4d& v)
+ {
+ Vector3d res;
+ res(0) = v(0)/v(3);
+ res(1) = v(1)/v(3);
+ res(2) = v(2)/v(3);
+ return res;
+ }
+
+ Vector3d unproject(const Vector2d& v)
+ {
+ Vector3d res;
+ res(0) = v(0);
+ res(1) = v(1);
+ res(2) = 1;
+ return res;
+ }
+
+ Vector4d unproject(const Vector3d& v)
+ {
+ Vector4d res;
+ res(0) = v(0);
+ res(1) = v(1);
+ res(2) = v(2);
+ res(3) = 1;
+ return res;
+ }
+
+
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3quat.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3quat.h
new file mode 100644
index 0000000000000000000000000000000000000000..ac5ec57cb670576399065a135f7d5822a6384738
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/se3quat.h
@@ -0,0 +1,306 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SE3QUAT_H_
+#define G2O_SE3QUAT_H_
+
+#include "se3_ops.h"
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace g2o {
+ using namespace Eigen;
+
+ typedef Matrix<double, 6, 1> Vector6d;
+ typedef Matrix<double, 7, 1> Vector7d;
+
+ class SE3Quat {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
+
+
+ protected:
+
+ Quaterniond _r;
+ Vector3d _t;
+
+
+ public:
+ SE3Quat(){
+ _r.setIdentity();
+ _t.setZero();
+ }
+
+ SE3Quat(const Matrix3d& R, const Vector3d& t):_r(Quaterniond(R)),_t(t){
+ normalizeRotation();
+ }
+
+ SE3Quat(const Quaterniond& q, const Vector3d& t):_r(q),_t(t){
+ normalizeRotation();
+ }
+
+ /**
+ * templaized constructor which allows v to be an arbitrary Eigen Vector type, e.g., Vector6d or Map<Vector6d>
+ */
+ template <typename Derived>
+ explicit SE3Quat(const MatrixBase<Derived>& v)
+ {
+ assert((v.size() == 6 || v.size() == 7) && "Vector dimension does not match");
+ if (v.size() == 6) {
+ for (int i=0; i<3; i++){
+ _t[i]=v[i];
+ _r.coeffs()(i)=v[i+3];
+ }
+ _r.w() = 0.; // recover the positive w
+ if (_r.norm()>1.){
+ _r.normalize();
+ } else {
+ double w2=1.-_r.squaredNorm();
+ _r.w()= (w2<0.) ? 0. : sqrt(w2);
+ }
+ }
+ else if (v.size() == 7) {
+ int idx = 0;
+ for (int i=0; i<3; ++i, ++idx)
+ _t(i) = v(idx);
+ for (int i=0; i<4; ++i, ++idx)
+ _r.coeffs()(i) = v(idx);
+ normalizeRotation();
+ }
+ }
+
+ inline const Vector3d& translation() const {return _t;}
+
+ inline void setTranslation(const Vector3d& t_) {_t = t_;}
+
+ inline const Quaterniond& rotation() const {return _r;}
+
+ void setRotation(const Quaterniond& r_) {_r=r_;}
+
+ inline SE3Quat operator* (const SE3Quat& tr2) const{
+ SE3Quat result(*this);
+ result._t += _r*tr2._t;
+ result._r*=tr2._r;
+ result.normalizeRotation();
+ return result;
+ }
+
+ inline SE3Quat& operator*= (const SE3Quat& tr2){
+ _t+=_r*tr2._t;
+ _r*=tr2._r;
+ normalizeRotation();
+ return *this;
+ }
+
+ inline Vector3d operator* (const Vector3d& v) const {
+ return _t+_r*v;
+ }
+
+ inline SE3Quat inverse() const{
+ SE3Quat ret;
+ ret._r=_r.conjugate();
+ ret._t=ret._r*(_t*-1.);
+ return ret;
+ }
+
+ inline double operator [](int i) const {
+ assert(i<7);
+ if (i<3)
+ return _t[i];
+ return _r.coeffs()[i-3];
+ }
+
+
+ inline Vector7d toVector() const{
+ Vector7d v;
+ v[0]=_t(0);
+ v[1]=_t(1);
+ v[2]=_t(2);
+ v[3]=_r.x();
+ v[4]=_r.y();
+ v[5]=_r.z();
+ v[6]=_r.w();
+ return v;
+ }
+
+ inline void fromVector(const Vector7d& v){
+ _r=Quaterniond(v[6], v[3], v[4], v[5]);
+ _t=Vector3d(v[0], v[1], v[2]);
+ }
+
+ inline Vector6d toMinimalVector() const{
+ Vector6d v;
+ v[0]=_t(0);
+ v[1]=_t(1);
+ v[2]=_t(2);
+ v[3]=_r.x();
+ v[4]=_r.y();
+ v[5]=_r.z();
+ return v;
+ }
+
+ inline void fromMinimalVector(const Vector6d& v){
+ double w = 1.-v[3]*v[3]-v[4]*v[4]-v[5]*v[5];
+ if (w>0){
+ _r=Quaterniond(sqrt(w), v[3], v[4], v[5]);
+ } else {
+ _r=Quaterniond(0, -v[3], -v[4], -v[5]);
+ }
+ _t=Vector3d(v[0], v[1], v[2]);
+ }
+
+
+
+ Vector6d log() const {
+ Vector6d res;
+ Matrix3d _R = _r.toRotationMatrix();
+ double d = 0.5*(_R(0,0)+_R(1,1)+_R(2,2)-1);
+ Vector3d omega;
+ Vector3d upsilon;
+
+
+ Vector3d dR = deltaR(_R);
+ Matrix3d V_inv;
+
+ if (d>0.99999)
+ {
+
+ omega=0.5*dR;
+ Matrix3d Omega = skew(omega);
+ V_inv = Matrix3d::Identity()- 0.5*Omega + (1./12.)*(Omega*Omega);
+ }
+ else
+ {
+ double theta = acos(d);
+ omega = theta/(2*sqrt(1-d*d))*dR;
+ Matrix3d Omega = skew(omega);
+ V_inv = ( Matrix3d::Identity() - 0.5*Omega
+ + ( 1-theta/(2*tan(theta/2)))/(theta*theta)*(Omega*Omega) );
+ }
+
+ upsilon = V_inv*_t;
+ for (int i=0; i<3;i++){
+ res[i]=omega[i];
+ }
+ for (int i=0; i<3;i++){
+ res[i+3]=upsilon[i];
+ }
+
+ return res;
+
+ }
+
+ Vector3d map(const Vector3d & xyz) const
+ {
+ return _r*xyz + _t;
+ }
+
+
+ static SE3Quat exp(const Vector6d & update)
+ {
+ Vector3d omega;
+ for (int i=0; i<3; i++)
+ omega[i]=update[i];
+ Vector3d upsilon;
+ for (int i=0; i<3; i++)
+ upsilon[i]=update[i+3];
+
+ double theta = omega.norm();
+ Matrix3d Omega = skew(omega);
+
+ Matrix3d R;
+ Matrix3d V;
+ if (theta<0.00001)
+ {
+ //TODO: CHECK WHETHER THIS IS CORRECT!!!
+ R = (Matrix3d::Identity() + Omega + Omega*Omega);
+
+ V = R;
+ }
+ else
+ {
+ Matrix3d Omega2 = Omega*Omega;
+
+ R = (Matrix3d::Identity()
+ + sin(theta)/theta *Omega
+ + (1-cos(theta))/(theta*theta)*Omega2);
+
+ V = (Matrix3d::Identity()
+ + (1-cos(theta))/(theta*theta)*Omega
+ + (theta-sin(theta))/(pow(theta,3))*Omega2);
+ }
+ return SE3Quat(Quaterniond(R),V*upsilon);
+ }
+
+ Matrix<double, 6, 6> adj() const
+ {
+ Matrix3d R = _r.toRotationMatrix();
+ Matrix<double, 6, 6> res;
+ res.block(0,0,3,3) = R;
+ res.block(3,3,3,3) = R;
+ res.block(3,0,3,3) = skew(_t)*R;
+ res.block(0,3,3,3) = Matrix3d::Zero(3,3);
+ return res;
+ }
+
+ Matrix<double,4,4> to_homogeneous_matrix() const
+ {
+ Matrix<double,4,4> homogeneous_matrix;
+ homogeneous_matrix.setIdentity();
+ homogeneous_matrix.block(0,0,3,3) = _r.toRotationMatrix();
+ homogeneous_matrix.col(3).head(3) = translation();
+
+ return homogeneous_matrix;
+ }
+
+ void normalizeRotation(){
+ if (_r.w()<0){
+ _r.coeffs() *= -1;
+ }
+ _r.normalize();
+ }
+
+ /**
+ * cast SE3Quat into an Eigen::Isometry3d
+ */
+ operator Eigen::Isometry3d() const
+ {
+ Eigen::Isometry3d result = (Eigen::Isometry3d) rotation();
+ result.translation() = translation();
+ return result;
+ }
+ };
+
+ inline std::ostream& operator <<(std::ostream& out_str, const SE3Quat& se3)
+ {
+ out_str << se3.to_homogeneous_matrix() << std::endl;
+ return out_str;
+ }
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/sim3.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/sim3.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfd6d5e9bd967a9555e83bb7ececbeb5407b8ec2
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/sim3.h
@@ -0,0 +1,307 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SIM_3
+#define G2O_SIM_3
+
+#include "se3_ops.h"
+#include <Eigen/Geometry>
+
+namespace g2o
+{
+ using namespace Eigen;
+
+ typedef Matrix <double, 7, 1> Vector7d;
+ typedef Matrix <double, 7, 7> Matrix7d;
+
+
+ struct Sim3
+ {
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ protected:
+ Quaterniond r;
+ Vector3d t;
+ double s;
+
+
+public:
+ Sim3()
+ {
+ r.setIdentity();
+ t.fill(0.);
+ s=1.;
+ }
+
+ Sim3(const Quaterniond & r, const Vector3d & t, double s)
+ : r(r),t(t),s(s)
+ {
+ }
+
+ Sim3(const Matrix3d & R, const Vector3d & t, double s)
+ : r(Quaterniond(R)),t(t),s(s)
+ {
+ }
+
+
+ Sim3(const Vector7d & update)
+ {
+
+ Vector3d omega;
+ for (int i=0; i<3; i++)
+ omega[i]=update[i];
+
+ Vector3d upsilon;
+ for (int i=0; i<3; i++)
+ upsilon[i]=update[i+3];
+
+ double sigma = update[6];
+ double theta = omega.norm();
+ Matrix3d Omega = skew(omega);
+ s = std::exp(sigma);
+ Matrix3d Omega2 = Omega*Omega;
+ Matrix3d I;
+ I.setIdentity();
+ Matrix3d R;
+
+ double eps = 0.00001;
+ double A,B,C;
+ if (fabs(sigma)<eps)
+ {
+ C = 1;
+ if (theta<eps)
+ {
+ A = 1./2.;
+ B = 1./6.;
+ R = (I + Omega + Omega*Omega);
+ }
+ else
+ {
+ double theta2= theta*theta;
+ A = (1-cos(theta))/(theta2);
+ B = (theta-sin(theta))/(theta2*theta);
+ R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
+ }
+ }
+ else
+ {
+ C=(s-1)/sigma;
+ if (theta<eps)
+ {
+ double sigma2= sigma*sigma;
+ A = ((sigma-1)*s+1)/sigma2;
+ B= ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
+ R = (I + Omega + Omega2);
+ }
+ else
+ {
+ R = I + sin(theta)/theta *Omega + (1-cos(theta))/(theta*theta)*Omega2;
+
+
+
+ double a=s*sin(theta);
+ double b=s*cos(theta);
+ double theta2= theta*theta;
+ double sigma2= sigma*sigma;
+
+ double c=theta2+sigma2;
+ A = (a*sigma+ (1-b)*theta)/(theta*c);
+ B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
+
+ }
+ }
+ r = Quaterniond(R);
+
+
+
+ Matrix3d W = A*Omega + B*Omega2 + C*I;
+ t = W*upsilon;
+ }
+
+ Vector3d map (const Vector3d& xyz) const {
+ return s*(r*xyz) + t;
+ }
+
+ Vector7d log() const
+ {
+ Vector7d res;
+ double sigma = std::log(s);
+
+
+
+
+ Vector3d omega;
+ Vector3d upsilon;
+
+
+ Matrix3d R = r.toRotationMatrix();
+ double d = 0.5*(R(0,0)+R(1,1)+R(2,2)-1);
+
+ Matrix3d Omega;
+
+ double eps = 0.00001;
+ Matrix3d I = Matrix3d::Identity();
+
+ double A,B,C;
+ if (fabs(sigma)<eps)
+ {
+ C = 1;
+ if (d>1-eps)
+ {
+ omega=0.5*deltaR(R);
+ Omega = skew(omega);
+ A = 1./2.;
+ B = 1./6.;
+ }
+ else
+ {
+ double theta = acos(d);
+ double theta2 = theta*theta;
+ omega = theta/(2*sqrt(1-d*d))*deltaR(R);
+ Omega = skew(omega);
+ A = (1-cos(theta))/(theta2);
+ B = (theta-sin(theta))/(theta2*theta);
+ }
+ }
+ else
+ {
+ C=(s-1)/sigma;
+ if (d>1-eps)
+ {
+
+ double sigma2 = sigma*sigma;
+ omega=0.5*deltaR(R);
+ Omega = skew(omega);
+ A = ((sigma-1)*s+1)/(sigma2);
+ B = ((0.5*sigma2-sigma+1)*s)/(sigma2*sigma);
+ }
+ else
+ {
+ double theta = acos(d);
+ omega = theta/(2*sqrt(1-d*d))*deltaR(R);
+ Omega = skew(omega);
+ double theta2 = theta*theta;
+ double a=s*sin(theta);
+ double b=s*cos(theta);
+ double c=theta2 + sigma*sigma;
+ A = (a*sigma+ (1-b)*theta)/(theta*c);
+ B = (C-((b-1)*sigma+a*theta)/(c))*1./(theta2);
+ }
+ }
+
+ Matrix3d W = A*Omega + B*Omega*Omega + C*I;
+
+ upsilon = W.lu().solve(t);
+
+
+ for (int i=0; i<3; i++)
+ res[i] = omega[i];
+
+ for (int i=0; i<3; i++)
+ res[i+3] = upsilon[i];
+
+ res[6] = sigma;
+
+ return res;
+
+ }
+
+
+ Sim3 inverse() const
+ {
+ return Sim3(r.conjugate(), r.conjugate()*((-1./s)*t), 1./s);
+ }
+
+
+ double operator[](int i) const
+ {
+ assert(i<8);
+ if (i<4){
+
+ return r.coeffs()[i];
+ }
+ if (i<7){
+ return t[i-4];
+ }
+ return s;
+ }
+
+ double& operator[](int i)
+ {
+ assert(i<8);
+ if (i<4){
+
+ return r.coeffs()[i];
+ }
+ if (i<7)
+ {
+ return t[i-4];
+ }
+ return s;
+ }
+
+ Sim3 operator *(const Sim3& other) const {
+ Sim3 ret;
+ ret.r = r*other.r;
+ ret.t=s*(r*other.t)+t;
+ ret.s=s*other.s;
+ return ret;
+ }
+
+ Sim3& operator *=(const Sim3& other){
+ Sim3 ret=(*this)*other;
+ *this=ret;
+ return *this;
+ }
+
+ inline const Vector3d& translation() const {return t;}
+
+ inline Vector3d& translation() {return t;}
+
+ inline const Quaterniond& rotation() const {return r;}
+
+ inline Quaterniond& rotation() {return r;}
+
+ inline const double& scale() const {return s;}
+
+ inline double& scale() {return s;}
+
+ };
+
+ inline std::ostream& operator <<(std::ostream& out_str,
+ const Sim3& sim3)
+ {
+ out_str << sim3.rotation().coeffs() << std::endl;
+ out_str << sim3.translation() << std::endl;
+ out_str << sim3.scale() << std::endl;
+
+ return out_str;
+ }
+
+} // end namespace
+
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d91b49829a7ab27ade9f3e10a92b98ddd8c91aa8
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.cpp
@@ -0,0 +1,56 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 Kurt Konolige
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "types_sba.h"
+#include <iostream>
+
+namespace g2o {
+
+ using namespace std;
+
+
+ VertexSBAPointXYZ::VertexSBAPointXYZ() : BaseVertex<3, Vector3d>()
+ {
+ }
+
+ bool VertexSBAPointXYZ::read(std::istream& is)
+ {
+ Vector3d lv;
+ for (int i=0; i<3; i++)
+ is >> _estimate[i];
+ return true;
+ }
+
+ bool VertexSBAPointXYZ::write(std::ostream& os) const
+ {
+ Vector3d lv=estimate();
+ for (int i=0; i<3; i++){
+ os << lv[i] << " ";
+ }
+ return os.good();
+ }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.h
new file mode 100644
index 0000000000000000000000000000000000000000..bb419dfaa7af06ddda58a73632975f0ff53b1975
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_sba.h
@@ -0,0 +1,61 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 Kurt Konolige
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#ifndef G2O_SBA_TYPES
+#define G2O_SBA_TYPES
+
+#include "../core/base_vertex.h"
+
+#include <Eigen/Geometry>
+#include <iostream>
+
+namespace g2o {
+
+/**
+ * \brief Point vertex, XYZ
+ */
+ class VertexSBAPointXYZ : public BaseVertex<3, Vector3d>
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ VertexSBAPointXYZ();
+ virtual bool read(std::istream& is);
+ virtual bool write(std::ostream& os) const;
+
+ virtual void setToOriginImpl() {
+ _estimate.fill(0.);
+ }
+
+ virtual void oplusImpl(const double* update)
+ {
+ Eigen::Map<const Vector3d> v(update);
+ _estimate += v;
+ }
+};
+
+} // end namespace
+
+#endif // SBA_TYPES
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abf1b1d8fb95a5f0f96e4b0b1215195120393276
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.cpp
@@ -0,0 +1,239 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "types_seven_dof_expmap.h"
+
+namespace g2o {
+
+ VertexSim3Expmap::VertexSim3Expmap() : BaseVertex<7, Sim3>()
+ {
+ _marginalized=false;
+ _fix_scale = false;
+ }
+
+
+ EdgeSim3::EdgeSim3() :
+ BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>()
+ {
+ }
+
+
+ bool VertexSim3Expmap::read(std::istream& is)
+ {
+ Vector7d cam2world;
+ for (int i=0; i<6; i++){
+ is >> cam2world[i];
+ }
+ is >> cam2world[6];
+// if (! is) {
+// // if the scale is not specified we set it to 1;
+// std::cerr << "!s";
+// cam2world[6]=0.;
+// }
+
+ for (int i=0; i<2; i++)
+ {
+ is >> _focal_length1[i];
+ }
+ for (int i=0; i<2; i++)
+ {
+ is >> _principle_point1[i];
+ }
+
+ setEstimate(Sim3(cam2world).inverse());
+ return true;
+ }
+
+ bool VertexSim3Expmap::write(std::ostream& os) const
+ {
+ Sim3 cam2world(estimate().inverse());
+ Vector7d lv=cam2world.log();
+ for (int i=0; i<7; i++){
+ os << lv[i] << " ";
+ }
+ for (int i=0; i<2; i++)
+ {
+ os << _focal_length1[i] << " ";
+ }
+ for (int i=0; i<2; i++)
+ {
+ os << _principle_point1[i] << " ";
+ }
+ return os.good();
+ }
+
+ bool EdgeSim3::read(std::istream& is)
+ {
+ Vector7d v7;
+ for (int i=0; i<7; i++){
+ is >> v7[i];
+ }
+
+ Sim3 cam2world(v7);
+ setMeasurement(cam2world.inverse());
+
+ for (int i=0; i<7; i++)
+ for (int j=i; j<7; j++)
+ {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+ }
+
+ bool EdgeSim3::write(std::ostream& os) const
+ {
+ Sim3 cam2world(measurement().inverse());
+ Vector7d v7 = cam2world.log();
+ for (int i=0; i<7; i++)
+ {
+ os << v7[i] << " ";
+ }
+ for (int i=0; i<7; i++)
+ for (int j=i; j<7; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+ }
+
+ /**Sim3ProjectXYZ*/
+
+ EdgeSim3ProjectXYZ::EdgeSim3ProjectXYZ() :
+ BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()
+ {
+ }
+
+ bool EdgeSim3ProjectXYZ::read(std::istream& is)
+ {
+ for (int i=0; i<2; i++)
+ {
+ is >> _measurement[i];
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+ }
+
+ bool EdgeSim3ProjectXYZ::write(std::ostream& os) const
+ {
+ for (int i=0; i<2; i++){
+ os << _measurement[i] << " ";
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+ }
+
+/**InverseSim3ProjectXYZ*/
+
+ EdgeInverseSim3ProjectXYZ::EdgeInverseSim3ProjectXYZ() :
+ BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>()
+ {
+ }
+
+ bool EdgeInverseSim3ProjectXYZ::read(std::istream& is)
+ {
+ for (int i=0; i<2; i++)
+ {
+ is >> _measurement[i];
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+ }
+
+ bool EdgeInverseSim3ProjectXYZ::write(std::ostream& os) const
+ {
+ for (int i=0; i<2; i++){
+ os << _measurement[i] << " ";
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+ }
+
+
+// void EdgeSim3ProjectXYZ::linearizeOplus()
+// {
+// VertexSim3Expmap * vj = static_cast<VertexSim3Expmap *>(_vertices[1]);
+// Sim3 T = vj->estimate();
+
+// VertexPointXYZ* vi = static_cast<VertexPointXYZ*>(_vertices[0]);
+// Vector3d xyz = vi->estimate();
+// Vector3d xyz_trans = T.map(xyz);
+
+// double x = xyz_trans[0];
+// double y = xyz_trans[1];
+// double z = xyz_trans[2];
+// double z_2 = z*z;
+
+// Matrix<double,2,3> tmp;
+// tmp(0,0) = _focal_length(0);
+// tmp(0,1) = 0;
+// tmp(0,2) = -x/z*_focal_length(0);
+
+// tmp(1,0) = 0;
+// tmp(1,1) = _focal_length(1);
+// tmp(1,2) = -y/z*_focal_length(1);
+
+// _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
+
+// _jacobianOplusXj(0,0) = x*y/z_2 * _focal_length(0);
+// _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *_focal_length(0);
+// _jacobianOplusXj(0,2) = y/z *_focal_length(0);
+// _jacobianOplusXj(0,3) = -1./z *_focal_length(0);
+// _jacobianOplusXj(0,4) = 0;
+// _jacobianOplusXj(0,5) = x/z_2 *_focal_length(0);
+// _jacobianOplusXj(0,6) = 0; // scale is ignored
+
+
+// _jacobianOplusXj(1,0) = (1+y*y/z_2) *_focal_length(1);
+// _jacobianOplusXj(1,1) = -x*y/z_2 *_focal_length(1);
+// _jacobianOplusXj(1,2) = -x/z *_focal_length(1);
+// _jacobianOplusXj(1,3) = 0;
+// _jacobianOplusXj(1,4) = -1./z *_focal_length(1);
+// _jacobianOplusXj(1,5) = y/z_2 *_focal_length(1);
+// _jacobianOplusXj(1,6) = 0; // scale is ignored
+// }
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h
new file mode 100644
index 0000000000000000000000000000000000000000..b63a585905b06dcd4ac4ace926753070d0d7ae9b
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h
@@ -0,0 +1,176 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Modified by Raúl Mur Artal (2014)
+// - Added EdgeInverseSim3ProjectXYZ
+// - Modified VertexSim3Expmap to represent relative transformation between two cameras. Includes calibration of both cameras.
+
+#ifndef G2O_SEVEN_DOF_EXPMAP_TYPES
+#define G2O_SEVEN_DOF_EXPMAP_TYPES
+
+#include "../core/base_vertex.h"
+#include "../core/base_binary_edge.h"
+#include "types_six_dof_expmap.h"
+#include "sim3.h"
+
+namespace g2o {
+
+ using namespace Eigen;
+
+ /**
+ * \brief Sim3 Vertex, (x,y,z,qw,qx,qy,qz)
+ * the parameterization for the increments constructed is a 7d vector
+ * (x,y,z,qx,qy,qz) (note that we leave out the w part of the quaternion.
+ */
+ class VertexSim3Expmap : public BaseVertex<7, Sim3>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ VertexSim3Expmap();
+ virtual bool read(std::istream& is);
+ virtual bool write(std::ostream& os) const;
+
+ virtual void setToOriginImpl() {
+ _estimate = Sim3();
+ }
+
+ virtual void oplusImpl(const double* update_)
+ {
+ Eigen::Map<Vector7d> update(const_cast<double*>(update_));
+
+ if (_fix_scale)
+ update[6] = 0;
+
+ Sim3 s(update);
+ setEstimate(s*estimate());
+ }
+
+ Vector2d _principle_point1, _principle_point2;
+ Vector2d _focal_length1, _focal_length2;
+
+ Vector2d cam_map1(const Vector2d & v) const
+ {
+ Vector2d res;
+ res[0] = v[0]*_focal_length1[0] + _principle_point1[0];
+ res[1] = v[1]*_focal_length1[1] + _principle_point1[1];
+ return res;
+ }
+
+ Vector2d cam_map2(const Vector2d & v) const
+ {
+ Vector2d res;
+ res[0] = v[0]*_focal_length2[0] + _principle_point2[0];
+ res[1] = v[1]*_focal_length2[1] + _principle_point2[1];
+ return res;
+ }
+
+ bool _fix_scale;
+
+
+ protected:
+ };
+
+ /**
+ * \brief 7D edge between two Vertex7
+ */
+ class EdgeSim3 : public BaseBinaryEdge<7, Sim3, VertexSim3Expmap, VertexSim3Expmap>
+ {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ EdgeSim3();
+ virtual bool read(std::istream& is);
+ virtual bool write(std::ostream& os) const;
+ void computeError()
+ {
+ const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[0]);
+ const VertexSim3Expmap* v2 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
+
+ Sim3 C(_measurement);
+ Sim3 error_=C*v1->estimate()*v2->estimate().inverse();
+ _error = error_.log();
+ }
+
+ virtual double initialEstimatePossible(const OptimizableGraph::VertexSet& , OptimizableGraph::Vertex* ) { return 1.;}
+ virtual void initialEstimate(const OptimizableGraph::VertexSet& from, OptimizableGraph::Vertex* /*to*/)
+ {
+ VertexSim3Expmap* v1 = static_cast<VertexSim3Expmap*>(_vertices[0]);
+ VertexSim3Expmap* v2 = static_cast<VertexSim3Expmap*>(_vertices[1]);
+ if (from.count(v1) > 0)
+ v2->setEstimate(measurement()*v1->estimate());
+ else
+ v1->setEstimate(measurement().inverse()*v2->estimate());
+ }
+ };
+
+
+/**/
+class EdgeSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ EdgeSim3ProjectXYZ();
+ virtual bool read(std::istream& is);
+ virtual bool write(std::ostream& os) const;
+
+ void computeError()
+ {
+ const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+
+ Vector2d obs(_measurement);
+ _error = obs-v1->cam_map1(project(v1->estimate().map(v2->estimate())));
+ }
+
+ // virtual void linearizeOplus();
+
+};
+
+/**/
+class EdgeInverseSim3ProjectXYZ : public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSim3Expmap>
+{
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ EdgeInverseSim3ProjectXYZ();
+ virtual bool read(std::istream& is);
+ virtual bool write(std::ostream& os) const;
+
+ void computeError()
+ {
+ const VertexSim3Expmap* v1 = static_cast<const VertexSim3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+
+ Vector2d obs(_measurement);
+ _error = obs-v1->cam_map2(project(v1->estimate().inverse().map(v2->estimate())));
+ }
+
+ // virtual void linearizeOplus();
+
+};
+
+} // end namespace
+
+#endif
+
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..498508ccf3bc8eaadb1e0287bce7c6f1a606e3a1
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.cpp
@@ -0,0 +1,367 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+#include "types_six_dof_expmap.h"
+
+#include "../core/factory.h"
+#include "../stuff/macros.h"
+
+namespace g2o {
+
+using namespace std;
+
+
+Vector2d project2d(const Vector3d& v) {
+ Vector2d res;
+ res(0) = v(0)/v(2);
+ res(1) = v(1)/v(2);
+ return res;
+}
+
+Vector3d unproject2d(const Vector2d& v) {
+ Vector3d res;
+ res(0) = v(0);
+ res(1) = v(1);
+ res(2) = 1;
+ return res;
+}
+
+VertexSE3Expmap::VertexSE3Expmap() : BaseVertex<6, SE3Quat>() {
+}
+
+bool VertexSE3Expmap::read(std::istream& is) {
+ Vector7d est;
+ for (int i=0; i<7; i++)
+ is >> est[i];
+ SE3Quat cam2world;
+ cam2world.fromVector(est);
+ setEstimate(cam2world.inverse());
+ return true;
+}
+
+bool VertexSE3Expmap::write(std::ostream& os) const {
+ SE3Quat cam2world(estimate().inverse());
+ for (int i=0; i<7; i++)
+ os << cam2world[i] << " ";
+ return os.good();
+}
+
+
+EdgeSE3ProjectXYZ::EdgeSE3ProjectXYZ() : BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>() {
+}
+
+bool EdgeSE3ProjectXYZ::read(std::istream& is){
+ for (int i=0; i<2; i++){
+ is >> _measurement[i];
+ }
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+}
+
+bool EdgeSE3ProjectXYZ::write(std::ostream& os) const {
+
+ for (int i=0; i<2; i++){
+ os << measurement()[i] << " ";
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+}
+
+
+void EdgeSE3ProjectXYZ::linearizeOplus() {
+ VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
+ SE3Quat T(vj->estimate());
+ VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
+ Vector3d xyz = vi->estimate();
+ Vector3d xyz_trans = T.map(xyz);
+
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double z = xyz_trans[2];
+ double z_2 = z*z;
+
+ Matrix<double,2,3> tmp;
+ tmp(0,0) = fx;
+ tmp(0,1) = 0;
+ tmp(0,2) = -x/z*fx;
+
+ tmp(1,0) = 0;
+ tmp(1,1) = fy;
+ tmp(1,2) = -y/z*fy;
+
+ _jacobianOplusXi = -1./z * tmp * T.rotation().toRotationMatrix();
+
+ _jacobianOplusXj(0,0) = x*y/z_2 *fx;
+ _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
+ _jacobianOplusXj(0,2) = y/z *fx;
+ _jacobianOplusXj(0,3) = -1./z *fx;
+ _jacobianOplusXj(0,4) = 0;
+ _jacobianOplusXj(0,5) = x/z_2 *fx;
+
+ _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
+ _jacobianOplusXj(1,1) = -x*y/z_2 *fy;
+ _jacobianOplusXj(1,2) = -x/z *fy;
+ _jacobianOplusXj(1,3) = 0;
+ _jacobianOplusXj(1,4) = -1./z *fy;
+ _jacobianOplusXj(1,5) = y/z_2 *fy;
+}
+
+Vector2d EdgeSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz) const{
+ Vector2d proj = project2d(trans_xyz);
+ Vector2d res;
+ res[0] = proj[0]*fx + cx;
+ res[1] = proj[1]*fy + cy;
+ return res;
+}
+
+
+Vector3d EdgeStereoSE3ProjectXYZ::cam_project(const Vector3d & trans_xyz, const float &bf) const{
+ const float invz = 1.0f/trans_xyz[2];
+ Vector3d res;
+ res[0] = trans_xyz[0]*invz*fx + cx;
+ res[1] = trans_xyz[1]*invz*fy + cy;
+ res[2] = res[0] - bf*invz;
+ return res;
+}
+
+EdgeStereoSE3ProjectXYZ::EdgeStereoSE3ProjectXYZ() : BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>() {
+}
+
+bool EdgeStereoSE3ProjectXYZ::read(std::istream& is){
+ for (int i=0; i<=3; i++){
+ is >> _measurement[i];
+ }
+ for (int i=0; i<=2; i++)
+ for (int j=i; j<=2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+}
+
+bool EdgeStereoSE3ProjectXYZ::write(std::ostream& os) const {
+
+ for (int i=0; i<=3; i++){
+ os << measurement()[i] << " ";
+ }
+
+ for (int i=0; i<=2; i++)
+ for (int j=i; j<=2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+}
+
+void EdgeStereoSE3ProjectXYZ::linearizeOplus() {
+ VertexSE3Expmap * vj = static_cast<VertexSE3Expmap *>(_vertices[1]);
+ SE3Quat T(vj->estimate());
+ VertexSBAPointXYZ* vi = static_cast<VertexSBAPointXYZ*>(_vertices[0]);
+ Vector3d xyz = vi->estimate();
+ Vector3d xyz_trans = T.map(xyz);
+
+ const Matrix3d R = T.rotation().toRotationMatrix();
+
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double z = xyz_trans[2];
+ double z_2 = z*z;
+
+ _jacobianOplusXi(0,0) = -fx*R(0,0)/z+fx*x*R(2,0)/z_2;
+ _jacobianOplusXi(0,1) = -fx*R(0,1)/z+fx*x*R(2,1)/z_2;
+ _jacobianOplusXi(0,2) = -fx*R(0,2)/z+fx*x*R(2,2)/z_2;
+
+ _jacobianOplusXi(1,0) = -fy*R(1,0)/z+fy*y*R(2,0)/z_2;
+ _jacobianOplusXi(1,1) = -fy*R(1,1)/z+fy*y*R(2,1)/z_2;
+ _jacobianOplusXi(1,2) = -fy*R(1,2)/z+fy*y*R(2,2)/z_2;
+
+ _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*R(2,0)/z_2;
+ _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)-bf*R(2,1)/z_2;
+ _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2)-bf*R(2,2)/z_2;
+
+ _jacobianOplusXj(0,0) = x*y/z_2 *fx;
+ _jacobianOplusXj(0,1) = -(1+(x*x/z_2)) *fx;
+ _jacobianOplusXj(0,2) = y/z *fx;
+ _jacobianOplusXj(0,3) = -1./z *fx;
+ _jacobianOplusXj(0,4) = 0;
+ _jacobianOplusXj(0,5) = x/z_2 *fx;
+
+ _jacobianOplusXj(1,0) = (1+y*y/z_2) *fy;
+ _jacobianOplusXj(1,1) = -x*y/z_2 *fy;
+ _jacobianOplusXj(1,2) = -x/z *fy;
+ _jacobianOplusXj(1,3) = 0;
+ _jacobianOplusXj(1,4) = -1./z *fy;
+ _jacobianOplusXj(1,5) = y/z_2 *fy;
+
+ _jacobianOplusXj(2,0) = _jacobianOplusXj(0,0)-bf*y/z_2;
+ _jacobianOplusXj(2,1) = _jacobianOplusXj(0,1)+bf*x/z_2;
+ _jacobianOplusXj(2,2) = _jacobianOplusXj(0,2);
+ _jacobianOplusXj(2,3) = _jacobianOplusXj(0,3);
+ _jacobianOplusXj(2,4) = 0;
+ _jacobianOplusXj(2,5) = _jacobianOplusXj(0,5)-bf/z_2;
+}
+
+
+//Only Pose
+
+bool EdgeSE3ProjectXYZOnlyPose::read(std::istream& is){
+ for (int i=0; i<2; i++){
+ is >> _measurement[i];
+ }
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+}
+
+bool EdgeSE3ProjectXYZOnlyPose::write(std::ostream& os) const {
+
+ for (int i=0; i<2; i++){
+ os << measurement()[i] << " ";
+ }
+
+ for (int i=0; i<2; i++)
+ for (int j=i; j<2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+}
+
+
+void EdgeSE3ProjectXYZOnlyPose::linearizeOplus() {
+ VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
+ Vector3d xyz_trans = vi->estimate().map(Xw);
+
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double invz = 1.0/xyz_trans[2];
+ double invz_2 = invz*invz;
+
+ _jacobianOplusXi(0,0) = x*y*invz_2 *fx;
+ _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
+ _jacobianOplusXi(0,2) = y*invz *fx;
+ _jacobianOplusXi(0,3) = -invz *fx;
+ _jacobianOplusXi(0,4) = 0;
+ _jacobianOplusXi(0,5) = x*invz_2 *fx;
+
+ _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
+ _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
+ _jacobianOplusXi(1,2) = -x*invz *fy;
+ _jacobianOplusXi(1,3) = 0;
+ _jacobianOplusXi(1,4) = -invz *fy;
+ _jacobianOplusXi(1,5) = y*invz_2 *fy;
+}
+
+Vector2d EdgeSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{
+ Vector2d proj = project2d(trans_xyz);
+ Vector2d res;
+ res[0] = proj[0]*fx + cx;
+ res[1] = proj[1]*fy + cy;
+ return res;
+}
+
+
+Vector3d EdgeStereoSE3ProjectXYZOnlyPose::cam_project(const Vector3d & trans_xyz) const{
+ const float invz = 1.0f/trans_xyz[2];
+ Vector3d res;
+ res[0] = trans_xyz[0]*invz*fx + cx;
+ res[1] = trans_xyz[1]*invz*fy + cy;
+ res[2] = res[0] - bf*invz;
+ return res;
+}
+
+
+bool EdgeStereoSE3ProjectXYZOnlyPose::read(std::istream& is){
+ for (int i=0; i<=3; i++){
+ is >> _measurement[i];
+ }
+ for (int i=0; i<=2; i++)
+ for (int j=i; j<=2; j++) {
+ is >> information()(i,j);
+ if (i!=j)
+ information()(j,i)=information()(i,j);
+ }
+ return true;
+}
+
+bool EdgeStereoSE3ProjectXYZOnlyPose::write(std::ostream& os) const {
+
+ for (int i=0; i<=3; i++){
+ os << measurement()[i] << " ";
+ }
+
+ for (int i=0; i<=2; i++)
+ for (int j=i; j<=2; j++){
+ os << " " << information()(i,j);
+ }
+ return os.good();
+}
+
+void EdgeStereoSE3ProjectXYZOnlyPose::linearizeOplus() {
+ VertexSE3Expmap * vi = static_cast<VertexSE3Expmap *>(_vertices[0]);
+ Vector3d xyz_trans = vi->estimate().map(Xw);
+
+ double x = xyz_trans[0];
+ double y = xyz_trans[1];
+ double invz = 1.0/xyz_trans[2];
+ double invz_2 = invz*invz;
+
+ _jacobianOplusXi(0,0) = x*y*invz_2 *fx;
+ _jacobianOplusXi(0,1) = -(1+(x*x*invz_2)) *fx;
+ _jacobianOplusXi(0,2) = y*invz *fx;
+ _jacobianOplusXi(0,3) = -invz *fx;
+ _jacobianOplusXi(0,4) = 0;
+ _jacobianOplusXi(0,5) = x*invz_2 *fx;
+
+ _jacobianOplusXi(1,0) = (1+y*y*invz_2) *fy;
+ _jacobianOplusXi(1,1) = -x*y*invz_2 *fy;
+ _jacobianOplusXi(1,2) = -x*invz *fy;
+ _jacobianOplusXi(1,3) = 0;
+ _jacobianOplusXi(1,4) = -invz *fy;
+ _jacobianOplusXi(1,5) = y*invz_2 *fy;
+
+ _jacobianOplusXi(2,0) = _jacobianOplusXi(0,0)-bf*y*invz_2;
+ _jacobianOplusXi(2,1) = _jacobianOplusXi(0,1)+bf*x*invz_2;
+ _jacobianOplusXi(2,2) = _jacobianOplusXi(0,2);
+ _jacobianOplusXi(2,3) = _jacobianOplusXi(0,3);
+ _jacobianOplusXi(2,4) = 0;
+ _jacobianOplusXi(2,5) = _jacobianOplusXi(0,5)-bf*invz_2;
+}
+
+
+} // end namespace
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h
new file mode 100644
index 0000000000000000000000000000000000000000..be765f111dc598f7b7396a0de9b40665c6e5b14f
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/g2o/types/types_six_dof_expmap.h
@@ -0,0 +1,208 @@
+// g2o - General Graph Optimization
+// Copyright (C) 2011 H. Strasdat
+// All rights reserved.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are
+// met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+// IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+// TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+// PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+// TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+// PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+// LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+// NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+// SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+// Modified by Raúl Mur Artal (2014)
+// Added EdgeSE3ProjectXYZ (project using focal_length in x,y directions)
+// Modified by Raúl Mur Artal (2016)
+// Added EdgeStereoSE3ProjectXYZ (project using focal_length in x,y directions)
+// Added EdgeSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
+// Added EdgeStereoSE3ProjectXYZOnlyPose (unary edge to optimize only the camera pose)
+
+#ifndef G2O_SIX_DOF_TYPES_EXPMAP
+#define G2O_SIX_DOF_TYPES_EXPMAP
+
+#include "../core/base_vertex.h"
+#include "../core/base_binary_edge.h"
+#include "../core/base_unary_edge.h"
+#include "se3_ops.h"
+#include "se3quat.h"
+#include "types_sba.h"
+#include <Eigen/Geometry>
+
+namespace g2o {
+namespace types_six_dof_expmap {
+void init();
+}
+
+using namespace Eigen;
+
+typedef Matrix<double, 6, 6> Matrix6d;
+
+
+/**
+ * \brief SE3 Vertex parameterized internally with a transformation matrix
+ and externally with its exponential map
+ */
+class VertexSE3Expmap : public BaseVertex<6, SE3Quat>{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ VertexSE3Expmap();
+
+ bool read(std::istream& is);
+
+ bool write(std::ostream& os) const;
+
+ virtual void setToOriginImpl() {
+ _estimate = SE3Quat();
+ }
+
+ virtual void oplusImpl(const double* update_) {
+ Eigen::Map<const Vector6d> update(update_);
+ setEstimate(SE3Quat::exp(update)*estimate());
+ }
+};
+
+
+class EdgeSE3ProjectXYZ: public BaseBinaryEdge<2, Vector2d, VertexSBAPointXYZ, VertexSE3Expmap>{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ EdgeSE3ProjectXYZ();
+
+ bool read(std::istream& is);
+
+ bool write(std::ostream& os) const;
+
+ void computeError() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+ Vector2d obs(_measurement);
+ _error = obs-cam_project(v1->estimate().map(v2->estimate()));
+ }
+
+ bool isDepthPositive() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+ return (v1->estimate().map(v2->estimate()))(2)>0.0;
+ }
+
+
+ virtual void linearizeOplus();
+
+ Vector2d cam_project(const Vector3d & trans_xyz) const;
+
+ double fx, fy, cx, cy;
+};
+
+
+class EdgeStereoSE3ProjectXYZ: public BaseBinaryEdge<3, Vector3d, VertexSBAPointXYZ, VertexSE3Expmap>{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ EdgeStereoSE3ProjectXYZ();
+
+ bool read(std::istream& is);
+
+ bool write(std::ostream& os) const;
+
+ void computeError() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+ Vector3d obs(_measurement);
+ _error = obs - cam_project(v1->estimate().map(v2->estimate()),bf);
+ }
+
+ bool isDepthPositive() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[1]);
+ const VertexSBAPointXYZ* v2 = static_cast<const VertexSBAPointXYZ*>(_vertices[0]);
+ return (v1->estimate().map(v2->estimate()))(2)>0.0;
+ }
+
+
+ virtual void linearizeOplus();
+
+ Vector3d cam_project(const Vector3d & trans_xyz, const float &bf) const;
+
+ double fx, fy, cx, cy, bf;
+};
+
+class EdgeSE3ProjectXYZOnlyPose: public BaseUnaryEdge<2, Vector2d, VertexSE3Expmap>{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ EdgeSE3ProjectXYZOnlyPose(){}
+
+ bool read(std::istream& is);
+
+ bool write(std::ostream& os) const;
+
+ void computeError() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
+ Vector2d obs(_measurement);
+ _error = obs-cam_project(v1->estimate().map(Xw));
+ }
+
+ bool isDepthPositive() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
+ return (v1->estimate().map(Xw))(2)>0.0;
+ }
+
+
+ virtual void linearizeOplus();
+
+ Vector2d cam_project(const Vector3d & trans_xyz) const;
+
+ Vector3d Xw;
+ double fx, fy, cx, cy;
+};
+
+
+class EdgeStereoSE3ProjectXYZOnlyPose: public BaseUnaryEdge<3, Vector3d, VertexSE3Expmap>{
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ EdgeStereoSE3ProjectXYZOnlyPose(){}
+
+ bool read(std::istream& is);
+
+ bool write(std::ostream& os) const;
+
+ void computeError() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
+ Vector3d obs(_measurement);
+ _error = obs - cam_project(v1->estimate().map(Xw));
+ }
+
+ bool isDepthPositive() {
+ const VertexSE3Expmap* v1 = static_cast<const VertexSE3Expmap*>(_vertices[0]);
+ return (v1->estimate().map(Xw))(2)>0.0;
+ }
+
+
+ virtual void linearizeOplus();
+
+ Vector3d cam_project(const Vector3d & trans_xyz) const;
+
+ Vector3d Xw;
+ double fx, fy, cx, cy, bf;
+};
+
+
+
+} // end namespace
+
+#endif
diff --git a/externals/ORB_SLAM2/Thirdparty/g2o/license-bsd.txt b/externals/ORB_SLAM2/Thirdparty/g2o/license-bsd.txt
new file mode 100644
index 0000000000000000000000000000000000000000..b9b8846be45416193b319d4fa9ef7922b1a42f6c
--- /dev/null
+++ b/externals/ORB_SLAM2/Thirdparty/g2o/license-bsd.txt
@@ -0,0 +1,27 @@
+g2o - General Graph Optimization
+Copyright (C) 2011 Rainer Kuemmerle, Giorgio Grisetti, Hauke Strasdat,
+Kurt Konolige, and Wolfram Burgard
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+* Redistributions of source code must retain the above copyright notice,
+ this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright
+ notice, this list of conditions and the following disclaimer in the
+ documentation and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
diff --git a/externals/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz b/externals/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz
new file mode 100755
index 0000000000000000000000000000000000000000..96f5a8184a1b401dae9b51fd8f3aae03de0a8b20
Binary files /dev/null and b/externals/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz differ
diff --git a/externals/ORB_SLAM2/build.sh b/externals/ORB_SLAM2/build.sh
new file mode 100755
index 0000000000000000000000000000000000000000..69d4ba228c72b3309fe9896e144618d6e1ecac3d
--- /dev/null
+++ b/externals/ORB_SLAM2/build.sh
@@ -0,0 +1,31 @@
+echo "Configuring and building Thirdparty/DBoW2 ..."
+
+cd Thirdparty/DBoW2
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
+
+cd ../../g2o
+
+echo "Configuring and building Thirdparty/g2o ..."
+
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
+
+cd ../../../
+
+echo "Uncompress vocabulary ..."
+
+cd Vocabulary
+tar -xf ORBvoc.txt.tar.gz
+cd ..
+
+echo "Configuring and building ORB_SLAM2 ..."
+
+mkdir build
+cd build
+cmake .. -DCMAKE_BUILD_TYPE=Release
+make -j
diff --git a/externals/ORB_SLAM2/build_ros.sh b/externals/ORB_SLAM2/build_ros.sh
new file mode 100755
index 0000000000000000000000000000000000000000..86b7c14b5bdebb7bd532941599a1a145d249dbcc
--- /dev/null
+++ b/externals/ORB_SLAM2/build_ros.sh
@@ -0,0 +1,7 @@
+echo "Building ROS nodes"
+
+cd Examples/ROS/ORB_SLAM2
+mkdir build
+cd build
+cmake .. -DROS_BUILD_TYPE=Release
+make -j
diff --git a/externals/ORB_SLAM2/include/Converter.h b/externals/ORB_SLAM2/include/Converter.h
new file mode 100644
index 0000000000000000000000000000000000000000..0002ad62c2e5b54308706de7b108cceb9b0bd9e2
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Converter.h
@@ -0,0 +1,57 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef CONVERTER_H
+#define CONVERTER_H
+
+#include<opencv2/core/core.hpp>
+
+#include<Eigen/Dense>
+#include"Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
+#include"Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class Converter
+{
+public:
+ static std::vector<cv::Mat> toDescriptorVector(const cv::Mat &Descriptors);
+
+ static g2o::SE3Quat toSE3Quat(const cv::Mat &cvT);
+ static g2o::SE3Quat toSE3Quat(const g2o::Sim3 &gSim3);
+
+ static cv::Mat toCvMat(const g2o::SE3Quat &SE3);
+ static cv::Mat toCvMat(const g2o::Sim3 &Sim3);
+ static cv::Mat toCvMat(const Eigen::Matrix<double,4,4> &m);
+ static cv::Mat toCvMat(const Eigen::Matrix3d &m);
+ static cv::Mat toCvMat(const Eigen::Matrix<double,3,1> &m);
+ static cv::Mat toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t);
+
+ static Eigen::Matrix<double,3,1> toVector3d(const cv::Mat &cvVector);
+ static Eigen::Matrix<double,3,1> toVector3d(const cv::Point3f &cvPoint);
+ static Eigen::Matrix<double,3,3> toMatrix3d(const cv::Mat &cvMat3);
+
+ static std::vector<float> toQuaternion(const cv::Mat &M);
+};
+
+}// namespace ORB_SLAM
+
+#endif // CONVERTER_H
diff --git a/externals/ORB_SLAM2/include/Frame.h b/externals/ORB_SLAM2/include/Frame.h
new file mode 100644
index 0000000000000000000000000000000000000000..a6a8032f570fa0eccd32c8cae02ce0017d18d5d4
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Frame.h
@@ -0,0 +1,213 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FRAME_H
+#define FRAME_H
+
+#include<vector>
+
+#include "MapPoint.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "ORBVocabulary.h"
+#include "KeyFrame.h"
+#include "ORBextractor.h"
+
+#include <opencv2/opencv.hpp>
+
+namespace ORB_SLAM2
+{
+#define FRAME_GRID_ROWS 48
+#define FRAME_GRID_COLS 64
+
+class MapPoint;
+class KeyFrame;
+
+class Frame
+{
+public:
+ Frame();
+
+ // Copy constructor.
+ Frame(const Frame &frame);
+
+ // Constructor for stereo cameras.
+ Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
+
+ // Constructor for RGB-D cameras.
+ Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
+
+ // Constructor for Monocular cameras.
+ Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth);
+
+ // Extract ORB on the image. 0 for left image and 1 for right image.
+ void ExtractORB(int flag, const cv::Mat &im);
+
+ // Compute Bag of Words representation.
+ void ComputeBoW();
+
+ // Set the camera pose.
+ void SetPose(cv::Mat Tcw);
+
+ // Computes rotation, translation and camera center matrices from the camera pose.
+ void UpdatePoseMatrices();
+
+ // Returns the camera center.
+ inline cv::Mat GetCameraCenter(){
+ return mOw.clone();
+ }
+
+ // Returns inverse of rotation
+ inline cv::Mat GetRotationInverse(){
+ return mRwc.clone();
+ }
+
+ // Check if a MapPoint is in the frustum of the camera
+ // and fill variables of the MapPoint to be used by the tracking
+ bool isInFrustum(MapPoint* pMP, float viewingCosLimit);
+
+ // Compute the cell of a keypoint (return false if outside the grid)
+ bool PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY);
+
+ vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel=-1, const int maxLevel=-1) const;
+
+ // Search a match for each keypoint in the left image to a keypoint in the right image.
+ // If there is a match, depth is computed and the right coordinate associated to the left keypoint is stored.
+ void ComputeStereoMatches();
+
+ // Associate a "right" coordinate to a keypoint if there is valid depth in the depthmap.
+ void ComputeStereoFromRGBD(const cv::Mat &imDepth);
+
+ // Backprojects a keypoint (if stereo/depth info available) into 3D world coordinates.
+ cv::Mat UnprojectStereo(const int &i);
+
+public:
+ // Vocabulary used for relocalization.
+ ORBVocabulary* mpORBvocabulary;
+
+ // Feature extractor. The right is used only in the stereo case.
+ ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
+
+ // Frame timestamp.
+ double mTimeStamp;
+
+ // Calibration matrix and OpenCV distortion parameters.
+ cv::Mat mK;
+ static float fx;
+ static float fy;
+ static float cx;
+ static float cy;
+ static float invfx;
+ static float invfy;
+ cv::Mat mDistCoef;
+
+ // Stereo baseline multiplied by fx.
+ float mbf;
+
+ // Stereo baseline in meters.
+ float mb;
+
+ // Threshold close/far points. Close points are inserted from 1 view.
+ // Far points are inserted as in the monocular case from 2 views.
+ float mThDepth;
+
+ // Number of KeyPoints.
+ int N;
+
+ // Vector of keypoints (original for visualization) and undistorted (actually used by the system).
+ // In the stereo case, mvKeysUn is redundant as images must be rectified.
+ // In the RGB-D case, RGB images can be distorted.
+ std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
+ std::vector<cv::KeyPoint> mvKeysUn;
+
+ // Corresponding stereo coordinate and depth for each keypoint.
+ // "Monocular" keypoints have a negative value.
+ std::vector<float> mvuRight;
+ std::vector<float> mvDepth;
+
+ // Bag of Words Vector structures.
+ DBoW2::BowVector mBowVec;
+ DBoW2::FeatureVector mFeatVec;
+
+ // ORB descriptor, each row associated to a keypoint.
+ cv::Mat mDescriptors, mDescriptorsRight;
+
+ // MapPoints associated to keypoints, NULL pointer if no association.
+ std::vector<MapPoint*> mvpMapPoints;
+
+ // Flag to identify outlier associations.
+ std::vector<bool> mvbOutlier;
+
+ // Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
+ static float mfGridElementWidthInv;
+ static float mfGridElementHeightInv;
+ std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];
+
+ // Camera pose.
+ cv::Mat mTcw;
+
+ // Current and Next Frame id.
+ static long unsigned int nNextId;
+ long unsigned int mnId;
+
+ // Reference Keyframe.
+ KeyFrame* mpReferenceKF;
+
+ // Scale pyramid info.
+ int mnScaleLevels;
+ float mfScaleFactor;
+ float mfLogScaleFactor;
+ vector<float> mvScaleFactors;
+ vector<float> mvInvScaleFactors;
+ vector<float> mvLevelSigma2;
+ vector<float> mvInvLevelSigma2;
+
+ // Undistorted Image Bounds (computed once).
+ static float mnMinX;
+ static float mnMaxX;
+ static float mnMinY;
+ static float mnMaxY;
+
+ static bool mbInitialComputations;
+
+
+private:
+
+ // Undistort keypoints given OpenCV distortion parameters.
+ // Only for the RGB-D case. Stereo must be already rectified!
+ // (called in the constructor).
+ void UndistortKeyPoints();
+
+ // Computes image bounds for the undistorted image (called in the constructor).
+ void ComputeImageBounds(const cv::Mat &imLeft);
+
+ // Assign keypoints to the grid for speed up feature matching (called in the constructor).
+ void AssignFeaturesToGrid();
+
+ // Rotation, translation and camera center
+ cv::Mat mRcw;
+ cv::Mat mtcw;
+ cv::Mat mRwc;
+ cv::Mat mOw; //==mtwc
+};
+
+}// namespace ORB_SLAM
+
+#endif // FRAME_H
diff --git a/externals/ORB_SLAM2/include/FrameDrawer.h b/externals/ORB_SLAM2/include/FrameDrawer.h
new file mode 100644
index 0000000000000000000000000000000000000000..95c1df9d9effb9890a2ba53bac3519a0a71c8779
--- /dev/null
+++ b/externals/ORB_SLAM2/include/FrameDrawer.h
@@ -0,0 +1,73 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef FRAMEDRAWER_H
+#define FRAMEDRAWER_H
+
+#include "Tracking.h"
+#include "MapPoint.h"
+#include "Map.h"
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include<mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class Viewer;
+
+class FrameDrawer
+{
+public:
+ FrameDrawer(Map* pMap);
+
+ // Update info from the last processed frame.
+ void Update(Tracking *pTracker);
+
+ // Draw last processed frame.
+ cv::Mat DrawFrame();
+
+protected:
+
+ void DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText);
+
+ // Info of the frame to be drawn
+ cv::Mat mIm;
+ int N;
+ vector<cv::KeyPoint> mvCurrentKeys;
+ vector<bool> mvbMap, mvbVO;
+ bool mbOnlyTracking;
+ int mnTracked, mnTrackedVO;
+ vector<cv::KeyPoint> mvIniKeys;
+ vector<int> mvIniMatches;
+ int mState;
+
+ Map* mpMap;
+
+ std::mutex mMutex;
+};
+
+} //namespace ORB_SLAM
+
+#endif // FRAMEDRAWER_H
diff --git a/externals/ORB_SLAM2/include/Initializer.h b/externals/ORB_SLAM2/include/Initializer.h
new file mode 100644
index 0000000000000000000000000000000000000000..09b27a65bb09cc2dd4f46b25cc4b133fe48320fb
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Initializer.h
@@ -0,0 +1,101 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+#ifndef INITIALIZER_H
+#define INITIALIZER_H
+
+#include<opencv2/opencv.hpp>
+#include "Frame.h"
+
+
+namespace ORB_SLAM2
+{
+
+// THIS IS THE INITIALIZER FOR MONOCULAR SLAM. NOT USED IN THE STEREO OR RGBD CASE.
+class Initializer
+{
+ typedef pair<int,int> Match;
+
+public:
+
+ // Fix the reference frame
+ Initializer(const Frame &ReferenceFrame, float sigma = 1.0, int iterations = 200);
+
+ // Computes in parallel a fundamental matrix and a homography
+ // Selects a model and tries to recover the motion and the structure from motion
+ bool Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12,
+ cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated);
+
+
+private:
+
+ void FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21);
+ void FindFundamental(vector<bool> &vbInliers, float &score, cv::Mat &F21);
+
+ cv::Mat ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
+ cv::Mat ComputeF21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2);
+
+ float CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma);
+
+ float CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma);
+
+ bool ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
+ cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+ bool ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
+ cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated);
+
+ void Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D);
+
+ void Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T);
+
+ int CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
+ const vector<Match> &vMatches12, vector<bool> &vbInliers,
+ const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax);
+
+ void DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t);
+
+
+ // Keypoints from Reference Frame (Frame 1)
+ vector<cv::KeyPoint> mvKeys1;
+
+ // Keypoints from Current Frame (Frame 2)
+ vector<cv::KeyPoint> mvKeys2;
+
+ // Current Matches from Reference to Current
+ vector<Match> mvMatches12;
+ vector<bool> mvbMatched1;
+
+ // Calibration
+ cv::Mat mK;
+
+ // Standard Deviation and Variance
+ float mSigma, mSigma2;
+
+ // Ransac max iterations
+ int mMaxIterations;
+
+ // Ransac sets
+ vector<vector<size_t> > mvSets;
+
+};
+
+} //namespace ORB_SLAM
+
+#endif // INITIALIZER_H
diff --git a/externals/ORB_SLAM2/include/KeyFrame.h b/externals/ORB_SLAM2/include/KeyFrame.h
new file mode 100644
index 0000000000000000000000000000000000000000..67f4348273bc5c313d30f5cf225378e71066e153
--- /dev/null
+++ b/externals/ORB_SLAM2/include/KeyFrame.h
@@ -0,0 +1,238 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef KEYFRAME_H
+#define KEYFRAME_H
+
+#include "MapPoint.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+#include "ORBVocabulary.h"
+#include "ORBextractor.h"
+#include "Frame.h"
+#include "KeyFrameDatabase.h"
+
+#include <mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Map;
+class MapPoint;
+class Frame;
+class KeyFrameDatabase;
+
+class KeyFrame
+{
+public:
+ KeyFrame(Frame &F, Map* pMap, KeyFrameDatabase* pKFDB);
+
+ // Pose functions
+ void SetPose(const cv::Mat &Tcw);
+ cv::Mat GetPose();
+ cv::Mat GetPoseInverse();
+ cv::Mat GetCameraCenter();
+ cv::Mat GetStereoCenter();
+ cv::Mat GetRotation();
+ cv::Mat GetTranslation();
+
+ // Bag of Words Representation
+ void ComputeBoW();
+
+ // Covisibility graph functions
+ void AddConnection(KeyFrame* pKF, const int &weight);
+ void EraseConnection(KeyFrame* pKF);
+ void UpdateConnections();
+ void UpdateBestCovisibles();
+ std::set<KeyFrame *> GetConnectedKeyFrames();
+ std::vector<KeyFrame* > GetVectorCovisibleKeyFrames();
+ std::vector<KeyFrame*> GetBestCovisibilityKeyFrames(const int &N);
+ std::vector<KeyFrame*> GetCovisiblesByWeight(const int &w);
+ int GetWeight(KeyFrame* pKF);
+
+ // Spanning tree functions
+ void AddChild(KeyFrame* pKF);
+ void EraseChild(KeyFrame* pKF);
+ void ChangeParent(KeyFrame* pKF);
+ std::set<KeyFrame*> GetChilds();
+ KeyFrame* GetParent();
+ bool hasChild(KeyFrame* pKF);
+
+ // Loop Edges
+ void AddLoopEdge(KeyFrame* pKF);
+ std::set<KeyFrame*> GetLoopEdges();
+
+ // MapPoint observation functions
+ void AddMapPoint(MapPoint* pMP, const size_t &idx);
+ void EraseMapPointMatch(const size_t &idx);
+ void EraseMapPointMatch(MapPoint* pMP);
+ void ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP);
+ std::set<MapPoint*> GetMapPoints();
+ std::vector<MapPoint*> GetMapPointMatches();
+ int TrackedMapPoints(const int &minObs);
+ MapPoint* GetMapPoint(const size_t &idx);
+
+ // KeyPoint functions
+ std::vector<size_t> GetFeaturesInArea(const float &x, const float &y, const float &r) const;
+ cv::Mat UnprojectStereo(int i);
+
+ // Image
+ bool IsInImage(const float &x, const float &y) const;
+
+ // Enable/Disable bad flag changes
+ void SetNotErase();
+ void SetErase();
+
+ // Set/check bad flag
+ void SetBadFlag();
+ bool isBad();
+
+ // Compute Scene Depth (q=2 median). Used in monocular.
+ float ComputeSceneMedianDepth(const int q);
+
+ static bool weightComp( int a, int b){
+ return a>b;
+ }
+
+ static bool lId(KeyFrame* pKF1, KeyFrame* pKF2){
+ return pKF1->mnId<pKF2->mnId;
+ }
+
+
+ // The following variables are accesed from only 1 thread or never change (no mutex needed).
+public:
+
+ static long unsigned int nNextId;
+ long unsigned int mnId;
+ const long unsigned int mnFrameId;
+
+ const double mTimeStamp;
+
+ // Grid (to speed up feature matching)
+ const int mnGridCols;
+ const int mnGridRows;
+ const float mfGridElementWidthInv;
+ const float mfGridElementHeightInv;
+
+ // Variables used by the tracking
+ long unsigned int mnTrackReferenceForFrame;
+ long unsigned int mnFuseTargetForKF;
+
+ // Variables used by the local mapping
+ long unsigned int mnBALocalForKF;
+ long unsigned int mnBAFixedForKF;
+
+ // Variables used by the keyframe database
+ long unsigned int mnLoopQuery;
+ int mnLoopWords;
+ float mLoopScore;
+ long unsigned int mnRelocQuery;
+ int mnRelocWords;
+ float mRelocScore;
+
+ // Variables used by loop closing
+ cv::Mat mTcwGBA;
+ cv::Mat mTcwBefGBA;
+ long unsigned int mnBAGlobalForKF;
+
+ // Calibration parameters
+ const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
+
+ // Number of KeyPoints
+ const int N;
+
+ // KeyPoints, stereo coordinate and descriptors (all associated by an index)
+ const std::vector<cv::KeyPoint> mvKeys;
+ const std::vector<cv::KeyPoint> mvKeysUn;
+ const std::vector<float> mvuRight; // negative value for monocular points
+ const std::vector<float> mvDepth; // negative value for monocular points
+ const cv::Mat mDescriptors;
+
+ //BoW
+ DBoW2::BowVector mBowVec;
+ DBoW2::FeatureVector mFeatVec;
+
+ // Pose relative to parent (this is computed when bad flag is activated)
+ cv::Mat mTcp;
+
+ // Scale
+ const int mnScaleLevels;
+ const float mfScaleFactor;
+ const float mfLogScaleFactor;
+ const std::vector<float> mvScaleFactors;
+ const std::vector<float> mvLevelSigma2;
+ const std::vector<float> mvInvLevelSigma2;
+
+ // Image bounds and calibration
+ const int mnMinX;
+ const int mnMinY;
+ const int mnMaxX;
+ const int mnMaxY;
+ const cv::Mat mK;
+
+
+ // The following variables need to be accessed trough a mutex to be thread safe.
+protected:
+
+ // SE3 Pose and camera center
+ cv::Mat Tcw;
+ cv::Mat Twc;
+ cv::Mat Ow;
+
+ cv::Mat Cw; // Stereo middel point. Only for visualization
+
+ // MapPoints associated to keypoints
+ std::vector<MapPoint*> mvpMapPoints;
+
+ // BoW
+ KeyFrameDatabase* mpKeyFrameDB;
+ ORBVocabulary* mpORBvocabulary;
+
+ // Grid over the image to speed up feature matching
+ std::vector< std::vector <std::vector<size_t> > > mGrid;
+
+ std::map<KeyFrame*,int> mConnectedKeyFrameWeights;
+ std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames;
+ std::vector<int> mvOrderedWeights;
+
+ // Spanning Tree and Loop Edges
+ bool mbFirstConnection;
+ KeyFrame* mpParent;
+ std::set<KeyFrame*> mspChildrens;
+ std::set<KeyFrame*> mspLoopEdges;
+
+ // Bad flags
+ bool mbNotErase;
+ bool mbToBeErased;
+ bool mbBad;
+
+ float mHalfBaseline; // Only for visualization
+
+ Map* mpMap;
+
+ std::mutex mMutexPose;
+ std::mutex mMutexConnections;
+ std::mutex mMutexFeatures;
+};
+
+} //namespace ORB_SLAM
+
+#endif // KEYFRAME_H
diff --git a/externals/ORB_SLAM2/include/KeyFrameDatabase.h b/externals/ORB_SLAM2/include/KeyFrameDatabase.h
new file mode 100644
index 0000000000000000000000000000000000000000..fa3735762d46867883e2d3ecec2e5ecde077d521
--- /dev/null
+++ b/externals/ORB_SLAM2/include/KeyFrameDatabase.h
@@ -0,0 +1,74 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef KEYFRAMEDATABASE_H
+#define KEYFRAMEDATABASE_H
+
+#include <vector>
+#include <list>
+#include <set>
+
+#include "KeyFrame.h"
+#include "Frame.h"
+#include "ORBVocabulary.h"
+
+#include<mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class KeyFrame;
+class Frame;
+
+
+class KeyFrameDatabase
+{
+public:
+
+ KeyFrameDatabase(const ORBVocabulary &voc);
+
+ void add(KeyFrame* pKF);
+
+ void erase(KeyFrame* pKF);
+
+ void clear();
+
+ // Loop Detection
+ std::vector<KeyFrame *> DetectLoopCandidates(KeyFrame* pKF, float minScore);
+
+ // Relocalization
+ std::vector<KeyFrame*> DetectRelocalizationCandidates(Frame* F);
+
+protected:
+
+ // Associated vocabulary
+ const ORBVocabulary* mpVoc;
+
+ // Inverted file
+ std::vector<list<KeyFrame*> > mvInvertedFile;
+
+ // Mutex
+ std::mutex mMutex;
+};
+
+} //namespace ORB_SLAM
+
+#endif
diff --git a/externals/ORB_SLAM2/include/LocalMapping.h b/externals/ORB_SLAM2/include/LocalMapping.h
new file mode 100644
index 0000000000000000000000000000000000000000..1361a5757ab46a4d17e0a25483542a755f2f8fb7
--- /dev/null
+++ b/externals/ORB_SLAM2/include/LocalMapping.h
@@ -0,0 +1,128 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef LOCALMAPPING_H
+#define LOCALMAPPING_H
+
+#include "KeyFrame.h"
+#include "Map.h"
+#include "LoopClosing.h"
+#include "Tracking.h"
+#include "KeyFrameDatabase.h"
+
+#include <mutex>
+
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class LoopClosing;
+class Map;
+
+class LocalMapping
+{
+public:
+ LocalMapping(Map* pMap, const float bMonocular);
+
+ void SetLoopCloser(LoopClosing* pLoopCloser);
+
+ void SetTracker(Tracking* pTracker);
+
+ // Main function
+ void Run();
+
+ void InsertKeyFrame(KeyFrame* pKF);
+
+ // Thread Synch
+ void RequestStop();
+ void RequestReset();
+ bool Stop();
+ void Release();
+ bool isStopped();
+ bool stopRequested();
+ bool AcceptKeyFrames();
+ void SetAcceptKeyFrames(bool flag);
+ bool SetNotStop(bool flag);
+
+ void InterruptBA();
+
+ void RequestFinish();
+ bool isFinished();
+
+ int KeyframesInQueue(){
+ unique_lock<std::mutex> lock(mMutexNewKFs);
+ return mlNewKeyFrames.size();
+ }
+
+protected:
+
+ bool CheckNewKeyFrames();
+ void ProcessNewKeyFrame();
+ void CreateNewMapPoints();
+
+ void MapPointCulling();
+ void SearchInNeighbors();
+
+ void KeyFrameCulling();
+
+ cv::Mat ComputeF12(KeyFrame* &pKF1, KeyFrame* &pKF2);
+
+ cv::Mat SkewSymmetricMatrix(const cv::Mat &v);
+
+ bool mbMonocular;
+
+ void ResetIfRequested();
+ bool mbResetRequested;
+ std::mutex mMutexReset;
+
+ bool CheckFinish();
+ void SetFinish();
+ bool mbFinishRequested;
+ bool mbFinished;
+ std::mutex mMutexFinish;
+
+ Map* mpMap;
+
+ LoopClosing* mpLoopCloser;
+ Tracking* mpTracker;
+
+ std::list<KeyFrame*> mlNewKeyFrames;
+
+ KeyFrame* mpCurrentKeyFrame;
+
+ std::list<MapPoint*> mlpRecentAddedMapPoints;
+
+ std::mutex mMutexNewKFs;
+
+ bool mbAbortBA;
+
+ bool mbStopped;
+ bool mbStopRequested;
+ bool mbNotStop;
+ std::mutex mMutexStop;
+
+ bool mbAcceptKeyFrames;
+ std::mutex mMutexAccept;
+};
+
+} //namespace ORB_SLAM
+
+#endif // LOCALMAPPING_H
diff --git a/externals/ORB_SLAM2/include/LoopClosing.h b/externals/ORB_SLAM2/include/LoopClosing.h
new file mode 100644
index 0000000000000000000000000000000000000000..d1ac7d55c724c353b55830419ac60e7b73ee5cd6
--- /dev/null
+++ b/externals/ORB_SLAM2/include/LoopClosing.h
@@ -0,0 +1,151 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef LOOPCLOSING_H
+#define LOOPCLOSING_H
+
+#include "KeyFrame.h"
+#include "LocalMapping.h"
+#include "Map.h"
+#include "ORBVocabulary.h"
+#include "Tracking.h"
+
+#include "KeyFrameDatabase.h"
+
+#include <thread>
+#include <mutex>
+#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class LocalMapping;
+class KeyFrameDatabase;
+
+
+class LoopClosing
+{
+public:
+
+ typedef pair<set<KeyFrame*>,int> ConsistentGroup;
+ typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
+ Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
+
+public:
+
+ LoopClosing(Map* pMap, KeyFrameDatabase* pDB, ORBVocabulary* pVoc,const bool bFixScale);
+
+ void SetTracker(Tracking* pTracker);
+
+ void SetLocalMapper(LocalMapping* pLocalMapper);
+
+ // Main function
+ void Run();
+
+ void InsertKeyFrame(KeyFrame *pKF);
+
+ void RequestReset();
+
+ // This function will run in a separate thread
+ void RunGlobalBundleAdjustment(unsigned long nLoopKF);
+
+ bool isRunningGBA(){
+ unique_lock<std::mutex> lock(mMutexGBA);
+ return mbRunningGBA;
+ }
+ bool isFinishedGBA(){
+ unique_lock<std::mutex> lock(mMutexGBA);
+ return mbFinishedGBA;
+ }
+
+ void RequestFinish();
+
+ bool isFinished();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+protected:
+
+ bool CheckNewKeyFrames();
+
+ bool DetectLoop();
+
+ bool ComputeSim3();
+
+ void SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap);
+
+ void CorrectLoop();
+
+ void ResetIfRequested();
+ bool mbResetRequested;
+ std::mutex mMutexReset;
+
+ bool CheckFinish();
+ void SetFinish();
+ bool mbFinishRequested;
+ bool mbFinished;
+ std::mutex mMutexFinish;
+
+ Map* mpMap;
+ Tracking* mpTracker;
+
+ KeyFrameDatabase* mpKeyFrameDB;
+ ORBVocabulary* mpORBVocabulary;
+
+ LocalMapping *mpLocalMapper;
+
+ std::list<KeyFrame*> mlpLoopKeyFrameQueue;
+
+ std::mutex mMutexLoopQueue;
+
+ // Loop detector parameters
+ float mnCovisibilityConsistencyTh;
+
+ // Loop detector variables
+ KeyFrame* mpCurrentKF;
+ KeyFrame* mpMatchedKF;
+ std::vector<ConsistentGroup> mvConsistentGroups;
+ std::vector<KeyFrame*> mvpEnoughConsistentCandidates;
+ std::vector<KeyFrame*> mvpCurrentConnectedKFs;
+ std::vector<MapPoint*> mvpCurrentMatchedPoints;
+ std::vector<MapPoint*> mvpLoopMapPoints;
+ cv::Mat mScw;
+ g2o::Sim3 mg2oScw;
+
+ long unsigned int mLastLoopKFid;
+
+ // Variables related to Global Bundle Adjustment
+ bool mbRunningGBA;
+ bool mbFinishedGBA;
+ bool mbStopGBA;
+ std::mutex mMutexGBA;
+ std::thread* mpThreadGBA;
+
+ // Fix scale in the stereo/RGB-D case
+ bool mbFixScale;
+
+
+ bool mnFullBAIdx;
+};
+
+} //namespace ORB_SLAM
+
+#endif // LOOPCLOSING_H
diff --git a/externals/ORB_SLAM2/include/Map.h b/externals/ORB_SLAM2/include/Map.h
new file mode 100644
index 0000000000000000000000000000000000000000..a75339feeabceadf29ae76f216363e2473fe79fe
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Map.h
@@ -0,0 +1,85 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAP_H
+#define MAP_H
+
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include <set>
+
+#include <mutex>
+
+
+
+namespace ORB_SLAM2
+{
+
+class MapPoint;
+class KeyFrame;
+
+class Map
+{
+public:
+ Map();
+
+ void AddKeyFrame(KeyFrame* pKF);
+ void AddMapPoint(MapPoint* pMP);
+ void EraseMapPoint(MapPoint* pMP);
+ void EraseKeyFrame(KeyFrame* pKF);
+ void SetReferenceMapPoints(const std::vector<MapPoint*> &vpMPs);
+ void InformNewBigChange();
+ int GetLastBigChangeIdx();
+
+ std::vector<KeyFrame*> GetAllKeyFrames();
+ std::vector<MapPoint*> GetAllMapPoints();
+ std::vector<MapPoint*> GetReferenceMapPoints();
+
+ long unsigned int MapPointsInMap();
+ long unsigned KeyFramesInMap();
+
+ long unsigned int GetMaxKFid();
+
+ void clear();
+
+ vector<KeyFrame*> mvpKeyFrameOrigins;
+
+ std::mutex mMutexMapUpdate;
+
+ // This avoid that two points are created simultaneously in separate threads (id conflict)
+ std::mutex mMutexPointCreation;
+
+protected:
+ std::set<MapPoint*> mspMapPoints;
+ std::set<KeyFrame*> mspKeyFrames;
+
+ std::vector<MapPoint*> mvpReferenceMapPoints;
+
+ long unsigned int mnMaxKFid;
+
+ // Index related to a big change in the map (loop closure, global BA)
+ int mnBigChangeIdx;
+
+ std::mutex mMutexMap;
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAP_H
diff --git a/externals/ORB_SLAM2/include/MapDrawer.h b/externals/ORB_SLAM2/include/MapDrawer.h
new file mode 100644
index 0000000000000000000000000000000000000000..6e6f151959711e1fcb6bfdc85fe80a4cb4cc8922
--- /dev/null
+++ b/externals/ORB_SLAM2/include/MapDrawer.h
@@ -0,0 +1,44 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAPDRAWER_H
+#define MAPDRAWER_H
+
+#include <QVector3D>
+#include <QList>
+
+#include"Map.h"
+#include"MapPoint.h"
+#include"KeyFrame.h"
+#include<pangolin/pangolin.h>
+
+#include<mutex>
+#include <QJsonObject>
+
+namespace ORB_SLAM2
+{
+
+class MapDrawer
+{
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAPDRAWER_H
diff --git a/externals/ORB_SLAM2/include/MapPoint.h b/externals/ORB_SLAM2/include/MapPoint.h
new file mode 100644
index 0000000000000000000000000000000000000000..f26893d89d6c138f747fed460df54accd9c717a1
--- /dev/null
+++ b/externals/ORB_SLAM2/include/MapPoint.h
@@ -0,0 +1,152 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef MAPPOINT_H
+#define MAPPOINT_H
+
+#include"KeyFrame.h"
+#include"Frame.h"
+#include"Map.h"
+
+#include<opencv2/core/core.hpp>
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+class KeyFrame;
+class Map;
+class Frame;
+
+
+class MapPoint
+{
+public:
+ MapPoint(const cv::Mat &Pos, KeyFrame* pRefKF, Map* pMap);
+ MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF);
+
+ void SetWorldPos(const cv::Mat &Pos);
+ cv::Mat GetWorldPos();
+
+ cv::Mat GetNormal();
+ KeyFrame* GetReferenceKeyFrame();
+
+ std::map<KeyFrame*,size_t> GetObservations();
+ int Observations();
+
+ void AddObservation(KeyFrame* pKF,size_t idx);
+ void EraseObservation(KeyFrame* pKF);
+
+ int GetIndexInKeyFrame(KeyFrame* pKF);
+ bool IsInKeyFrame(KeyFrame* pKF);
+
+ void SetBadFlag();
+ bool isBad();
+
+ void Replace(MapPoint* pMP);
+ MapPoint* GetReplaced();
+
+ void IncreaseVisible(int n=1);
+ void IncreaseFound(int n=1);
+ float GetFoundRatio();
+ inline int GetFound(){
+ return mnFound;
+ }
+
+ void ComputeDistinctiveDescriptors();
+
+ cv::Mat GetDescriptor();
+
+ void UpdateNormalAndDepth();
+
+ float GetMinDistanceInvariance();
+ float GetMaxDistanceInvariance();
+ int PredictScale(const float ¤tDist, KeyFrame*pKF);
+ int PredictScale(const float ¤tDist, Frame* pF);
+
+public:
+ long unsigned int mnId;
+ static long unsigned int nNextId;
+ long int mnFirstKFid;
+ long int mnFirstFrame;
+ int nObs;
+
+ // Variables used by the tracking
+ float mTrackProjX;
+ float mTrackProjY;
+ float mTrackProjXR;
+ bool mbTrackInView;
+ int mnTrackScaleLevel;
+ float mTrackViewCos;
+ long unsigned int mnTrackReferenceForFrame;
+ long unsigned int mnLastFrameSeen;
+
+ // Variables used by local mapping
+ long unsigned int mnBALocalForKF;
+ long unsigned int mnFuseCandidateForKF;
+
+ // Variables used by loop closing
+ long unsigned int mnLoopPointForKF;
+ long unsigned int mnCorrectedByKF;
+ long unsigned int mnCorrectedReference;
+ cv::Mat mPosGBA;
+ long unsigned int mnBAGlobalForKF;
+
+
+ static std::mutex mGlobalMutex;
+
+protected:
+
+ // Position in absolute coordinates
+ cv::Mat mWorldPos;
+
+ // Keyframes observing the point and associated index in keyframe
+ std::map<KeyFrame*,size_t> mObservations;
+
+ // Mean viewing direction
+ cv::Mat mNormalVector;
+
+ // Best descriptor to fast matching
+ cv::Mat mDescriptor;
+
+ // Reference KeyFrame
+ KeyFrame* mpRefKF;
+
+ // Tracking counters
+ int mnVisible;
+ int mnFound;
+
+ // Bad flag (we do not currently erase MapPoint from memory)
+ bool mbBad;
+ MapPoint* mpReplaced;
+
+ // Scale invariance distances
+ float mfMinDistance;
+ float mfMaxDistance;
+
+ Map* mpMap;
+
+ std::mutex mMutexPos;
+ std::mutex mMutexFeatures;
+};
+
+} //namespace ORB_SLAM
+
+#endif // MAPPOINT_H
diff --git a/externals/ORB_SLAM2/include/ORBVocabulary.h b/externals/ORB_SLAM2/include/ORBVocabulary.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca5fa8d7778236eca2d739cb9585f68f8c4490b1
--- /dev/null
+++ b/externals/ORB_SLAM2/include/ORBVocabulary.h
@@ -0,0 +1,36 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef ORBVOCABULARY_H
+#define ORBVOCABULARY_H
+
+#include"Thirdparty/DBoW2/DBoW2/FORB.h"
+#include"Thirdparty/DBoW2/DBoW2/TemplatedVocabulary.h"
+
+namespace ORB_SLAM2
+{
+
+typedef DBoW2::TemplatedVocabulary<DBoW2::FORB::TDescriptor, DBoW2::FORB>
+ ORBVocabulary;
+
+} //namespace ORB_SLAM
+
+#endif // ORBVOCABULARY_H
diff --git a/externals/ORB_SLAM2/include/ORBextractor.h b/externals/ORB_SLAM2/include/ORBextractor.h
new file mode 100644
index 0000000000000000000000000000000000000000..66e8e7a547fc1c6c1582ca93cc86cd1b8f7b74fb
--- /dev/null
+++ b/externals/ORB_SLAM2/include/ORBextractor.h
@@ -0,0 +1,116 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef ORBEXTRACTOR_H
+#define ORBEXTRACTOR_H
+
+#include <vector>
+#include <list>
+#include <opencv/cv.h>
+
+
+namespace ORB_SLAM2
+{
+
+class ExtractorNode
+{
+public:
+ ExtractorNode():bNoMore(false){}
+
+ void DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4);
+
+ std::vector<cv::KeyPoint> vKeys;
+ cv::Point2i UL, UR, BL, BR;
+ std::list<ExtractorNode>::iterator lit;
+ bool bNoMore;
+};
+
+class ORBextractor
+{
+public:
+
+ enum {HARRIS_SCORE=0, FAST_SCORE=1 };
+
+ ORBextractor(int nfeatures, float scaleFactor, int nlevels,
+ int iniThFAST, int minThFAST);
+
+ ~ORBextractor(){}
+
+ // Compute the ORB features and descriptors on an image.
+ // ORB are dispersed on the image using an octree.
+ // Mask is ignored in the current implementation.
+ void operator()( cv::InputArray image, cv::InputArray mask,
+ std::vector<cv::KeyPoint>& keypoints,
+ cv::OutputArray descriptors);
+
+ int inline GetLevels(){
+ return nlevels;}
+
+ float inline GetScaleFactor(){
+ return scaleFactor;}
+
+ std::vector<float> inline GetScaleFactors(){
+ return mvScaleFactor;
+ }
+
+ std::vector<float> inline GetInverseScaleFactors(){
+ return mvInvScaleFactor;
+ }
+
+ std::vector<float> inline GetScaleSigmaSquares(){
+ return mvLevelSigma2;
+ }
+
+ std::vector<float> inline GetInverseScaleSigmaSquares(){
+ return mvInvLevelSigma2;
+ }
+
+ std::vector<cv::Mat> mvImagePyramid;
+
+protected:
+
+ void ComputePyramid(cv::Mat image);
+ void ComputeKeyPointsOctTree(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
+ std::vector<cv::KeyPoint> DistributeOctTree(const std::vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
+ const int &maxX, const int &minY, const int &maxY, const int &nFeatures, const int &level);
+
+ void ComputeKeyPointsOld(std::vector<std::vector<cv::KeyPoint> >& allKeypoints);
+ std::vector<cv::Point> pattern;
+
+ int nfeatures;
+ double scaleFactor;
+ int nlevels;
+ int iniThFAST;
+ int minThFAST;
+
+ std::vector<int> mnFeaturesPerLevel;
+
+ std::vector<int> umax;
+
+ std::vector<float> mvScaleFactor;
+ std::vector<float> mvInvScaleFactor;
+ std::vector<float> mvLevelSigma2;
+ std::vector<float> mvInvLevelSigma2;
+};
+
+} //namespace ORB_SLAM
+
+#endif
+
diff --git a/externals/ORB_SLAM2/include/ORBmatcher.h b/externals/ORB_SLAM2/include/ORBmatcher.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad881ee04ae9556a1b184f6c29b98eee62dddd01
--- /dev/null
+++ b/externals/ORB_SLAM2/include/ORBmatcher.h
@@ -0,0 +1,106 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef ORBMATCHER_H
+#define ORBMATCHER_H
+
+#include<vector>
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include"MapPoint.h"
+#include"KeyFrame.h"
+#include"Frame.h"
+
+
+namespace ORB_SLAM2
+{
+
+class ORBmatcher
+{
+public:
+
+ ORBmatcher(float nnratio=0.6, bool checkOri=true);
+
+ // Computes the Hamming distance between two ORB descriptors
+ static int DescriptorDistance(const cv::Mat &a, const cv::Mat &b);
+
+ // Search matches between Frame keypoints and projected MapPoints. Returns number of matches
+ // Used to track the local map (Tracking)
+ int SearchByProjection(Frame &F, const std::vector<MapPoint*> &vpMapPoints, const float th=3);
+
+ // Project MapPoints tracked in last frame into the current frame and search matches.
+ // Used to track from previous frame (Tracking)
+ int SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono);
+
+ // Project MapPoints seen in KeyFrame into the Frame and search matches.
+ // Used in relocalisation (Tracking)
+ int SearchByProjection(Frame &CurrentFrame, KeyFrame* pKF, const std::set<MapPoint*> &sAlreadyFound, const float th, const int ORBdist);
+
+ // Project MapPoints using a Similarity Transformation and search matches.
+ // Used in loop detection (Loop Closing)
+ int SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, std::vector<MapPoint*> &vpMatched, int th);
+
+ // Search matches between MapPoints in a KeyFrame and ORB in a Frame.
+ // Brute force constrained to ORB that belong to the same vocabulary node (at a certain level)
+ // Used in Relocalisation and Loop Detection
+ int SearchByBoW(KeyFrame *pKF, Frame &F, std::vector<MapPoint*> &vpMapPointMatches);
+ int SearchByBoW(KeyFrame *pKF1, KeyFrame* pKF2, std::vector<MapPoint*> &vpMatches12);
+
+ // Matching for the Map Initialization (only used in the monocular case)
+ int SearchForInitialization(Frame &F1, Frame &F2, std::vector<cv::Point2f> &vbPrevMatched, std::vector<int> &vnMatches12, int windowSize=10);
+
+ // Matching to triangulate new MapPoints. Check Epipolar Constraint.
+ int SearchForTriangulation(KeyFrame *pKF1, KeyFrame* pKF2, cv::Mat F12,
+ std::vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo);
+
+ // Search matches between MapPoints seen in KF1 and KF2 transforming by a Sim3 [s12*R12|t12]
+ // In the stereo and RGB-D case, s12=1
+ int SearchBySim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches12, const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th);
+
+ // Project MapPoints into KeyFrame and search for duplicated MapPoints.
+ int Fuse(KeyFrame* pKF, const vector<MapPoint *> &vpMapPoints, const float th=3.0);
+
+ // Project MapPoints into KeyFrame using a given Sim3 and search for duplicated MapPoints.
+ int Fuse(KeyFrame* pKF, cv::Mat Scw, const std::vector<MapPoint*> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint);
+
+public:
+
+ static const int TH_LOW;
+ static const int TH_HIGH;
+ static const int HISTO_LENGTH;
+
+
+protected:
+
+ bool CheckDistEpipolarLine(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &F12, const KeyFrame *pKF);
+
+ float RadiusByViewingCos(const float &viewCos);
+
+ void ComputeThreeMaxima(std::vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3);
+
+ float mfNNratio;
+ bool mbCheckOrientation;
+};
+
+}// namespace ORB_SLAM
+
+#endif // ORBMATCHER_H
diff --git a/externals/ORB_SLAM2/include/Optimizer.h b/externals/ORB_SLAM2/include/Optimizer.h
new file mode 100644
index 0000000000000000000000000000000000000000..2c4378ed3b5a7c4b6b4f47efc5e1dbcf76dc6723
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Optimizer.h
@@ -0,0 +1,62 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#ifndef OPTIMIZER_H
+#define OPTIMIZER_H
+
+#include "Map.h"
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include "LoopClosing.h"
+#include "Frame.h"
+
+#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+namespace ORB_SLAM2
+{
+
+class LoopClosing;
+
+class Optimizer
+{
+public:
+ void static BundleAdjustment(const std::vector<KeyFrame*> &vpKF, const std::vector<MapPoint*> &vpMP,
+ int nIterations = 5, bool *pbStopFlag=NULL, const unsigned long nLoopKF=0,
+ const bool bRobust = true);
+ void static GlobalBundleAdjustemnt(Map* pMap, int nIterations=5, bool *pbStopFlag=NULL,
+ const unsigned long nLoopKF=0, const bool bRobust = true);
+ void static LocalBundleAdjustment(KeyFrame* pKF, bool *pbStopFlag, Map *pMap);
+ int static PoseOptimization(Frame* pFrame);
+
+ // if bFixScale is true, 6DoF optimization (stereo,rgbd), 7DoF otherwise (mono)
+ void static OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+ const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
+ const LoopClosing::KeyFrameAndPose &CorrectedSim3,
+ const map<KeyFrame *, set<KeyFrame *> > &LoopConnections,
+ const bool &bFixScale);
+
+ // if bFixScale is true, optimize SE3 (stereo,rgbd), Sim3 otherwise (mono)
+ static int OptimizeSim3(KeyFrame* pKF1, KeyFrame* pKF2, std::vector<MapPoint *> &vpMatches1,
+ g2o::Sim3 &g2oS12, const float th2, const bool bFixScale);
+};
+
+} //namespace ORB_SLAM
+
+#endif // OPTIMIZER_H
diff --git a/externals/ORB_SLAM2/include/PnPsolver.h b/externals/ORB_SLAM2/include/PnPsolver.h
new file mode 100644
index 0000000000000000000000000000000000000000..f92544fc8fe3be7ac8bcfdcdeefd1521ab0842ac
--- /dev/null
+++ b/externals/ORB_SLAM2/include/PnPsolver.h
@@ -0,0 +1,198 @@
+/**
+* This file is part of ORB-SLAM2.
+* This file is a modified version of EPnP <http://cvlab.epfl.ch/EPnP/index.php>, see FreeBSD license below.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+* Copyright (c) 2009, V. Lepetit, EPFL
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright notice, this
+* list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright notice,
+* this list of conditions and the following disclaimer in the documentation
+* and/or other materials provided with the distribution.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+* The views and conclusions contained in the software and documentation are those
+* of the authors and should not be interpreted as representing official policies,
+* either expressed or implied, of the FreeBSD Project
+*/
+
+#ifndef PNPSOLVER_H
+#define PNPSOLVER_H
+
+#include <opencv2/core/core.hpp>
+#include "MapPoint.h"
+#include "Frame.h"
+
+namespace ORB_SLAM2
+{
+
+class PnPsolver {
+ public:
+ PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches);
+
+ ~PnPsolver();
+
+ void SetRansacParameters(double probability = 0.99, int minInliers = 8 , int maxIterations = 300, int minSet = 4, float epsilon = 0.4,
+ float th2 = 5.991);
+
+ cv::Mat find(vector<bool> &vbInliers, int &nInliers);
+
+ cv::Mat iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers);
+
+ private:
+
+ void CheckInliers();
+ bool Refine();
+
+ // Functions from the original EPnP code
+ void set_maximum_number_of_correspondences(const int n);
+ void reset_correspondences(void);
+ void add_correspondence(const double X, const double Y, const double Z,
+ const double u, const double v);
+
+ double compute_pose(double R[3][3], double T[3]);
+
+ void relative_error(double & rot_err, double & transl_err,
+ const double Rtrue[3][3], const double ttrue[3],
+ const double Rest[3][3], const double test[3]);
+
+ void print_pose(const double R[3][3], const double t[3]);
+ double reprojection_error(const double R[3][3], const double t[3]);
+
+ void choose_control_points(void);
+ void compute_barycentric_coordinates(void);
+ void fill_M(CvMat * M, const int row, const double * alphas, const double u, const double v);
+ void compute_ccs(const double * betas, const double * ut);
+ void compute_pcs(void);
+
+ void solve_for_sign(void);
+
+ void find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+ void find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+ void find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho, double * betas);
+ void qr_solve(CvMat * A, CvMat * b, CvMat * X);
+
+ double dot(const double * v1, const double * v2);
+ double dist2(const double * p1, const double * p2);
+
+ void compute_rho(double * rho);
+ void compute_L_6x10(const double * ut, double * l_6x10);
+
+ void gauss_newton(const CvMat * L_6x10, const CvMat * Rho, double current_betas[4]);
+ void compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
+ double cb[4], CvMat * A, CvMat * b);
+
+ double compute_R_and_t(const double * ut, const double * betas,
+ double R[3][3], double t[3]);
+
+ void estimate_R_and_t(double R[3][3], double t[3]);
+
+ void copy_R_and_t(const double R_dst[3][3], const double t_dst[3],
+ double R_src[3][3], double t_src[3]);
+
+ void mat_to_quat(const double R[3][3], double q[4]);
+
+
+ double uc, vc, fu, fv;
+
+ double * pws, * us, * alphas, * pcs;
+ int maximum_number_of_correspondences;
+ int number_of_correspondences;
+
+ double cws[4][3], ccs[4][3];
+ double cws_determinant;
+
+ vector<MapPoint*> mvpMapPointMatches;
+
+ // 2D Points
+ vector<cv::Point2f> mvP2D;
+ vector<float> mvSigma2;
+
+ // 3D Points
+ vector<cv::Point3f> mvP3Dw;
+
+ // Index in Frame
+ vector<size_t> mvKeyPointIndices;
+
+ // Current Estimation
+ double mRi[3][3];
+ double mti[3];
+ cv::Mat mTcwi;
+ vector<bool> mvbInliersi;
+ int mnInliersi;
+
+ // Current Ransac State
+ int mnIterations;
+ vector<bool> mvbBestInliers;
+ int mnBestInliers;
+ cv::Mat mBestTcw;
+
+ // Refined
+ cv::Mat mRefinedTcw;
+ vector<bool> mvbRefinedInliers;
+ int mnRefinedInliers;
+
+ // Number of Correspondences
+ int N;
+
+ // Indices for random selection [0 .. N-1]
+ vector<size_t> mvAllIndices;
+
+ // RANSAC probability
+ double mRansacProb;
+
+ // RANSAC min inliers
+ int mRansacMinInliers;
+
+ // RANSAC max iterations
+ int mRansacMaxIts;
+
+ // RANSAC expected inliers/total ratio
+ float mRansacEpsilon;
+
+ // RANSAC Threshold inlier/outlier. Max error e = dist(P1,T_12*P2)^2
+ float mRansacTh;
+
+ // RANSAC Minimun Set used at each iteration
+ int mRansacMinSet;
+
+ // Max square error associated with scale level. Max error = th*th*sigma(level)*sigma(level)
+ vector<float> mvMaxError;
+
+};
+
+} //namespace ORB_SLAM
+
+#endif //PNPSOLVER_H
diff --git a/externals/ORB_SLAM2/include/Sim3Solver.h b/externals/ORB_SLAM2/include/Sim3Solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..9af66cb26fbc2b4a91d04247973f5eaae2ddbfdf
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Sim3Solver.h
@@ -0,0 +1,133 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef SIM3SOLVER_H
+#define SIM3SOLVER_H
+
+#include <opencv2/opencv.hpp>
+#include <vector>
+
+#include "KeyFrame.h"
+
+
+
+namespace ORB_SLAM2
+{
+
+class Sim3Solver
+{
+public:
+
+ Sim3Solver(KeyFrame* pKF1, KeyFrame* pKF2, const std::vector<MapPoint*> &vpMatched12, const bool bFixScale = true);
+
+ void SetRansacParameters(double probability = 0.99, int minInliers = 6 , int maxIterations = 300);
+
+ cv::Mat find(std::vector<bool> &vbInliers12, int &nInliers);
+
+ cv::Mat iterate(int nIterations, bool &bNoMore, std::vector<bool> &vbInliers, int &nInliers);
+
+ cv::Mat GetEstimatedRotation();
+ cv::Mat GetEstimatedTranslation();
+ float GetEstimatedScale();
+
+
+protected:
+
+ void ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C);
+
+ void ComputeSim3(cv::Mat &P1, cv::Mat &P2);
+
+ void CheckInliers();
+
+ void Project(const std::vector<cv::Mat> &vP3Dw, std::vector<cv::Mat> &vP2D, cv::Mat Tcw, cv::Mat K);
+ void FromCameraToImage(const std::vector<cv::Mat> &vP3Dc, std::vector<cv::Mat> &vP2D, cv::Mat K);
+
+
+protected:
+
+ // KeyFrames and matches
+ KeyFrame* mpKF1;
+ KeyFrame* mpKF2;
+
+ std::vector<cv::Mat> mvX3Dc1;
+ std::vector<cv::Mat> mvX3Dc2;
+ std::vector<MapPoint*> mvpMapPoints1;
+ std::vector<MapPoint*> mvpMapPoints2;
+ std::vector<MapPoint*> mvpMatches12;
+ std::vector<size_t> mvnIndices1;
+ std::vector<size_t> mvSigmaSquare1;
+ std::vector<size_t> mvSigmaSquare2;
+ std::vector<size_t> mvnMaxError1;
+ std::vector<size_t> mvnMaxError2;
+
+ int N;
+ int mN1;
+
+ // Current Estimation
+ cv::Mat mR12i;
+ cv::Mat mt12i;
+ float ms12i;
+ cv::Mat mT12i;
+ cv::Mat mT21i;
+ std::vector<bool> mvbInliersi;
+ int mnInliersi;
+
+ // Current Ransac State
+ int mnIterations;
+ std::vector<bool> mvbBestInliers;
+ int mnBestInliers;
+ cv::Mat mBestT12;
+ cv::Mat mBestRotation;
+ cv::Mat mBestTranslation;
+ float mBestScale;
+
+ // Scale is fixed to 1 in the stereo/RGBD case
+ bool mbFixScale;
+
+ // Indices for random selection
+ std::vector<size_t> mvAllIndices;
+
+ // Projections
+ std::vector<cv::Mat> mvP1im1;
+ std::vector<cv::Mat> mvP2im2;
+
+ // RANSAC probability
+ double mRansacProb;
+
+ // RANSAC min inliers
+ int mRansacMinInliers;
+
+ // RANSAC max iterations
+ int mRansacMaxIts;
+
+ // Threshold inlier/outlier. e = dist(Pi,T_ij*Pj)^2 < 5.991*mSigma2
+ float mTh;
+ float mSigma2;
+
+ // Calibration
+ cv::Mat mK1;
+ cv::Mat mK2;
+
+};
+
+} //namespace ORB_SLAM
+
+#endif // SIM3SOLVER_H
diff --git a/externals/ORB_SLAM2/include/System.h b/externals/ORB_SLAM2/include/System.h
new file mode 100644
index 0000000000000000000000000000000000000000..8aed7cb91df06be8e261ef22eafc73f8f44b8385
--- /dev/null
+++ b/externals/ORB_SLAM2/include/System.h
@@ -0,0 +1,186 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef SYSTEM_H
+#define SYSTEM_H
+
+#include <unistd.h>
+#include<string>
+#include<thread>
+#include<opencv2/core/core.hpp>
+#include <QJsonObject>
+
+#include "Tracking.h"
+#include "FrameDrawer.h"
+#include "MapDrawer.h"
+#include "Map.h"
+#include "LocalMapping.h"
+#include "LoopClosing.h"
+#include "KeyFrameDatabase.h"
+#include "ORBVocabulary.h"
+#include "Viewer.h"
+
+namespace ORB_SLAM2
+{
+
+class Viewer;
+class FrameDrawer;
+class Map;
+class Tracking;
+class LocalMapping;
+class LoopClosing;
+
+class System
+{
+public:
+ // Input sensor
+ enum eSensor{
+ MONOCULAR=0,
+ STEREO=1,
+ RGBD=2
+ };
+
+public:
+
+ // Initialize the SLAM system. It launches the Local Mapping, Loop Closing and Viewer threads.
+ System(ORBVocabulary *voc, const QJsonObject settingsFile, const eSensor sensor,
+ const bool bUseViewer, bool loopClosing);
+
+ // Proccess the given stereo frame. Images must be synchronized and rectified.
+ // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+ // Returns the camera pose (empty if tracking fails).
+ cv::Mat TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp);
+
+ // Process the given rgbd frame. Depthmap must be registered to the RGB frame.
+ // Input image: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+ // Input depthmap: Float (CV_32F).
+ // Returns the camera pose (empty if tracking fails).
+ cv::Mat TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp);
+
+ // Proccess the given monocular frame
+ // Input images: RGB (CV_8UC3) or grayscale (CV_8U). RGB is converted to grayscale.
+ // Returns the camera pose (empty if tracking fails).
+ cv::Mat TrackMonocular(const cv::Mat &im, const double ×tamp);
+
+ // This stops local mapping thread (map building) and performs only camera tracking.
+ void ActivateLocalizationMode();
+ // This resumes local mapping thread and performs SLAM again.
+ void DeactivateLocalizationMode();
+
+ // Returns true if there have been a big map change (loop closure, global BA)
+ // since last call to this function
+ bool MapChanged();
+
+ // Reset the system (clear map)
+ void Reset();
+
+ // All threads will be requested to finish.
+ // It waits until all threads have finished.
+ // This function must be called before saving the trajectory.
+ void Shutdown();
+
+ // Save camera trajectory in the TUM RGB-D dataset format.
+ // Only for stereo and RGB-D. This method does not work for monocular.
+ // Call first Shutdown()
+ // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+ void SaveTrajectoryTUM(const string &filename);
+
+ // Save keyframe poses in the TUM RGB-D dataset format.
+ // This method works for all sensor input.
+ // Call first Shutdown()
+ // See format details at: http://vision.in.tum.de/data/datasets/rgbd-dataset
+ void SaveKeyFrameTrajectoryTUM(const string &filename);
+
+ // Save camera trajectory in the KITTI dataset format.
+ // Only for stereo and RGB-D. This method does not work for monocular.
+ // Call first Shutdown()
+ // See format details at: http://www.cvlibs.net/datasets/kitti/eval_odometry.php
+ void SaveTrajectoryKITTI(const string &filename);
+
+ // TODO: Save/Load functions
+ // SaveMap(const string &filename);
+ // LoadMap(const string &filename);
+
+ // Information from most recent processed frame
+ // You can call this right after TrackMonocular (or stereo or RGBD)
+ int GetTrackingState();
+ std::vector<MapPoint*> GetTrackedMapPoints();
+ std::vector<cv::KeyPoint> GetTrackedKeyPointsUn();
+
+private:
+
+ // Input sensor
+ eSensor mSensor;
+
+ // ORB vocabulary used for place recognition and feature matching.
+ ORBVocabulary* mpVocabulary;
+
+ // KeyFrame database for place recognition (relocalization and loop detection).
+ KeyFrameDatabase* mpKeyFrameDatabase;
+
+ // Map structure that stores the pointers to all KeyFrames and MapPoints.
+ Map* mpMap;
+
+ // Tracker. It receives a frame and computes the associated camera pose.
+ // It also decides when to insert a new keyframe, create some new MapPoints and
+ // performs relocalization if tracking fails.
+ Tracking* mpTracker;
+
+ // Local Mapper. It manages the local map and performs local bundle adjustment.
+ LocalMapping* mpLocalMapper;
+
+ // Loop Closer. It searches loops with every new keyframe. If there is a loop it performs
+ // a pose graph optimization and full bundle adjustment (in a new thread) afterwards.
+ LoopClosing* mpLoopCloser;
+
+ // The viewer draws the map and the current camera pose. It uses Pangolin.
+ Viewer* mpViewer;
+
+ FrameDrawer* mpFrameDrawer;
+ MapDrawer* mpMapDrawer;
+
+ // System threads: Local Mapping, Loop Closing, Viewer.
+ // The Tracking thread "lives" in the main execution thread that creates the System object.
+ std::thread* mptLocalMapping;
+ std::thread* mptLoopClosing;
+ std::thread* mptViewer;
+
+ // Reset flag
+ std::mutex mMutexReset;
+ bool mbReset;
+
+ // Change mode flags
+ std::mutex mMutexMode;
+ bool mbActivateLocalizationMode;
+ bool mbDeactivateLocalizationMode;
+
+ // Tracking state
+ int mTrackingState;
+ std::vector<MapPoint*> mTrackedMapPoints;
+ std::vector<cv::KeyPoint> mTrackedKeyPointsUn;
+ std::mutex mMutexState;
+
+ bool mLoopClosing;
+};
+
+}// namespace ORB_SLAM
+
+#endif // SYSTEM_H
diff --git a/externals/ORB_SLAM2/include/Tracking.h b/externals/ORB_SLAM2/include/Tracking.h
new file mode 100644
index 0000000000000000000000000000000000000000..db5b2f05c0b2b5319916fc77246e9dd165991b42
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Tracking.h
@@ -0,0 +1,224 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef TRACKING_H
+#define TRACKING_H
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include"Viewer.h"
+#include"FrameDrawer.h"
+#include"Map.h"
+#include"LocalMapping.h"
+#include"LoopClosing.h"
+#include"Frame.h"
+#include "ORBVocabulary.h"
+#include"KeyFrameDatabase.h"
+#include"ORBextractor.h"
+#include "Initializer.h"
+#include "MapDrawer.h"
+#include "System.h"
+
+#include <mutex>
+
+#include <QJsonObject>
+
+
+namespace ORB_SLAM2
+{
+
+class Viewer;
+class FrameDrawer;
+class Map;
+class LocalMapping;
+class LoopClosing;
+class System;
+
+class Tracking
+{
+
+public:
+ Tracking(System* pSys, ORBVocabulary* pVoc, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Map* pMap,
+ KeyFrameDatabase* pKFDB, const QJsonObject settings, const int sensor);
+
+ // Preprocess the input and call Track(). Extract features and performs stereo matching.
+ cv::Mat GrabImageStereo(const cv::Mat &imRectLeft,const cv::Mat &imRectRight, const double ×tamp);
+ cv::Mat GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp);
+ cv::Mat GrabImageMonocular(const cv::Mat &im, const double ×tamp);
+
+ void SetLocalMapper(LocalMapping* pLocalMapper);
+ void SetLoopClosing(LoopClosing* pLoopClosing);
+ void SetViewer(Viewer* pViewer);
+
+ // Load new settings
+ // The focal lenght should be similar or scale prediction will fail when projecting points
+ // TODO: Modify MapPoint::PredictScale to take into account focal lenght
+ void ChangeCalibration(const string &strSettingPath);
+
+ // Use this function if you have deactivated local mapping and you only want to localize the camera.
+ void InformOnlyTracking(const bool &flag);
+
+
+public:
+
+ // Tracking states
+ enum eTrackingState{
+ SYSTEM_NOT_READY=-1,
+ NO_IMAGES_YET=0,
+ NOT_INITIALIZED=1,
+ OK=2,
+ LOST=3
+ };
+
+ eTrackingState mState;
+ eTrackingState mLastProcessedState;
+
+ // Input sensor
+ int mSensor;
+
+ // Current Frame
+ Frame mCurrentFrame;
+ cv::Mat mImGray;
+
+ // Initialization Variables (Monocular)
+ std::vector<int> mvIniLastMatches;
+ std::vector<int> mvIniMatches;
+ std::vector<cv::Point2f> mvbPrevMatched;
+ std::vector<cv::Point3f> mvIniP3D;
+ Frame mInitialFrame;
+
+ // Lists used to recover the full camera trajectory at the end of the execution.
+ // Basically we store the reference keyframe for each frame and its relative transformation
+ list<cv::Mat> mlRelativeFramePoses;
+ list<KeyFrame*> mlpReferences;
+ list<double> mlFrameTimes;
+ list<bool> mlbLost;
+
+ // True if local mapping is deactivated and we are performing only localization
+ bool mbOnlyTracking;
+
+ void Reset();
+
+protected:
+
+ // Main tracking function. It is independent of the input sensor.
+ void Track();
+
+ // Map initialization for stereo and RGB-D
+ void StereoInitialization();
+
+ // Map initialization for monocular
+ void MonocularInitialization();
+ void CreateInitialMapMonocular();
+
+ void CheckReplacedInLastFrame();
+ bool TrackReferenceKeyFrame();
+ void UpdateLastFrame();
+ bool TrackWithMotionModel();
+
+ bool Relocalization();
+
+ void UpdateLocalMap();
+ void UpdateLocalPoints();
+ void UpdateLocalKeyFrames();
+
+ bool TrackLocalMap();
+ void SearchLocalPoints();
+
+ bool NeedNewKeyFrame();
+ void CreateNewKeyFrame();
+
+ // In case of performing only localization, this flag is true when there are no matches to
+ // points in the map. Still tracking will continue if there are enough matches with temporal points.
+ // In that case we are doing visual odometry. The system will try to do relocalization to recover
+ // "zero-drift" localization to the map.
+ bool mbVO;
+
+ //Other Thread Pointers
+ LocalMapping* mpLocalMapper;
+ LoopClosing* mpLoopClosing;
+
+ //ORB
+ ORBextractor* mpORBextractorLeft, *mpORBextractorRight;
+ ORBextractor* mpIniORBextractor;
+
+ //BoW
+ ORBVocabulary* mpORBVocabulary;
+ KeyFrameDatabase* mpKeyFrameDB;
+
+ // Initalization (only for monocular)
+ Initializer* mpInitializer;
+
+ //Local Map
+ KeyFrame* mpReferenceKF;
+ std::vector<KeyFrame*> mvpLocalKeyFrames;
+ std::vector<MapPoint*> mvpLocalMapPoints;
+
+ // System
+ System* mpSystem;
+
+ //Drawers
+ Viewer* mpViewer;
+ FrameDrawer* mpFrameDrawer;
+ MapDrawer* mpMapDrawer;
+
+ //Map
+ Map* mpMap;
+
+ //Calibration matrix
+ cv::Mat mK;
+ cv::Mat mDistCoef;
+ float mbf;
+
+ //New KeyFrame rules (according to fps)
+ int mMinFrames;
+ int mMaxFrames;
+
+ // Threshold close/far points
+ // Points seen as close by the stereo/RGBD sensor are considered reliable
+ // and inserted from just one frame. Far points requiere a match in two keyframes.
+ float mThDepth;
+
+ // For RGB-D inputs only. For some datasets (e.g. TUM) the depthmap values are scaled.
+ float mDepthMapFactor;
+
+ //Current matches in frame
+ int mnMatchesInliers;
+
+ //Last Frame, KeyFrame and Relocalisation Info
+ KeyFrame* mpLastKeyFrame;
+ Frame mLastFrame;
+ unsigned int mnLastKeyFrameId;
+ unsigned int mnLastRelocFrameId;
+
+ //Motion Model
+ cv::Mat mVelocity;
+
+ //Color order (true RGB, false BGR, ignored if grayscale)
+ bool mbRGB;
+
+ list<MapPoint*> mlpTemporalPoints;
+};
+
+} //namespace ORB_SLAM
+
+#endif // TRACKING_H
diff --git a/externals/ORB_SLAM2/include/Viewer.h b/externals/ORB_SLAM2/include/Viewer.h
new file mode 100644
index 0000000000000000000000000000000000000000..251e223c54f5b41ad460dd96ae1bbdd5ad0bb9ef
--- /dev/null
+++ b/externals/ORB_SLAM2/include/Viewer.h
@@ -0,0 +1,91 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#ifndef VIEWER_H
+#define VIEWER_H
+
+#include "FrameDrawer.h"
+#include "MapDrawer.h"
+#include "Tracking.h"
+#include "System.h"
+
+#include <mutex>
+
+namespace ORB_SLAM2
+{
+
+class Tracking;
+class FrameDrawer;
+class MapDrawer;
+class System;
+
+class Viewer
+{
+public:
+ Viewer(System* pSystem, FrameDrawer* pFrameDrawer, MapDrawer* pMapDrawer, Tracking *pTracking, const string &strSettingPath);
+
+ // Main thread function. Draw points, keyframes, the current camera pose and the last processed
+ // frame. Drawing is refreshed according to the camera fps. We use Pangolin.
+ void Run();
+
+ void RequestFinish();
+
+ void RequestStop();
+
+ bool isFinished();
+
+ bool isStopped();
+
+ void Release();
+
+private:
+
+ bool Stop();
+
+ System* mpSystem;
+ FrameDrawer* mpFrameDrawer;
+ MapDrawer* mpMapDrawer;
+ Tracking* mpTracker;
+
+ // 1/fps in ms
+ double mT;
+ float mImageWidth, mImageHeight;
+
+ float mViewpointX, mViewpointY, mViewpointZ, mViewpointF;
+
+ bool CheckFinish();
+ void SetFinish();
+ bool mbFinishRequested;
+ bool mbFinished;
+ std::mutex mMutexFinish;
+
+ bool mbStopped;
+ bool mbStopRequested;
+ std::mutex mMutexStop;
+
+};
+
+}
+
+
+#endif // VIEWER_H
+
+
diff --git a/externals/ORB_SLAM2/src/Converter.cc b/externals/ORB_SLAM2/src/Converter.cc
new file mode 100644
index 0000000000000000000000000000000000000000..00ea8ee597861d515190c39c343d9a3903c978c3
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Converter.cc
@@ -0,0 +1,151 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Converter.h"
+
+namespace ORB_SLAM2
+{
+
+std::vector<cv::Mat> Converter::toDescriptorVector(const cv::Mat &Descriptors)
+{
+ std::vector<cv::Mat> vDesc;
+ vDesc.reserve(Descriptors.rows);
+ for (int j=0;j<Descriptors.rows;j++)
+ vDesc.push_back(Descriptors.row(j));
+
+ return vDesc;
+}
+
+g2o::SE3Quat Converter::toSE3Quat(const cv::Mat &cvT)
+{
+ Eigen::Matrix<double,3,3> R;
+ R << cvT.at<float>(0,0), cvT.at<float>(0,1), cvT.at<float>(0,2),
+ cvT.at<float>(1,0), cvT.at<float>(1,1), cvT.at<float>(1,2),
+ cvT.at<float>(2,0), cvT.at<float>(2,1), cvT.at<float>(2,2);
+
+ Eigen::Matrix<double,3,1> t(cvT.at<float>(0,3), cvT.at<float>(1,3), cvT.at<float>(2,3));
+
+ return g2o::SE3Quat(R,t);
+}
+
+cv::Mat Converter::toCvMat(const g2o::SE3Quat &SE3)
+{
+ Eigen::Matrix<double,4,4> eigMat = SE3.to_homogeneous_matrix();
+ return toCvMat(eigMat);
+}
+
+cv::Mat Converter::toCvMat(const g2o::Sim3 &Sim3)
+{
+ Eigen::Matrix3d eigR = Sim3.rotation().toRotationMatrix();
+ Eigen::Vector3d eigt = Sim3.translation();
+ double s = Sim3.scale();
+ return toCvSE3(s*eigR,eigt);
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix<double,4,4> &m)
+{
+ cv::Mat cvMat(4,4,CV_32F);
+ for(int i=0;i<4;i++)
+ for(int j=0; j<4; j++)
+ cvMat.at<float>(i,j)=m(i,j);
+
+ return cvMat.clone();
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix3d &m)
+{
+ cv::Mat cvMat(3,3,CV_32F);
+ for(int i=0;i<3;i++)
+ for(int j=0; j<3; j++)
+ cvMat.at<float>(i,j)=m(i,j);
+
+ return cvMat.clone();
+}
+
+cv::Mat Converter::toCvMat(const Eigen::Matrix<double,3,1> &m)
+{
+ cv::Mat cvMat(3,1,CV_32F);
+ for(int i=0;i<3;i++)
+ cvMat.at<float>(i)=m(i);
+
+ return cvMat.clone();
+}
+
+cv::Mat Converter::toCvSE3(const Eigen::Matrix<double,3,3> &R, const Eigen::Matrix<double,3,1> &t)
+{
+ cv::Mat cvMat = cv::Mat::eye(4,4,CV_32F);
+ for(int i=0;i<3;i++)
+ {
+ for(int j=0;j<3;j++)
+ {
+ cvMat.at<float>(i,j)=R(i,j);
+ }
+ }
+ for(int i=0;i<3;i++)
+ {
+ cvMat.at<float>(i,3)=t(i);
+ }
+
+ return cvMat.clone();
+}
+
+Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Mat &cvVector)
+{
+ Eigen::Matrix<double,3,1> v;
+ v << cvVector.at<float>(0), cvVector.at<float>(1), cvVector.at<float>(2);
+
+ return v;
+}
+
+Eigen::Matrix<double,3,1> Converter::toVector3d(const cv::Point3f &cvPoint)
+{
+ Eigen::Matrix<double,3,1> v;
+ v << cvPoint.x, cvPoint.y, cvPoint.z;
+
+ return v;
+}
+
+Eigen::Matrix<double,3,3> Converter::toMatrix3d(const cv::Mat &cvMat3)
+{
+ Eigen::Matrix<double,3,3> M;
+
+ M << cvMat3.at<float>(0,0), cvMat3.at<float>(0,1), cvMat3.at<float>(0,2),
+ cvMat3.at<float>(1,0), cvMat3.at<float>(1,1), cvMat3.at<float>(1,2),
+ cvMat3.at<float>(2,0), cvMat3.at<float>(2,1), cvMat3.at<float>(2,2);
+
+ return M;
+}
+
+std::vector<float> Converter::toQuaternion(const cv::Mat &M)
+{
+ Eigen::Matrix<double,3,3> eigMat = toMatrix3d(M);
+ Eigen::Quaterniond q(eigMat);
+
+ std::vector<float> v(4);
+ v[0] = q.x();
+ v[1] = q.y();
+ v[2] = q.z();
+ v[3] = q.w();
+
+ return v;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Frame.cc b/externals/ORB_SLAM2/src/Frame.cc
new file mode 100644
index 0000000000000000000000000000000000000000..0e37d493358c74d0ac90983407acb78fc86fd1e5
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Frame.cc
@@ -0,0 +1,682 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Frame.h"
+#include "Converter.h"
+#include "ORBmatcher.h"
+#include <thread>
+
+namespace ORB_SLAM2
+{
+
+long unsigned int Frame::nNextId=0;
+bool Frame::mbInitialComputations=true;
+float Frame::cx, Frame::cy, Frame::fx, Frame::fy, Frame::invfx, Frame::invfy;
+float Frame::mnMinX, Frame::mnMinY, Frame::mnMaxX, Frame::mnMaxY;
+float Frame::mfGridElementWidthInv, Frame::mfGridElementHeightInv;
+
+Frame::Frame()
+{}
+
+//Copy Constructor
+Frame::Frame(const Frame &frame)
+ :mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight),
+ mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()),
+ mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys),
+ mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn), mvuRight(frame.mvuRight),
+ mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec),
+ mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()),
+ mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mnId(frame.mnId),
+ mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels),
+ mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor),
+ mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors),
+ mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2)
+{
+ for(int i=0;i<FRAME_GRID_COLS;i++)
+ for(int j=0; j<FRAME_GRID_ROWS; j++)
+ mGrid[i][j]=frame.mGrid[i][j];
+
+ if(!frame.mTcw.empty())
+ SetPose(frame.mTcw);
+}
+
+
+Frame::Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
+ :mpORBvocabulary(voc),mpORBextractorLeft(extractorLeft),mpORBextractorRight(extractorRight), mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
+ mpReferenceKF(static_cast<KeyFrame*>(NULL))
+{
+ // Frame ID
+ mnId=nNextId++;
+
+ // Scale Level Info
+ mnScaleLevels = mpORBextractorLeft->GetLevels();
+ mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
+ mfLogScaleFactor = log(mfScaleFactor);
+ mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
+ mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
+ mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
+ mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
+
+ // ORB extraction
+ thread threadLeft(&Frame::ExtractORB,this,0,imLeft);
+ thread threadRight(&Frame::ExtractORB,this,1,imRight);
+ threadLeft.join();
+ threadRight.join();
+
+ N = mvKeys.size();
+
+ if(mvKeys.empty())
+ return;
+
+ UndistortKeyPoints();
+
+ ComputeStereoMatches();
+
+ mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
+ mvbOutlier = vector<bool>(N,false);
+
+
+ // This is done only for the first Frame (or after a change in the calibration)
+ if(mbInitialComputations)
+ {
+ ComputeImageBounds(imLeft);
+
+ mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/(mnMaxX-mnMinX);
+ mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/(mnMaxY-mnMinY);
+
+ fx = K.at<float>(0,0);
+ fy = K.at<float>(1,1);
+ cx = K.at<float>(0,2);
+ cy = K.at<float>(1,2);
+ invfx = 1.0f/fx;
+ invfy = 1.0f/fy;
+
+ mbInitialComputations=false;
+ }
+
+ mb = mbf/fx;
+
+ AssignFeaturesToGrid();
+}
+
+Frame::Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
+ :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
+ mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
+{
+ // Frame ID
+ mnId=nNextId++;
+
+ // Scale Level Info
+ mnScaleLevels = mpORBextractorLeft->GetLevels();
+ mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
+ mfLogScaleFactor = log(mfScaleFactor);
+ mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
+ mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
+ mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
+ mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
+
+ // ORB extraction
+ ExtractORB(0,imGray);
+
+ N = mvKeys.size();
+
+ if(mvKeys.empty())
+ return;
+
+ UndistortKeyPoints();
+
+ ComputeStereoFromRGBD(imDepth);
+
+ mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
+ mvbOutlier = vector<bool>(N,false);
+
+ // This is done only for the first Frame (or after a change in the calibration)
+ if(mbInitialComputations)
+ {
+ ComputeImageBounds(imGray);
+
+ mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
+ mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
+
+ fx = K.at<float>(0,0);
+ fy = K.at<float>(1,1);
+ cx = K.at<float>(0,2);
+ cy = K.at<float>(1,2);
+ invfx = 1.0f/fx;
+ invfy = 1.0f/fy;
+
+ mbInitialComputations=false;
+ }
+
+ mb = mbf/fx;
+
+ AssignFeaturesToGrid();
+}
+
+
+Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth)
+ :mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
+ mTimeStamp(timeStamp), mK(K.clone()),mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth)
+{
+ // Frame ID
+ mnId=nNextId++;
+
+ // Scale Level Info
+ mnScaleLevels = mpORBextractorLeft->GetLevels();
+ mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
+ mfLogScaleFactor = log(mfScaleFactor);
+ mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
+ mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
+ mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
+ mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();
+
+ // ORB extraction
+ ExtractORB(0,imGray);
+
+ N = mvKeys.size();
+
+ if(mvKeys.empty())
+ return;
+
+ UndistortKeyPoints();
+
+ // Set no stereo information
+ mvuRight = vector<float>(N,-1);
+ mvDepth = vector<float>(N,-1);
+
+ mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
+ mvbOutlier = vector<bool>(N,false);
+
+ // This is done only for the first Frame (or after a change in the calibration)
+ if(mbInitialComputations)
+ {
+ ComputeImageBounds(imGray);
+
+ mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
+ mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);
+
+ fx = K.at<float>(0,0);
+ fy = K.at<float>(1,1);
+ cx = K.at<float>(0,2);
+ cy = K.at<float>(1,2);
+ invfx = 1.0f/fx;
+ invfy = 1.0f/fy;
+
+ mbInitialComputations=false;
+ }
+
+ mb = mbf/fx;
+
+ AssignFeaturesToGrid();
+}
+
+void Frame::AssignFeaturesToGrid()
+{
+ int nReserve = 0.5f*N/(FRAME_GRID_COLS*FRAME_GRID_ROWS);
+ for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
+ for (unsigned int j=0; j<FRAME_GRID_ROWS;j++)
+ mGrid[i][j].reserve(nReserve);
+
+ for(int i=0;i<N;i++)
+ {
+ const cv::KeyPoint &kp = mvKeysUn[i];
+
+ int nGridPosX, nGridPosY;
+ if(PosInGrid(kp,nGridPosX,nGridPosY))
+ mGrid[nGridPosX][nGridPosY].push_back(i);
+ }
+}
+
+void Frame::ExtractORB(int flag, const cv::Mat &im)
+{
+ if(flag==0)
+ (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors);
+ else
+ (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight);
+}
+
+void Frame::SetPose(cv::Mat Tcw)
+{
+ mTcw = Tcw.clone();
+ UpdatePoseMatrices();
+}
+
+void Frame::UpdatePoseMatrices()
+{
+ mRcw = mTcw.rowRange(0,3).colRange(0,3);
+ mRwc = mRcw.t();
+ mtcw = mTcw.rowRange(0,3).col(3);
+ mOw = -mRcw.t()*mtcw;
+}
+
+bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
+{
+ pMP->mbTrackInView = false;
+
+ // 3D in absolute coordinates
+ cv::Mat P = pMP->GetWorldPos();
+
+ // 3D in camera coordinates
+ const cv::Mat Pc = mRcw*P+mtcw;
+ const float &PcX = Pc.at<float>(0);
+ const float &PcY= Pc.at<float>(1);
+ const float &PcZ = Pc.at<float>(2);
+
+ // Check positive depth
+ if(PcZ<0.0f)
+ return false;
+
+ // Project in image and check it is not outside
+ const float invz = 1.0f/PcZ;
+ const float u=fx*PcX*invz+cx;
+ const float v=fy*PcY*invz+cy;
+
+ if(u<mnMinX || u>mnMaxX)
+ return false;
+ if(v<mnMinY || v>mnMaxY)
+ return false;
+
+ // Check distance is in the scale invariance region of the MapPoint
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ const cv::Mat PO = P-mOw;
+ const float dist = cv::norm(PO);
+
+ if(dist<minDistance || dist>maxDistance)
+ return false;
+
+ // Check viewing angle
+ cv::Mat Pn = pMP->GetNormal();
+
+ const float viewCos = PO.dot(Pn)/dist;
+
+ if(viewCos<viewingCosLimit)
+ return false;
+
+ // Predict scale in the image
+ const int nPredictedLevel = pMP->PredictScale(dist,this);
+
+ // Data used by the tracking
+ pMP->mbTrackInView = true;
+ pMP->mTrackProjX = u;
+ pMP->mTrackProjXR = u - mbf*invz;
+ pMP->mTrackProjY = v;
+ pMP->mnTrackScaleLevel= nPredictedLevel;
+ pMP->mTrackViewCos = viewCos;
+
+ return true;
+}
+
+vector<size_t> Frame::GetFeaturesInArea(const float &x, const float &y, const float &r, const int minLevel, const int maxLevel) const
+{
+ vector<size_t> vIndices;
+ vIndices.reserve(N);
+
+ const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
+ if(nMinCellX>=FRAME_GRID_COLS)
+ return vIndices;
+
+ const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
+ if(nMaxCellX<0)
+ return vIndices;
+
+ const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
+ if(nMinCellY>=FRAME_GRID_ROWS)
+ return vIndices;
+
+ const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
+ if(nMaxCellY<0)
+ return vIndices;
+
+ const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);
+
+ for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
+ {
+ for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
+ {
+ const vector<size_t> vCell = mGrid[ix][iy];
+ if(vCell.empty())
+ continue;
+
+ for(size_t j=0, jend=vCell.size(); j<jend; j++)
+ {
+ const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
+ if(bCheckLevels)
+ {
+ if(kpUn.octave<minLevel)
+ continue;
+ if(maxLevel>=0)
+ if(kpUn.octave>maxLevel)
+ continue;
+ }
+
+ const float distx = kpUn.pt.x-x;
+ const float disty = kpUn.pt.y-y;
+
+ if(fabs(distx)<r && fabs(disty)<r)
+ vIndices.push_back(vCell[j]);
+ }
+ }
+ }
+
+ return vIndices;
+}
+
+bool Frame::PosInGrid(const cv::KeyPoint &kp, int &posX, int &posY)
+{
+ posX = round((kp.pt.x-mnMinX)*mfGridElementWidthInv);
+ posY = round((kp.pt.y-mnMinY)*mfGridElementHeightInv);
+
+ //Keypoint's coordinates are undistorted, which could cause to go out of the image
+ if(posX<0 || posX>=FRAME_GRID_COLS || posY<0 || posY>=FRAME_GRID_ROWS)
+ return false;
+
+ return true;
+}
+
+
+void Frame::ComputeBoW()
+{
+ if(mBowVec.empty())
+ {
+ vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
+ mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
+ }
+}
+
+void Frame::UndistortKeyPoints()
+{
+ if(mDistCoef.at<float>(0)==0.0)
+ {
+ mvKeysUn=mvKeys;
+ return;
+ }
+
+ // Fill matrix with points
+ cv::Mat mat(N,2,CV_32F);
+ for(int i=0; i<N; i++)
+ {
+ mat.at<float>(i,0)=mvKeys[i].pt.x;
+ mat.at<float>(i,1)=mvKeys[i].pt.y;
+ }
+
+ // Undistort points
+ mat=mat.reshape(2);
+ cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
+ mat=mat.reshape(1);
+
+ // Fill undistorted keypoint vector
+ mvKeysUn.resize(N);
+ for(int i=0; i<N; i++)
+ {
+ cv::KeyPoint kp = mvKeys[i];
+ kp.pt.x=mat.at<float>(i,0);
+ kp.pt.y=mat.at<float>(i,1);
+ mvKeysUn[i]=kp;
+ }
+}
+
+void Frame::ComputeImageBounds(const cv::Mat &imLeft)
+{
+ if(mDistCoef.at<float>(0)!=0.0)
+ {
+ cv::Mat mat(4,2,CV_32F);
+ mat.at<float>(0,0)=0.0; mat.at<float>(0,1)=0.0;
+ mat.at<float>(1,0)=imLeft.cols; mat.at<float>(1,1)=0.0;
+ mat.at<float>(2,0)=0.0; mat.at<float>(2,1)=imLeft.rows;
+ mat.at<float>(3,0)=imLeft.cols; mat.at<float>(3,1)=imLeft.rows;
+
+ // Undistort corners
+ mat=mat.reshape(2);
+ cv::undistortPoints(mat,mat,mK,mDistCoef,cv::Mat(),mK);
+ mat=mat.reshape(1);
+
+ mnMinX = min(mat.at<float>(0,0),mat.at<float>(2,0));
+ mnMaxX = max(mat.at<float>(1,0),mat.at<float>(3,0));
+ mnMinY = min(mat.at<float>(0,1),mat.at<float>(1,1));
+ mnMaxY = max(mat.at<float>(2,1),mat.at<float>(3,1));
+
+ }
+ else
+ {
+ mnMinX = 0.0f;
+ mnMaxX = imLeft.cols;
+ mnMinY = 0.0f;
+ mnMaxY = imLeft.rows;
+ }
+}
+
+void Frame::ComputeStereoMatches()
+{
+ mvuRight = vector<float>(N,-1.0f);
+ mvDepth = vector<float>(N,-1.0f);
+
+ const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
+
+ const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;
+
+ //Assign keypoints to row table
+ vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
+
+ for(int i=0; i<nRows; i++)
+ vRowIndices[i].reserve(200);
+
+ const int Nr = mvKeysRight.size();
+
+ for(int iR=0; iR<Nr; iR++)
+ {
+ const cv::KeyPoint &kp = mvKeysRight[iR];
+ const float &kpY = kp.pt.y;
+ const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];
+ const int maxr = ceil(kpY+r);
+ const int minr = floor(kpY-r);
+
+ for(int yi=minr;yi<=maxr;yi++)
+ vRowIndices[yi].push_back(iR);
+ }
+
+ // Set limits for search
+ const float minZ = mb;
+ const float minD = 0;
+ const float maxD = mbf/minZ;
+
+ // For each left keypoint search a match in the right image
+ vector<pair<int, int> > vDistIdx;
+ vDistIdx.reserve(N);
+
+ for(int iL=0; iL<N; iL++)
+ {
+ const cv::KeyPoint &kpL = mvKeys[iL];
+ const int &levelL = kpL.octave;
+ const float &vL = kpL.pt.y;
+ const float &uL = kpL.pt.x;
+
+ const vector<size_t> &vCandidates = vRowIndices[vL];
+
+ if(vCandidates.empty())
+ continue;
+
+ const float minU = uL-maxD;
+ const float maxU = uL-minD;
+
+ if(maxU<0)
+ continue;
+
+ int bestDist = ORBmatcher::TH_HIGH;
+ size_t bestIdxR = 0;
+
+ const cv::Mat &dL = mDescriptors.row(iL);
+
+ // Compare descriptor to right keypoints
+ for(size_t iC=0; iC<vCandidates.size(); iC++)
+ {
+ const size_t iR = vCandidates[iC];
+ const cv::KeyPoint &kpR = mvKeysRight[iR];
+
+ if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
+ continue;
+
+ const float &uR = kpR.pt.x;
+
+ if(uR>=minU && uR<=maxU)
+ {
+ const cv::Mat &dR = mDescriptorsRight.row(iR);
+ const int dist = ORBmatcher::DescriptorDistance(dL,dR);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdxR = iR;
+ }
+ }
+ }
+
+ // Subpixel match by correlation
+ if(bestDist<thOrbDist)
+ {
+ // coordinates in image pyramid at keypoint scale
+ const float uR0 = mvKeysRight[bestIdxR].pt.x;
+ const float scaleFactor = mvInvScaleFactors[kpL.octave];
+ const float scaleduL = round(kpL.pt.x*scaleFactor);
+ const float scaledvL = round(kpL.pt.y*scaleFactor);
+ const float scaleduR0 = round(uR0*scaleFactor);
+
+ // sliding window search
+ const int w = 5;
+ cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
+ IL.convertTo(IL,CV_32F);
+ IL = IL - IL.at<float>(w,w) *cv::Mat::ones(IL.rows,IL.cols,CV_32F);
+
+ int bestDist = INT_MAX;
+ int bestincR = 0;
+ const int L = 5;
+ vector<float> vDists;
+ vDists.resize(2*L+1);
+
+ const float iniu = scaleduR0+L-w;
+ const float endu = scaleduR0+L+w+1;
+ if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)
+ continue;
+
+ for(int incR=-L; incR<=+L; incR++)
+ {
+ cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
+ IR.convertTo(IR,CV_32F);
+ IR = IR - IR.at<float>(w,w) *cv::Mat::ones(IR.rows,IR.cols,CV_32F);
+
+ float dist = cv::norm(IL,IR,cv::NORM_L1);
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestincR = incR;
+ }
+
+ vDists[L+incR] = dist;
+ }
+
+ if(bestincR==-L || bestincR==L)
+ continue;
+
+ // Sub-pixel match (Parabola fitting)
+ const float dist1 = vDists[L+bestincR-1];
+ const float dist2 = vDists[L+bestincR];
+ const float dist3 = vDists[L+bestincR+1];
+
+ const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));
+
+ if(deltaR<-1 || deltaR>1)
+ continue;
+
+ // Re-scaled coordinate
+ float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
+
+ float disparity = (uL-bestuR);
+
+ if(disparity>=minD && disparity<maxD)
+ {
+ if(disparity<=0)
+ {
+ disparity=0.01;
+ bestuR = uL-0.01;
+ }
+ mvDepth[iL]=mbf/disparity;
+ mvuRight[iL] = bestuR;
+ vDistIdx.push_back(pair<int,int>(bestDist,iL));
+ }
+ }
+ }
+
+ sort(vDistIdx.begin(),vDistIdx.end());
+ const float median = vDistIdx[vDistIdx.size()/2].first;
+ const float thDist = 1.5f*1.4f*median;
+
+ for(int i=vDistIdx.size()-1;i>=0;i--)
+ {
+ if(vDistIdx[i].first<thDist)
+ break;
+ else
+ {
+ mvuRight[vDistIdx[i].second]=-1;
+ mvDepth[vDistIdx[i].second]=-1;
+ }
+ }
+}
+
+
+void Frame::ComputeStereoFromRGBD(const cv::Mat &imDepth)
+{
+ mvuRight = vector<float>(N,-1);
+ mvDepth = vector<float>(N,-1);
+
+ for(int i=0; i<N; i++)
+ {
+ const cv::KeyPoint &kp = mvKeys[i];
+ const cv::KeyPoint &kpU = mvKeysUn[i];
+
+ const float &v = kp.pt.y;
+ const float &u = kp.pt.x;
+
+ const float d = imDepth.at<float>(v,u);
+
+ if(d>0)
+ {
+ mvDepth[i] = d;
+ mvuRight[i] = kpU.pt.x-mbf/d;
+ }
+ }
+}
+
+cv::Mat Frame::UnprojectStereo(const int &i)
+{
+ const float z = mvDepth[i];
+ if(z>0)
+ {
+ const float u = mvKeysUn[i].pt.x;
+ const float v = mvKeysUn[i].pt.y;
+ const float x = (u-cx)*z*invfx;
+ const float y = (v-cy)*z*invfy;
+ cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
+ return mRwc*x3Dc+mOw;
+ }
+ else
+ return cv::Mat();
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/FrameDrawer.cc b/externals/ORB_SLAM2/src/FrameDrawer.cc
new file mode 100644
index 0000000000000000000000000000000000000000..29c3bb0d0ca05627404721bc6f285016fccd985c
--- /dev/null
+++ b/externals/ORB_SLAM2/src/FrameDrawer.cc
@@ -0,0 +1,51 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "FrameDrawer.h"
+#include "Tracking.h"
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+FrameDrawer::FrameDrawer(Map* pMap):mpMap(pMap)
+{
+}
+
+cv::Mat FrameDrawer::DrawFrame()
+{
+ cv::Mat imWithInfo;
+ return imWithInfo;
+}
+
+
+void FrameDrawer::DrawTextInfo(cv::Mat &im, int nState, cv::Mat &imText)
+{
+}
+
+void FrameDrawer::Update(Tracking *pTracker)
+{
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Initializer.cc b/externals/ORB_SLAM2/src/Initializer.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6094b8f51b3273cfc90a37f008eebe4e19c7e6ad
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Initializer.cc
@@ -0,0 +1,931 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Initializer.h"
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+
+#include "Optimizer.h"
+#include "ORBmatcher.h"
+
+#include<thread>
+
+namespace ORB_SLAM2
+{
+
+Initializer::Initializer(const Frame &ReferenceFrame, float sigma, int iterations)
+{
+ mK = ReferenceFrame.mK.clone();
+
+ mvKeys1 = ReferenceFrame.mvKeysUn;
+
+ mSigma = sigma;
+ mSigma2 = sigma*sigma;
+ mMaxIterations = iterations;
+}
+
+bool Initializer::Initialize(const Frame &CurrentFrame, const vector<int> &vMatches12, cv::Mat &R21, cv::Mat &t21,
+ vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated)
+{
+ // Fill structures with current keypoints and matches with reference frame
+ // Reference Frame: 1, Current Frame: 2
+ mvKeys2 = CurrentFrame.mvKeysUn;
+
+ mvMatches12.clear();
+ mvMatches12.reserve(mvKeys2.size());
+ mvbMatched1.resize(mvKeys1.size());
+ for(size_t i=0, iend=vMatches12.size();i<iend; i++)
+ {
+ if(vMatches12[i]>=0)
+ {
+ mvMatches12.push_back(make_pair(i,vMatches12[i]));
+ mvbMatched1[i]=true;
+ }
+ else
+ mvbMatched1[i]=false;
+ }
+
+ const int N = mvMatches12.size();
+
+ // Indices for minimum set selection
+ vector<size_t> vAllIndices;
+ vAllIndices.reserve(N);
+ vector<size_t> vAvailableIndices;
+
+ for(int i=0; i<N; i++)
+ {
+ vAllIndices.push_back(i);
+ }
+
+ // Generate sets of 8 points for each RANSAC iteration
+ mvSets = vector< vector<size_t> >(mMaxIterations,vector<size_t>(8,0));
+
+ DUtils::Random::SeedRandOnce(0);
+
+ for(int it=0; it<mMaxIterations; it++)
+ {
+ vAvailableIndices = vAllIndices;
+
+ // Select a minimum set
+ for(size_t j=0; j<8; j++)
+ {
+ int randi = DUtils::Random::RandomInt(0,vAvailableIndices.size()-1);
+ int idx = vAvailableIndices[randi];
+
+ mvSets[it][j] = idx;
+
+ vAvailableIndices[randi] = vAvailableIndices.back();
+ vAvailableIndices.pop_back();
+ }
+ }
+
+ // Launch threads to compute in parallel a fundamental matrix and a homography
+ vector<bool> vbMatchesInliersH, vbMatchesInliersF;
+ float SH, SF;
+ cv::Mat H, F;
+
+ thread threadH(&Initializer::FindHomography,this,ref(vbMatchesInliersH), ref(SH), ref(H));
+ thread threadF(&Initializer::FindFundamental,this,ref(vbMatchesInliersF), ref(SF), ref(F));
+
+ // Wait until both threads have finished
+ threadH.join();
+ threadF.join();
+
+ // Compute ratio of scores
+ float RH = SH/(SH+SF);
+
+ // Try to reconstruct from homography or fundamental depending on the ratio (0.40-0.45)
+ if(RH>0.40)
+ return ReconstructH(vbMatchesInliersH,H,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
+ else //if(pF_HF>0.6)
+ return ReconstructF(vbMatchesInliersF,F,mK,R21,t21,vP3D,vbTriangulated,1.0,50);
+
+ return false;
+}
+
+
+void Initializer::FindHomography(vector<bool> &vbMatchesInliers, float &score, cv::Mat &H21)
+{
+ // Number of putative matches
+ const int N = mvMatches12.size();
+
+ // Normalize coordinates
+ vector<cv::Point2f> vPn1, vPn2;
+ cv::Mat T1, T2;
+ Normalize(mvKeys1,vPn1, T1);
+ Normalize(mvKeys2,vPn2, T2);
+ cv::Mat T2inv = T2.inv();
+
+ // Best Results variables
+ score = 0.0;
+ vbMatchesInliers = vector<bool>(N,false);
+
+ // Iteration variables
+ vector<cv::Point2f> vPn1i(8);
+ vector<cv::Point2f> vPn2i(8);
+ cv::Mat H21i, H12i;
+ vector<bool> vbCurrentInliers(N,false);
+ float currentScore;
+
+ // Perform all RANSAC iterations and save the solution with highest score
+ for(int it=0; it<mMaxIterations; it++)
+ {
+ // Select a minimum set
+ for(size_t j=0; j<8; j++)
+ {
+ int idx = mvSets[it][j];
+
+ vPn1i[j] = vPn1[mvMatches12[idx].first];
+ vPn2i[j] = vPn2[mvMatches12[idx].second];
+ }
+
+ cv::Mat Hn = ComputeH21(vPn1i,vPn2i);
+ H21i = T2inv*Hn*T1;
+ H12i = H21i.inv();
+
+ currentScore = CheckHomography(H21i, H12i, vbCurrentInliers, mSigma);
+
+ if(currentScore>score)
+ {
+ H21 = H21i.clone();
+ vbMatchesInliers = vbCurrentInliers;
+ score = currentScore;
+ }
+ }
+}
+
+
+void Initializer::FindFundamental(vector<bool> &vbMatchesInliers, float &score, cv::Mat &F21)
+{
+ // Number of putative matches
+ const int N = vbMatchesInliers.size();
+
+ // Normalize coordinates
+ vector<cv::Point2f> vPn1, vPn2;
+ cv::Mat T1, T2;
+ Normalize(mvKeys1,vPn1, T1);
+ Normalize(mvKeys2,vPn2, T2);
+ cv::Mat T2t = T2.t();
+
+ // Best Results variables
+ score = 0.0;
+ vbMatchesInliers = vector<bool>(N,false);
+
+ // Iteration variables
+ vector<cv::Point2f> vPn1i(8);
+ vector<cv::Point2f> vPn2i(8);
+ cv::Mat F21i;
+ vector<bool> vbCurrentInliers(N,false);
+ float currentScore;
+
+ // Perform all RANSAC iterations and save the solution with highest score
+ for(int it=0; it<mMaxIterations; it++)
+ {
+ // Select a minimum set
+ for(int j=0; j<8; j++)
+ {
+ int idx = mvSets[it][j];
+
+ vPn1i[j] = vPn1[mvMatches12[idx].first];
+ vPn2i[j] = vPn2[mvMatches12[idx].second];
+ }
+
+ cv::Mat Fn = ComputeF21(vPn1i,vPn2i);
+
+ F21i = T2t*Fn*T1;
+
+ currentScore = CheckFundamental(F21i, vbCurrentInliers, mSigma);
+
+ if(currentScore>score)
+ {
+ F21 = F21i.clone();
+ vbMatchesInliers = vbCurrentInliers;
+ score = currentScore;
+ }
+ }
+}
+
+
+cv::Mat Initializer::ComputeH21(const vector<cv::Point2f> &vP1, const vector<cv::Point2f> &vP2)
+{
+ const int N = vP1.size();
+
+ cv::Mat A(2*N,9,CV_32F);
+
+ for(int i=0; i<N; i++)
+ {
+ const float u1 = vP1[i].x;
+ const float v1 = vP1[i].y;
+ const float u2 = vP2[i].x;
+ const float v2 = vP2[i].y;
+
+ A.at<float>(2*i,0) = 0.0;
+ A.at<float>(2*i,1) = 0.0;
+ A.at<float>(2*i,2) = 0.0;
+ A.at<float>(2*i,3) = -u1;
+ A.at<float>(2*i,4) = -v1;
+ A.at<float>(2*i,5) = -1;
+ A.at<float>(2*i,6) = v2*u1;
+ A.at<float>(2*i,7) = v2*v1;
+ A.at<float>(2*i,8) = v2;
+
+ A.at<float>(2*i+1,0) = u1;
+ A.at<float>(2*i+1,1) = v1;
+ A.at<float>(2*i+1,2) = 1;
+ A.at<float>(2*i+1,3) = 0.0;
+ A.at<float>(2*i+1,4) = 0.0;
+ A.at<float>(2*i+1,5) = 0.0;
+ A.at<float>(2*i+1,6) = -u2*u1;
+ A.at<float>(2*i+1,7) = -u2*v1;
+ A.at<float>(2*i+1,8) = -u2;
+
+ }
+
+ cv::Mat u,w,vt;
+
+ cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+ return vt.row(8).reshape(0, 3);
+}
+
+cv::Mat Initializer::ComputeF21(const vector<cv::Point2f> &vP1,const vector<cv::Point2f> &vP2)
+{
+ const int N = vP1.size();
+
+ cv::Mat A(N,9,CV_32F);
+
+ for(int i=0; i<N; i++)
+ {
+ const float u1 = vP1[i].x;
+ const float v1 = vP1[i].y;
+ const float u2 = vP2[i].x;
+ const float v2 = vP2[i].y;
+
+ A.at<float>(i,0) = u2*u1;
+ A.at<float>(i,1) = u2*v1;
+ A.at<float>(i,2) = u2;
+ A.at<float>(i,3) = v2*u1;
+ A.at<float>(i,4) = v2*v1;
+ A.at<float>(i,5) = v2;
+ A.at<float>(i,6) = u1;
+ A.at<float>(i,7) = v1;
+ A.at<float>(i,8) = 1;
+ }
+
+ cv::Mat u,w,vt;
+
+ cv::SVDecomp(A,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+ cv::Mat Fpre = vt.row(8).reshape(0, 3);
+
+ cv::SVDecomp(Fpre,w,u,vt,cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+
+ w.at<float>(2)=0;
+
+ return u*cv::Mat::diag(w)*vt;
+}
+
+float Initializer::CheckHomography(const cv::Mat &H21, const cv::Mat &H12, vector<bool> &vbMatchesInliers, float sigma)
+{
+ const int N = mvMatches12.size();
+
+ const float h11 = H21.at<float>(0,0);
+ const float h12 = H21.at<float>(0,1);
+ const float h13 = H21.at<float>(0,2);
+ const float h21 = H21.at<float>(1,0);
+ const float h22 = H21.at<float>(1,1);
+ const float h23 = H21.at<float>(1,2);
+ const float h31 = H21.at<float>(2,0);
+ const float h32 = H21.at<float>(2,1);
+ const float h33 = H21.at<float>(2,2);
+
+ const float h11inv = H12.at<float>(0,0);
+ const float h12inv = H12.at<float>(0,1);
+ const float h13inv = H12.at<float>(0,2);
+ const float h21inv = H12.at<float>(1,0);
+ const float h22inv = H12.at<float>(1,1);
+ const float h23inv = H12.at<float>(1,2);
+ const float h31inv = H12.at<float>(2,0);
+ const float h32inv = H12.at<float>(2,1);
+ const float h33inv = H12.at<float>(2,2);
+
+ vbMatchesInliers.resize(N);
+
+ float score = 0;
+
+ const float th = 5.991;
+
+ const float invSigmaSquare = 1.0/(sigma*sigma);
+
+ for(int i=0; i<N; i++)
+ {
+ bool bIn = true;
+
+ const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+ const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+ const float u1 = kp1.pt.x;
+ const float v1 = kp1.pt.y;
+ const float u2 = kp2.pt.x;
+ const float v2 = kp2.pt.y;
+
+ // Reprojection error in first image
+ // x2in1 = H12*x2
+
+ const float w2in1inv = 1.0/(h31inv*u2+h32inv*v2+h33inv);
+ const float u2in1 = (h11inv*u2+h12inv*v2+h13inv)*w2in1inv;
+ const float v2in1 = (h21inv*u2+h22inv*v2+h23inv)*w2in1inv;
+
+ const float squareDist1 = (u1-u2in1)*(u1-u2in1)+(v1-v2in1)*(v1-v2in1);
+
+ const float chiSquare1 = squareDist1*invSigmaSquare;
+
+ if(chiSquare1>th)
+ bIn = false;
+ else
+ score += th - chiSquare1;
+
+ // Reprojection error in second image
+ // x1in2 = H21*x1
+
+ const float w1in2inv = 1.0/(h31*u1+h32*v1+h33);
+ const float u1in2 = (h11*u1+h12*v1+h13)*w1in2inv;
+ const float v1in2 = (h21*u1+h22*v1+h23)*w1in2inv;
+
+ const float squareDist2 = (u2-u1in2)*(u2-u1in2)+(v2-v1in2)*(v2-v1in2);
+
+ const float chiSquare2 = squareDist2*invSigmaSquare;
+
+ if(chiSquare2>th)
+ bIn = false;
+ else
+ score += th - chiSquare2;
+
+ if(bIn)
+ vbMatchesInliers[i]=true;
+ else
+ vbMatchesInliers[i]=false;
+ }
+
+ return score;
+}
+
+float Initializer::CheckFundamental(const cv::Mat &F21, vector<bool> &vbMatchesInliers, float sigma)
+{
+ const int N = mvMatches12.size();
+
+ const float f11 = F21.at<float>(0,0);
+ const float f12 = F21.at<float>(0,1);
+ const float f13 = F21.at<float>(0,2);
+ const float f21 = F21.at<float>(1,0);
+ const float f22 = F21.at<float>(1,1);
+ const float f23 = F21.at<float>(1,2);
+ const float f31 = F21.at<float>(2,0);
+ const float f32 = F21.at<float>(2,1);
+ const float f33 = F21.at<float>(2,2);
+
+ vbMatchesInliers.resize(N);
+
+ float score = 0;
+
+ const float th = 3.841;
+ const float thScore = 5.991;
+
+ const float invSigmaSquare = 1.0/(sigma*sigma);
+
+ for(int i=0; i<N; i++)
+ {
+ bool bIn = true;
+
+ const cv::KeyPoint &kp1 = mvKeys1[mvMatches12[i].first];
+ const cv::KeyPoint &kp2 = mvKeys2[mvMatches12[i].second];
+
+ const float u1 = kp1.pt.x;
+ const float v1 = kp1.pt.y;
+ const float u2 = kp2.pt.x;
+ const float v2 = kp2.pt.y;
+
+ // Reprojection error in second image
+ // l2=F21x1=(a2,b2,c2)
+
+ const float a2 = f11*u1+f12*v1+f13;
+ const float b2 = f21*u1+f22*v1+f23;
+ const float c2 = f31*u1+f32*v1+f33;
+
+ const float num2 = a2*u2+b2*v2+c2;
+
+ const float squareDist1 = num2*num2/(a2*a2+b2*b2);
+
+ const float chiSquare1 = squareDist1*invSigmaSquare;
+
+ if(chiSquare1>th)
+ bIn = false;
+ else
+ score += thScore - chiSquare1;
+
+ // Reprojection error in second image
+ // l1 =x2tF21=(a1,b1,c1)
+
+ const float a1 = f11*u2+f21*v2+f31;
+ const float b1 = f12*u2+f22*v2+f32;
+ const float c1 = f13*u2+f23*v2+f33;
+
+ const float num1 = a1*u1+b1*v1+c1;
+
+ const float squareDist2 = num1*num1/(a1*a1+b1*b1);
+
+ const float chiSquare2 = squareDist2*invSigmaSquare;
+
+ if(chiSquare2>th)
+ bIn = false;
+ else
+ score += thScore - chiSquare2;
+
+ if(bIn)
+ vbMatchesInliers[i]=true;
+ else
+ vbMatchesInliers[i]=false;
+ }
+
+ return score;
+}
+
+bool Initializer::ReconstructF(vector<bool> &vbMatchesInliers, cv::Mat &F21, cv::Mat &K,
+ cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
+{
+ int N=0;
+ for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+ if(vbMatchesInliers[i])
+ N++;
+
+ // Compute Essential Matrix from Fundamental Matrix
+ cv::Mat E21 = K.t()*F21*K;
+
+ cv::Mat R1, R2, t;
+
+ // Recover the 4 motion hypotheses
+ DecomposeE(E21,R1,R2,t);
+
+ cv::Mat t1=t;
+ cv::Mat t2=-t;
+
+ // Reconstruct with the 4 hyphoteses and check
+ vector<cv::Point3f> vP3D1, vP3D2, vP3D3, vP3D4;
+ vector<bool> vbTriangulated1,vbTriangulated2,vbTriangulated3, vbTriangulated4;
+ float parallax1,parallax2, parallax3, parallax4;
+
+ int nGood1 = CheckRT(R1,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D1, 4.0*mSigma2, vbTriangulated1, parallax1);
+ int nGood2 = CheckRT(R2,t1,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D2, 4.0*mSigma2, vbTriangulated2, parallax2);
+ int nGood3 = CheckRT(R1,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D3, 4.0*mSigma2, vbTriangulated3, parallax3);
+ int nGood4 = CheckRT(R2,t2,mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K, vP3D4, 4.0*mSigma2, vbTriangulated4, parallax4);
+
+ int maxGood = max(nGood1,max(nGood2,max(nGood3,nGood4)));
+
+ R21 = cv::Mat();
+ t21 = cv::Mat();
+
+ int nMinGood = max(static_cast<int>(0.9*N),minTriangulated);
+
+ int nsimilar = 0;
+ if(nGood1>0.7*maxGood)
+ nsimilar++;
+ if(nGood2>0.7*maxGood)
+ nsimilar++;
+ if(nGood3>0.7*maxGood)
+ nsimilar++;
+ if(nGood4>0.7*maxGood)
+ nsimilar++;
+
+ // If there is not a clear winner or not enough triangulated points reject initialization
+ if(maxGood<nMinGood || nsimilar>1)
+ {
+ return false;
+ }
+
+ // If best reconstruction has enough parallax initialize
+ if(maxGood==nGood1)
+ {
+ if(parallax1>minParallax)
+ {
+ vP3D = vP3D1;
+ vbTriangulated = vbTriangulated1;
+
+ R1.copyTo(R21);
+ t1.copyTo(t21);
+ return true;
+ }
+ }else if(maxGood==nGood2)
+ {
+ if(parallax2>minParallax)
+ {
+ vP3D = vP3D2;
+ vbTriangulated = vbTriangulated2;
+
+ R2.copyTo(R21);
+ t1.copyTo(t21);
+ return true;
+ }
+ }else if(maxGood==nGood3)
+ {
+ if(parallax3>minParallax)
+ {
+ vP3D = vP3D3;
+ vbTriangulated = vbTriangulated3;
+
+ R1.copyTo(R21);
+ t2.copyTo(t21);
+ return true;
+ }
+ }else if(maxGood==nGood4)
+ {
+ if(parallax4>minParallax)
+ {
+ vP3D = vP3D4;
+ vbTriangulated = vbTriangulated4;
+
+ R2.copyTo(R21);
+ t2.copyTo(t21);
+ return true;
+ }
+ }
+
+ return false;
+}
+
+bool Initializer::ReconstructH(vector<bool> &vbMatchesInliers, cv::Mat &H21, cv::Mat &K,
+ cv::Mat &R21, cv::Mat &t21, vector<cv::Point3f> &vP3D, vector<bool> &vbTriangulated, float minParallax, int minTriangulated)
+{
+ int N=0;
+ for(size_t i=0, iend = vbMatchesInliers.size() ; i<iend; i++)
+ if(vbMatchesInliers[i])
+ N++;
+
+ // We recover 8 motion hypotheses using the method of Faugeras et al.
+ // Motion and structure from motion in a piecewise planar environment.
+ // International Journal of Pattern Recognition and Artificial Intelligence, 1988
+
+ cv::Mat invK = K.inv();
+ cv::Mat A = invK*H21*K;
+
+ cv::Mat U,w,Vt,V;
+ cv::SVD::compute(A,w,U,Vt,cv::SVD::FULL_UV);
+ V=Vt.t();
+
+ float s = cv::determinant(U)*cv::determinant(Vt);
+
+ float d1 = w.at<float>(0);
+ float d2 = w.at<float>(1);
+ float d3 = w.at<float>(2);
+
+ if(d1/d2<1.00001 || d2/d3<1.00001)
+ {
+ return false;
+ }
+
+ vector<cv::Mat> vR, vt, vn;
+ vR.reserve(8);
+ vt.reserve(8);
+ vn.reserve(8);
+
+ //n'=[x1 0 x3] 4 posibilities e1=e3=1, e1=1 e3=-1, e1=-1 e3=1, e1=e3=-1
+ float aux1 = sqrt((d1*d1-d2*d2)/(d1*d1-d3*d3));
+ float aux3 = sqrt((d2*d2-d3*d3)/(d1*d1-d3*d3));
+ float x1[] = {aux1,aux1,-aux1,-aux1};
+ float x3[] = {aux3,-aux3,aux3,-aux3};
+
+ //case d'=d2
+ float aux_stheta = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1+d3)*d2);
+
+ float ctheta = (d2*d2+d1*d3)/((d1+d3)*d2);
+ float stheta[] = {aux_stheta, -aux_stheta, -aux_stheta, aux_stheta};
+
+ for(int i=0; i<4; i++)
+ {
+ cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+ Rp.at<float>(0,0)=ctheta;
+ Rp.at<float>(0,2)=-stheta[i];
+ Rp.at<float>(2,0)=stheta[i];
+ Rp.at<float>(2,2)=ctheta;
+
+ cv::Mat R = s*U*Rp*Vt;
+ vR.push_back(R);
+
+ cv::Mat tp(3,1,CV_32F);
+ tp.at<float>(0)=x1[i];
+ tp.at<float>(1)=0;
+ tp.at<float>(2)=-x3[i];
+ tp*=d1-d3;
+
+ cv::Mat t = U*tp;
+ vt.push_back(t/cv::norm(t));
+
+ cv::Mat np(3,1,CV_32F);
+ np.at<float>(0)=x1[i];
+ np.at<float>(1)=0;
+ np.at<float>(2)=x3[i];
+
+ cv::Mat n = V*np;
+ if(n.at<float>(2)<0)
+ n=-n;
+ vn.push_back(n);
+ }
+
+ //case d'=-d2
+ float aux_sphi = sqrt((d1*d1-d2*d2)*(d2*d2-d3*d3))/((d1-d3)*d2);
+
+ float cphi = (d1*d3-d2*d2)/((d1-d3)*d2);
+ float sphi[] = {aux_sphi, -aux_sphi, -aux_sphi, aux_sphi};
+
+ for(int i=0; i<4; i++)
+ {
+ cv::Mat Rp=cv::Mat::eye(3,3,CV_32F);
+ Rp.at<float>(0,0)=cphi;
+ Rp.at<float>(0,2)=sphi[i];
+ Rp.at<float>(1,1)=-1;
+ Rp.at<float>(2,0)=sphi[i];
+ Rp.at<float>(2,2)=-cphi;
+
+ cv::Mat R = s*U*Rp*Vt;
+ vR.push_back(R);
+
+ cv::Mat tp(3,1,CV_32F);
+ tp.at<float>(0)=x1[i];
+ tp.at<float>(1)=0;
+ tp.at<float>(2)=x3[i];
+ tp*=d1+d3;
+
+ cv::Mat t = U*tp;
+ vt.push_back(t/cv::norm(t));
+
+ cv::Mat np(3,1,CV_32F);
+ np.at<float>(0)=x1[i];
+ np.at<float>(1)=0;
+ np.at<float>(2)=x3[i];
+
+ cv::Mat n = V*np;
+ if(n.at<float>(2)<0)
+ n=-n;
+ vn.push_back(n);
+ }
+
+
+ int bestGood = 0;
+ int secondBestGood = 0;
+ int bestSolutionIdx = -1;
+ float bestParallax = -1;
+ vector<cv::Point3f> bestP3D;
+ vector<bool> bestTriangulated;
+
+ // Instead of applying the visibility constraints proposed in the Faugeras' paper (which could fail for points seen with low parallax)
+ // We reconstruct all hypotheses and check in terms of triangulated points and parallax
+ for(size_t i=0; i<8; i++)
+ {
+ float parallaxi;
+ vector<cv::Point3f> vP3Di;
+ vector<bool> vbTriangulatedi;
+ int nGood = CheckRT(vR[i],vt[i],mvKeys1,mvKeys2,mvMatches12,vbMatchesInliers,K,vP3Di, 4.0*mSigma2, vbTriangulatedi, parallaxi);
+
+ if(nGood>bestGood)
+ {
+ secondBestGood = bestGood;
+ bestGood = nGood;
+ bestSolutionIdx = i;
+ bestParallax = parallaxi;
+ bestP3D = vP3Di;
+ bestTriangulated = vbTriangulatedi;
+ }
+ else if(nGood>secondBestGood)
+ {
+ secondBestGood = nGood;
+ }
+ }
+
+
+ if(secondBestGood<0.75*bestGood && bestParallax>=minParallax && bestGood>minTriangulated && bestGood>0.9*N)
+ {
+ vR[bestSolutionIdx].copyTo(R21);
+ vt[bestSolutionIdx].copyTo(t21);
+ vP3D = bestP3D;
+ vbTriangulated = bestTriangulated;
+
+ return true;
+ }
+
+ return false;
+}
+
+void Initializer::Triangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, const cv::Mat &P1, const cv::Mat &P2, cv::Mat &x3D)
+{
+ cv::Mat A(4,4,CV_32F);
+
+ A.row(0) = kp1.pt.x*P1.row(2)-P1.row(0);
+ A.row(1) = kp1.pt.y*P1.row(2)-P1.row(1);
+ A.row(2) = kp2.pt.x*P2.row(2)-P2.row(0);
+ A.row(3) = kp2.pt.y*P2.row(2)-P2.row(1);
+
+ cv::Mat u,w,vt;
+ cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+ x3D = vt.row(3).t();
+ x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+}
+
+void Initializer::Normalize(const vector<cv::KeyPoint> &vKeys, vector<cv::Point2f> &vNormalizedPoints, cv::Mat &T)
+{
+ float meanX = 0;
+ float meanY = 0;
+ const int N = vKeys.size();
+
+ vNormalizedPoints.resize(N);
+
+ for(int i=0; i<N; i++)
+ {
+ meanX += vKeys[i].pt.x;
+ meanY += vKeys[i].pt.y;
+ }
+
+ meanX = meanX/N;
+ meanY = meanY/N;
+
+ float meanDevX = 0;
+ float meanDevY = 0;
+
+ for(int i=0; i<N; i++)
+ {
+ vNormalizedPoints[i].x = vKeys[i].pt.x - meanX;
+ vNormalizedPoints[i].y = vKeys[i].pt.y - meanY;
+
+ meanDevX += fabs(vNormalizedPoints[i].x);
+ meanDevY += fabs(vNormalizedPoints[i].y);
+ }
+
+ meanDevX = meanDevX/N;
+ meanDevY = meanDevY/N;
+
+ float sX = 1.0/meanDevX;
+ float sY = 1.0/meanDevY;
+
+ for(int i=0; i<N; i++)
+ {
+ vNormalizedPoints[i].x = vNormalizedPoints[i].x * sX;
+ vNormalizedPoints[i].y = vNormalizedPoints[i].y * sY;
+ }
+
+ T = cv::Mat::eye(3,3,CV_32F);
+ T.at<float>(0,0) = sX;
+ T.at<float>(1,1) = sY;
+ T.at<float>(0,2) = -meanX*sX;
+ T.at<float>(1,2) = -meanY*sY;
+}
+
+
+int Initializer::CheckRT(const cv::Mat &R, const cv::Mat &t, const vector<cv::KeyPoint> &vKeys1, const vector<cv::KeyPoint> &vKeys2,
+ const vector<Match> &vMatches12, vector<bool> &vbMatchesInliers,
+ const cv::Mat &K, vector<cv::Point3f> &vP3D, float th2, vector<bool> &vbGood, float ¶llax)
+{
+ // Calibration parameters
+ const float fx = K.at<float>(0,0);
+ const float fy = K.at<float>(1,1);
+ const float cx = K.at<float>(0,2);
+ const float cy = K.at<float>(1,2);
+
+ vbGood = vector<bool>(vKeys1.size(),false);
+ vP3D.resize(vKeys1.size());
+
+ vector<float> vCosParallax;
+ vCosParallax.reserve(vKeys1.size());
+
+ // Camera 1 Projection Matrix K[I|0]
+ cv::Mat P1(3,4,CV_32F,cv::Scalar(0));
+ K.copyTo(P1.rowRange(0,3).colRange(0,3));
+
+ cv::Mat O1 = cv::Mat::zeros(3,1,CV_32F);
+
+ // Camera 2 Projection Matrix K[R|t]
+ cv::Mat P2(3,4,CV_32F);
+ R.copyTo(P2.rowRange(0,3).colRange(0,3));
+ t.copyTo(P2.rowRange(0,3).col(3));
+ P2 = K*P2;
+
+ cv::Mat O2 = -R.t()*t;
+
+ int nGood=0;
+
+ for(size_t i=0, iend=vMatches12.size();i<iend;i++)
+ {
+ if(!vbMatchesInliers[i])
+ continue;
+
+ const cv::KeyPoint &kp1 = vKeys1[vMatches12[i].first];
+ const cv::KeyPoint &kp2 = vKeys2[vMatches12[i].second];
+ cv::Mat p3dC1;
+
+ Triangulate(kp1,kp2,P1,P2,p3dC1);
+
+ if(!isfinite(p3dC1.at<float>(0)) || !isfinite(p3dC1.at<float>(1)) || !isfinite(p3dC1.at<float>(2)))
+ {
+ vbGood[vMatches12[i].first]=false;
+ continue;
+ }
+
+ // Check parallax
+ cv::Mat normal1 = p3dC1 - O1;
+ float dist1 = cv::norm(normal1);
+
+ cv::Mat normal2 = p3dC1 - O2;
+ float dist2 = cv::norm(normal2);
+
+ float cosParallax = normal1.dot(normal2)/(dist1*dist2);
+
+ // Check depth in front of first camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+ if(p3dC1.at<float>(2)<=0 && cosParallax<0.99998)
+ continue;
+
+ // Check depth in front of second camera (only if enough parallax, as "infinite" points can easily go to negative depth)
+ cv::Mat p3dC2 = R*p3dC1+t;
+
+ if(p3dC2.at<float>(2)<=0 && cosParallax<0.99998)
+ continue;
+
+ // Check reprojection error in first image
+ float im1x, im1y;
+ float invZ1 = 1.0/p3dC1.at<float>(2);
+ im1x = fx*p3dC1.at<float>(0)*invZ1+cx;
+ im1y = fy*p3dC1.at<float>(1)*invZ1+cy;
+
+ float squareError1 = (im1x-kp1.pt.x)*(im1x-kp1.pt.x)+(im1y-kp1.pt.y)*(im1y-kp1.pt.y);
+
+ if(squareError1>th2)
+ continue;
+
+ // Check reprojection error in second image
+ float im2x, im2y;
+ float invZ2 = 1.0/p3dC2.at<float>(2);
+ im2x = fx*p3dC2.at<float>(0)*invZ2+cx;
+ im2y = fy*p3dC2.at<float>(1)*invZ2+cy;
+
+ float squareError2 = (im2x-kp2.pt.x)*(im2x-kp2.pt.x)+(im2y-kp2.pt.y)*(im2y-kp2.pt.y);
+
+ if(squareError2>th2)
+ continue;
+
+ vCosParallax.push_back(cosParallax);
+ vP3D[vMatches12[i].first] = cv::Point3f(p3dC1.at<float>(0),p3dC1.at<float>(1),p3dC1.at<float>(2));
+ nGood++;
+
+ if(cosParallax<0.99998)
+ vbGood[vMatches12[i].first]=true;
+ }
+
+ if(nGood>0)
+ {
+ sort(vCosParallax.begin(),vCosParallax.end());
+
+ size_t idx = min(50,int(vCosParallax.size()-1));
+ parallax = acos(vCosParallax[idx])*180/CV_PI;
+ }
+ else
+ parallax=0;
+
+ return nGood;
+}
+
+void Initializer::DecomposeE(const cv::Mat &E, cv::Mat &R1, cv::Mat &R2, cv::Mat &t)
+{
+ cv::Mat u,w,vt;
+ cv::SVD::compute(E,w,u,vt);
+
+ u.col(2).copyTo(t);
+ t=t/cv::norm(t);
+
+ cv::Mat W(3,3,CV_32F,cv::Scalar(0));
+ W.at<float>(0,1)=-1;
+ W.at<float>(1,0)=1;
+ W.at<float>(2,2)=1;
+
+ R1 = u*W*vt;
+ if(cv::determinant(R1)<0)
+ R1=-R1;
+
+ R2 = u*W.t()*vt;
+ if(cv::determinant(R2)<0)
+ R2=-R2;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/KeyFrame.cc b/externals/ORB_SLAM2/src/KeyFrame.cc
new file mode 100644
index 0000000000000000000000000000000000000000..4ef1e78e0f7d0ae3a3ae8c1739227599277ec046
--- /dev/null
+++ b/externals/ORB_SLAM2/src/KeyFrame.cc
@@ -0,0 +1,665 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "KeyFrame.h"
+#include "Converter.h"
+#include "ORBmatcher.h"
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+long unsigned int KeyFrame::nNextId=0;
+
+KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
+ mnFrameId(F.mnId), mTimeStamp(F.mTimeStamp), mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS),
+ mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv),
+ mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0),
+ mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0),
+ fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy),
+ mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
+ mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
+ mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
+ mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
+ mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
+ mnMaxY(F.mnMaxY), mK(F.mK), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB),
+ mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mbNotErase(false),
+ mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap)
+{
+ mnId=nNextId++;
+
+ mGrid.resize(mnGridCols);
+ for(int i=0; i<mnGridCols;i++)
+ {
+ mGrid[i].resize(mnGridRows);
+ for(int j=0; j<mnGridRows; j++)
+ mGrid[i][j] = F.mGrid[i][j];
+ }
+
+ SetPose(F.mTcw);
+}
+
+void KeyFrame::ComputeBoW()
+{
+ if(mBowVec.empty() || mFeatVec.empty())
+ {
+ vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
+ // Feature vector associate features with nodes in the 4th level (from leaves up)
+ // We assume the vocabulary tree has 6 levels, change the 4 otherwise
+ mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
+ }
+}
+
+void KeyFrame::SetPose(const cv::Mat &Tcw_)
+{
+ unique_lock<mutex> lock(mMutexPose);
+ Tcw_.copyTo(Tcw);
+ cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
+ cv::Mat tcw = Tcw.rowRange(0,3).col(3);
+ cv::Mat Rwc = Rcw.t();
+ Ow = -Rwc*tcw;
+
+ Twc = cv::Mat::eye(4,4,Tcw.type());
+ Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
+ Ow.copyTo(Twc.rowRange(0,3).col(3));
+ cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
+ Cw = Twc*center;
+}
+
+cv::Mat KeyFrame::GetPose()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Tcw.clone();
+}
+
+cv::Mat KeyFrame::GetPoseInverse()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Twc.clone();
+}
+
+cv::Mat KeyFrame::GetCameraCenter()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Ow.clone();
+}
+
+cv::Mat KeyFrame::GetStereoCenter()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Cw.clone();
+}
+
+
+cv::Mat KeyFrame::GetRotation()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Tcw.rowRange(0,3).colRange(0,3).clone();
+}
+
+cv::Mat KeyFrame::GetTranslation()
+{
+ unique_lock<mutex> lock(mMutexPose);
+ return Tcw.rowRange(0,3).col(3).clone();
+}
+
+void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
+{
+ {
+ unique_lock<mutex> lock(mMutexConnections);
+ if(!mConnectedKeyFrameWeights.count(pKF))
+ mConnectedKeyFrameWeights[pKF]=weight;
+ else if(mConnectedKeyFrameWeights[pKF]!=weight)
+ mConnectedKeyFrameWeights[pKF]=weight;
+ else
+ return;
+ }
+
+ UpdateBestCovisibles();
+}
+
+void KeyFrame::UpdateBestCovisibles()
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ vector<pair<int,KeyFrame*> > vPairs;
+ vPairs.reserve(mConnectedKeyFrameWeights.size());
+ for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
+ vPairs.push_back(make_pair(mit->second,mit->first));
+
+ sort(vPairs.begin(),vPairs.end());
+ list<KeyFrame*> lKFs;
+ list<int> lWs;
+ for(size_t i=0, iend=vPairs.size(); i<iend;i++)
+ {
+ lKFs.push_front(vPairs[i].second);
+ lWs.push_front(vPairs[i].first);
+ }
+
+ mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
+ mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
+}
+
+set<KeyFrame*> KeyFrame::GetConnectedKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ set<KeyFrame*> s;
+ for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin();mit!=mConnectedKeyFrameWeights.end();mit++)
+ s.insert(mit->first);
+ return s;
+}
+
+vector<KeyFrame*> KeyFrame::GetVectorCovisibleKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ return mvpOrderedConnectedKeyFrames;
+}
+
+vector<KeyFrame*> KeyFrame::GetBestCovisibilityKeyFrames(const int &N)
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ if((int)mvpOrderedConnectedKeyFrames.size()<N)
+ return mvpOrderedConnectedKeyFrames;
+ else
+ return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(),mvpOrderedConnectedKeyFrames.begin()+N);
+
+}
+
+vector<KeyFrame*> KeyFrame::GetCovisiblesByWeight(const int &w)
+{
+ unique_lock<mutex> lock(mMutexConnections);
+
+ if(mvpOrderedConnectedKeyFrames.empty())
+ return vector<KeyFrame*>();
+
+ vector<int>::iterator it = upper_bound(mvOrderedWeights.begin(),mvOrderedWeights.end(),w,KeyFrame::weightComp);
+ if(it==mvOrderedWeights.end())
+ return vector<KeyFrame*>();
+ else
+ {
+ int n = it-mvOrderedWeights.begin();
+ return vector<KeyFrame*>(mvpOrderedConnectedKeyFrames.begin(), mvpOrderedConnectedKeyFrames.begin()+n);
+ }
+}
+
+int KeyFrame::GetWeight(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ if(mConnectedKeyFrameWeights.count(pKF))
+ return mConnectedKeyFrameWeights[pKF];
+ else
+ return 0;
+}
+
+void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ mvpMapPoints[idx]=pMP;
+}
+
+void KeyFrame::EraseMapPointMatch(const size_t &idx)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
+}
+
+void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
+{
+ int idx = pMP->GetIndexInKeyFrame(this);
+ if(idx>=0)
+ mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
+}
+
+
+void KeyFrame::ReplaceMapPointMatch(const size_t &idx, MapPoint* pMP)
+{
+ mvpMapPoints[idx]=pMP;
+}
+
+set<MapPoint*> KeyFrame::GetMapPoints()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ set<MapPoint*> s;
+ for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
+ {
+ if(!mvpMapPoints[i])
+ continue;
+ MapPoint* pMP = mvpMapPoints[i];
+ if(!pMP->isBad())
+ s.insert(pMP);
+ }
+ return s;
+}
+
+int KeyFrame::TrackedMapPoints(const int &minObs)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+
+ int nPoints=0;
+ const bool bCheckObs = minObs>0;
+ for(int i=0; i<N; i++)
+ {
+ MapPoint* pMP = mvpMapPoints[i];
+ if(pMP)
+ {
+ if(!pMP->isBad())
+ {
+ if(bCheckObs)
+ {
+ if(mvpMapPoints[i]->Observations()>=minObs)
+ nPoints++;
+ }
+ else
+ nPoints++;
+ }
+ }
+ }
+
+ return nPoints;
+}
+
+vector<MapPoint*> KeyFrame::GetMapPointMatches()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return mvpMapPoints;
+}
+
+MapPoint* KeyFrame::GetMapPoint(const size_t &idx)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return mvpMapPoints[idx];
+}
+
+void KeyFrame::UpdateConnections()
+{
+ map<KeyFrame*,int> KFcounter;
+
+ vector<MapPoint*> vpMP;
+
+ {
+ unique_lock<mutex> lockMPs(mMutexFeatures);
+ vpMP = mvpMapPoints;
+ }
+
+ //For all map points in keyframe check in which other keyframes are they seen
+ //Increase counter for those keyframes
+ for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
+ {
+ MapPoint* pMP = *vit;
+
+ if(!pMP)
+ continue;
+
+ if(pMP->isBad())
+ continue;
+
+ map<KeyFrame*,size_t> observations = pMP->GetObservations();
+
+ for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ if(mit->first->mnId==mnId)
+ continue;
+ KFcounter[mit->first]++;
+ }
+ }
+
+ // This should not happen
+ if(KFcounter.empty())
+ return;
+
+ //If the counter is greater than threshold add connection
+ //In case no keyframe counter is over threshold add the one with maximum counter
+ int nmax=0;
+ KeyFrame* pKFmax=NULL;
+ int th = 15;
+
+ vector<pair<int,KeyFrame*> > vPairs;
+ vPairs.reserve(KFcounter.size());
+ for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
+ {
+ if(mit->second>nmax)
+ {
+ nmax=mit->second;
+ pKFmax=mit->first;
+ }
+ if(mit->second>=th)
+ {
+ vPairs.push_back(make_pair(mit->second,mit->first));
+ (mit->first)->AddConnection(this,mit->second);
+ }
+ }
+
+ if(vPairs.empty())
+ {
+ vPairs.push_back(make_pair(nmax,pKFmax));
+ pKFmax->AddConnection(this,nmax);
+ }
+
+ sort(vPairs.begin(),vPairs.end());
+ list<KeyFrame*> lKFs;
+ list<int> lWs;
+ for(size_t i=0; i<vPairs.size();i++)
+ {
+ lKFs.push_front(vPairs[i].second);
+ lWs.push_front(vPairs[i].first);
+ }
+
+ {
+ unique_lock<mutex> lockCon(mMutexConnections);
+
+ // mspConnectedKeyFrames = spConnectedKeyFrames;
+ mConnectedKeyFrameWeights = KFcounter;
+ mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
+ mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
+
+ if(mbFirstConnection && mnId!=0)
+ {
+ mpParent = mvpOrderedConnectedKeyFrames.front();
+ mpParent->AddChild(this);
+ mbFirstConnection = false;
+ }
+
+ }
+}
+
+void KeyFrame::AddChild(KeyFrame *pKF)
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ mspChildrens.insert(pKF);
+}
+
+void KeyFrame::EraseChild(KeyFrame *pKF)
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ mspChildrens.erase(pKF);
+}
+
+void KeyFrame::ChangeParent(KeyFrame *pKF)
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ mpParent = pKF;
+ pKF->AddChild(this);
+}
+
+set<KeyFrame*> KeyFrame::GetChilds()
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ return mspChildrens;
+}
+
+KeyFrame* KeyFrame::GetParent()
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ return mpParent;
+}
+
+bool KeyFrame::hasChild(KeyFrame *pKF)
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ return mspChildrens.count(pKF);
+}
+
+void KeyFrame::AddLoopEdge(KeyFrame *pKF)
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ mbNotErase = true;
+ mspLoopEdges.insert(pKF);
+}
+
+set<KeyFrame*> KeyFrame::GetLoopEdges()
+{
+ unique_lock<mutex> lockCon(mMutexConnections);
+ return mspLoopEdges;
+}
+
+void KeyFrame::SetNotErase()
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ mbNotErase = true;
+}
+
+void KeyFrame::SetErase()
+{
+ {
+ unique_lock<mutex> lock(mMutexConnections);
+ if(mspLoopEdges.empty())
+ {
+ mbNotErase = false;
+ }
+ }
+
+ if(mbToBeErased)
+ {
+ SetBadFlag();
+ }
+}
+
+void KeyFrame::SetBadFlag()
+{
+ {
+ unique_lock<mutex> lock(mMutexConnections);
+ if(mnId==0)
+ return;
+ else if(mbNotErase)
+ {
+ mbToBeErased = true;
+ return;
+ }
+ }
+
+ for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
+ mit->first->EraseConnection(this);
+
+ for(size_t i=0; i<mvpMapPoints.size(); i++)
+ if(mvpMapPoints[i])
+ mvpMapPoints[i]->EraseObservation(this);
+ {
+ unique_lock<mutex> lock(mMutexConnections);
+ unique_lock<mutex> lock1(mMutexFeatures);
+
+ mConnectedKeyFrameWeights.clear();
+ mvpOrderedConnectedKeyFrames.clear();
+
+ // Update Spanning Tree
+ set<KeyFrame*> sParentCandidates;
+ sParentCandidates.insert(mpParent);
+
+ // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
+ // Include that children as new parent candidate for the rest
+ while(!mspChildrens.empty())
+ {
+ bool bContinue = false;
+
+ int max = -1;
+ KeyFrame* pC;
+ KeyFrame* pP;
+
+ for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
+ {
+ KeyFrame* pKF = *sit;
+ if(pKF->isBad())
+ continue;
+
+ // Check if a parent candidate is connected to the keyframe
+ vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
+ for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
+ {
+ for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
+ {
+ if(vpConnected[i]->mnId == (*spcit)->mnId)
+ {
+ int w = pKF->GetWeight(vpConnected[i]);
+ if(w>max)
+ {
+ pC = pKF;
+ pP = vpConnected[i];
+ max = w;
+ bContinue = true;
+ }
+ }
+ }
+ }
+ }
+
+ if(bContinue)
+ {
+ pC->ChangeParent(pP);
+ sParentCandidates.insert(pC);
+ mspChildrens.erase(pC);
+ }
+ else
+ break;
+ }
+
+ // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
+ if(!mspChildrens.empty())
+ for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
+ {
+ (*sit)->ChangeParent(mpParent);
+ }
+
+ mpParent->EraseChild(this);
+ mTcp = Tcw*mpParent->GetPoseInverse();
+ mbBad = true;
+ }
+
+
+ mpMap->EraseKeyFrame(this);
+ mpKeyFrameDB->erase(this);
+}
+
+bool KeyFrame::isBad()
+{
+ unique_lock<mutex> lock(mMutexConnections);
+ return mbBad;
+}
+
+void KeyFrame::EraseConnection(KeyFrame* pKF)
+{
+ bool bUpdate = false;
+ {
+ unique_lock<mutex> lock(mMutexConnections);
+ if(mConnectedKeyFrameWeights.count(pKF))
+ {
+ mConnectedKeyFrameWeights.erase(pKF);
+ bUpdate=true;
+ }
+ }
+
+ if(bUpdate)
+ UpdateBestCovisibles();
+}
+
+vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
+{
+ vector<size_t> vIndices;
+ vIndices.reserve(N);
+
+ const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
+ if(nMinCellX>=mnGridCols)
+ return vIndices;
+
+ const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
+ if(nMaxCellX<0)
+ return vIndices;
+
+ const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
+ if(nMinCellY>=mnGridRows)
+ return vIndices;
+
+ const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
+ if(nMaxCellY<0)
+ return vIndices;
+
+ for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
+ {
+ for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
+ {
+ const vector<size_t> vCell = mGrid[ix][iy];
+ for(size_t j=0, jend=vCell.size(); j<jend; j++)
+ {
+ const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
+ const float distx = kpUn.pt.x-x;
+ const float disty = kpUn.pt.y-y;
+
+ if(fabs(distx)<r && fabs(disty)<r)
+ vIndices.push_back(vCell[j]);
+ }
+ }
+ }
+
+ return vIndices;
+}
+
+bool KeyFrame::IsInImage(const float &x, const float &y) const
+{
+ return (x>=mnMinX && x<mnMaxX && y>=mnMinY && y<mnMaxY);
+}
+
+cv::Mat KeyFrame::UnprojectStereo(int i)
+{
+ const float z = mvDepth[i];
+ if(z>0)
+ {
+ const float u = mvKeys[i].pt.x;
+ const float v = mvKeys[i].pt.y;
+ const float x = (u-cx)*z*invfx;
+ const float y = (v-cy)*z*invfy;
+ cv::Mat x3Dc = (cv::Mat_<float>(3,1) << x, y, z);
+
+ unique_lock<mutex> lock(mMutexPose);
+ return Twc.rowRange(0,3).colRange(0,3)*x3Dc+Twc.rowRange(0,3).col(3);
+ }
+ else
+ return cv::Mat();
+}
+
+float KeyFrame::ComputeSceneMedianDepth(const int q)
+{
+ vector<MapPoint*> vpMapPoints;
+ cv::Mat Tcw_;
+ {
+ unique_lock<mutex> lock(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPose);
+ vpMapPoints = mvpMapPoints;
+ Tcw_ = Tcw.clone();
+ }
+
+ vector<float> vDepths;
+ vDepths.reserve(N);
+ cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
+ Rcw2 = Rcw2.t();
+ float zcw = Tcw_.at<float>(2,3);
+ for(int i=0; i<N; i++)
+ {
+ if(mvpMapPoints[i])
+ {
+ MapPoint* pMP = mvpMapPoints[i];
+ cv::Mat x3Dw = pMP->GetWorldPos();
+ float z = Rcw2.dot(x3Dw)+zcw;
+ vDepths.push_back(z);
+ }
+ }
+
+ sort(vDepths.begin(),vDepths.end());
+
+ return vDepths[(vDepths.size()-1)/q];
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/KeyFrameDatabase.cc b/externals/ORB_SLAM2/src/KeyFrameDatabase.cc
new file mode 100644
index 0000000000000000000000000000000000000000..826860cab9e825c4e51e365e1e4eaad987f13d27
--- /dev/null
+++ b/externals/ORB_SLAM2/src/KeyFrameDatabase.cc
@@ -0,0 +1,311 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "KeyFrameDatabase.h"
+
+#include "KeyFrame.h"
+#include "Thirdparty/DBoW2/DBoW2/BowVector.h"
+
+#include<mutex>
+
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+KeyFrameDatabase::KeyFrameDatabase (const ORBVocabulary &voc):
+ mpVoc(&voc)
+{
+ mvInvertedFile.resize(voc.size());
+}
+
+
+void KeyFrameDatabase::add(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutex);
+
+ for(DBoW2::BowVector::const_iterator vit= pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
+ mvInvertedFile[vit->first].push_back(pKF);
+}
+
+void KeyFrameDatabase::erase(KeyFrame* pKF)
+{
+ unique_lock<mutex> lock(mMutex);
+
+ // Erase elements in the Inverse File for the entry
+ for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit!=vend; vit++)
+ {
+ // List of keyframes that share the word
+ list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
+
+ for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+ {
+ if(pKF==*lit)
+ {
+ lKFs.erase(lit);
+ break;
+ }
+ }
+ }
+}
+
+void KeyFrameDatabase::clear()
+{
+ mvInvertedFile.clear();
+ mvInvertedFile.resize(mpVoc->size());
+}
+
+
+vector<KeyFrame*> KeyFrameDatabase::DetectLoopCandidates(KeyFrame* pKF, float minScore)
+{
+ set<KeyFrame*> spConnectedKeyFrames = pKF->GetConnectedKeyFrames();
+ list<KeyFrame*> lKFsSharingWords;
+
+ // Search all keyframes that share a word with current keyframes
+ // Discard keyframes connected to the query keyframe
+ {
+ unique_lock<mutex> lock(mMutex);
+
+ for(DBoW2::BowVector::const_iterator vit=pKF->mBowVec.begin(), vend=pKF->mBowVec.end(); vit != vend; vit++)
+ {
+ list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
+
+ for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi=*lit;
+ if(pKFi->mnLoopQuery!=pKF->mnId)
+ {
+ pKFi->mnLoopWords=0;
+ if(!spConnectedKeyFrames.count(pKFi))
+ {
+ pKFi->mnLoopQuery=pKF->mnId;
+ lKFsSharingWords.push_back(pKFi);
+ }
+ }
+ pKFi->mnLoopWords++;
+ }
+ }
+ }
+
+ if(lKFsSharingWords.empty())
+ return vector<KeyFrame*>();
+
+ list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+ // Only compare against those keyframes that share enough words
+ int maxCommonWords=0;
+ for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+ {
+ if((*lit)->mnLoopWords>maxCommonWords)
+ maxCommonWords=(*lit)->mnLoopWords;
+ }
+
+ int minCommonWords = maxCommonWords*0.8f;
+
+ int nscores=0;
+
+ // Compute similarity score. Retain the matches whose score is higher than minScore
+ for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi = *lit;
+
+ if(pKFi->mnLoopWords>minCommonWords)
+ {
+ nscores++;
+
+ float si = mpVoc->score(pKF->mBowVec,pKFi->mBowVec);
+
+ pKFi->mLoopScore = si;
+ if(si>=minScore)
+ lScoreAndMatch.push_back(make_pair(si,pKFi));
+ }
+ }
+
+ if(lScoreAndMatch.empty())
+ return vector<KeyFrame*>();
+
+ list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+ float bestAccScore = minScore;
+
+ // Lets now accumulate score by covisibility
+ for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+ {
+ KeyFrame* pKFi = it->second;
+ vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+ float bestScore = it->first;
+ float accScore = it->first;
+ KeyFrame* pBestKF = pKFi;
+ for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKF2 = *vit;
+ if(pKF2->mnLoopQuery==pKF->mnId && pKF2->mnLoopWords>minCommonWords)
+ {
+ accScore+=pKF2->mLoopScore;
+ if(pKF2->mLoopScore>bestScore)
+ {
+ pBestKF=pKF2;
+ bestScore = pKF2->mLoopScore;
+ }
+ }
+ }
+
+ lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+ if(accScore>bestAccScore)
+ bestAccScore=accScore;
+ }
+
+ // Return all those keyframes with a score higher than 0.75*bestScore
+ float minScoreToRetain = 0.75f*bestAccScore;
+
+ set<KeyFrame*> spAlreadyAddedKF;
+ vector<KeyFrame*> vpLoopCandidates;
+ vpLoopCandidates.reserve(lAccScoreAndMatch.size());
+
+ for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+ {
+ if(it->first>minScoreToRetain)
+ {
+ KeyFrame* pKFi = it->second;
+ if(!spAlreadyAddedKF.count(pKFi))
+ {
+ vpLoopCandidates.push_back(pKFi);
+ spAlreadyAddedKF.insert(pKFi);
+ }
+ }
+ }
+
+
+ return vpLoopCandidates;
+}
+
+vector<KeyFrame*> KeyFrameDatabase::DetectRelocalizationCandidates(Frame *F)
+{
+ list<KeyFrame*> lKFsSharingWords;
+
+ // Search all keyframes that share a word with current frame
+ {
+ unique_lock<mutex> lock(mMutex);
+
+ for(DBoW2::BowVector::const_iterator vit=F->mBowVec.begin(), vend=F->mBowVec.end(); vit != vend; vit++)
+ {
+ list<KeyFrame*> &lKFs = mvInvertedFile[vit->first];
+
+ for(list<KeyFrame*>::iterator lit=lKFs.begin(), lend= lKFs.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi=*lit;
+ if(pKFi->mnRelocQuery!=F->mnId)
+ {
+ pKFi->mnRelocWords=0;
+ pKFi->mnRelocQuery=F->mnId;
+ lKFsSharingWords.push_back(pKFi);
+ }
+ pKFi->mnRelocWords++;
+ }
+ }
+ }
+ if(lKFsSharingWords.empty())
+ return vector<KeyFrame*>();
+
+ // Only compare against those keyframes that share enough words
+ int maxCommonWords=0;
+ for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+ {
+ if((*lit)->mnRelocWords>maxCommonWords)
+ maxCommonWords=(*lit)->mnRelocWords;
+ }
+
+ int minCommonWords = maxCommonWords*0.8f;
+
+ list<pair<float,KeyFrame*> > lScoreAndMatch;
+
+ int nscores=0;
+
+ // Compute similarity score.
+ for(list<KeyFrame*>::iterator lit=lKFsSharingWords.begin(), lend= lKFsSharingWords.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi = *lit;
+
+ if(pKFi->mnRelocWords>minCommonWords)
+ {
+ nscores++;
+ float si = mpVoc->score(F->mBowVec,pKFi->mBowVec);
+ pKFi->mRelocScore=si;
+ lScoreAndMatch.push_back(make_pair(si,pKFi));
+ }
+ }
+
+ if(lScoreAndMatch.empty())
+ return vector<KeyFrame*>();
+
+ list<pair<float,KeyFrame*> > lAccScoreAndMatch;
+ float bestAccScore = 0;
+
+ // Lets now accumulate score by covisibility
+ for(list<pair<float,KeyFrame*> >::iterator it=lScoreAndMatch.begin(), itend=lScoreAndMatch.end(); it!=itend; it++)
+ {
+ KeyFrame* pKFi = it->second;
+ vector<KeyFrame*> vpNeighs = pKFi->GetBestCovisibilityKeyFrames(10);
+
+ float bestScore = it->first;
+ float accScore = bestScore;
+ KeyFrame* pBestKF = pKFi;
+ for(vector<KeyFrame*>::iterator vit=vpNeighs.begin(), vend=vpNeighs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKF2 = *vit;
+ if(pKF2->mnRelocQuery!=F->mnId)
+ continue;
+
+ accScore+=pKF2->mRelocScore;
+ if(pKF2->mRelocScore>bestScore)
+ {
+ pBestKF=pKF2;
+ bestScore = pKF2->mRelocScore;
+ }
+
+ }
+ lAccScoreAndMatch.push_back(make_pair(accScore,pBestKF));
+ if(accScore>bestAccScore)
+ bestAccScore=accScore;
+ }
+
+ // Return all those keyframes with a score higher than 0.75*bestScore
+ float minScoreToRetain = 0.75f*bestAccScore;
+ set<KeyFrame*> spAlreadyAddedKF;
+ vector<KeyFrame*> vpRelocCandidates;
+ vpRelocCandidates.reserve(lAccScoreAndMatch.size());
+ for(list<pair<float,KeyFrame*> >::iterator it=lAccScoreAndMatch.begin(), itend=lAccScoreAndMatch.end(); it!=itend; it++)
+ {
+ const float &si = it->first;
+ if(si>minScoreToRetain)
+ {
+ KeyFrame* pKFi = it->second;
+ if(!spAlreadyAddedKF.count(pKFi))
+ {
+ vpRelocCandidates.push_back(pKFi);
+ spAlreadyAddedKF.insert(pKFi);
+ }
+ }
+ }
+
+ return vpRelocCandidates;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/LocalMapping.cc b/externals/ORB_SLAM2/src/LocalMapping.cc
new file mode 100644
index 0000000000000000000000000000000000000000..12040308f795ab361d2739a2e7ee87b84fab86c4
--- /dev/null
+++ b/externals/ORB_SLAM2/src/LocalMapping.cc
@@ -0,0 +1,760 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "LocalMapping.h"
+#include "LoopClosing.h"
+#include "ORBmatcher.h"
+#include "Optimizer.h"
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+LocalMapping::LocalMapping(Map *pMap, const float bMonocular):
+ mbMonocular(bMonocular), mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
+ mbAbortBA(false), mbStopped(false), mbStopRequested(false), mbNotStop(false), mbAcceptKeyFrames(true)
+{
+}
+
+void LocalMapping::SetLoopCloser(LoopClosing* pLoopCloser)
+{
+ mpLoopCloser = pLoopCloser;
+}
+
+void LocalMapping::SetTracker(Tracking *pTracker)
+{
+ mpTracker=pTracker;
+}
+
+void LocalMapping::Run()
+{
+
+ mbFinished = false;
+
+ while(1)
+ {
+ // Tracking will see that Local Mapping is busy
+ SetAcceptKeyFrames(false);
+
+ // Check if there are keyframes in the queue
+ if(CheckNewKeyFrames())
+ {
+ // BoW conversion and insertion in Map
+ ProcessNewKeyFrame();
+
+ // Check recent MapPoints
+ MapPointCulling();
+
+ // Triangulate new MapPoints
+ CreateNewMapPoints();
+
+ if(!CheckNewKeyFrames())
+ {
+ // Find more matches in neighbor keyframes and fuse point duplications
+ SearchInNeighbors();
+ }
+
+ mbAbortBA = false;
+
+ if(!CheckNewKeyFrames() && !stopRequested())
+ {
+ // Local BA
+ if(mpMap->KeyFramesInMap()>2)
+ Optimizer::LocalBundleAdjustment(mpCurrentKeyFrame,&mbAbortBA, mpMap);
+
+ // Check redundant local Keyframes
+ KeyFrameCulling();
+ }
+
+ mpLoopCloser->InsertKeyFrame(mpCurrentKeyFrame);
+ }
+ else if(Stop())
+ {
+ // Safe area to stop
+ while(isStopped() && !CheckFinish())
+ {
+ usleep(3000);
+ }
+ if(CheckFinish())
+ break;
+ }
+
+ ResetIfRequested();
+
+ // Tracking will see that Local Mapping is busy
+ SetAcceptKeyFrames(true);
+
+ if(CheckFinish())
+ break;
+
+ usleep(3000);
+ }
+
+ SetFinish();
+}
+
+void LocalMapping::InsertKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexNewKFs);
+ mlNewKeyFrames.push_back(pKF);
+ mbAbortBA=true;
+}
+
+
+bool LocalMapping::CheckNewKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexNewKFs);
+ return(!mlNewKeyFrames.empty());
+}
+
+void LocalMapping::ProcessNewKeyFrame()
+{
+ {
+ unique_lock<mutex> lock(mMutexNewKFs);
+ mpCurrentKeyFrame = mlNewKeyFrames.front();
+ mlNewKeyFrames.pop_front();
+ }
+
+ // Compute Bags of Words structures
+ mpCurrentKeyFrame->ComputeBoW();
+
+ // Associate MapPoints to the new keyframe and update normal and descriptor
+ const vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
+
+ for(size_t i=0; i<vpMapPointMatches.size(); i++)
+ {
+ MapPoint* pMP = vpMapPointMatches[i];
+ if(pMP)
+ {
+ if(!pMP->isBad())
+ {
+ if(!pMP->IsInKeyFrame(mpCurrentKeyFrame))
+ {
+ pMP->AddObservation(mpCurrentKeyFrame, i);
+ pMP->UpdateNormalAndDepth();
+ pMP->ComputeDistinctiveDescriptors();
+ }
+ else // this can only happen for new stereo points inserted by the Tracking
+ {
+ mlpRecentAddedMapPoints.push_back(pMP);
+ }
+ }
+ }
+ }
+
+ // Update links in the Covisibility Graph
+ mpCurrentKeyFrame->UpdateConnections();
+
+ // Insert Keyframe in Map
+ mpMap->AddKeyFrame(mpCurrentKeyFrame);
+}
+
+void LocalMapping::MapPointCulling()
+{
+ // Check Recent Added MapPoints
+ list<MapPoint*>::iterator lit = mlpRecentAddedMapPoints.begin();
+ const unsigned long int nCurrentKFid = mpCurrentKeyFrame->mnId;
+
+ int nThObs;
+ if(mbMonocular)
+ nThObs = 2;
+ else
+ nThObs = 3;
+ const int cnThObs = nThObs;
+
+ while(lit!=mlpRecentAddedMapPoints.end())
+ {
+ MapPoint* pMP = *lit;
+ if(pMP->isBad())
+ {
+ lit = mlpRecentAddedMapPoints.erase(lit);
+ }
+ else if(pMP->GetFoundRatio()<0.25f )
+ {
+ pMP->SetBadFlag();
+ lit = mlpRecentAddedMapPoints.erase(lit);
+ }
+ else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=2 && pMP->Observations()<=cnThObs)
+ {
+ pMP->SetBadFlag();
+ lit = mlpRecentAddedMapPoints.erase(lit);
+ }
+ else if(((int)nCurrentKFid-(int)pMP->mnFirstKFid)>=3)
+ lit = mlpRecentAddedMapPoints.erase(lit);
+ else
+ lit++;
+ }
+}
+
+void LocalMapping::CreateNewMapPoints()
+{
+ // Retrieve neighbor keyframes in covisibility graph
+ int nn = 10;
+ if(mbMonocular)
+ nn=20;
+ const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
+
+ ORBmatcher matcher(0.6,false);
+
+ cv::Mat Rcw1 = mpCurrentKeyFrame->GetRotation();
+ cv::Mat Rwc1 = Rcw1.t();
+ cv::Mat tcw1 = mpCurrentKeyFrame->GetTranslation();
+ cv::Mat Tcw1(3,4,CV_32F);
+ Rcw1.copyTo(Tcw1.colRange(0,3));
+ tcw1.copyTo(Tcw1.col(3));
+ cv::Mat Ow1 = mpCurrentKeyFrame->GetCameraCenter();
+
+ const float &fx1 = mpCurrentKeyFrame->fx;
+ const float &fy1 = mpCurrentKeyFrame->fy;
+ const float &cx1 = mpCurrentKeyFrame->cx;
+ const float &cy1 = mpCurrentKeyFrame->cy;
+ const float &invfx1 = mpCurrentKeyFrame->invfx;
+ const float &invfy1 = mpCurrentKeyFrame->invfy;
+
+ const float ratioFactor = 1.5f*mpCurrentKeyFrame->mfScaleFactor;
+
+ int nnew=0;
+
+ // Search matches with epipolar restriction and triangulate
+ for(size_t i=0; i<vpNeighKFs.size(); i++)
+ {
+ if(i>0 && CheckNewKeyFrames())
+ return;
+
+ KeyFrame* pKF2 = vpNeighKFs[i];
+
+ // Check first that baseline is not too short
+ cv::Mat Ow2 = pKF2->GetCameraCenter();
+ cv::Mat vBaseline = Ow2-Ow1;
+ const float baseline = cv::norm(vBaseline);
+
+ if(!mbMonocular)
+ {
+ if(baseline<pKF2->mb)
+ continue;
+ }
+ else
+ {
+ const float medianDepthKF2 = pKF2->ComputeSceneMedianDepth(2);
+ const float ratioBaselineDepth = baseline/medianDepthKF2;
+
+ if(ratioBaselineDepth<0.01)
+ continue;
+ }
+
+ // Compute Fundamental Matrix
+ cv::Mat F12 = ComputeF12(mpCurrentKeyFrame,pKF2);
+
+ // Search matches that fullfil epipolar constraint
+ vector<pair<size_t,size_t> > vMatchedIndices;
+ matcher.SearchForTriangulation(mpCurrentKeyFrame,pKF2,F12,vMatchedIndices,false);
+
+ cv::Mat Rcw2 = pKF2->GetRotation();
+ cv::Mat Rwc2 = Rcw2.t();
+ cv::Mat tcw2 = pKF2->GetTranslation();
+ cv::Mat Tcw2(3,4,CV_32F);
+ Rcw2.copyTo(Tcw2.colRange(0,3));
+ tcw2.copyTo(Tcw2.col(3));
+
+ const float &fx2 = pKF2->fx;
+ const float &fy2 = pKF2->fy;
+ const float &cx2 = pKF2->cx;
+ const float &cy2 = pKF2->cy;
+ const float &invfx2 = pKF2->invfx;
+ const float &invfy2 = pKF2->invfy;
+
+ // Triangulate each match
+ const int nmatches = vMatchedIndices.size();
+ for(int ikp=0; ikp<nmatches; ikp++)
+ {
+ const int &idx1 = vMatchedIndices[ikp].first;
+ const int &idx2 = vMatchedIndices[ikp].second;
+
+ const cv::KeyPoint &kp1 = mpCurrentKeyFrame->mvKeysUn[idx1];
+ const float kp1_ur=mpCurrentKeyFrame->mvuRight[idx1];
+ bool bStereo1 = kp1_ur>=0;
+
+ const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
+ const float kp2_ur = pKF2->mvuRight[idx2];
+ bool bStereo2 = kp2_ur>=0;
+
+ // Check parallax between rays
+ cv::Mat xn1 = (cv::Mat_<float>(3,1) << (kp1.pt.x-cx1)*invfx1, (kp1.pt.y-cy1)*invfy1, 1.0);
+ cv::Mat xn2 = (cv::Mat_<float>(3,1) << (kp2.pt.x-cx2)*invfx2, (kp2.pt.y-cy2)*invfy2, 1.0);
+
+ cv::Mat ray1 = Rwc1*xn1;
+ cv::Mat ray2 = Rwc2*xn2;
+ const float cosParallaxRays = ray1.dot(ray2)/(cv::norm(ray1)*cv::norm(ray2));
+
+ float cosParallaxStereo = cosParallaxRays+1;
+ float cosParallaxStereo1 = cosParallaxStereo;
+ float cosParallaxStereo2 = cosParallaxStereo;
+
+ if(bStereo1)
+ cosParallaxStereo1 = cos(2*atan2(mpCurrentKeyFrame->mb/2,mpCurrentKeyFrame->mvDepth[idx1]));
+ else if(bStereo2)
+ cosParallaxStereo2 = cos(2*atan2(pKF2->mb/2,pKF2->mvDepth[idx2]));
+
+ cosParallaxStereo = min(cosParallaxStereo1,cosParallaxStereo2);
+
+ cv::Mat x3D;
+ if(cosParallaxRays<cosParallaxStereo && cosParallaxRays>0 && (bStereo1 || bStereo2 || cosParallaxRays<0.9998))
+ {
+ // Linear Triangulation Method
+ cv::Mat A(4,4,CV_32F);
+ A.row(0) = xn1.at<float>(0)*Tcw1.row(2)-Tcw1.row(0);
+ A.row(1) = xn1.at<float>(1)*Tcw1.row(2)-Tcw1.row(1);
+ A.row(2) = xn2.at<float>(0)*Tcw2.row(2)-Tcw2.row(0);
+ A.row(3) = xn2.at<float>(1)*Tcw2.row(2)-Tcw2.row(1);
+
+ cv::Mat w,u,vt;
+ cv::SVD::compute(A,w,u,vt,cv::SVD::MODIFY_A| cv::SVD::FULL_UV);
+
+ x3D = vt.row(3).t();
+
+ if(x3D.at<float>(3)==0)
+ continue;
+
+ // Euclidean coordinates
+ x3D = x3D.rowRange(0,3)/x3D.at<float>(3);
+
+ }
+ else if(bStereo1 && cosParallaxStereo1<cosParallaxStereo2)
+ {
+ x3D = mpCurrentKeyFrame->UnprojectStereo(idx1);
+ }
+ else if(bStereo2 && cosParallaxStereo2<cosParallaxStereo1)
+ {
+ x3D = pKF2->UnprojectStereo(idx2);
+ }
+ else
+ continue; //No stereo and very low parallax
+
+ cv::Mat x3Dt = x3D.t();
+
+ //Check triangulation in front of cameras
+ float z1 = Rcw1.row(2).dot(x3Dt)+tcw1.at<float>(2);
+ if(z1<=0)
+ continue;
+
+ float z2 = Rcw2.row(2).dot(x3Dt)+tcw2.at<float>(2);
+ if(z2<=0)
+ continue;
+
+ //Check reprojection error in first keyframe
+ const float &sigmaSquare1 = mpCurrentKeyFrame->mvLevelSigma2[kp1.octave];
+ const float x1 = Rcw1.row(0).dot(x3Dt)+tcw1.at<float>(0);
+ const float y1 = Rcw1.row(1).dot(x3Dt)+tcw1.at<float>(1);
+ const float invz1 = 1.0/z1;
+
+ if(!bStereo1)
+ {
+ float u1 = fx1*x1*invz1+cx1;
+ float v1 = fy1*y1*invz1+cy1;
+ float errX1 = u1 - kp1.pt.x;
+ float errY1 = v1 - kp1.pt.y;
+ if((errX1*errX1+errY1*errY1)>5.991*sigmaSquare1)
+ continue;
+ }
+ else
+ {
+ float u1 = fx1*x1*invz1+cx1;
+ float u1_r = u1 - mpCurrentKeyFrame->mbf*invz1;
+ float v1 = fy1*y1*invz1+cy1;
+ float errX1 = u1 - kp1.pt.x;
+ float errY1 = v1 - kp1.pt.y;
+ float errX1_r = u1_r - kp1_ur;
+ if((errX1*errX1+errY1*errY1+errX1_r*errX1_r)>7.8*sigmaSquare1)
+ continue;
+ }
+
+ //Check reprojection error in second keyframe
+ const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
+ const float x2 = Rcw2.row(0).dot(x3Dt)+tcw2.at<float>(0);
+ const float y2 = Rcw2.row(1).dot(x3Dt)+tcw2.at<float>(1);
+ const float invz2 = 1.0/z2;
+ if(!bStereo2)
+ {
+ float u2 = fx2*x2*invz2+cx2;
+ float v2 = fy2*y2*invz2+cy2;
+ float errX2 = u2 - kp2.pt.x;
+ float errY2 = v2 - kp2.pt.y;
+ if((errX2*errX2+errY2*errY2)>5.991*sigmaSquare2)
+ continue;
+ }
+ else
+ {
+ float u2 = fx2*x2*invz2+cx2;
+ float u2_r = u2 - mpCurrentKeyFrame->mbf*invz2;
+ float v2 = fy2*y2*invz2+cy2;
+ float errX2 = u2 - kp2.pt.x;
+ float errY2 = v2 - kp2.pt.y;
+ float errX2_r = u2_r - kp2_ur;
+ if((errX2*errX2+errY2*errY2+errX2_r*errX2_r)>7.8*sigmaSquare2)
+ continue;
+ }
+
+ //Check scale consistency
+ cv::Mat normal1 = x3D-Ow1;
+ float dist1 = cv::norm(normal1);
+
+ cv::Mat normal2 = x3D-Ow2;
+ float dist2 = cv::norm(normal2);
+
+ if(dist1==0 || dist2==0)
+ continue;
+
+ const float ratioDist = dist2/dist1;
+ const float ratioOctave = mpCurrentKeyFrame->mvScaleFactors[kp1.octave]/pKF2->mvScaleFactors[kp2.octave];
+
+ /*if(fabs(ratioDist-ratioOctave)>ratioFactor)
+ continue;*/
+ if(ratioDist*ratioFactor<ratioOctave || ratioDist>ratioOctave*ratioFactor)
+ continue;
+
+ // Triangulation is succesfull
+ MapPoint* pMP = new MapPoint(x3D,mpCurrentKeyFrame,mpMap);
+
+ pMP->AddObservation(mpCurrentKeyFrame,idx1);
+ pMP->AddObservation(pKF2,idx2);
+
+ mpCurrentKeyFrame->AddMapPoint(pMP,idx1);
+ pKF2->AddMapPoint(pMP,idx2);
+
+ pMP->ComputeDistinctiveDescriptors();
+
+ pMP->UpdateNormalAndDepth();
+
+ mpMap->AddMapPoint(pMP);
+ mlpRecentAddedMapPoints.push_back(pMP);
+
+ nnew++;
+ }
+ }
+}
+
+void LocalMapping::SearchInNeighbors()
+{
+ // Retrieve neighbor keyframes
+ int nn = 10;
+ if(mbMonocular)
+ nn=20;
+ const vector<KeyFrame*> vpNeighKFs = mpCurrentKeyFrame->GetBestCovisibilityKeyFrames(nn);
+ vector<KeyFrame*> vpTargetKFs;
+ for(vector<KeyFrame*>::const_iterator vit=vpNeighKFs.begin(), vend=vpNeighKFs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKFi = *vit;
+ if(pKFi->isBad() || pKFi->mnFuseTargetForKF == mpCurrentKeyFrame->mnId)
+ continue;
+ vpTargetKFs.push_back(pKFi);
+ pKFi->mnFuseTargetForKF = mpCurrentKeyFrame->mnId;
+
+ // Extend to some second neighbors
+ const vector<KeyFrame*> vpSecondNeighKFs = pKFi->GetBestCovisibilityKeyFrames(5);
+ for(vector<KeyFrame*>::const_iterator vit2=vpSecondNeighKFs.begin(), vend2=vpSecondNeighKFs.end(); vit2!=vend2; vit2++)
+ {
+ KeyFrame* pKFi2 = *vit2;
+ if(pKFi2->isBad() || pKFi2->mnFuseTargetForKF==mpCurrentKeyFrame->mnId || pKFi2->mnId==mpCurrentKeyFrame->mnId)
+ continue;
+ vpTargetKFs.push_back(pKFi2);
+ }
+ }
+
+
+ // Search matches by projection from current KF in target KFs
+ ORBmatcher matcher;
+ vector<MapPoint*> vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
+ for(vector<KeyFrame*>::iterator vit=vpTargetKFs.begin(), vend=vpTargetKFs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKFi = *vit;
+
+ matcher.Fuse(pKFi,vpMapPointMatches);
+ }
+
+ // Search matches by projection from target KFs in current KF
+ vector<MapPoint*> vpFuseCandidates;
+ vpFuseCandidates.reserve(vpTargetKFs.size()*vpMapPointMatches.size());
+
+ for(vector<KeyFrame*>::iterator vitKF=vpTargetKFs.begin(), vendKF=vpTargetKFs.end(); vitKF!=vendKF; vitKF++)
+ {
+ KeyFrame* pKFi = *vitKF;
+
+ vector<MapPoint*> vpMapPointsKFi = pKFi->GetMapPointMatches();
+
+ for(vector<MapPoint*>::iterator vitMP=vpMapPointsKFi.begin(), vendMP=vpMapPointsKFi.end(); vitMP!=vendMP; vitMP++)
+ {
+ MapPoint* pMP = *vitMP;
+ if(!pMP)
+ continue;
+ if(pMP->isBad() || pMP->mnFuseCandidateForKF == mpCurrentKeyFrame->mnId)
+ continue;
+ pMP->mnFuseCandidateForKF = mpCurrentKeyFrame->mnId;
+ vpFuseCandidates.push_back(pMP);
+ }
+ }
+
+ matcher.Fuse(mpCurrentKeyFrame,vpFuseCandidates);
+
+
+ // Update points
+ vpMapPointMatches = mpCurrentKeyFrame->GetMapPointMatches();
+ for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
+ {
+ MapPoint* pMP=vpMapPointMatches[i];
+ if(pMP)
+ {
+ if(!pMP->isBad())
+ {
+ pMP->ComputeDistinctiveDescriptors();
+ pMP->UpdateNormalAndDepth();
+ }
+ }
+ }
+
+ // Update connections in covisibility graph
+ mpCurrentKeyFrame->UpdateConnections();
+}
+
+cv::Mat LocalMapping::ComputeF12(KeyFrame *&pKF1, KeyFrame *&pKF2)
+{
+ cv::Mat R1w = pKF1->GetRotation();
+ cv::Mat t1w = pKF1->GetTranslation();
+ cv::Mat R2w = pKF2->GetRotation();
+ cv::Mat t2w = pKF2->GetTranslation();
+
+ cv::Mat R12 = R1w*R2w.t();
+ cv::Mat t12 = -R1w*R2w.t()*t2w+t1w;
+
+ cv::Mat t12x = SkewSymmetricMatrix(t12);
+
+ const cv::Mat &K1 = pKF1->mK;
+ const cv::Mat &K2 = pKF2->mK;
+
+
+ return K1.t().inv()*t12x*R12*K2.inv();
+}
+
+void LocalMapping::RequestStop()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ mbStopRequested = true;
+ unique_lock<mutex> lock2(mMutexNewKFs);
+ mbAbortBA = true;
+}
+
+bool LocalMapping::Stop()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ if(mbStopRequested && !mbNotStop)
+ {
+ mbStopped = true;
+ cout << "Local Mapping STOP" << endl;
+ return true;
+ }
+
+ return false;
+}
+
+bool LocalMapping::isStopped()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ return mbStopped;
+}
+
+bool LocalMapping::stopRequested()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ return mbStopRequested;
+}
+
+void LocalMapping::Release()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ unique_lock<mutex> lock2(mMutexFinish);
+ if(mbFinished)
+ return;
+ mbStopped = false;
+ mbStopRequested = false;
+ for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++)
+ delete *lit;
+ mlNewKeyFrames.clear();
+
+ cout << "Local Mapping RELEASE" << endl;
+}
+
+bool LocalMapping::AcceptKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexAccept);
+ return mbAcceptKeyFrames;
+}
+
+void LocalMapping::SetAcceptKeyFrames(bool flag)
+{
+ unique_lock<mutex> lock(mMutexAccept);
+ mbAcceptKeyFrames=flag;
+}
+
+bool LocalMapping::SetNotStop(bool flag)
+{
+ unique_lock<mutex> lock(mMutexStop);
+
+ if(flag && mbStopped)
+ return false;
+
+ mbNotStop = flag;
+
+ return true;
+}
+
+void LocalMapping::InterruptBA()
+{
+ mbAbortBA = true;
+}
+
+void LocalMapping::KeyFrameCulling()
+{
+ // Check redundant keyframes (only local keyframes)
+ // A keyframe is considered redundant if the 90% of the MapPoints it sees, are seen
+ // in at least other 3 keyframes (in the same or finer scale)
+ // We only consider close stereo points
+ vector<KeyFrame*> vpLocalKeyFrames = mpCurrentKeyFrame->GetVectorCovisibleKeyFrames();
+
+ for(vector<KeyFrame*>::iterator vit=vpLocalKeyFrames.begin(), vend=vpLocalKeyFrames.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKF = *vit;
+ if(pKF->mnId==0)
+ continue;
+ const vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
+
+ int nObs = 3;
+ const int thObs=nObs;
+ int nRedundantObservations=0;
+ int nMPs=0;
+ for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
+ {
+ MapPoint* pMP = vpMapPoints[i];
+ if(pMP)
+ {
+ if(!pMP->isBad())
+ {
+ if(!mbMonocular)
+ {
+ if(pKF->mvDepth[i]>pKF->mThDepth || pKF->mvDepth[i]<0)
+ continue;
+ }
+
+ nMPs++;
+ if(pMP->Observations()>thObs)
+ {
+ const int &scaleLevel = pKF->mvKeysUn[i].octave;
+ const map<KeyFrame*, size_t> observations = pMP->GetObservations();
+ int nObs=0;
+ for(map<KeyFrame*, size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKFi = mit->first;
+ if(pKFi==pKF)
+ continue;
+ const int &scaleLeveli = pKFi->mvKeysUn[mit->second].octave;
+
+ if(scaleLeveli<=scaleLevel+1)
+ {
+ nObs++;
+ if(nObs>=thObs)
+ break;
+ }
+ }
+ if(nObs>=thObs)
+ {
+ nRedundantObservations++;
+ }
+ }
+ }
+ }
+ }
+
+ if(nRedundantObservations>0.9*nMPs)
+ pKF->SetBadFlag();
+ }
+}
+
+cv::Mat LocalMapping::SkewSymmetricMatrix(const cv::Mat &v)
+{
+ return (cv::Mat_<float>(3,3) << 0, -v.at<float>(2), v.at<float>(1),
+ v.at<float>(2), 0,-v.at<float>(0),
+ -v.at<float>(1), v.at<float>(0), 0);
+}
+
+void LocalMapping::RequestReset()
+{
+ {
+ unique_lock<mutex> lock(mMutexReset);
+ mbResetRequested = true;
+ }
+
+ while(1)
+ {
+ {
+ unique_lock<mutex> lock2(mMutexReset);
+ if(!mbResetRequested)
+ break;
+ }
+ usleep(3000);
+ }
+}
+
+void LocalMapping::ResetIfRequested()
+{
+ unique_lock<mutex> lock(mMutexReset);
+ if(mbResetRequested)
+ {
+ mlNewKeyFrames.clear();
+ mlpRecentAddedMapPoints.clear();
+ mbResetRequested=false;
+ }
+}
+
+void LocalMapping::RequestFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinishRequested = true;
+}
+
+bool LocalMapping::CheckFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinishRequested;
+}
+
+void LocalMapping::SetFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinished = true;
+ unique_lock<mutex> lock2(mMutexStop);
+ mbStopped = true;
+}
+
+bool LocalMapping::isFinished()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinished;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/LoopClosing.cc b/externals/ORB_SLAM2/src/LoopClosing.cc
new file mode 100644
index 0000000000000000000000000000000000000000..5e317dd420278d8145f102bb7903cc6920978b10
--- /dev/null
+++ b/externals/ORB_SLAM2/src/LoopClosing.cc
@@ -0,0 +1,776 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "LoopClosing.h"
+
+#include "Sim3Solver.h"
+
+#include "Converter.h"
+
+#include "Optimizer.h"
+
+#include "ORBmatcher.h"
+
+#include<mutex>
+#include<thread>
+
+
+namespace ORB_SLAM2
+{
+
+LoopClosing::LoopClosing(Map *pMap, KeyFrameDatabase *pDB, ORBVocabulary *pVoc, const bool bFixScale):
+ mbResetRequested(false), mbFinishRequested(false), mbFinished(true), mpMap(pMap),
+ mpKeyFrameDB(pDB), mpORBVocabulary(pVoc), mpMatchedKF(NULL), mLastLoopKFid(0), mbRunningGBA(false), mbFinishedGBA(true),
+ mbStopGBA(false), mpThreadGBA(NULL), mbFixScale(bFixScale), mnFullBAIdx(0)
+{
+ mnCovisibilityConsistencyTh = 3;
+}
+
+void LoopClosing::SetTracker(Tracking *pTracker)
+{
+ mpTracker=pTracker;
+}
+
+void LoopClosing::SetLocalMapper(LocalMapping *pLocalMapper)
+{
+ mpLocalMapper=pLocalMapper;
+}
+
+
+void LoopClosing::Run()
+{
+ mbFinished =false;
+
+ while(1)
+ {
+ // Check if there are keyframes in the queue
+ if(CheckNewKeyFrames())
+ {
+ // Detect loop candidates and check covisibility consistency
+ if(DetectLoop())
+ {
+ // Compute similarity transformation [sR|t]
+ // In the stereo/RGBD case s=1
+ if(ComputeSim3())
+ {
+ // Perform loop fusion and pose graph optimization
+ CorrectLoop();
+ }
+ }
+ }
+
+ ResetIfRequested();
+
+ if(CheckFinish())
+ break;
+
+ usleep(5000);
+ }
+
+ SetFinish();
+}
+
+void LoopClosing::InsertKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexLoopQueue);
+ if(pKF->mnId!=0)
+ mlpLoopKeyFrameQueue.push_back(pKF);
+}
+
+bool LoopClosing::CheckNewKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexLoopQueue);
+ return(!mlpLoopKeyFrameQueue.empty());
+}
+
+bool LoopClosing::DetectLoop()
+{
+ {
+ unique_lock<mutex> lock(mMutexLoopQueue);
+ mpCurrentKF = mlpLoopKeyFrameQueue.front();
+ mlpLoopKeyFrameQueue.pop_front();
+ // Avoid that a keyframe can be erased while it is being process by this thread
+ mpCurrentKF->SetNotErase();
+ }
+
+ //If the map contains less than 10 KF or less than 10 KF have passed from last loop detection
+ if(mpCurrentKF->mnId<mLastLoopKFid+10)
+ {
+ mpKeyFrameDB->add(mpCurrentKF);
+ mpCurrentKF->SetErase();
+ return false;
+ }
+
+ // Compute reference BoW similarity score
+ // This is the lowest score to a connected keyframe in the covisibility graph
+ // We will impose loop candidates to have a higher similarity than this
+ const vector<KeyFrame*> vpConnectedKeyFrames = mpCurrentKF->GetVectorCovisibleKeyFrames();
+ const DBoW2::BowVector &CurrentBowVec = mpCurrentKF->mBowVec;
+ float minScore = 1;
+ for(size_t i=0; i<vpConnectedKeyFrames.size(); i++)
+ {
+ KeyFrame* pKF = vpConnectedKeyFrames[i];
+ if(pKF->isBad())
+ continue;
+ const DBoW2::BowVector &BowVec = pKF->mBowVec;
+
+ float score = mpORBVocabulary->score(CurrentBowVec, BowVec);
+
+ if(score<minScore)
+ minScore = score;
+ }
+
+ // Query the database imposing the minimum score
+ vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectLoopCandidates(mpCurrentKF, minScore);
+
+ // If there are no loop candidates, just add new keyframe and return false
+ if(vpCandidateKFs.empty())
+ {
+ mpKeyFrameDB->add(mpCurrentKF);
+ mvConsistentGroups.clear();
+ mpCurrentKF->SetErase();
+ return false;
+ }
+
+ // For each loop candidate check consistency with previous loop candidates
+ // Each candidate expands a covisibility group (keyframes connected to the loop candidate in the covisibility graph)
+ // A group is consistent with a previous group if they share at least a keyframe
+ // We must detect a consistent loop in several consecutive keyframes to accept it
+ mvpEnoughConsistentCandidates.clear();
+
+ vector<ConsistentGroup> vCurrentConsistentGroups;
+ vector<bool> vbConsistentGroup(mvConsistentGroups.size(),false);
+ for(size_t i=0, iend=vpCandidateKFs.size(); i<iend; i++)
+ {
+ KeyFrame* pCandidateKF = vpCandidateKFs[i];
+
+ set<KeyFrame*> spCandidateGroup = pCandidateKF->GetConnectedKeyFrames();
+ spCandidateGroup.insert(pCandidateKF);
+
+ bool bEnoughConsistent = false;
+ bool bConsistentForSomeGroup = false;
+ for(size_t iG=0, iendG=mvConsistentGroups.size(); iG<iendG; iG++)
+ {
+ set<KeyFrame*> sPreviousGroup = mvConsistentGroups[iG].first;
+
+ bool bConsistent = false;
+ for(set<KeyFrame*>::iterator sit=spCandidateGroup.begin(), send=spCandidateGroup.end(); sit!=send;sit++)
+ {
+ if(sPreviousGroup.count(*sit))
+ {
+ bConsistent=true;
+ bConsistentForSomeGroup=true;
+ break;
+ }
+ }
+
+ if(bConsistent)
+ {
+ int nPreviousConsistency = mvConsistentGroups[iG].second;
+ int nCurrentConsistency = nPreviousConsistency + 1;
+ if(!vbConsistentGroup[iG])
+ {
+ ConsistentGroup cg = make_pair(spCandidateGroup,nCurrentConsistency);
+ vCurrentConsistentGroups.push_back(cg);
+ vbConsistentGroup[iG]=true; //this avoid to include the same group more than once
+ }
+ if(nCurrentConsistency>=mnCovisibilityConsistencyTh && !bEnoughConsistent)
+ {
+ mvpEnoughConsistentCandidates.push_back(pCandidateKF);
+ bEnoughConsistent=true; //this avoid to insert the same candidate more than once
+ }
+ }
+ }
+
+ // If the group is not consistent with any previous group insert with consistency counter set to zero
+ if(!bConsistentForSomeGroup)
+ {
+ ConsistentGroup cg = make_pair(spCandidateGroup,0);
+ vCurrentConsistentGroups.push_back(cg);
+ }
+ }
+
+ // Update Covisibility Consistent Groups
+ mvConsistentGroups = vCurrentConsistentGroups;
+
+
+ // Add Current Keyframe to database
+ mpKeyFrameDB->add(mpCurrentKF);
+
+ if(mvpEnoughConsistentCandidates.empty())
+ {
+ mpCurrentKF->SetErase();
+ return false;
+ }
+ else
+ {
+ return true;
+ }
+
+ mpCurrentKF->SetErase();
+ return false;
+}
+
+bool LoopClosing::ComputeSim3()
+{
+ // For each consistent loop candidate we try to compute a Sim3
+
+ const int nInitialCandidates = mvpEnoughConsistentCandidates.size();
+
+ // We compute first ORB matches for each candidate
+ // If enough matches are found, we setup a Sim3Solver
+ ORBmatcher matcher(0.75,true);
+
+ vector<Sim3Solver*> vpSim3Solvers;
+ vpSim3Solvers.resize(nInitialCandidates);
+
+ vector<vector<MapPoint*> > vvpMapPointMatches;
+ vvpMapPointMatches.resize(nInitialCandidates);
+
+ vector<bool> vbDiscarded;
+ vbDiscarded.resize(nInitialCandidates);
+
+ int nCandidates=0; //candidates with enough matches
+
+ for(int i=0; i<nInitialCandidates; i++)
+ {
+ KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
+
+ // avoid that local mapping erase it while it is being processed in this thread
+ pKF->SetNotErase();
+
+ if(pKF->isBad())
+ {
+ vbDiscarded[i] = true;
+ continue;
+ }
+
+ int nmatches = matcher.SearchByBoW(mpCurrentKF,pKF,vvpMapPointMatches[i]);
+
+ if(nmatches<20)
+ {
+ vbDiscarded[i] = true;
+ continue;
+ }
+ else
+ {
+ Sim3Solver* pSolver = new Sim3Solver(mpCurrentKF,pKF,vvpMapPointMatches[i],mbFixScale);
+ pSolver->SetRansacParameters(0.99,20,300);
+ vpSim3Solvers[i] = pSolver;
+ }
+
+ nCandidates++;
+ }
+
+ bool bMatch = false;
+
+ // Perform alternatively RANSAC iterations for each candidate
+ // until one is succesful or all fail
+ while(nCandidates>0 && !bMatch)
+ {
+ for(int i=0; i<nInitialCandidates; i++)
+ {
+ if(vbDiscarded[i])
+ continue;
+
+ KeyFrame* pKF = mvpEnoughConsistentCandidates[i];
+
+ // Perform 5 Ransac Iterations
+ vector<bool> vbInliers;
+ int nInliers;
+ bool bNoMore;
+
+ Sim3Solver* pSolver = vpSim3Solvers[i];
+ cv::Mat Scm = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
+
+ // If Ransac reachs max. iterations discard keyframe
+ if(bNoMore)
+ {
+ vbDiscarded[i]=true;
+ nCandidates--;
+ }
+
+ // If RANSAC returns a Sim3, perform a guided matching and optimize with all correspondences
+ if(!Scm.empty())
+ {
+ vector<MapPoint*> vpMapPointMatches(vvpMapPointMatches[i].size(), static_cast<MapPoint*>(NULL));
+ for(size_t j=0, jend=vbInliers.size(); j<jend; j++)
+ {
+ if(vbInliers[j])
+ vpMapPointMatches[j]=vvpMapPointMatches[i][j];
+ }
+
+ cv::Mat R = pSolver->GetEstimatedRotation();
+ cv::Mat t = pSolver->GetEstimatedTranslation();
+ const float s = pSolver->GetEstimatedScale();
+ matcher.SearchBySim3(mpCurrentKF,pKF,vpMapPointMatches,s,R,t,7.5);
+
+ g2o::Sim3 gScm(Converter::toMatrix3d(R),Converter::toVector3d(t),s);
+ const int nInliers = Optimizer::OptimizeSim3(mpCurrentKF, pKF, vpMapPointMatches, gScm, 10, mbFixScale);
+
+ // If optimization is succesful stop ransacs and continue
+ if(nInliers>=20)
+ {
+ bMatch = true;
+ mpMatchedKF = pKF;
+ g2o::Sim3 gSmw(Converter::toMatrix3d(pKF->GetRotation()),Converter::toVector3d(pKF->GetTranslation()),1.0);
+ mg2oScw = gScm*gSmw;
+ mScw = Converter::toCvMat(mg2oScw);
+
+ mvpCurrentMatchedPoints = vpMapPointMatches;
+ break;
+ }
+ }
+ }
+ }
+
+ if(!bMatch)
+ {
+ for(int i=0; i<nInitialCandidates; i++)
+ mvpEnoughConsistentCandidates[i]->SetErase();
+ mpCurrentKF->SetErase();
+ return false;
+ }
+
+ // Retrieve MapPoints seen in Loop Keyframe and neighbors
+ vector<KeyFrame*> vpLoopConnectedKFs = mpMatchedKF->GetVectorCovisibleKeyFrames();
+ vpLoopConnectedKFs.push_back(mpMatchedKF);
+ mvpLoopMapPoints.clear();
+ for(vector<KeyFrame*>::iterator vit=vpLoopConnectedKFs.begin(); vit!=vpLoopConnectedKFs.end(); vit++)
+ {
+ KeyFrame* pKF = *vit;
+ vector<MapPoint*> vpMapPoints = pKF->GetMapPointMatches();
+ for(size_t i=0, iend=vpMapPoints.size(); i<iend; i++)
+ {
+ MapPoint* pMP = vpMapPoints[i];
+ if(pMP)
+ {
+ if(!pMP->isBad() && pMP->mnLoopPointForKF!=mpCurrentKF->mnId)
+ {
+ mvpLoopMapPoints.push_back(pMP);
+ pMP->mnLoopPointForKF=mpCurrentKF->mnId;
+ }
+ }
+ }
+ }
+
+ // Find more matches projecting with the computed Sim3
+ matcher.SearchByProjection(mpCurrentKF, mScw, mvpLoopMapPoints, mvpCurrentMatchedPoints,10);
+
+ // If enough matches accept Loop
+ int nTotalMatches = 0;
+ for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
+ {
+ if(mvpCurrentMatchedPoints[i])
+ nTotalMatches++;
+ }
+
+ if(nTotalMatches>=40)
+ {
+ for(int i=0; i<nInitialCandidates; i++)
+ if(mvpEnoughConsistentCandidates[i]!=mpMatchedKF)
+ mvpEnoughConsistentCandidates[i]->SetErase();
+ return true;
+ }
+ else
+ {
+ for(int i=0; i<nInitialCandidates; i++)
+ mvpEnoughConsistentCandidates[i]->SetErase();
+ mpCurrentKF->SetErase();
+ return false;
+ }
+
+}
+
+void LoopClosing::CorrectLoop()
+{
+ cout << "Loop detected!" << endl;
+
+ // Send a stop signal to Local Mapping
+ // Avoid new keyframes are inserted while correcting the loop
+ mpLocalMapper->RequestStop();
+
+ // If a Global Bundle Adjustment is running, abort it
+ if(isRunningGBA())
+ {
+ unique_lock<mutex> lock(mMutexGBA);
+ mbStopGBA = true;
+
+ mnFullBAIdx++;
+
+ if(mpThreadGBA)
+ {
+ mpThreadGBA->detach();
+ delete mpThreadGBA;
+ }
+ }
+
+ // Wait until Local Mapping has effectively stopped
+ while(!mpLocalMapper->isStopped())
+ {
+ usleep(1000);
+ }
+
+ // Ensure current keyframe is updated
+ mpCurrentKF->UpdateConnections();
+
+ // Retrive keyframes connected to the current keyframe and compute corrected Sim3 pose by propagation
+ mvpCurrentConnectedKFs = mpCurrentKF->GetVectorCovisibleKeyFrames();
+ mvpCurrentConnectedKFs.push_back(mpCurrentKF);
+
+ KeyFrameAndPose CorrectedSim3, NonCorrectedSim3;
+ CorrectedSim3[mpCurrentKF]=mg2oScw;
+ cv::Mat Twc = mpCurrentKF->GetPoseInverse();
+
+
+ {
+ // Get Map Mutex
+ unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
+
+ for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKFi = *vit;
+
+ cv::Mat Tiw = pKFi->GetPose();
+
+ if(pKFi!=mpCurrentKF)
+ {
+ cv::Mat Tic = Tiw*Twc;
+ cv::Mat Ric = Tic.rowRange(0,3).colRange(0,3);
+ cv::Mat tic = Tic.rowRange(0,3).col(3);
+ g2o::Sim3 g2oSic(Converter::toMatrix3d(Ric),Converter::toVector3d(tic),1.0);
+ g2o::Sim3 g2oCorrectedSiw = g2oSic*mg2oScw;
+ //Pose corrected with the Sim3 of the loop closure
+ CorrectedSim3[pKFi]=g2oCorrectedSiw;
+ }
+
+ cv::Mat Riw = Tiw.rowRange(0,3).colRange(0,3);
+ cv::Mat tiw = Tiw.rowRange(0,3).col(3);
+ g2o::Sim3 g2oSiw(Converter::toMatrix3d(Riw),Converter::toVector3d(tiw),1.0);
+ //Pose without correction
+ NonCorrectedSim3[pKFi]=g2oSiw;
+ }
+
+ // Correct all MapPoints obsrved by current keyframe and neighbors, so that they align with the other side of the loop
+ for(KeyFrameAndPose::iterator mit=CorrectedSim3.begin(), mend=CorrectedSim3.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKFi = mit->first;
+ g2o::Sim3 g2oCorrectedSiw = mit->second;
+ g2o::Sim3 g2oCorrectedSwi = g2oCorrectedSiw.inverse();
+
+ g2o::Sim3 g2oSiw =NonCorrectedSim3[pKFi];
+
+ vector<MapPoint*> vpMPsi = pKFi->GetMapPointMatches();
+ for(size_t iMP=0, endMPi = vpMPsi.size(); iMP<endMPi; iMP++)
+ {
+ MapPoint* pMPi = vpMPsi[iMP];
+ if(!pMPi)
+ continue;
+ if(pMPi->isBad())
+ continue;
+ if(pMPi->mnCorrectedByKF==mpCurrentKF->mnId)
+ continue;
+
+ // Project with non-corrected pose and project back with corrected pose
+ cv::Mat P3Dw = pMPi->GetWorldPos();
+ Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
+ Eigen::Matrix<double,3,1> eigCorrectedP3Dw = g2oCorrectedSwi.map(g2oSiw.map(eigP3Dw));
+
+ cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
+ pMPi->SetWorldPos(cvCorrectedP3Dw);
+ pMPi->mnCorrectedByKF = mpCurrentKF->mnId;
+ pMPi->mnCorrectedReference = pKFi->mnId;
+ pMPi->UpdateNormalAndDepth();
+ }
+
+ // Update keyframe pose with corrected Sim3. First transform Sim3 to SE3 (scale translation)
+ Eigen::Matrix3d eigR = g2oCorrectedSiw.rotation().toRotationMatrix();
+ Eigen::Vector3d eigt = g2oCorrectedSiw.translation();
+ double s = g2oCorrectedSiw.scale();
+
+ eigt *=(1./s); //[R t/s;0 1]
+
+ cv::Mat correctedTiw = Converter::toCvSE3(eigR,eigt);
+
+ pKFi->SetPose(correctedTiw);
+
+ // Make sure connections are updated
+ pKFi->UpdateConnections();
+ }
+
+ // Start Loop Fusion
+ // Update matched map points and replace if duplicated
+ for(size_t i=0; i<mvpCurrentMatchedPoints.size(); i++)
+ {
+ if(mvpCurrentMatchedPoints[i])
+ {
+ MapPoint* pLoopMP = mvpCurrentMatchedPoints[i];
+ MapPoint* pCurMP = mpCurrentKF->GetMapPoint(i);
+ if(pCurMP)
+ pCurMP->Replace(pLoopMP);
+ else
+ {
+ mpCurrentKF->AddMapPoint(pLoopMP,i);
+ pLoopMP->AddObservation(mpCurrentKF,i);
+ pLoopMP->ComputeDistinctiveDescriptors();
+ }
+ }
+ }
+
+ }
+
+ // Project MapPoints observed in the neighborhood of the loop keyframe
+ // into the current keyframe and neighbors using corrected poses.
+ // Fuse duplications.
+ SearchAndFuse(CorrectedSim3);
+
+
+ // After the MapPoint fusion, new links in the covisibility graph will appear attaching both sides of the loop
+ map<KeyFrame*, set<KeyFrame*> > LoopConnections;
+
+ for(vector<KeyFrame*>::iterator vit=mvpCurrentConnectedKFs.begin(), vend=mvpCurrentConnectedKFs.end(); vit!=vend; vit++)
+ {
+ KeyFrame* pKFi = *vit;
+ vector<KeyFrame*> vpPreviousNeighbors = pKFi->GetVectorCovisibleKeyFrames();
+
+ // Update connections. Detect new links.
+ pKFi->UpdateConnections();
+ LoopConnections[pKFi]=pKFi->GetConnectedKeyFrames();
+ for(vector<KeyFrame*>::iterator vit_prev=vpPreviousNeighbors.begin(), vend_prev=vpPreviousNeighbors.end(); vit_prev!=vend_prev; vit_prev++)
+ {
+ LoopConnections[pKFi].erase(*vit_prev);
+ }
+ for(vector<KeyFrame*>::iterator vit2=mvpCurrentConnectedKFs.begin(), vend2=mvpCurrentConnectedKFs.end(); vit2!=vend2; vit2++)
+ {
+ LoopConnections[pKFi].erase(*vit2);
+ }
+ }
+
+ // Optimize graph
+ Optimizer::OptimizeEssentialGraph(mpMap, mpMatchedKF, mpCurrentKF, NonCorrectedSim3, CorrectedSim3, LoopConnections, mbFixScale);
+
+ mpMap->InformNewBigChange();
+
+ // Add loop edge
+ mpMatchedKF->AddLoopEdge(mpCurrentKF);
+ mpCurrentKF->AddLoopEdge(mpMatchedKF);
+
+ // Launch a new thread to perform Global Bundle Adjustment
+ mbRunningGBA = true;
+ mbFinishedGBA = false;
+ mbStopGBA = false;
+ mpThreadGBA = new thread(&LoopClosing::RunGlobalBundleAdjustment,this,mpCurrentKF->mnId);
+
+ // Loop closed. Release Local Mapping.
+ mpLocalMapper->Release();
+
+ mLastLoopKFid = mpCurrentKF->mnId;
+}
+
+void LoopClosing::SearchAndFuse(const KeyFrameAndPose &CorrectedPosesMap)
+{
+ ORBmatcher matcher(0.8);
+
+ for(KeyFrameAndPose::const_iterator mit=CorrectedPosesMap.begin(), mend=CorrectedPosesMap.end(); mit!=mend;mit++)
+ {
+ KeyFrame* pKF = mit->first;
+
+ g2o::Sim3 g2oScw = mit->second;
+ cv::Mat cvScw = Converter::toCvMat(g2oScw);
+
+ vector<MapPoint*> vpReplacePoints(mvpLoopMapPoints.size(),static_cast<MapPoint*>(NULL));
+ matcher.Fuse(pKF,cvScw,mvpLoopMapPoints,4,vpReplacePoints);
+
+ // Get Map Mutex
+ unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
+ const int nLP = mvpLoopMapPoints.size();
+ for(int i=0; i<nLP;i++)
+ {
+ MapPoint* pRep = vpReplacePoints[i];
+ if(pRep)
+ {
+ pRep->Replace(mvpLoopMapPoints[i]);
+ }
+ }
+ }
+}
+
+
+void LoopClosing::RequestReset()
+{
+ {
+ unique_lock<mutex> lock(mMutexReset);
+ mbResetRequested = true;
+ }
+
+ while(1)
+ {
+ {
+ unique_lock<mutex> lock2(mMutexReset);
+ if(!mbResetRequested)
+ break;
+ }
+ usleep(5000);
+ }
+}
+
+void LoopClosing::ResetIfRequested()
+{
+ unique_lock<mutex> lock(mMutexReset);
+ if(mbResetRequested)
+ {
+ mlpLoopKeyFrameQueue.clear();
+ mLastLoopKFid=0;
+ mbResetRequested=false;
+ }
+}
+
+void LoopClosing::RunGlobalBundleAdjustment(unsigned long nLoopKF)
+{
+ cout << "Starting Global Bundle Adjustment" << endl;
+
+ int idx = mnFullBAIdx;
+ Optimizer::GlobalBundleAdjustemnt(mpMap,10,&mbStopGBA,nLoopKF,false);
+
+ // Update all MapPoints and KeyFrames
+ // Local Mapping was active during BA, that means that there might be new keyframes
+ // not included in the Global BA and they are not consistent with the updated map.
+ // We need to propagate the correction through the spanning tree
+ {
+ unique_lock<mutex> lock(mMutexGBA);
+ if(idx!=mnFullBAIdx)
+ return;
+
+ if(!mbStopGBA)
+ {
+ cout << "Global Bundle Adjustment finished" << endl;
+ cout << "Updating map ..." << endl;
+ mpLocalMapper->RequestStop();
+ // Wait until Local Mapping has effectively stopped
+
+ while(!mpLocalMapper->isStopped() && !mpLocalMapper->isFinished())
+ {
+ usleep(1000);
+ }
+
+ // Get Map Mutex
+ unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
+
+ // Correct keyframes starting at map first keyframe
+ list<KeyFrame*> lpKFtoCheck(mpMap->mvpKeyFrameOrigins.begin(),mpMap->mvpKeyFrameOrigins.end());
+
+ while(!lpKFtoCheck.empty())
+ {
+ KeyFrame* pKF = lpKFtoCheck.front();
+ const set<KeyFrame*> sChilds = pKF->GetChilds();
+ cv::Mat Twc = pKF->GetPoseInverse();
+ for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++)
+ {
+ KeyFrame* pChild = *sit;
+ if(pChild->mnBAGlobalForKF!=nLoopKF)
+ {
+ cv::Mat Tchildc = pChild->GetPose()*Twc;
+ pChild->mTcwGBA = Tchildc*pKF->mTcwGBA;//*Tcorc*pKF->mTcwGBA;
+ pChild->mnBAGlobalForKF=nLoopKF;
+
+ }
+ lpKFtoCheck.push_back(pChild);
+ }
+
+ pKF->mTcwBefGBA = pKF->GetPose();
+ pKF->SetPose(pKF->mTcwGBA);
+ lpKFtoCheck.pop_front();
+ }
+
+ // Correct MapPoints
+ const vector<MapPoint*> vpMPs = mpMap->GetAllMapPoints();
+
+ for(size_t i=0; i<vpMPs.size(); i++)
+ {
+ MapPoint* pMP = vpMPs[i];
+
+ if(pMP->isBad())
+ continue;
+
+ if(pMP->mnBAGlobalForKF==nLoopKF)
+ {
+ // If optimized by Global BA, just update
+ pMP->SetWorldPos(pMP->mPosGBA);
+ }
+ else
+ {
+ // Update according to the correction of its reference keyframe
+ KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
+
+ if(pRefKF->mnBAGlobalForKF!=nLoopKF)
+ continue;
+
+ // Map to non-corrected camera
+ cv::Mat Rcw = pRefKF->mTcwBefGBA.rowRange(0,3).colRange(0,3);
+ cv::Mat tcw = pRefKF->mTcwBefGBA.rowRange(0,3).col(3);
+ cv::Mat Xc = Rcw*pMP->GetWorldPos()+tcw;
+
+ // Backproject using corrected camera
+ cv::Mat Twc = pRefKF->GetPoseInverse();
+ cv::Mat Rwc = Twc.rowRange(0,3).colRange(0,3);
+ cv::Mat twc = Twc.rowRange(0,3).col(3);
+
+ pMP->SetWorldPos(Rwc*Xc+twc);
+ }
+ }
+
+ mpMap->InformNewBigChange();
+
+ mpLocalMapper->Release();
+
+ cout << "Map updated!" << endl;
+ }
+
+ mbFinishedGBA = true;
+ mbRunningGBA = false;
+ }
+}
+
+void LoopClosing::RequestFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinishRequested = true;
+}
+
+bool LoopClosing::CheckFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinishRequested;
+}
+
+void LoopClosing::SetFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinished = true;
+}
+
+bool LoopClosing::isFinished()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinished;
+}
+
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Map.cc b/externals/ORB_SLAM2/src/Map.cc
new file mode 100644
index 0000000000000000000000000000000000000000..15fcd8691484514b7aa189fe4ce8821dfa85a8ab
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Map.cc
@@ -0,0 +1,133 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Map.h"
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+Map::Map():mnMaxKFid(0),mnBigChangeIdx(0)
+{
+}
+
+void Map::AddKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mspKeyFrames.insert(pKF);
+ if(pKF->mnId>mnMaxKFid)
+ mnMaxKFid=pKF->mnId;
+}
+
+void Map::AddMapPoint(MapPoint *pMP)
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mspMapPoints.insert(pMP);
+}
+
+void Map::EraseMapPoint(MapPoint *pMP)
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mspMapPoints.erase(pMP);
+
+ // TODO: This only erase the pointer.
+ // Delete the MapPoint
+}
+
+void Map::EraseKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mspKeyFrames.erase(pKF);
+
+ // TODO: This only erase the pointer.
+ // Delete the MapPoint
+}
+
+void Map::SetReferenceMapPoints(const vector<MapPoint *> &vpMPs)
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mvpReferenceMapPoints = vpMPs;
+}
+
+void Map::InformNewBigChange()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ mnBigChangeIdx++;
+}
+
+int Map::GetLastBigChangeIdx()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return mnBigChangeIdx;
+}
+
+vector<KeyFrame*> Map::GetAllKeyFrames()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return vector<KeyFrame*>(mspKeyFrames.begin(),mspKeyFrames.end());
+}
+
+vector<MapPoint*> Map::GetAllMapPoints()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return vector<MapPoint*>(mspMapPoints.begin(),mspMapPoints.end());
+}
+
+long unsigned int Map::MapPointsInMap()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return mspMapPoints.size();
+}
+
+long unsigned int Map::KeyFramesInMap()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return mspKeyFrames.size();
+}
+
+vector<MapPoint*> Map::GetReferenceMapPoints()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return mvpReferenceMapPoints;
+}
+
+long unsigned int Map::GetMaxKFid()
+{
+ unique_lock<mutex> lock(mMutexMap);
+ return mnMaxKFid;
+}
+
+void Map::clear()
+{
+ for(set<MapPoint*>::iterator sit=mspMapPoints.begin(), send=mspMapPoints.end(); sit!=send; sit++)
+ delete *sit;
+
+ for(set<KeyFrame*>::iterator sit=mspKeyFrames.begin(), send=mspKeyFrames.end(); sit!=send; sit++)
+ delete *sit;
+
+ mspMapPoints.clear();
+ mspKeyFrames.clear();
+ mnMaxKFid = 0;
+ mvpReferenceMapPoints.clear();
+ mvpKeyFrameOrigins.clear();
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/MapDrawer.cc b/externals/ORB_SLAM2/src/MapDrawer.cc
new file mode 100644
index 0000000000000000000000000000000000000000..17eaa461d03dee4346ba00b3a4c619a179e0a31d
--- /dev/null
+++ b/externals/ORB_SLAM2/src/MapDrawer.cc
@@ -0,0 +1,29 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "MapDrawer.h"
+#include "MapPoint.h"
+#include "KeyFrame.h"
+#include <pangolin/pangolin.h>
+#include <mutex>
+
+namespace ORB_SLAM2
+{
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/MapPoint.cc b/externals/ORB_SLAM2/src/MapPoint.cc
new file mode 100644
index 0000000000000000000000000000000000000000..3b2921197b11c37a4c7d8c914a1d0ea987d94720
--- /dev/null
+++ b/externals/ORB_SLAM2/src/MapPoint.cc
@@ -0,0 +1,421 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "MapPoint.h"
+#include "ORBmatcher.h"
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+long unsigned int MapPoint::nNextId=0;
+mutex MapPoint::mGlobalMutex;
+
+MapPoint::MapPoint(const cv::Mat &Pos, KeyFrame *pRefKF, Map* pMap):
+ mnFirstKFid(pRefKF->mnId), mnFirstFrame(pRefKF->mnFrameId), nObs(0), mnTrackReferenceForFrame(0),
+ mnLastFrameSeen(0), mnBALocalForKF(0), mnFuseCandidateForKF(0), mnLoopPointForKF(0), mnCorrectedByKF(0),
+ mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(pRefKF), mnVisible(1), mnFound(1), mbBad(false),
+ mpReplaced(static_cast<MapPoint*>(NULL)), mfMinDistance(0), mfMaxDistance(0), mpMap(pMap)
+{
+ Pos.copyTo(mWorldPos);
+ mNormalVector = cv::Mat::zeros(3,1,CV_32F);
+
+ // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
+ unique_lock<mutex> lock(mpMap->mMutexPointCreation);
+ mnId=nNextId++;
+}
+
+MapPoint::MapPoint(const cv::Mat &Pos, Map* pMap, Frame* pFrame, const int &idxF):
+ mnFirstKFid(-1), mnFirstFrame(pFrame->mnId), nObs(0), mnTrackReferenceForFrame(0), mnLastFrameSeen(0),
+ mnBALocalForKF(0), mnFuseCandidateForKF(0),mnLoopPointForKF(0), mnCorrectedByKF(0),
+ mnCorrectedReference(0), mnBAGlobalForKF(0), mpRefKF(static_cast<KeyFrame*>(NULL)), mnVisible(1),
+ mnFound(1), mbBad(false), mpReplaced(NULL), mpMap(pMap)
+{
+ Pos.copyTo(mWorldPos);
+ cv::Mat Ow = pFrame->GetCameraCenter();
+ mNormalVector = mWorldPos - Ow;
+ mNormalVector = mNormalVector/cv::norm(mNormalVector);
+
+ cv::Mat PC = Pos - Ow;
+ const float dist = cv::norm(PC);
+ const int level = pFrame->mvKeysUn[idxF].octave;
+ const float levelScaleFactor = pFrame->mvScaleFactors[level];
+ const int nLevels = pFrame->mnScaleLevels;
+
+ mfMaxDistance = dist*levelScaleFactor;
+ mfMinDistance = mfMaxDistance/pFrame->mvScaleFactors[nLevels-1];
+
+ pFrame->mDescriptors.row(idxF).copyTo(mDescriptor);
+
+ // MapPoints can be created from Tracking and Local Mapping. This mutex avoid conflicts with id.
+ unique_lock<mutex> lock(mpMap->mMutexPointCreation);
+ mnId=nNextId++;
+}
+
+void MapPoint::SetWorldPos(const cv::Mat &Pos)
+{
+ unique_lock<mutex> lock2(mGlobalMutex);
+ unique_lock<mutex> lock(mMutexPos);
+ Pos.copyTo(mWorldPos);
+}
+
+cv::Mat MapPoint::GetWorldPos()
+{
+ unique_lock<mutex> lock(mMutexPos);
+ return mWorldPos.clone();
+}
+
+cv::Mat MapPoint::GetNormal()
+{
+ unique_lock<mutex> lock(mMutexPos);
+ return mNormalVector.clone();
+}
+
+KeyFrame* MapPoint::GetReferenceKeyFrame()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return mpRefKF;
+}
+
+void MapPoint::AddObservation(KeyFrame* pKF, size_t idx)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ if(mObservations.count(pKF))
+ return;
+ mObservations[pKF]=idx;
+
+ if(pKF->mvuRight[idx]>=0)
+ nObs+=2;
+ else
+ nObs++;
+}
+
+void MapPoint::EraseObservation(KeyFrame* pKF)
+{
+ bool bBad=false;
+ {
+ unique_lock<mutex> lock(mMutexFeatures);
+ if(mObservations.count(pKF))
+ {
+ int idx = mObservations[pKF];
+ if(pKF->mvuRight[idx]>=0)
+ nObs-=2;
+ else
+ nObs--;
+
+ mObservations.erase(pKF);
+
+ if(mpRefKF==pKF)
+ mpRefKF=mObservations.begin()->first;
+
+ // If only 2 observations or less, discard point
+ if(nObs<=2)
+ bBad=true;
+ }
+ }
+
+ if(bBad)
+ SetBadFlag();
+}
+
+map<KeyFrame*, size_t> MapPoint::GetObservations()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return mObservations;
+}
+
+int MapPoint::Observations()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return nObs;
+}
+
+void MapPoint::SetBadFlag()
+{
+ map<KeyFrame*,size_t> obs;
+ {
+ unique_lock<mutex> lock1(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPos);
+ mbBad=true;
+ obs = mObservations;
+ mObservations.clear();
+ }
+ for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKF = mit->first;
+ pKF->EraseMapPointMatch(mit->second);
+ }
+
+ mpMap->EraseMapPoint(this);
+}
+
+MapPoint* MapPoint::GetReplaced()
+{
+ unique_lock<mutex> lock1(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPos);
+ return mpReplaced;
+}
+
+void MapPoint::Replace(MapPoint* pMP)
+{
+ if(pMP->mnId==this->mnId)
+ return;
+
+ int nvisible, nfound;
+ map<KeyFrame*,size_t> obs;
+ {
+ unique_lock<mutex> lock1(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPos);
+ obs=mObservations;
+ mObservations.clear();
+ mbBad=true;
+ nvisible = mnVisible;
+ nfound = mnFound;
+ mpReplaced = pMP;
+ }
+
+ for(map<KeyFrame*,size_t>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
+ {
+ // Replace measurement in keyframe
+ KeyFrame* pKF = mit->first;
+
+ if(!pMP->IsInKeyFrame(pKF))
+ {
+ pKF->ReplaceMapPointMatch(mit->second, pMP);
+ pMP->AddObservation(pKF,mit->second);
+ }
+ else
+ {
+ pKF->EraseMapPointMatch(mit->second);
+ }
+ }
+ pMP->IncreaseFound(nfound);
+ pMP->IncreaseVisible(nvisible);
+ pMP->ComputeDistinctiveDescriptors();
+
+ mpMap->EraseMapPoint(this);
+}
+
+bool MapPoint::isBad()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPos);
+ return mbBad;
+}
+
+void MapPoint::IncreaseVisible(int n)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ mnVisible+=n;
+}
+
+void MapPoint::IncreaseFound(int n)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ mnFound+=n;
+}
+
+float MapPoint::GetFoundRatio()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return static_cast<float>(mnFound)/mnVisible;
+}
+
+void MapPoint::ComputeDistinctiveDescriptors()
+{
+ // Retrieve all observed descriptors
+ vector<cv::Mat> vDescriptors;
+
+ map<KeyFrame*,size_t> observations;
+
+ {
+ unique_lock<mutex> lock1(mMutexFeatures);
+ if(mbBad)
+ return;
+ observations=mObservations;
+ }
+
+ if(observations.empty())
+ return;
+
+ vDescriptors.reserve(observations.size());
+
+ for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKF = mit->first;
+
+ if(!pKF->isBad())
+ vDescriptors.push_back(pKF->mDescriptors.row(mit->second));
+ }
+
+ if(vDescriptors.empty())
+ return;
+
+ // Compute distances between them
+ const size_t N = vDescriptors.size();
+
+ float Distances[N][N];
+ for(size_t i=0;i<N;i++)
+ {
+ Distances[i][i]=0;
+ for(size_t j=i+1;j<N;j++)
+ {
+ int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
+ Distances[i][j]=distij;
+ Distances[j][i]=distij;
+ }
+ }
+
+ // Take the descriptor with least median distance to the rest
+ int BestMedian = INT_MAX;
+ int BestIdx = 0;
+ for(size_t i=0;i<N;i++)
+ {
+ vector<int> vDists(Distances[i],Distances[i]+N);
+ sort(vDists.begin(),vDists.end());
+ int median = vDists[0.5*(N-1)];
+
+ if(median<BestMedian)
+ {
+ BestMedian = median;
+ BestIdx = i;
+ }
+ }
+
+ {
+ unique_lock<mutex> lock(mMutexFeatures);
+ mDescriptor = vDescriptors[BestIdx].clone();
+ }
+}
+
+cv::Mat MapPoint::GetDescriptor()
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return mDescriptor.clone();
+}
+
+int MapPoint::GetIndexInKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ if(mObservations.count(pKF))
+ return mObservations[pKF];
+ else
+ return -1;
+}
+
+bool MapPoint::IsInKeyFrame(KeyFrame *pKF)
+{
+ unique_lock<mutex> lock(mMutexFeatures);
+ return (mObservations.count(pKF));
+}
+
+void MapPoint::UpdateNormalAndDepth()
+{
+ map<KeyFrame*,size_t> observations;
+ KeyFrame* pRefKF;
+ cv::Mat Pos;
+ {
+ unique_lock<mutex> lock1(mMutexFeatures);
+ unique_lock<mutex> lock2(mMutexPos);
+ if(mbBad)
+ return;
+ observations=mObservations;
+ pRefKF=mpRefKF;
+ Pos = mWorldPos.clone();
+ }
+
+ if(observations.empty())
+ return;
+
+ cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
+ int n=0;
+ for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKF = mit->first;
+ cv::Mat Owi = pKF->GetCameraCenter();
+ cv::Mat normali = mWorldPos - Owi;
+ normal = normal + normali/cv::norm(normali);
+ n++;
+ }
+
+ cv::Mat PC = Pos - pRefKF->GetCameraCenter();
+ const float dist = cv::norm(PC);
+ const int level = pRefKF->mvKeysUn[observations[pRefKF]].octave;
+ const float levelScaleFactor = pRefKF->mvScaleFactors[level];
+ const int nLevels = pRefKF->mnScaleLevels;
+
+ {
+ unique_lock<mutex> lock3(mMutexPos);
+ mfMaxDistance = dist*levelScaleFactor;
+ mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];
+ mNormalVector = normal/n;
+ }
+}
+
+float MapPoint::GetMinDistanceInvariance()
+{
+ unique_lock<mutex> lock(mMutexPos);
+ return 0.8f*mfMinDistance;
+}
+
+float MapPoint::GetMaxDistanceInvariance()
+{
+ unique_lock<mutex> lock(mMutexPos);
+ return 1.2f*mfMaxDistance;
+}
+
+int MapPoint::PredictScale(const float ¤tDist, KeyFrame* pKF)
+{
+ float ratio;
+ {
+ unique_lock<mutex> lock(mMutexPos);
+ ratio = mfMaxDistance/currentDist;
+ }
+
+ int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
+ if(nScale<0)
+ nScale = 0;
+ else if(nScale>=pKF->mnScaleLevels)
+ nScale = pKF->mnScaleLevels-1;
+
+ return nScale;
+}
+
+int MapPoint::PredictScale(const float ¤tDist, Frame* pF)
+{
+ float ratio;
+ {
+ unique_lock<mutex> lock(mMutexPos);
+ ratio = mfMaxDistance/currentDist;
+ }
+
+ int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
+ if(nScale<0)
+ nScale = 0;
+ else if(nScale>=pF->mnScaleLevels)
+ nScale = pF->mnScaleLevels-1;
+
+ return nScale;
+}
+
+
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/ORBextractor.cc b/externals/ORB_SLAM2/src/ORBextractor.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6e96c4d14558ecb8ed1ea8df676bc0a1fe85772a
--- /dev/null
+++ b/externals/ORB_SLAM2/src/ORBextractor.cc
@@ -0,0 +1,1135 @@
+/**
+* This file is part of ORB-SLAM2.
+* This file is based on the file orb.cpp from the OpenCV library (see BSD license below).
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+/**
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2009, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*
+*/
+
+
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/features2d/features2d.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <vector>
+
+#include "ORBextractor.h"
+
+
+using namespace cv;
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+const int PATCH_SIZE = 31;
+const int HALF_PATCH_SIZE = 15;
+const int EDGE_THRESHOLD = 19;
+
+
+static float IC_Angle(const Mat& image, Point2f pt, const vector<int> & u_max)
+{
+ int m_01 = 0, m_10 = 0;
+
+ const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
+
+ // Treat the center line differently, v=0
+ for (int u = -HALF_PATCH_SIZE; u <= HALF_PATCH_SIZE; ++u)
+ m_10 += u * center[u];
+
+ // Go line by line in the circuI853lar patch
+ int step = (int)image.step1();
+ for (int v = 1; v <= HALF_PATCH_SIZE; ++v)
+ {
+ // Proceed over the two lines
+ int v_sum = 0;
+ int d = u_max[v];
+ for (int u = -d; u <= d; ++u)
+ {
+ int val_plus = center[u + v*step], val_minus = center[u - v*step];
+ v_sum += (val_plus - val_minus);
+ m_10 += u * (val_plus + val_minus);
+ }
+ m_01 += v * v_sum;
+ }
+
+ return fastAtan2((float)m_01, (float)m_10);
+}
+
+
+const float factorPI = (float)(CV_PI/180.f);
+static void computeOrbDescriptor(const KeyPoint& kpt,
+ const Mat& img, const Point* pattern,
+ uchar* desc)
+{
+ float angle = (float)kpt.angle*factorPI;
+ float a = (float)cos(angle), b = (float)sin(angle);
+
+ const uchar* center = &img.at<uchar>(cvRound(kpt.pt.y), cvRound(kpt.pt.x));
+ const int step = (int)img.step;
+
+ #define GET_VALUE(idx) \
+ center[cvRound(pattern[idx].x*b + pattern[idx].y*a)*step + \
+ cvRound(pattern[idx].x*a - pattern[idx].y*b)]
+
+
+ for (int i = 0; i < 32; ++i, pattern += 16)
+ {
+ int t0, t1, val;
+ t0 = GET_VALUE(0); t1 = GET_VALUE(1);
+ val = t0 < t1;
+ t0 = GET_VALUE(2); t1 = GET_VALUE(3);
+ val |= (t0 < t1) << 1;
+ t0 = GET_VALUE(4); t1 = GET_VALUE(5);
+ val |= (t0 < t1) << 2;
+ t0 = GET_VALUE(6); t1 = GET_VALUE(7);
+ val |= (t0 < t1) << 3;
+ t0 = GET_VALUE(8); t1 = GET_VALUE(9);
+ val |= (t0 < t1) << 4;
+ t0 = GET_VALUE(10); t1 = GET_VALUE(11);
+ val |= (t0 < t1) << 5;
+ t0 = GET_VALUE(12); t1 = GET_VALUE(13);
+ val |= (t0 < t1) << 6;
+ t0 = GET_VALUE(14); t1 = GET_VALUE(15);
+ val |= (t0 < t1) << 7;
+
+ desc[i] = (uchar)val;
+ }
+
+ #undef GET_VALUE
+}
+
+
+static int bit_pattern_31_[256*4] =
+{
+ 8,-3, 9,5/*mean (0), correlation (0)*/,
+ 4,2, 7,-12/*mean (1.12461e-05), correlation (0.0437584)*/,
+ -11,9, -8,2/*mean (3.37382e-05), correlation (0.0617409)*/,
+ 7,-12, 12,-13/*mean (5.62303e-05), correlation (0.0636977)*/,
+ 2,-13, 2,12/*mean (0.000134953), correlation (0.085099)*/,
+ 1,-7, 1,6/*mean (0.000528565), correlation (0.0857175)*/,
+ -2,-10, -2,-4/*mean (0.0188821), correlation (0.0985774)*/,
+ -13,-13, -11,-8/*mean (0.0363135), correlation (0.0899616)*/,
+ -13,-3, -12,-9/*mean (0.121806), correlation (0.099849)*/,
+ 10,4, 11,9/*mean (0.122065), correlation (0.093285)*/,
+ -13,-8, -8,-9/*mean (0.162787), correlation (0.0942748)*/,
+ -11,7, -9,12/*mean (0.21561), correlation (0.0974438)*/,
+ 7,7, 12,6/*mean (0.160583), correlation (0.130064)*/,
+ -4,-5, -3,0/*mean (0.228171), correlation (0.132998)*/,
+ -13,2, -12,-3/*mean (0.00997526), correlation (0.145926)*/,
+ -9,0, -7,5/*mean (0.198234), correlation (0.143636)*/,
+ 12,-6, 12,-1/*mean (0.0676226), correlation (0.16689)*/,
+ -3,6, -2,12/*mean (0.166847), correlation (0.171682)*/,
+ -6,-13, -4,-8/*mean (0.101215), correlation (0.179716)*/,
+ 11,-13, 12,-8/*mean (0.200641), correlation (0.192279)*/,
+ 4,7, 5,1/*mean (0.205106), correlation (0.186848)*/,
+ 5,-3, 10,-3/*mean (0.234908), correlation (0.192319)*/,
+ 3,-7, 6,12/*mean (0.0709964), correlation (0.210872)*/,
+ -8,-7, -6,-2/*mean (0.0939834), correlation (0.212589)*/,
+ -2,11, -1,-10/*mean (0.127778), correlation (0.20866)*/,
+ -13,12, -8,10/*mean (0.14783), correlation (0.206356)*/,
+ -7,3, -5,-3/*mean (0.182141), correlation (0.198942)*/,
+ -4,2, -3,7/*mean (0.188237), correlation (0.21384)*/,
+ -10,-12, -6,11/*mean (0.14865), correlation (0.23571)*/,
+ 5,-12, 6,-7/*mean (0.222312), correlation (0.23324)*/,
+ 5,-6, 7,-1/*mean (0.229082), correlation (0.23389)*/,
+ 1,0, 4,-5/*mean (0.241577), correlation (0.215286)*/,
+ 9,11, 11,-13/*mean (0.00338507), correlation (0.251373)*/,
+ 4,7, 4,12/*mean (0.131005), correlation (0.257622)*/,
+ 2,-1, 4,4/*mean (0.152755), correlation (0.255205)*/,
+ -4,-12, -2,7/*mean (0.182771), correlation (0.244867)*/,
+ -8,-5, -7,-10/*mean (0.186898), correlation (0.23901)*/,
+ 4,11, 9,12/*mean (0.226226), correlation (0.258255)*/,
+ 0,-8, 1,-13/*mean (0.0897886), correlation (0.274827)*/,
+ -13,-2, -8,2/*mean (0.148774), correlation (0.28065)*/,
+ -3,-2, -2,3/*mean (0.153048), correlation (0.283063)*/,
+ -6,9, -4,-9/*mean (0.169523), correlation (0.278248)*/,
+ 8,12, 10,7/*mean (0.225337), correlation (0.282851)*/,
+ 0,9, 1,3/*mean (0.226687), correlation (0.278734)*/,
+ 7,-5, 11,-10/*mean (0.00693882), correlation (0.305161)*/,
+ -13,-6, -11,0/*mean (0.0227283), correlation (0.300181)*/,
+ 10,7, 12,1/*mean (0.125517), correlation (0.31089)*/,
+ -6,-3, -6,12/*mean (0.131748), correlation (0.312779)*/,
+ 10,-9, 12,-4/*mean (0.144827), correlation (0.292797)*/,
+ -13,8, -8,-12/*mean (0.149202), correlation (0.308918)*/,
+ -13,0, -8,-4/*mean (0.160909), correlation (0.310013)*/,
+ 3,3, 7,8/*mean (0.177755), correlation (0.309394)*/,
+ 5,7, 10,-7/*mean (0.212337), correlation (0.310315)*/,
+ -1,7, 1,-12/*mean (0.214429), correlation (0.311933)*/,
+ 3,-10, 5,6/*mean (0.235807), correlation (0.313104)*/,
+ 2,-4, 3,-10/*mean (0.00494827), correlation (0.344948)*/,
+ -13,0, -13,5/*mean (0.0549145), correlation (0.344675)*/,
+ -13,-7, -12,12/*mean (0.103385), correlation (0.342715)*/,
+ -13,3, -11,8/*mean (0.134222), correlation (0.322922)*/,
+ -7,12, -4,7/*mean (0.153284), correlation (0.337061)*/,
+ 6,-10, 12,8/*mean (0.154881), correlation (0.329257)*/,
+ -9,-1, -7,-6/*mean (0.200967), correlation (0.33312)*/,
+ -2,-5, 0,12/*mean (0.201518), correlation (0.340635)*/,
+ -12,5, -7,5/*mean (0.207805), correlation (0.335631)*/,
+ 3,-10, 8,-13/*mean (0.224438), correlation (0.34504)*/,
+ -7,-7, -4,5/*mean (0.239361), correlation (0.338053)*/,
+ -3,-2, -1,-7/*mean (0.240744), correlation (0.344322)*/,
+ 2,9, 5,-11/*mean (0.242949), correlation (0.34145)*/,
+ -11,-13, -5,-13/*mean (0.244028), correlation (0.336861)*/,
+ -1,6, 0,-1/*mean (0.247571), correlation (0.343684)*/,
+ 5,-3, 5,2/*mean (0.000697256), correlation (0.357265)*/,
+ -4,-13, -4,12/*mean (0.00213675), correlation (0.373827)*/,
+ -9,-6, -9,6/*mean (0.0126856), correlation (0.373938)*/,
+ -12,-10, -8,-4/*mean (0.0152497), correlation (0.364237)*/,
+ 10,2, 12,-3/*mean (0.0299933), correlation (0.345292)*/,
+ 7,12, 12,12/*mean (0.0307242), correlation (0.366299)*/,
+ -7,-13, -6,5/*mean (0.0534975), correlation (0.368357)*/,
+ -4,9, -3,4/*mean (0.099865), correlation (0.372276)*/,
+ 7,-1, 12,2/*mean (0.117083), correlation (0.364529)*/,
+ -7,6, -5,1/*mean (0.126125), correlation (0.369606)*/,
+ -13,11, -12,5/*mean (0.130364), correlation (0.358502)*/,
+ -3,7, -2,-6/*mean (0.131691), correlation (0.375531)*/,
+ 7,-8, 12,-7/*mean (0.160166), correlation (0.379508)*/,
+ -13,-7, -11,-12/*mean (0.167848), correlation (0.353343)*/,
+ 1,-3, 12,12/*mean (0.183378), correlation (0.371916)*/,
+ 2,-6, 3,0/*mean (0.228711), correlation (0.371761)*/,
+ -4,3, -2,-13/*mean (0.247211), correlation (0.364063)*/,
+ -1,-13, 1,9/*mean (0.249325), correlation (0.378139)*/,
+ 7,1, 8,-6/*mean (0.000652272), correlation (0.411682)*/,
+ 1,-1, 3,12/*mean (0.00248538), correlation (0.392988)*/,
+ 9,1, 12,6/*mean (0.0206815), correlation (0.386106)*/,
+ -1,-9, -1,3/*mean (0.0364485), correlation (0.410752)*/,
+ -13,-13, -10,5/*mean (0.0376068), correlation (0.398374)*/,
+ 7,7, 10,12/*mean (0.0424202), correlation (0.405663)*/,
+ 12,-5, 12,9/*mean (0.0942645), correlation (0.410422)*/,
+ 6,3, 7,11/*mean (0.1074), correlation (0.413224)*/,
+ 5,-13, 6,10/*mean (0.109256), correlation (0.408646)*/,
+ 2,-12, 2,3/*mean (0.131691), correlation (0.416076)*/,
+ 3,8, 4,-6/*mean (0.165081), correlation (0.417569)*/,
+ 2,6, 12,-13/*mean (0.171874), correlation (0.408471)*/,
+ 9,-12, 10,3/*mean (0.175146), correlation (0.41296)*/,
+ -8,4, -7,9/*mean (0.183682), correlation (0.402956)*/,
+ -11,12, -4,-6/*mean (0.184672), correlation (0.416125)*/,
+ 1,12, 2,-8/*mean (0.191487), correlation (0.386696)*/,
+ 6,-9, 7,-4/*mean (0.192668), correlation (0.394771)*/,
+ 2,3, 3,-2/*mean (0.200157), correlation (0.408303)*/,
+ 6,3, 11,0/*mean (0.204588), correlation (0.411762)*/,
+ 3,-3, 8,-8/*mean (0.205904), correlation (0.416294)*/,
+ 7,8, 9,3/*mean (0.213237), correlation (0.409306)*/,
+ -11,-5, -6,-4/*mean (0.243444), correlation (0.395069)*/,
+ -10,11, -5,10/*mean (0.247672), correlation (0.413392)*/,
+ -5,-8, -3,12/*mean (0.24774), correlation (0.411416)*/,
+ -10,5, -9,0/*mean (0.00213675), correlation (0.454003)*/,
+ 8,-1, 12,-6/*mean (0.0293635), correlation (0.455368)*/,
+ 4,-6, 6,-11/*mean (0.0404971), correlation (0.457393)*/,
+ -10,12, -8,7/*mean (0.0481107), correlation (0.448364)*/,
+ 4,-2, 6,7/*mean (0.050641), correlation (0.455019)*/,
+ -2,0, -2,12/*mean (0.0525978), correlation (0.44338)*/,
+ -5,-8, -5,2/*mean (0.0629667), correlation (0.457096)*/,
+ 7,-6, 10,12/*mean (0.0653846), correlation (0.445623)*/,
+ -9,-13, -8,-8/*mean (0.0858749), correlation (0.449789)*/,
+ -5,-13, -5,-2/*mean (0.122402), correlation (0.450201)*/,
+ 8,-8, 9,-13/*mean (0.125416), correlation (0.453224)*/,
+ -9,-11, -9,0/*mean (0.130128), correlation (0.458724)*/,
+ 1,-8, 1,-2/*mean (0.132467), correlation (0.440133)*/,
+ 7,-4, 9,1/*mean (0.132692), correlation (0.454)*/,
+ -2,1, -1,-4/*mean (0.135695), correlation (0.455739)*/,
+ 11,-6, 12,-11/*mean (0.142904), correlation (0.446114)*/,
+ -12,-9, -6,4/*mean (0.146165), correlation (0.451473)*/,
+ 3,7, 7,12/*mean (0.147627), correlation (0.456643)*/,
+ 5,5, 10,8/*mean (0.152901), correlation (0.455036)*/,
+ 0,-4, 2,8/*mean (0.167083), correlation (0.459315)*/,
+ -9,12, -5,-13/*mean (0.173234), correlation (0.454706)*/,
+ 0,7, 2,12/*mean (0.18312), correlation (0.433855)*/,
+ -1,2, 1,7/*mean (0.185504), correlation (0.443838)*/,
+ 5,11, 7,-9/*mean (0.185706), correlation (0.451123)*/,
+ 3,5, 6,-8/*mean (0.188968), correlation (0.455808)*/,
+ -13,-4, -8,9/*mean (0.191667), correlation (0.459128)*/,
+ -5,9, -3,-3/*mean (0.193196), correlation (0.458364)*/,
+ -4,-7, -3,-12/*mean (0.196536), correlation (0.455782)*/,
+ 6,5, 8,0/*mean (0.1972), correlation (0.450481)*/,
+ -7,6, -6,12/*mean (0.199438), correlation (0.458156)*/,
+ -13,6, -5,-2/*mean (0.211224), correlation (0.449548)*/,
+ 1,-10, 3,10/*mean (0.211718), correlation (0.440606)*/,
+ 4,1, 8,-4/*mean (0.213034), correlation (0.443177)*/,
+ -2,-2, 2,-13/*mean (0.234334), correlation (0.455304)*/,
+ 2,-12, 12,12/*mean (0.235684), correlation (0.443436)*/,
+ -2,-13, 0,-6/*mean (0.237674), correlation (0.452525)*/,
+ 4,1, 9,3/*mean (0.23962), correlation (0.444824)*/,
+ -6,-10, -3,-5/*mean (0.248459), correlation (0.439621)*/,
+ -3,-13, -1,1/*mean (0.249505), correlation (0.456666)*/,
+ 7,5, 12,-11/*mean (0.00119208), correlation (0.495466)*/,
+ 4,-2, 5,-7/*mean (0.00372245), correlation (0.484214)*/,
+ -13,9, -9,-5/*mean (0.00741116), correlation (0.499854)*/,
+ 7,1, 8,6/*mean (0.0208952), correlation (0.499773)*/,
+ 7,-8, 7,6/*mean (0.0220085), correlation (0.501609)*/,
+ -7,-4, -7,1/*mean (0.0233806), correlation (0.496568)*/,
+ -8,11, -7,-8/*mean (0.0236505), correlation (0.489719)*/,
+ -13,6, -12,-8/*mean (0.0268781), correlation (0.503487)*/,
+ 2,4, 3,9/*mean (0.0323324), correlation (0.501938)*/,
+ 10,-5, 12,3/*mean (0.0399235), correlation (0.494029)*/,
+ -6,-5, -6,7/*mean (0.0420153), correlation (0.486579)*/,
+ 8,-3, 9,-8/*mean (0.0548021), correlation (0.484237)*/,
+ 2,-12, 2,8/*mean (0.0616622), correlation (0.496642)*/,
+ -11,-2, -10,3/*mean (0.0627755), correlation (0.498563)*/,
+ -12,-13, -7,-9/*mean (0.0829622), correlation (0.495491)*/,
+ -11,0, -10,-5/*mean (0.0843342), correlation (0.487146)*/,
+ 5,-3, 11,8/*mean (0.0929937), correlation (0.502315)*/,
+ -2,-13, -1,12/*mean (0.113327), correlation (0.48941)*/,
+ -1,-8, 0,9/*mean (0.132119), correlation (0.467268)*/,
+ -13,-11, -12,-5/*mean (0.136269), correlation (0.498771)*/,
+ -10,-2, -10,11/*mean (0.142173), correlation (0.498714)*/,
+ -3,9, -2,-13/*mean (0.144141), correlation (0.491973)*/,
+ 2,-3, 3,2/*mean (0.14892), correlation (0.500782)*/,
+ -9,-13, -4,0/*mean (0.150371), correlation (0.498211)*/,
+ -4,6, -3,-10/*mean (0.152159), correlation (0.495547)*/,
+ -4,12, -2,-7/*mean (0.156152), correlation (0.496925)*/,
+ -6,-11, -4,9/*mean (0.15749), correlation (0.499222)*/,
+ 6,-3, 6,11/*mean (0.159211), correlation (0.503821)*/,
+ -13,11, -5,5/*mean (0.162427), correlation (0.501907)*/,
+ 11,11, 12,6/*mean (0.16652), correlation (0.497632)*/,
+ 7,-5, 12,-2/*mean (0.169141), correlation (0.484474)*/,
+ -1,12, 0,7/*mean (0.169456), correlation (0.495339)*/,
+ -4,-8, -3,-2/*mean (0.171457), correlation (0.487251)*/,
+ -7,1, -6,7/*mean (0.175), correlation (0.500024)*/,
+ -13,-12, -8,-13/*mean (0.175866), correlation (0.497523)*/,
+ -7,-2, -6,-8/*mean (0.178273), correlation (0.501854)*/,
+ -8,5, -6,-9/*mean (0.181107), correlation (0.494888)*/,
+ -5,-1, -4,5/*mean (0.190227), correlation (0.482557)*/,
+ -13,7, -8,10/*mean (0.196739), correlation (0.496503)*/,
+ 1,5, 5,-13/*mean (0.19973), correlation (0.499759)*/,
+ 1,0, 10,-13/*mean (0.204465), correlation (0.49873)*/,
+ 9,12, 10,-1/*mean (0.209334), correlation (0.49063)*/,
+ 5,-8, 10,-9/*mean (0.211134), correlation (0.503011)*/,
+ -1,11, 1,-13/*mean (0.212), correlation (0.499414)*/,
+ -9,-3, -6,2/*mean (0.212168), correlation (0.480739)*/,
+ -1,-10, 1,12/*mean (0.212731), correlation (0.502523)*/,
+ -13,1, -8,-10/*mean (0.21327), correlation (0.489786)*/,
+ 8,-11, 10,-6/*mean (0.214159), correlation (0.488246)*/,
+ 2,-13, 3,-6/*mean (0.216993), correlation (0.50287)*/,
+ 7,-13, 12,-9/*mean (0.223639), correlation (0.470502)*/,
+ -10,-10, -5,-7/*mean (0.224089), correlation (0.500852)*/,
+ -10,-8, -8,-13/*mean (0.228666), correlation (0.502629)*/,
+ 4,-6, 8,5/*mean (0.22906), correlation (0.498305)*/,
+ 3,12, 8,-13/*mean (0.233378), correlation (0.503825)*/,
+ -4,2, -3,-3/*mean (0.234323), correlation (0.476692)*/,
+ 5,-13, 10,-12/*mean (0.236392), correlation (0.475462)*/,
+ 4,-13, 5,-1/*mean (0.236842), correlation (0.504132)*/,
+ -9,9, -4,3/*mean (0.236977), correlation (0.497739)*/,
+ 0,3, 3,-9/*mean (0.24314), correlation (0.499398)*/,
+ -12,1, -6,1/*mean (0.243297), correlation (0.489447)*/,
+ 3,2, 4,-8/*mean (0.00155196), correlation (0.553496)*/,
+ -10,-10, -10,9/*mean (0.00239541), correlation (0.54297)*/,
+ 8,-13, 12,12/*mean (0.0034413), correlation (0.544361)*/,
+ -8,-12, -6,-5/*mean (0.003565), correlation (0.551225)*/,
+ 2,2, 3,7/*mean (0.00835583), correlation (0.55285)*/,
+ 10,6, 11,-8/*mean (0.00885065), correlation (0.540913)*/,
+ 6,8, 8,-12/*mean (0.0101552), correlation (0.551085)*/,
+ -7,10, -6,5/*mean (0.0102227), correlation (0.533635)*/,
+ -3,-9, -3,9/*mean (0.0110211), correlation (0.543121)*/,
+ -1,-13, -1,5/*mean (0.0113473), correlation (0.550173)*/,
+ -3,-7, -3,4/*mean (0.0140913), correlation (0.554774)*/,
+ -8,-2, -8,3/*mean (0.017049), correlation (0.55461)*/,
+ 4,2, 12,12/*mean (0.01778), correlation (0.546921)*/,
+ 2,-5, 3,11/*mean (0.0224022), correlation (0.549667)*/,
+ 6,-9, 11,-13/*mean (0.029161), correlation (0.546295)*/,
+ 3,-1, 7,12/*mean (0.0303081), correlation (0.548599)*/,
+ 11,-1, 12,4/*mean (0.0355151), correlation (0.523943)*/,
+ -3,0, -3,6/*mean (0.0417904), correlation (0.543395)*/,
+ 4,-11, 4,12/*mean (0.0487292), correlation (0.542818)*/,
+ 2,-4, 2,1/*mean (0.0575124), correlation (0.554888)*/,
+ -10,-6, -8,1/*mean (0.0594242), correlation (0.544026)*/,
+ -13,7, -11,1/*mean (0.0597391), correlation (0.550524)*/,
+ -13,12, -11,-13/*mean (0.0608974), correlation (0.55383)*/,
+ 6,0, 11,-13/*mean (0.065126), correlation (0.552006)*/,
+ 0,-1, 1,4/*mean (0.074224), correlation (0.546372)*/,
+ -13,3, -9,-2/*mean (0.0808592), correlation (0.554875)*/,
+ -9,8, -6,-3/*mean (0.0883378), correlation (0.551178)*/,
+ -13,-6, -8,-2/*mean (0.0901035), correlation (0.548446)*/,
+ 5,-9, 8,10/*mean (0.0949843), correlation (0.554694)*/,
+ 2,7, 3,-9/*mean (0.0994152), correlation (0.550979)*/,
+ -1,-6, -1,-1/*mean (0.10045), correlation (0.552714)*/,
+ 9,5, 11,-2/*mean (0.100686), correlation (0.552594)*/,
+ 11,-3, 12,-8/*mean (0.101091), correlation (0.532394)*/,
+ 3,0, 3,5/*mean (0.101147), correlation (0.525576)*/,
+ -1,4, 0,10/*mean (0.105263), correlation (0.531498)*/,
+ 3,-6, 4,5/*mean (0.110785), correlation (0.540491)*/,
+ -13,0, -10,5/*mean (0.112798), correlation (0.536582)*/,
+ 5,8, 12,11/*mean (0.114181), correlation (0.555793)*/,
+ 8,9, 9,-6/*mean (0.117431), correlation (0.553763)*/,
+ 7,-4, 8,-12/*mean (0.118522), correlation (0.553452)*/,
+ -10,4, -10,9/*mean (0.12094), correlation (0.554785)*/,
+ 7,3, 12,4/*mean (0.122582), correlation (0.555825)*/,
+ 9,-7, 10,-2/*mean (0.124978), correlation (0.549846)*/,
+ 7,0, 12,-2/*mean (0.127002), correlation (0.537452)*/,
+ -1,-6, 0,-11/*mean (0.127148), correlation (0.547401)*/
+};
+
+ORBextractor::ORBextractor(int _nfeatures, float _scaleFactor, int _nlevels,
+ int _iniThFAST, int _minThFAST):
+ nfeatures(_nfeatures), scaleFactor(_scaleFactor), nlevels(_nlevels),
+ iniThFAST(_iniThFAST), minThFAST(_minThFAST)
+{
+ mvScaleFactor.resize(nlevels);
+ mvLevelSigma2.resize(nlevels);
+ mvScaleFactor[0]=1.0f;
+ mvLevelSigma2[0]=1.0f;
+ for(int i=1; i<nlevels; i++)
+ {
+ mvScaleFactor[i]=mvScaleFactor[i-1]*scaleFactor;
+ mvLevelSigma2[i]=mvScaleFactor[i]*mvScaleFactor[i];
+ }
+
+ mvInvScaleFactor.resize(nlevels);
+ mvInvLevelSigma2.resize(nlevels);
+ for(int i=0; i<nlevels; i++)
+ {
+ mvInvScaleFactor[i]=1.0f/mvScaleFactor[i];
+ mvInvLevelSigma2[i]=1.0f/mvLevelSigma2[i];
+ }
+
+ mvImagePyramid.resize(nlevels);
+
+ mnFeaturesPerLevel.resize(nlevels);
+ float factor = 1.0f / scaleFactor;
+ float nDesiredFeaturesPerScale = nfeatures*(1 - factor)/(1 - (float)pow((double)factor, (double)nlevels));
+
+ int sumFeatures = 0;
+ for( int level = 0; level < nlevels-1; level++ )
+ {
+ mnFeaturesPerLevel[level] = cvRound(nDesiredFeaturesPerScale);
+ sumFeatures += mnFeaturesPerLevel[level];
+ nDesiredFeaturesPerScale *= factor;
+ }
+ mnFeaturesPerLevel[nlevels-1] = std::max(nfeatures - sumFeatures, 0);
+
+ const int npoints = 512;
+ const Point* pattern0 = (const Point*)bit_pattern_31_;
+ std::copy(pattern0, pattern0 + npoints, std::back_inserter(pattern));
+
+ //This is for orientation
+ // pre-compute the end of a row in a circular patch
+ umax.resize(HALF_PATCH_SIZE + 1);
+
+ int v, v0, vmax = cvFloor(HALF_PATCH_SIZE * sqrt(2.f) / 2 + 1);
+ int vmin = cvCeil(HALF_PATCH_SIZE * sqrt(2.f) / 2);
+ const double hp2 = HALF_PATCH_SIZE*HALF_PATCH_SIZE;
+ for (v = 0; v <= vmax; ++v)
+ umax[v] = cvRound(sqrt(hp2 - v * v));
+
+ // Make sure we are symmetric
+ for (v = HALF_PATCH_SIZE, v0 = 0; v >= vmin; --v)
+ {
+ while (umax[v0] == umax[v0 + 1])
+ ++v0;
+ umax[v] = v0;
+ ++v0;
+ }
+}
+
+static void computeOrientation(const Mat& image, vector<KeyPoint>& keypoints, const vector<int>& umax)
+{
+ for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
+ keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
+ {
+ keypoint->angle = IC_Angle(image, keypoint->pt, umax);
+ }
+}
+
+void ExtractorNode::DivideNode(ExtractorNode &n1, ExtractorNode &n2, ExtractorNode &n3, ExtractorNode &n4)
+{
+ const int halfX = ceil(static_cast<float>(UR.x-UL.x)/2);
+ const int halfY = ceil(static_cast<float>(BR.y-UL.y)/2);
+
+ //Define boundaries of childs
+ n1.UL = UL;
+ n1.UR = cv::Point2i(UL.x+halfX,UL.y);
+ n1.BL = cv::Point2i(UL.x,UL.y+halfY);
+ n1.BR = cv::Point2i(UL.x+halfX,UL.y+halfY);
+ n1.vKeys.reserve(vKeys.size());
+
+ n2.UL = n1.UR;
+ n2.UR = UR;
+ n2.BL = n1.BR;
+ n2.BR = cv::Point2i(UR.x,UL.y+halfY);
+ n2.vKeys.reserve(vKeys.size());
+
+ n3.UL = n1.BL;
+ n3.UR = n1.BR;
+ n3.BL = BL;
+ n3.BR = cv::Point2i(n1.BR.x,BL.y);
+ n3.vKeys.reserve(vKeys.size());
+
+ n4.UL = n3.UR;
+ n4.UR = n2.BR;
+ n4.BL = n3.BR;
+ n4.BR = BR;
+ n4.vKeys.reserve(vKeys.size());
+
+ //Associate points to childs
+ for(size_t i=0;i<vKeys.size();i++)
+ {
+ const cv::KeyPoint &kp = vKeys[i];
+ if(kp.pt.x<n1.UR.x)
+ {
+ if(kp.pt.y<n1.BR.y)
+ n1.vKeys.push_back(kp);
+ else
+ n3.vKeys.push_back(kp);
+ }
+ else if(kp.pt.y<n1.BR.y)
+ n2.vKeys.push_back(kp);
+ else
+ n4.vKeys.push_back(kp);
+ }
+
+ if(n1.vKeys.size()==1)
+ n1.bNoMore = true;
+ if(n2.vKeys.size()==1)
+ n2.bNoMore = true;
+ if(n3.vKeys.size()==1)
+ n3.bNoMore = true;
+ if(n4.vKeys.size()==1)
+ n4.bNoMore = true;
+
+}
+
+vector<cv::KeyPoint> ORBextractor::DistributeOctTree(const vector<cv::KeyPoint>& vToDistributeKeys, const int &minX,
+ const int &maxX, const int &minY, const int &maxY, const int &N, const int &level)
+{
+ (void)(level);
+ // Compute how many initial nodes
+ const int nIni = round(static_cast<float>(maxX-minX)/(maxY-minY));
+
+ const float hX = static_cast<float>(maxX-minX)/nIni;
+
+ list<ExtractorNode> lNodes;
+
+ vector<ExtractorNode*> vpIniNodes;
+ vpIniNodes.resize(nIni);
+
+ for(int i=0; i<nIni; i++)
+ {
+ ExtractorNode ni;
+ ni.UL = cv::Point2i(hX*static_cast<float>(i),0);
+ ni.UR = cv::Point2i(hX*static_cast<float>(i+1),0);
+ ni.BL = cv::Point2i(ni.UL.x,maxY-minY);
+ ni.BR = cv::Point2i(ni.UR.x,maxY-minY);
+ ni.vKeys.reserve(vToDistributeKeys.size());
+
+ lNodes.push_back(ni);
+ vpIniNodes[i] = &lNodes.back();
+ }
+
+ //Associate points to childs
+ for(size_t i=0;i<vToDistributeKeys.size();i++)
+ {
+ const cv::KeyPoint &kp = vToDistributeKeys[i];
+ vpIniNodes[kp.pt.x/hX]->vKeys.push_back(kp);
+ }
+
+ list<ExtractorNode>::iterator lit = lNodes.begin();
+
+ while(lit!=lNodes.end())
+ {
+ if(lit->vKeys.size()==1)
+ {
+ lit->bNoMore=true;
+ lit++;
+ }
+ else if(lit->vKeys.empty())
+ lit = lNodes.erase(lit);
+ else
+ lit++;
+ }
+
+ bool bFinish = false;
+
+ int iteration = 0;
+
+ vector<pair<int,ExtractorNode*> > vSizeAndPointerToNode;
+ vSizeAndPointerToNode.reserve(lNodes.size()*4);
+
+ while(!bFinish)
+ {
+ iteration++;
+
+ int prevSize = lNodes.size();
+
+ lit = lNodes.begin();
+
+ int nToExpand = 0;
+
+ vSizeAndPointerToNode.clear();
+
+ while(lit!=lNodes.end())
+ {
+ if(lit->bNoMore)
+ {
+ // If node only contains one point do not subdivide and continue
+ lit++;
+ continue;
+ }
+ else
+ {
+ // If more than one point, subdivide
+ ExtractorNode n1,n2,n3,n4;
+ lit->DivideNode(n1,n2,n3,n4);
+
+ // Add childs if they contain points
+ if(n1.vKeys.size()>0)
+ {
+ lNodes.push_front(n1);
+ if(n1.vKeys.size()>1)
+ {
+ nToExpand++;
+ vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n2.vKeys.size()>0)
+ {
+ lNodes.push_front(n2);
+ if(n2.vKeys.size()>1)
+ {
+ nToExpand++;
+ vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n3.vKeys.size()>0)
+ {
+ lNodes.push_front(n3);
+ if(n3.vKeys.size()>1)
+ {
+ nToExpand++;
+ vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n4.vKeys.size()>0)
+ {
+ lNodes.push_front(n4);
+ if(n4.vKeys.size()>1)
+ {
+ nToExpand++;
+ vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+
+ lit=lNodes.erase(lit);
+ continue;
+ }
+ }
+
+ // Finish if there are more nodes than required features
+ // or all nodes contain just one point
+ if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
+ {
+ bFinish = true;
+ }
+ else if(((int)lNodes.size()+nToExpand*3)>N)
+ {
+
+ while(!bFinish)
+ {
+
+ prevSize = lNodes.size();
+
+ vector<pair<int,ExtractorNode*> > vPrevSizeAndPointerToNode = vSizeAndPointerToNode;
+ vSizeAndPointerToNode.clear();
+
+ sort(vPrevSizeAndPointerToNode.begin(),vPrevSizeAndPointerToNode.end());
+ for(int j=vPrevSizeAndPointerToNode.size()-1;j>=0;j--)
+ {
+ ExtractorNode n1,n2,n3,n4;
+ vPrevSizeAndPointerToNode[j].second->DivideNode(n1,n2,n3,n4);
+
+ // Add childs if they contain points
+ if(n1.vKeys.size()>0)
+ {
+ lNodes.push_front(n1);
+ if(n1.vKeys.size()>1)
+ {
+ vSizeAndPointerToNode.push_back(make_pair(n1.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n2.vKeys.size()>0)
+ {
+ lNodes.push_front(n2);
+ if(n2.vKeys.size()>1)
+ {
+ vSizeAndPointerToNode.push_back(make_pair(n2.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n3.vKeys.size()>0)
+ {
+ lNodes.push_front(n3);
+ if(n3.vKeys.size()>1)
+ {
+ vSizeAndPointerToNode.push_back(make_pair(n3.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+ if(n4.vKeys.size()>0)
+ {
+ lNodes.push_front(n4);
+ if(n4.vKeys.size()>1)
+ {
+ vSizeAndPointerToNode.push_back(make_pair(n4.vKeys.size(),&lNodes.front()));
+ lNodes.front().lit = lNodes.begin();
+ }
+ }
+
+ lNodes.erase(vPrevSizeAndPointerToNode[j].second->lit);
+
+ if((int)lNodes.size()>=N)
+ break;
+ }
+
+ if((int)lNodes.size()>=N || (int)lNodes.size()==prevSize)
+ bFinish = true;
+
+ }
+ }
+ }
+
+ // Retain the best point in each node
+ vector<cv::KeyPoint> vResultKeys;
+ vResultKeys.reserve(nfeatures);
+ for(list<ExtractorNode>::iterator lit=lNodes.begin(); lit!=lNodes.end(); lit++)
+ {
+ vector<cv::KeyPoint> &vNodeKeys = lit->vKeys;
+ cv::KeyPoint* pKP = &vNodeKeys[0];
+ float maxResponse = pKP->response;
+
+ for(size_t k=1;k<vNodeKeys.size();k++)
+ {
+ if(vNodeKeys[k].response>maxResponse)
+ {
+ pKP = &vNodeKeys[k];
+ maxResponse = vNodeKeys[k].response;
+ }
+ }
+
+ vResultKeys.push_back(*pKP);
+ }
+
+ return vResultKeys;
+}
+
+void ORBextractor::ComputeKeyPointsOctTree(vector<vector<KeyPoint> >& allKeypoints)
+{
+ allKeypoints.resize(nlevels);
+
+ const float W = 30;
+
+ for (int level = 0; level < nlevels; ++level)
+ {
+ const int minBorderX = EDGE_THRESHOLD-3;
+ const int minBorderY = minBorderX;
+ const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD+3;
+ const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD+3;
+
+ vector<cv::KeyPoint> vToDistributeKeys;
+ vToDistributeKeys.reserve(nfeatures*10);
+
+ const float width = (maxBorderX-minBorderX);
+ const float height = (maxBorderY-minBorderY);
+
+ const int nCols = width/W;
+ const int nRows = height/W;
+ const int wCell = ceil(width/nCols);
+ const int hCell = ceil(height/nRows);
+
+ for(int i=0; i<nRows; i++)
+ {
+ const float iniY =minBorderY+i*hCell;
+ float maxY = iniY+hCell+6;
+
+ if(iniY>=maxBorderY-3)
+ continue;
+ if(maxY>maxBorderY)
+ maxY = maxBorderY;
+
+ for(int j=0; j<nCols; j++)
+ {
+ const float iniX =minBorderX+j*wCell;
+ float maxX = iniX+wCell+6;
+ if(iniX>=maxBorderX-6)
+ continue;
+ if(maxX>maxBorderX)
+ maxX = maxBorderX;
+
+ vector<cv::KeyPoint> vKeysCell;
+ FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
+ vKeysCell,iniThFAST,true);
+
+ if(vKeysCell.empty())
+ {
+ FAST(mvImagePyramid[level].rowRange(iniY,maxY).colRange(iniX,maxX),
+ vKeysCell,minThFAST,true);
+ }
+
+ if(!vKeysCell.empty())
+ {
+ for(vector<cv::KeyPoint>::iterator vit=vKeysCell.begin(); vit!=vKeysCell.end();vit++)
+ {
+ (*vit).pt.x+=j*wCell;
+ (*vit).pt.y+=i*hCell;
+ vToDistributeKeys.push_back(*vit);
+ }
+ }
+
+ }
+ }
+
+ vector<KeyPoint> & keypoints = allKeypoints[level];
+ keypoints.reserve(nfeatures);
+
+ keypoints = DistributeOctTree(vToDistributeKeys, minBorderX, maxBorderX,
+ minBorderY, maxBorderY,mnFeaturesPerLevel[level], level);
+
+ const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
+
+ // Add border to coordinates and scale information
+ const int nkps = keypoints.size();
+ for(int i=0; i<nkps ; i++)
+ {
+ keypoints[i].pt.x+=minBorderX;
+ keypoints[i].pt.y+=minBorderY;
+ keypoints[i].octave=level;
+ keypoints[i].size = scaledPatchSize;
+ }
+ }
+
+ // compute orientations
+ for (int level = 0; level < nlevels; ++level)
+ computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
+}
+
+void ORBextractor::ComputeKeyPointsOld(std::vector<std::vector<KeyPoint> > &allKeypoints)
+{
+ allKeypoints.resize(nlevels);
+
+ float imageRatio = (float)mvImagePyramid[0].cols/mvImagePyramid[0].rows;
+
+ for (int level = 0; level < nlevels; ++level)
+ {
+ const int nDesiredFeatures = mnFeaturesPerLevel[level];
+
+ const int levelCols = sqrt((float)nDesiredFeatures/(5*imageRatio));
+ const int levelRows = imageRatio*levelCols;
+
+ const int minBorderX = EDGE_THRESHOLD;
+ const int minBorderY = minBorderX;
+ const int maxBorderX = mvImagePyramid[level].cols-EDGE_THRESHOLD;
+ const int maxBorderY = mvImagePyramid[level].rows-EDGE_THRESHOLD;
+
+ const int W = maxBorderX - minBorderX;
+ const int H = maxBorderY - minBorderY;
+ const int cellW = ceil((float)W/levelCols);
+ const int cellH = ceil((float)H/levelRows);
+
+ const int nCells = levelRows*levelCols;
+ const int nfeaturesCell = ceil((float)nDesiredFeatures/nCells);
+
+ vector<vector<vector<KeyPoint> > > cellKeyPoints(levelRows, vector<vector<KeyPoint> >(levelCols));
+
+ vector<vector<int> > nToRetain(levelRows,vector<int>(levelCols,0));
+ vector<vector<int> > nTotal(levelRows,vector<int>(levelCols,0));
+ vector<vector<bool> > bNoMore(levelRows,vector<bool>(levelCols,false));
+ vector<int> iniXCol(levelCols);
+ vector<int> iniYRow(levelRows);
+ int nNoMore = 0;
+ int nToDistribute = 0;
+
+
+ float hY = cellH + 6;
+
+ for(int i=0; i<levelRows; i++)
+ {
+ const float iniY = minBorderY + i*cellH - 3;
+ iniYRow[i] = iniY;
+
+ if(i == levelRows-1)
+ {
+ hY = maxBorderY+3-iniY;
+ if(hY<=0)
+ continue;
+ }
+
+ float hX = cellW + 6;
+
+ for(int j=0; j<levelCols; j++)
+ {
+ float iniX;
+
+ if(i==0)
+ {
+ iniX = minBorderX + j*cellW - 3;
+ iniXCol[j] = iniX;
+ }
+ else
+ {
+ iniX = iniXCol[j];
+ }
+
+
+ if(j == levelCols-1)
+ {
+ hX = maxBorderX+3-iniX;
+ if(hX<=0)
+ continue;
+ }
+
+
+ Mat cellImage = mvImagePyramid[level].rowRange(iniY,iniY+hY).colRange(iniX,iniX+hX);
+
+ cellKeyPoints[i][j].reserve(nfeaturesCell*5);
+
+ FAST(cellImage,cellKeyPoints[i][j],iniThFAST,true);
+
+ if(cellKeyPoints[i][j].size()<=3)
+ {
+ cellKeyPoints[i][j].clear();
+
+ FAST(cellImage,cellKeyPoints[i][j],minThFAST,true);
+ }
+
+
+ const int nKeys = cellKeyPoints[i][j].size();
+ nTotal[i][j] = nKeys;
+
+ if(nKeys>nfeaturesCell)
+ {
+ nToRetain[i][j] = nfeaturesCell;
+ bNoMore[i][j] = false;
+ }
+ else
+ {
+ nToRetain[i][j] = nKeys;
+ nToDistribute += nfeaturesCell-nKeys;
+ bNoMore[i][j] = true;
+ nNoMore++;
+ }
+
+ }
+ }
+
+
+ // Retain by score
+
+ while(nToDistribute>0 && nNoMore<nCells)
+ {
+ int nNewFeaturesCell = nfeaturesCell + ceil((float)nToDistribute/(nCells-nNoMore));
+ nToDistribute = 0;
+
+ for(int i=0; i<levelRows; i++)
+ {
+ for(int j=0; j<levelCols; j++)
+ {
+ if(!bNoMore[i][j])
+ {
+ if(nTotal[i][j]>nNewFeaturesCell)
+ {
+ nToRetain[i][j] = nNewFeaturesCell;
+ bNoMore[i][j] = false;
+ }
+ else
+ {
+ nToRetain[i][j] = nTotal[i][j];
+ nToDistribute += nNewFeaturesCell-nTotal[i][j];
+ bNoMore[i][j] = true;
+ nNoMore++;
+ }
+ }
+ }
+ }
+ }
+
+ vector<KeyPoint> & keypoints = allKeypoints[level];
+ keypoints.reserve(nDesiredFeatures*2);
+
+ const int scaledPatchSize = PATCH_SIZE*mvScaleFactor[level];
+
+ // Retain by score and transform coordinates
+ for(int i=0; i<levelRows; i++)
+ {
+ for(int j=0; j<levelCols; j++)
+ {
+ vector<KeyPoint> &keysCell = cellKeyPoints[i][j];
+ KeyPointsFilter::retainBest(keysCell,nToRetain[i][j]);
+ if((int)keysCell.size()>nToRetain[i][j])
+ keysCell.resize(nToRetain[i][j]);
+
+
+ for(size_t k=0, kend=keysCell.size(); k<kend; k++)
+ {
+ keysCell[k].pt.x+=iniXCol[j];
+ keysCell[k].pt.y+=iniYRow[i];
+ keysCell[k].octave=level;
+ keysCell[k].size = scaledPatchSize;
+ keypoints.push_back(keysCell[k]);
+ }
+ }
+ }
+
+ if((int)keypoints.size()>nDesiredFeatures)
+ {
+ KeyPointsFilter::retainBest(keypoints,nDesiredFeatures);
+ keypoints.resize(nDesiredFeatures);
+ }
+ }
+
+ // and compute orientations
+ for (int level = 0; level < nlevels; ++level)
+ computeOrientation(mvImagePyramid[level], allKeypoints[level], umax);
+}
+
+static void computeDescriptors(const Mat& image, vector<KeyPoint>& keypoints, Mat& descriptors,
+ const vector<Point>& pattern)
+{
+ descriptors = Mat::zeros((int)keypoints.size(), 32, CV_8UC1);
+
+ for (size_t i = 0; i < keypoints.size(); i++)
+ computeOrbDescriptor(keypoints[i], image, &pattern[0], descriptors.ptr((int)i));
+}
+
+void ORBextractor::operator()( InputArray _image, InputArray _mask, vector<KeyPoint>& _keypoints,
+ OutputArray _descriptors)
+{
+ if(_image.empty())
+ return;
+
+ Mat image = _image.getMat();
+ assert(image.type() == CV_8UC1 );
+
+ // Pre-compute the scale pyramid
+ ComputePyramid(image);
+
+ vector < vector<KeyPoint> > allKeypoints;
+ ComputeKeyPointsOctTree(allKeypoints);
+ //ComputeKeyPointsOld(allKeypoints);
+
+ Mat descriptors;
+
+ int nkeypoints = 0;
+ for (int level = 0; level < nlevels; ++level)
+ nkeypoints += (int)allKeypoints[level].size();
+ if( nkeypoints == 0 )
+ _descriptors.release();
+ else
+ {
+ _descriptors.create(nkeypoints, 32, CV_8U);
+ descriptors = _descriptors.getMat();
+ }
+
+ _keypoints.clear();
+ _keypoints.reserve(nkeypoints);
+
+ int offset = 0;
+ for (int level = 0; level < nlevels; ++level)
+ {
+ vector<KeyPoint>& keypoints = allKeypoints[level];
+ int nkeypointsLevel = (int)keypoints.size();
+
+ if(nkeypointsLevel==0)
+ continue;
+
+ // preprocess the resized image
+ Mat workingMat = mvImagePyramid[level].clone();
+ GaussianBlur(workingMat, workingMat, Size(7, 7), 2, 2, BORDER_REFLECT_101);
+
+ // Compute the descriptors
+ Mat desc = descriptors.rowRange(offset, offset + nkeypointsLevel);
+ computeDescriptors(workingMat, keypoints, desc, pattern);
+
+ offset += nkeypointsLevel;
+
+ // Scale keypoint coordinates
+ if (level != 0)
+ {
+ float scale = mvScaleFactor[level]; //getScale(level, firstLevel, scaleFactor);
+ for (vector<KeyPoint>::iterator keypoint = keypoints.begin(),
+ keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
+ keypoint->pt *= scale;
+ }
+ // And add the keypoints to the output
+ _keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
+ }
+}
+
+void ORBextractor::ComputePyramid(cv::Mat image)
+{
+ for (int level = 0; level < nlevels; ++level)
+ {
+ float scale = mvInvScaleFactor[level];
+ Size sz(cvRound((float)image.cols*scale), cvRound((float)image.rows*scale));
+ Size wholeSize(sz.width + EDGE_THRESHOLD*2, sz.height + EDGE_THRESHOLD*2);
+ Mat temp(wholeSize, image.type()), masktemp;
+ mvImagePyramid[level] = temp(Rect(EDGE_THRESHOLD, EDGE_THRESHOLD, sz.width, sz.height));
+
+ // Compute the resized image
+ if( level != 0 )
+ {
+ resize(mvImagePyramid[level-1], mvImagePyramid[level], sz, 0, 0, INTER_LINEAR);
+
+ copyMakeBorder(mvImagePyramid[level], temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
+ BORDER_REFLECT_101+BORDER_ISOLATED);
+ }
+ else
+ {
+ copyMakeBorder(image, temp, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD, EDGE_THRESHOLD,
+ BORDER_REFLECT_101);
+ }
+ }
+
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/ORBmatcher.cc b/externals/ORB_SLAM2/src/ORBmatcher.cc
new file mode 100644
index 0000000000000000000000000000000000000000..2cd7224494a6217f9831c45ff166864e8c664ef3
--- /dev/null
+++ b/externals/ORB_SLAM2/src/ORBmatcher.cc
@@ -0,0 +1,1665 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "ORBmatcher.h"
+
+#include<limits.h>
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include "Thirdparty/DBoW2/DBoW2/FeatureVector.h"
+
+#include<stdint-gcc.h>
+
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+const int ORBmatcher::TH_HIGH = 100;
+const int ORBmatcher::TH_LOW = 50;
+const int ORBmatcher::HISTO_LENGTH = 30;
+
+ORBmatcher::ORBmatcher(float nnratio, bool checkOri): mfNNratio(nnratio), mbCheckOrientation(checkOri)
+{
+}
+
+int ORBmatcher::SearchByProjection(Frame &F, const vector<MapPoint*> &vpMapPoints, const float th)
+{
+ int nmatches=0;
+
+ const bool bFactor = th!=1.0;
+
+ for(size_t iMP=0; iMP<vpMapPoints.size(); iMP++)
+ {
+ MapPoint* pMP = vpMapPoints[iMP];
+ if(!pMP->mbTrackInView)
+ continue;
+
+ if(pMP->isBad())
+ continue;
+
+ const int &nPredictedLevel = pMP->mnTrackScaleLevel;
+
+ // The size of the window will depend on the viewing direction
+ float r = RadiusByViewingCos(pMP->mTrackViewCos);
+
+ if(bFactor)
+ r*=th;
+
+ const vector<size_t> vIndices =
+ F.GetFeaturesInArea(pMP->mTrackProjX,pMP->mTrackProjY,r*F.mvScaleFactors[nPredictedLevel],nPredictedLevel-1,nPredictedLevel);
+
+ if(vIndices.empty())
+ continue;
+
+ const cv::Mat MPdescriptor = pMP->GetDescriptor();
+
+ int bestDist=256;
+ int bestLevel= -1;
+ int bestDist2=256;
+ int bestLevel2 = -1;
+ int bestIdx =-1 ;
+
+ // Get best and second matches with near keypoints
+ for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
+ {
+ const size_t idx = *vit;
+
+ if(F.mvpMapPoints[idx])
+ if(F.mvpMapPoints[idx]->Observations()>0)
+ continue;
+
+ if(F.mvuRight[idx]>0)
+ {
+ const float er = fabs(pMP->mTrackProjXR-F.mvuRight[idx]);
+ if(er>r*F.mvScaleFactors[nPredictedLevel])
+ continue;
+ }
+
+ const cv::Mat &d = F.mDescriptors.row(idx);
+
+ const int dist = DescriptorDistance(MPdescriptor,d);
+
+ if(dist<bestDist)
+ {
+ bestDist2=bestDist;
+ bestDist=dist;
+ bestLevel2 = bestLevel;
+ bestLevel = F.mvKeysUn[idx].octave;
+ bestIdx=idx;
+ }
+ else if(dist<bestDist2)
+ {
+ bestLevel2 = F.mvKeysUn[idx].octave;
+ bestDist2=dist;
+ }
+ }
+
+ // Apply ratio to second match (only if best and second are in the same scale level)
+ if(bestDist<=TH_HIGH)
+ {
+ if(bestLevel==bestLevel2 && bestDist>mfNNratio*bestDist2)
+ continue;
+
+ F.mvpMapPoints[bestIdx]=pMP;
+ nmatches++;
+ }
+ }
+
+ return nmatches;
+}
+
+float ORBmatcher::RadiusByViewingCos(const float &viewCos)
+{
+ if(viewCos>0.998)
+ return 2.5;
+ else
+ return 4.0;
+}
+
+
+bool ORBmatcher::CheckDistEpipolarLine(const cv::KeyPoint &kp1,const cv::KeyPoint &kp2,const cv::Mat &F12,const KeyFrame* pKF2)
+{
+ // Epipolar line in second image l = x1'F12 = [a b c]
+ const float a = kp1.pt.x*F12.at<float>(0,0)+kp1.pt.y*F12.at<float>(1,0)+F12.at<float>(2,0);
+ const float b = kp1.pt.x*F12.at<float>(0,1)+kp1.pt.y*F12.at<float>(1,1)+F12.at<float>(2,1);
+ const float c = kp1.pt.x*F12.at<float>(0,2)+kp1.pt.y*F12.at<float>(1,2)+F12.at<float>(2,2);
+
+ const float num = a*kp2.pt.x+b*kp2.pt.y+c;
+
+ const float den = a*a+b*b;
+
+ if(den==0)
+ return false;
+
+ const float dsqr = num*num/den;
+
+ return dsqr<3.84*pKF2->mvLevelSigma2[kp2.octave];
+}
+
+int ORBmatcher::SearchByBoW(KeyFrame* pKF,Frame &F, vector<MapPoint*> &vpMapPointMatches)
+{
+ const vector<MapPoint*> vpMapPointsKF = pKF->GetMapPointMatches();
+
+ vpMapPointMatches = vector<MapPoint*>(F.N,static_cast<MapPoint*>(NULL));
+
+ const DBoW2::FeatureVector &vFeatVecKF = pKF->mFeatVec;
+
+ int nmatches=0;
+
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ // We perform the matching over ORB that belong to the same vocabulary node (at a certain level)
+ DBoW2::FeatureVector::const_iterator KFit = vFeatVecKF.begin();
+ DBoW2::FeatureVector::const_iterator Fit = F.mFeatVec.begin();
+ DBoW2::FeatureVector::const_iterator KFend = vFeatVecKF.end();
+ DBoW2::FeatureVector::const_iterator Fend = F.mFeatVec.end();
+
+ while(KFit != KFend && Fit != Fend)
+ {
+ if(KFit->first == Fit->first)
+ {
+ const vector<unsigned int> vIndicesKF = KFit->second;
+ const vector<unsigned int> vIndicesF = Fit->second;
+
+ for(size_t iKF=0; iKF<vIndicesKF.size(); iKF++)
+ {
+ const unsigned int realIdxKF = vIndicesKF[iKF];
+
+ MapPoint* pMP = vpMapPointsKF[realIdxKF];
+
+ if(!pMP)
+ continue;
+
+ if(pMP->isBad())
+ continue;
+
+ const cv::Mat &dKF= pKF->mDescriptors.row(realIdxKF);
+
+ int bestDist1=256;
+ int bestIdxF =-1 ;
+ int bestDist2=256;
+
+ for(size_t iF=0; iF<vIndicesF.size(); iF++)
+ {
+ const unsigned int realIdxF = vIndicesF[iF];
+
+ if(vpMapPointMatches[realIdxF])
+ continue;
+
+ const cv::Mat &dF = F.mDescriptors.row(realIdxF);
+
+ const int dist = DescriptorDistance(dKF,dF);
+
+ if(dist<bestDist1)
+ {
+ bestDist2=bestDist1;
+ bestDist1=dist;
+ bestIdxF=realIdxF;
+ }
+ else if(dist<bestDist2)
+ {
+ bestDist2=dist;
+ }
+ }
+
+ if(bestDist1<=TH_LOW)
+ {
+ if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
+ {
+ vpMapPointMatches[bestIdxF]=pMP;
+
+ const cv::KeyPoint &kp = pKF->mvKeysUn[realIdxKF];
+
+ if(mbCheckOrientation)
+ {
+ float rot = kp.angle-F.mvKeys[bestIdxF].angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(bestIdxF);
+ }
+ nmatches++;
+ }
+ }
+
+ }
+
+ KFit++;
+ Fit++;
+ }
+ else if(KFit->first < Fit->first)
+ {
+ KFit = vFeatVecKF.lower_bound(Fit->first);
+ }
+ else
+ {
+ Fit = F.mFeatVec.lower_bound(KFit->first);
+ }
+ }
+
+
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i==ind1 || i==ind2 || i==ind3)
+ continue;
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ vpMapPointMatches[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
+ nmatches--;
+ }
+ }
+ }
+
+ return nmatches;
+}
+
+int ORBmatcher::SearchByProjection(KeyFrame* pKF, cv::Mat Scw, const vector<MapPoint*> &vpPoints, vector<MapPoint*> &vpMatched, int th)
+{
+ // Get Calibration Parameters for later projection
+ const float &fx = pKF->fx;
+ const float &fy = pKF->fy;
+ const float &cx = pKF->cx;
+ const float &cy = pKF->cy;
+
+ // Decompose Scw
+ cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
+ const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));
+ cv::Mat Rcw = sRcw/scw;
+ cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;
+ cv::Mat Ow = -Rcw.t()*tcw;
+
+ // Set of MapPoints already found in the KeyFrame
+ set<MapPoint*> spAlreadyFound(vpMatched.begin(), vpMatched.end());
+ spAlreadyFound.erase(static_cast<MapPoint*>(NULL));
+
+ int nmatches=0;
+
+ // For each Candidate MapPoint Project and Match
+ for(int iMP=0, iendMP=vpPoints.size(); iMP<iendMP; iMP++)
+ {
+ MapPoint* pMP = vpPoints[iMP];
+
+ // Discard Bad MapPoints and already found
+ if(pMP->isBad() || spAlreadyFound.count(pMP))
+ continue;
+
+ // Get 3D Coords.
+ cv::Mat p3Dw = pMP->GetWorldPos();
+
+ // Transform into Camera Coords.
+ cv::Mat p3Dc = Rcw*p3Dw+tcw;
+
+ // Depth must be positive
+ if(p3Dc.at<float>(2)<0.0)
+ continue;
+
+ // Project into Image
+ const float invz = 1/p3Dc.at<float>(2);
+ const float x = p3Dc.at<float>(0)*invz;
+ const float y = p3Dc.at<float>(1)*invz;
+
+ const float u = fx*x+cx;
+ const float v = fy*y+cy;
+
+ // Point must be inside the image
+ if(!pKF->IsInImage(u,v))
+ continue;
+
+ // Depth must be inside the scale invariance region of the point
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ cv::Mat PO = p3Dw-Ow;
+ const float dist = cv::norm(PO);
+
+ if(dist<minDistance || dist>maxDistance)
+ continue;
+
+ // Viewing angle must be less than 60 deg
+ cv::Mat Pn = pMP->GetNormal();
+
+ if(PO.dot(Pn)<0.5*dist)
+ continue;
+
+ int nPredictedLevel = pMP->PredictScale(dist,pKF);
+
+ // Search in a radius
+ const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
+
+ if(vIndices.empty())
+ continue;
+
+ // Match to the most similar keypoint in the radius
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = 256;
+ int bestIdx = -1;
+ for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
+ {
+ const size_t idx = *vit;
+ if(vpMatched[idx])
+ continue;
+
+ const int &kpLevel= pKF->mvKeysUn[idx].octave;
+
+ if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
+ continue;
+
+ const cv::Mat &dKF = pKF->mDescriptors.row(idx);
+
+ const int dist = DescriptorDistance(dMP,dKF);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdx = idx;
+ }
+ }
+
+ if(bestDist<=TH_LOW)
+ {
+ vpMatched[bestIdx]=pMP;
+ nmatches++;
+ }
+
+ }
+
+ return nmatches;
+}
+
+int ORBmatcher::SearchForInitialization(Frame &F1, Frame &F2, vector<cv::Point2f> &vbPrevMatched, vector<int> &vnMatches12, int windowSize)
+{
+ int nmatches=0;
+ vnMatches12 = vector<int>(F1.mvKeysUn.size(),-1);
+
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ vector<int> vMatchedDistance(F2.mvKeysUn.size(),INT_MAX);
+ vector<int> vnMatches21(F2.mvKeysUn.size(),-1);
+
+ for(size_t i1=0, iend1=F1.mvKeysUn.size(); i1<iend1; i1++)
+ {
+ cv::KeyPoint kp1 = F1.mvKeysUn[i1];
+ int level1 = kp1.octave;
+ if(level1>0)
+ continue;
+
+ vector<size_t> vIndices2 = F2.GetFeaturesInArea(vbPrevMatched[i1].x,vbPrevMatched[i1].y, windowSize,level1,level1);
+
+ if(vIndices2.empty())
+ continue;
+
+ cv::Mat d1 = F1.mDescriptors.row(i1);
+
+ int bestDist = INT_MAX;
+ int bestDist2 = INT_MAX;
+ int bestIdx2 = -1;
+
+ for(vector<size_t>::iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
+ {
+ size_t i2 = *vit;
+
+ cv::Mat d2 = F2.mDescriptors.row(i2);
+
+ int dist = DescriptorDistance(d1,d2);
+
+ if(vMatchedDistance[i2]<=dist)
+ continue;
+
+ if(dist<bestDist)
+ {
+ bestDist2=bestDist;
+ bestDist=dist;
+ bestIdx2=i2;
+ }
+ else if(dist<bestDist2)
+ {
+ bestDist2=dist;
+ }
+ }
+
+ if(bestDist<=TH_LOW)
+ {
+ if(bestDist<(float)bestDist2*mfNNratio)
+ {
+ if(vnMatches21[bestIdx2]>=0)
+ {
+ vnMatches12[vnMatches21[bestIdx2]]=-1;
+ nmatches--;
+ }
+ vnMatches12[i1]=bestIdx2;
+ vnMatches21[bestIdx2]=i1;
+ vMatchedDistance[bestIdx2]=bestDist;
+ nmatches++;
+
+ if(mbCheckOrientation)
+ {
+ float rot = F1.mvKeysUn[i1].angle-F2.mvKeysUn[bestIdx2].angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(i1);
+ }
+ }
+ }
+
+ }
+
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i==ind1 || i==ind2 || i==ind3)
+ continue;
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ int idx1 = rotHist[i][j];
+ if(vnMatches12[idx1]>=0)
+ {
+ vnMatches12[idx1]=-1;
+ nmatches--;
+ }
+ }
+ }
+
+ }
+
+ //Update prev matched
+ for(size_t i1=0, iend1=vnMatches12.size(); i1<iend1; i1++)
+ if(vnMatches12[i1]>=0)
+ vbPrevMatched[i1]=F2.mvKeysUn[vnMatches12[i1]].pt;
+
+ return nmatches;
+}
+
+int ORBmatcher::SearchByBoW(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches12)
+{
+ const vector<cv::KeyPoint> &vKeysUn1 = pKF1->mvKeysUn;
+ const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
+ const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
+ const cv::Mat &Descriptors1 = pKF1->mDescriptors;
+
+ const vector<cv::KeyPoint> &vKeysUn2 = pKF2->mvKeysUn;
+ const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
+ const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
+ const cv::Mat &Descriptors2 = pKF2->mDescriptors;
+
+ vpMatches12 = vector<MapPoint*>(vpMapPoints1.size(),static_cast<MapPoint*>(NULL));
+ vector<bool> vbMatched2(vpMapPoints2.size(),false);
+
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ int nmatches = 0;
+
+ DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
+ DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
+ DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
+ DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
+
+ while(f1it != f1end && f2it != f2end)
+ {
+ if(f1it->first == f2it->first)
+ {
+ for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
+ {
+ const size_t idx1 = f1it->second[i1];
+
+ MapPoint* pMP1 = vpMapPoints1[idx1];
+ if(!pMP1)
+ continue;
+ if(pMP1->isBad())
+ continue;
+
+ const cv::Mat &d1 = Descriptors1.row(idx1);
+
+ int bestDist1=256;
+ int bestIdx2 =-1 ;
+ int bestDist2=256;
+
+ for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
+ {
+ const size_t idx2 = f2it->second[i2];
+
+ MapPoint* pMP2 = vpMapPoints2[idx2];
+
+ if(vbMatched2[idx2] || !pMP2)
+ continue;
+
+ if(pMP2->isBad())
+ continue;
+
+ const cv::Mat &d2 = Descriptors2.row(idx2);
+
+ int dist = DescriptorDistance(d1,d2);
+
+ if(dist<bestDist1)
+ {
+ bestDist2=bestDist1;
+ bestDist1=dist;
+ bestIdx2=idx2;
+ }
+ else if(dist<bestDist2)
+ {
+ bestDist2=dist;
+ }
+ }
+
+ if(bestDist1<TH_LOW)
+ {
+ if(static_cast<float>(bestDist1)<mfNNratio*static_cast<float>(bestDist2))
+ {
+ vpMatches12[idx1]=vpMapPoints2[bestIdx2];
+ vbMatched2[bestIdx2]=true;
+
+ if(mbCheckOrientation)
+ {
+ float rot = vKeysUn1[idx1].angle-vKeysUn2[bestIdx2].angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(idx1);
+ }
+ nmatches++;
+ }
+ }
+ }
+
+ f1it++;
+ f2it++;
+ }
+ else if(f1it->first < f2it->first)
+ {
+ f1it = vFeatVec1.lower_bound(f2it->first);
+ }
+ else
+ {
+ f2it = vFeatVec2.lower_bound(f1it->first);
+ }
+ }
+
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i==ind1 || i==ind2 || i==ind3)
+ continue;
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ vpMatches12[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
+ nmatches--;
+ }
+ }
+ }
+
+ return nmatches;
+}
+
+int ORBmatcher::SearchForTriangulation(KeyFrame *pKF1, KeyFrame *pKF2, cv::Mat F12,
+ vector<pair<size_t, size_t> > &vMatchedPairs, const bool bOnlyStereo)
+{
+ const DBoW2::FeatureVector &vFeatVec1 = pKF1->mFeatVec;
+ const DBoW2::FeatureVector &vFeatVec2 = pKF2->mFeatVec;
+
+ //Compute epipole in second image
+ cv::Mat Cw = pKF1->GetCameraCenter();
+ cv::Mat R2w = pKF2->GetRotation();
+ cv::Mat t2w = pKF2->GetTranslation();
+ cv::Mat C2 = R2w*Cw+t2w;
+ const float invz = 1.0f/C2.at<float>(2);
+ const float ex =pKF2->fx*C2.at<float>(0)*invz+pKF2->cx;
+ const float ey =pKF2->fy*C2.at<float>(1)*invz+pKF2->cy;
+
+ // Find matches between not tracked keypoints
+ // Matching speed-up by ORB Vocabulary
+ // Compare only ORB that share the same node
+
+ int nmatches=0;
+ vector<bool> vbMatched2(pKF2->N,false);
+ vector<int> vMatches12(pKF1->N,-1);
+
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ DBoW2::FeatureVector::const_iterator f1it = vFeatVec1.begin();
+ DBoW2::FeatureVector::const_iterator f2it = vFeatVec2.begin();
+ DBoW2::FeatureVector::const_iterator f1end = vFeatVec1.end();
+ DBoW2::FeatureVector::const_iterator f2end = vFeatVec2.end();
+
+ while(f1it!=f1end && f2it!=f2end)
+ {
+ if(f1it->first == f2it->first)
+ {
+ for(size_t i1=0, iend1=f1it->second.size(); i1<iend1; i1++)
+ {
+ const size_t idx1 = f1it->second[i1];
+
+ MapPoint* pMP1 = pKF1->GetMapPoint(idx1);
+
+ // If there is already a MapPoint skip
+ if(pMP1)
+ continue;
+
+ const bool bStereo1 = pKF1->mvuRight[idx1]>=0;
+
+ if(bOnlyStereo)
+ if(!bStereo1)
+ continue;
+
+ const cv::KeyPoint &kp1 = pKF1->mvKeysUn[idx1];
+
+ const cv::Mat &d1 = pKF1->mDescriptors.row(idx1);
+
+ int bestDist = TH_LOW;
+ int bestIdx2 = -1;
+
+ for(size_t i2=0, iend2=f2it->second.size(); i2<iend2; i2++)
+ {
+ size_t idx2 = f2it->second[i2];
+
+ MapPoint* pMP2 = pKF2->GetMapPoint(idx2);
+
+ // If we have already matched or there is a MapPoint skip
+ if(vbMatched2[idx2] || pMP2)
+ continue;
+
+ const bool bStereo2 = pKF2->mvuRight[idx2]>=0;
+
+ if(bOnlyStereo)
+ if(!bStereo2)
+ continue;
+
+ const cv::Mat &d2 = pKF2->mDescriptors.row(idx2);
+
+ const int dist = DescriptorDistance(d1,d2);
+
+ if(dist>TH_LOW || dist>bestDist)
+ continue;
+
+ const cv::KeyPoint &kp2 = pKF2->mvKeysUn[idx2];
+
+ if(!bStereo1 && !bStereo2)
+ {
+ const float distex = ex-kp2.pt.x;
+ const float distey = ey-kp2.pt.y;
+ if(distex*distex+distey*distey<100*pKF2->mvScaleFactors[kp2.octave])
+ continue;
+ }
+
+ if(CheckDistEpipolarLine(kp1,kp2,F12,pKF2))
+ {
+ bestIdx2 = idx2;
+ bestDist = dist;
+ }
+ }
+
+ if(bestIdx2>=0)
+ {
+ const cv::KeyPoint &kp2 = pKF2->mvKeysUn[bestIdx2];
+ vMatches12[idx1]=bestIdx2;
+ nmatches++;
+
+ if(mbCheckOrientation)
+ {
+ float rot = kp1.angle-kp2.angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(idx1);
+ }
+ }
+ }
+
+ f1it++;
+ f2it++;
+ }
+ else if(f1it->first < f2it->first)
+ {
+ f1it = vFeatVec1.lower_bound(f2it->first);
+ }
+ else
+ {
+ f2it = vFeatVec2.lower_bound(f1it->first);
+ }
+ }
+
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i==ind1 || i==ind2 || i==ind3)
+ continue;
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ vMatches12[rotHist[i][j]]=-1;
+ nmatches--;
+ }
+ }
+
+ }
+
+ vMatchedPairs.clear();
+ vMatchedPairs.reserve(nmatches);
+
+ for(size_t i=0, iend=vMatches12.size(); i<iend; i++)
+ {
+ if(vMatches12[i]<0)
+ continue;
+ vMatchedPairs.push_back(make_pair(i,vMatches12[i]));
+ }
+
+ return nmatches;
+}
+
+int ORBmatcher::Fuse(KeyFrame *pKF, const vector<MapPoint *> &vpMapPoints, const float th)
+{
+ cv::Mat Rcw = pKF->GetRotation();
+ cv::Mat tcw = pKF->GetTranslation();
+
+ const float &fx = pKF->fx;
+ const float &fy = pKF->fy;
+ const float &cx = pKF->cx;
+ const float &cy = pKF->cy;
+ const float &bf = pKF->mbf;
+
+ cv::Mat Ow = pKF->GetCameraCenter();
+
+ int nFused=0;
+
+ const int nMPs = vpMapPoints.size();
+
+ for(int i=0; i<nMPs; i++)
+ {
+ MapPoint* pMP = vpMapPoints[i];
+
+ if(!pMP)
+ continue;
+
+ if(pMP->isBad() || pMP->IsInKeyFrame(pKF))
+ continue;
+
+ cv::Mat p3Dw = pMP->GetWorldPos();
+ cv::Mat p3Dc = Rcw*p3Dw + tcw;
+
+ // Depth must be positive
+ if(p3Dc.at<float>(2)<0.0f)
+ continue;
+
+ const float invz = 1/p3Dc.at<float>(2);
+ const float x = p3Dc.at<float>(0)*invz;
+ const float y = p3Dc.at<float>(1)*invz;
+
+ const float u = fx*x+cx;
+ const float v = fy*y+cy;
+
+ // Point must be inside the image
+ if(!pKF->IsInImage(u,v))
+ continue;
+
+ const float ur = u-bf*invz;
+
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ cv::Mat PO = p3Dw-Ow;
+ const float dist3D = cv::norm(PO);
+
+ // Depth must be inside the scale pyramid of the image
+ if(dist3D<minDistance || dist3D>maxDistance )
+ continue;
+
+ // Viewing angle must be less than 60 deg
+ cv::Mat Pn = pMP->GetNormal();
+
+ if(PO.dot(Pn)<0.5*dist3D)
+ continue;
+
+ int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
+
+ // Search in a radius
+ const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
+
+ if(vIndices.empty())
+ continue;
+
+ // Match to the most similar keypoint in the radius
+
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = 256;
+ int bestIdx = -1;
+ for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
+ {
+ const size_t idx = *vit;
+
+ const cv::KeyPoint &kp = pKF->mvKeysUn[idx];
+
+ const int &kpLevel= kp.octave;
+
+ if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
+ continue;
+
+ if(pKF->mvuRight[idx]>=0)
+ {
+ // Check reprojection error in stereo
+ const float &kpx = kp.pt.x;
+ const float &kpy = kp.pt.y;
+ const float &kpr = pKF->mvuRight[idx];
+ const float ex = u-kpx;
+ const float ey = v-kpy;
+ const float er = ur-kpr;
+ const float e2 = ex*ex+ey*ey+er*er;
+
+ if(e2*pKF->mvInvLevelSigma2[kpLevel]>7.8)
+ continue;
+ }
+ else
+ {
+ const float &kpx = kp.pt.x;
+ const float &kpy = kp.pt.y;
+ const float ex = u-kpx;
+ const float ey = v-kpy;
+ const float e2 = ex*ex+ey*ey;
+
+ if(e2*pKF->mvInvLevelSigma2[kpLevel]>5.99)
+ continue;
+ }
+
+ const cv::Mat &dKF = pKF->mDescriptors.row(idx);
+
+ const int dist = DescriptorDistance(dMP,dKF);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdx = idx;
+ }
+ }
+
+ // If there is already a MapPoint replace otherwise add new measurement
+ if(bestDist<=TH_LOW)
+ {
+ MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
+ if(pMPinKF)
+ {
+ if(!pMPinKF->isBad())
+ {
+ if(pMPinKF->Observations()>pMP->Observations())
+ pMP->Replace(pMPinKF);
+ else
+ pMPinKF->Replace(pMP);
+ }
+ }
+ else
+ {
+ pMP->AddObservation(pKF,bestIdx);
+ pKF->AddMapPoint(pMP,bestIdx);
+ }
+ nFused++;
+ }
+ }
+
+ return nFused;
+}
+
+int ORBmatcher::Fuse(KeyFrame *pKF, cv::Mat Scw, const vector<MapPoint *> &vpPoints, float th, vector<MapPoint *> &vpReplacePoint)
+{
+ // Get Calibration Parameters for later projection
+ const float &fx = pKF->fx;
+ const float &fy = pKF->fy;
+ const float &cx = pKF->cx;
+ const float &cy = pKF->cy;
+
+ // Decompose Scw
+ cv::Mat sRcw = Scw.rowRange(0,3).colRange(0,3);
+ const float scw = sqrt(sRcw.row(0).dot(sRcw.row(0)));
+ cv::Mat Rcw = sRcw/scw;
+ cv::Mat tcw = Scw.rowRange(0,3).col(3)/scw;
+ cv::Mat Ow = -Rcw.t()*tcw;
+
+ // Set of MapPoints already found in the KeyFrame
+ const set<MapPoint*> spAlreadyFound = pKF->GetMapPoints();
+
+ int nFused=0;
+
+ const int nPoints = vpPoints.size();
+
+ // For each candidate MapPoint project and match
+ for(int iMP=0; iMP<nPoints; iMP++)
+ {
+ MapPoint* pMP = vpPoints[iMP];
+
+ // Discard Bad MapPoints and already found
+ if(pMP->isBad() || spAlreadyFound.count(pMP))
+ continue;
+
+ // Get 3D Coords.
+ cv::Mat p3Dw = pMP->GetWorldPos();
+
+ // Transform into Camera Coords.
+ cv::Mat p3Dc = Rcw*p3Dw+tcw;
+
+ // Depth must be positive
+ if(p3Dc.at<float>(2)<0.0f)
+ continue;
+
+ // Project into Image
+ const float invz = 1.0/p3Dc.at<float>(2);
+ const float x = p3Dc.at<float>(0)*invz;
+ const float y = p3Dc.at<float>(1)*invz;
+
+ const float u = fx*x+cx;
+ const float v = fy*y+cy;
+
+ // Point must be inside the image
+ if(!pKF->IsInImage(u,v))
+ continue;
+
+ // Depth must be inside the scale pyramid of the image
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ cv::Mat PO = p3Dw-Ow;
+ const float dist3D = cv::norm(PO);
+
+ if(dist3D<minDistance || dist3D>maxDistance)
+ continue;
+
+ // Viewing angle must be less than 60 deg
+ cv::Mat Pn = pMP->GetNormal();
+
+ if(PO.dot(Pn)<0.5*dist3D)
+ continue;
+
+ // Compute predicted scale level
+ const int nPredictedLevel = pMP->PredictScale(dist3D,pKF);
+
+ // Search in a radius
+ const float radius = th*pKF->mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices = pKF->GetFeaturesInArea(u,v,radius);
+
+ if(vIndices.empty())
+ continue;
+
+ // Match to the most similar keypoint in the radius
+
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = INT_MAX;
+ int bestIdx = -1;
+ for(vector<size_t>::const_iterator vit=vIndices.begin(); vit!=vIndices.end(); vit++)
+ {
+ const size_t idx = *vit;
+ const int &kpLevel = pKF->mvKeysUn[idx].octave;
+
+ if(kpLevel<nPredictedLevel-1 || kpLevel>nPredictedLevel)
+ continue;
+
+ const cv::Mat &dKF = pKF->mDescriptors.row(idx);
+
+ int dist = DescriptorDistance(dMP,dKF);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdx = idx;
+ }
+ }
+
+ // If there is already a MapPoint replace otherwise add new measurement
+ if(bestDist<=TH_LOW)
+ {
+ MapPoint* pMPinKF = pKF->GetMapPoint(bestIdx);
+ if(pMPinKF)
+ {
+ if(!pMPinKF->isBad())
+ vpReplacePoint[iMP] = pMPinKF;
+ }
+ else
+ {
+ pMP->AddObservation(pKF,bestIdx);
+ pKF->AddMapPoint(pMP,bestIdx);
+ }
+ nFused++;
+ }
+ }
+
+ return nFused;
+}
+
+int ORBmatcher::SearchBySim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint*> &vpMatches12,
+ const float &s12, const cv::Mat &R12, const cv::Mat &t12, const float th)
+{
+ const float &fx = pKF1->fx;
+ const float &fy = pKF1->fy;
+ const float &cx = pKF1->cx;
+ const float &cy = pKF1->cy;
+
+ // Camera 1 from world
+ cv::Mat R1w = pKF1->GetRotation();
+ cv::Mat t1w = pKF1->GetTranslation();
+
+ //Camera 2 from world
+ cv::Mat R2w = pKF2->GetRotation();
+ cv::Mat t2w = pKF2->GetTranslation();
+
+ //Transformation between cameras
+ cv::Mat sR12 = s12*R12;
+ cv::Mat sR21 = (1.0/s12)*R12.t();
+ cv::Mat t21 = -sR21*t12;
+
+ const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
+ const int N1 = vpMapPoints1.size();
+
+ const vector<MapPoint*> vpMapPoints2 = pKF2->GetMapPointMatches();
+ const int N2 = vpMapPoints2.size();
+
+ vector<bool> vbAlreadyMatched1(N1,false);
+ vector<bool> vbAlreadyMatched2(N2,false);
+
+ for(int i=0; i<N1; i++)
+ {
+ MapPoint* pMP = vpMatches12[i];
+ if(pMP)
+ {
+ vbAlreadyMatched1[i]=true;
+ int idx2 = pMP->GetIndexInKeyFrame(pKF2);
+ if(idx2>=0 && idx2<N2)
+ vbAlreadyMatched2[idx2]=true;
+ }
+ }
+
+ vector<int> vnMatch1(N1,-1);
+ vector<int> vnMatch2(N2,-1);
+
+ // Transform from KF1 to KF2 and search
+ for(int i1=0; i1<N1; i1++)
+ {
+ MapPoint* pMP = vpMapPoints1[i1];
+
+ if(!pMP || vbAlreadyMatched1[i1])
+ continue;
+
+ if(pMP->isBad())
+ continue;
+
+ cv::Mat p3Dw = pMP->GetWorldPos();
+ cv::Mat p3Dc1 = R1w*p3Dw + t1w;
+ cv::Mat p3Dc2 = sR21*p3Dc1 + t21;
+
+ // Depth must be positive
+ if(p3Dc2.at<float>(2)<0.0)
+ continue;
+
+ const float invz = 1.0/p3Dc2.at<float>(2);
+ const float x = p3Dc2.at<float>(0)*invz;
+ const float y = p3Dc2.at<float>(1)*invz;
+
+ const float u = fx*x+cx;
+ const float v = fy*y+cy;
+
+ // Point must be inside the image
+ if(!pKF2->IsInImage(u,v))
+ continue;
+
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ const float dist3D = cv::norm(p3Dc2);
+
+ // Depth must be inside the scale invariance region
+ if(dist3D<minDistance || dist3D>maxDistance )
+ continue;
+
+ // Compute predicted octave
+ const int nPredictedLevel = pMP->PredictScale(dist3D,pKF2);
+
+ // Search in a radius
+ const float radius = th*pKF2->mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices = pKF2->GetFeaturesInArea(u,v,radius);
+
+ if(vIndices.empty())
+ continue;
+
+ // Match to the most similar keypoint in the radius
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = INT_MAX;
+ int bestIdx = -1;
+ for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
+ {
+ const size_t idx = *vit;
+
+ const cv::KeyPoint &kp = pKF2->mvKeysUn[idx];
+
+ if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
+ continue;
+
+ const cv::Mat &dKF = pKF2->mDescriptors.row(idx);
+
+ const int dist = DescriptorDistance(dMP,dKF);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdx = idx;
+ }
+ }
+
+ if(bestDist<=TH_HIGH)
+ {
+ vnMatch1[i1]=bestIdx;
+ }
+ }
+
+ // Transform from KF2 to KF2 and search
+ for(int i2=0; i2<N2; i2++)
+ {
+ MapPoint* pMP = vpMapPoints2[i2];
+
+ if(!pMP || vbAlreadyMatched2[i2])
+ continue;
+
+ if(pMP->isBad())
+ continue;
+
+ cv::Mat p3Dw = pMP->GetWorldPos();
+ cv::Mat p3Dc2 = R2w*p3Dw + t2w;
+ cv::Mat p3Dc1 = sR12*p3Dc2 + t12;
+
+ // Depth must be positive
+ if(p3Dc1.at<float>(2)<0.0)
+ continue;
+
+ const float invz = 1.0/p3Dc1.at<float>(2);
+ const float x = p3Dc1.at<float>(0)*invz;
+ const float y = p3Dc1.at<float>(1)*invz;
+
+ const float u = fx*x+cx;
+ const float v = fy*y+cy;
+
+ // Point must be inside the image
+ if(!pKF1->IsInImage(u,v))
+ continue;
+
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+ const float dist3D = cv::norm(p3Dc1);
+
+ // Depth must be inside the scale pyramid of the image
+ if(dist3D<minDistance || dist3D>maxDistance)
+ continue;
+
+ // Compute predicted octave
+ const int nPredictedLevel = pMP->PredictScale(dist3D,pKF1);
+
+ // Search in a radius of 2.5*sigma(ScaleLevel)
+ const float radius = th*pKF1->mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices = pKF1->GetFeaturesInArea(u,v,radius);
+
+ if(vIndices.empty())
+ continue;
+
+ // Match to the most similar keypoint in the radius
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = INT_MAX;
+ int bestIdx = -1;
+ for(vector<size_t>::const_iterator vit=vIndices.begin(), vend=vIndices.end(); vit!=vend; vit++)
+ {
+ const size_t idx = *vit;
+
+ const cv::KeyPoint &kp = pKF1->mvKeysUn[idx];
+
+ if(kp.octave<nPredictedLevel-1 || kp.octave>nPredictedLevel)
+ continue;
+
+ const cv::Mat &dKF = pKF1->mDescriptors.row(idx);
+
+ const int dist = DescriptorDistance(dMP,dKF);
+
+ if(dist<bestDist)
+ {
+ bestDist = dist;
+ bestIdx = idx;
+ }
+ }
+
+ if(bestDist<=TH_HIGH)
+ {
+ vnMatch2[i2]=bestIdx;
+ }
+ }
+
+ // Check agreement
+ int nFound = 0;
+
+ for(int i1=0; i1<N1; i1++)
+ {
+ int idx2 = vnMatch1[i1];
+
+ if(idx2>=0)
+ {
+ int idx1 = vnMatch2[idx2];
+ if(idx1==i1)
+ {
+ vpMatches12[i1] = vpMapPoints2[idx2];
+ nFound++;
+ }
+ }
+ }
+
+ return nFound;
+}
+
+int ORBmatcher::SearchByProjection(Frame &CurrentFrame, const Frame &LastFrame, const float th, const bool bMono)
+{
+ int nmatches = 0;
+
+ // Rotation Histogram (to check rotation consistency)
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
+ const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
+
+ const cv::Mat twc = -Rcw.t()*tcw;
+
+ const cv::Mat Rlw = LastFrame.mTcw.rowRange(0,3).colRange(0,3);
+ const cv::Mat tlw = LastFrame.mTcw.rowRange(0,3).col(3);
+
+ const cv::Mat tlc = Rlw*twc+tlw;
+
+ const bool bForward = tlc.at<float>(2)>CurrentFrame.mb && !bMono;
+ const bool bBackward = -tlc.at<float>(2)>CurrentFrame.mb && !bMono;
+
+ for(int i=0; i<LastFrame.N; i++)
+ {
+ MapPoint* pMP = LastFrame.mvpMapPoints[i];
+
+ if(pMP)
+ {
+ if(!LastFrame.mvbOutlier[i])
+ {
+ // Project
+ cv::Mat x3Dw = pMP->GetWorldPos();
+ cv::Mat x3Dc = Rcw*x3Dw+tcw;
+
+ const float xc = x3Dc.at<float>(0);
+ const float yc = x3Dc.at<float>(1);
+ const float invzc = 1.0/x3Dc.at<float>(2);
+
+ if(invzc<0)
+ continue;
+
+ float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
+ float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
+
+ if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
+ continue;
+ if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
+ continue;
+
+ int nLastOctave = LastFrame.mvKeys[i].octave;
+
+ // Search in a window. Size depends on scale
+ float radius = th*CurrentFrame.mvScaleFactors[nLastOctave];
+
+ vector<size_t> vIndices2;
+
+ if(bForward)
+ vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave);
+ else if(bBackward)
+ vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, 0, nLastOctave);
+ else
+ vIndices2 = CurrentFrame.GetFeaturesInArea(u,v, radius, nLastOctave-1, nLastOctave+1);
+
+ if(vIndices2.empty())
+ continue;
+
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = 256;
+ int bestIdx2 = -1;
+
+ for(vector<size_t>::const_iterator vit=vIndices2.begin(), vend=vIndices2.end(); vit!=vend; vit++)
+ {
+ const size_t i2 = *vit;
+ if(CurrentFrame.mvpMapPoints[i2])
+ if(CurrentFrame.mvpMapPoints[i2]->Observations()>0)
+ continue;
+
+ if(CurrentFrame.mvuRight[i2]>0)
+ {
+ const float ur = u - CurrentFrame.mbf*invzc;
+ const float er = fabs(ur - CurrentFrame.mvuRight[i2]);
+ if(er>radius)
+ continue;
+ }
+
+ const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
+
+ const int dist = DescriptorDistance(dMP,d);
+
+ if(dist<bestDist)
+ {
+ bestDist=dist;
+ bestIdx2=i2;
+ }
+ }
+
+ if(bestDist<=TH_HIGH)
+ {
+ CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
+ nmatches++;
+
+ if(mbCheckOrientation)
+ {
+ float rot = LastFrame.mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(bestIdx2);
+ }
+ }
+ }
+ }
+ }
+
+ //Apply rotation consistency
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i!=ind1 && i!=ind2 && i!=ind3)
+ {
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ CurrentFrame.mvpMapPoints[rotHist[i][j]]=static_cast<MapPoint*>(NULL);
+ nmatches--;
+ }
+ }
+ }
+ }
+
+ return nmatches;
+}
+
+int ORBmatcher::SearchByProjection(Frame &CurrentFrame, KeyFrame *pKF, const set<MapPoint*> &sAlreadyFound, const float th , const int ORBdist)
+{
+ int nmatches = 0;
+
+ const cv::Mat Rcw = CurrentFrame.mTcw.rowRange(0,3).colRange(0,3);
+ const cv::Mat tcw = CurrentFrame.mTcw.rowRange(0,3).col(3);
+ const cv::Mat Ow = -Rcw.t()*tcw;
+
+ // Rotation Histogram (to check rotation consistency)
+ vector<int> rotHist[HISTO_LENGTH];
+ for(int i=0;i<HISTO_LENGTH;i++)
+ rotHist[i].reserve(500);
+ const float factor = 1.0f/HISTO_LENGTH;
+
+ const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
+
+ for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
+ {
+ MapPoint* pMP = vpMPs[i];
+
+ if(pMP)
+ {
+ if(!pMP->isBad() && !sAlreadyFound.count(pMP))
+ {
+ //Project
+ cv::Mat x3Dw = pMP->GetWorldPos();
+ cv::Mat x3Dc = Rcw*x3Dw+tcw;
+
+ const float xc = x3Dc.at<float>(0);
+ const float yc = x3Dc.at<float>(1);
+ const float invzc = 1.0/x3Dc.at<float>(2);
+
+ const float u = CurrentFrame.fx*xc*invzc+CurrentFrame.cx;
+ const float v = CurrentFrame.fy*yc*invzc+CurrentFrame.cy;
+
+ if(u<CurrentFrame.mnMinX || u>CurrentFrame.mnMaxX)
+ continue;
+ if(v<CurrentFrame.mnMinY || v>CurrentFrame.mnMaxY)
+ continue;
+
+ // Compute predicted scale level
+ cv::Mat PO = x3Dw-Ow;
+ float dist3D = cv::norm(PO);
+
+ const float maxDistance = pMP->GetMaxDistanceInvariance();
+ const float minDistance = pMP->GetMinDistanceInvariance();
+
+ // Depth must be inside the scale pyramid of the image
+ if(dist3D<minDistance || dist3D>maxDistance)
+ continue;
+
+ int nPredictedLevel = pMP->PredictScale(dist3D,&CurrentFrame);
+
+ // Search in a window
+ const float radius = th*CurrentFrame.mvScaleFactors[nPredictedLevel];
+
+ const vector<size_t> vIndices2 = CurrentFrame.GetFeaturesInArea(u, v, radius, nPredictedLevel-1, nPredictedLevel+1);
+
+ if(vIndices2.empty())
+ continue;
+
+ const cv::Mat dMP = pMP->GetDescriptor();
+
+ int bestDist = 256;
+ int bestIdx2 = -1;
+
+ for(vector<size_t>::const_iterator vit=vIndices2.begin(); vit!=vIndices2.end(); vit++)
+ {
+ const size_t i2 = *vit;
+ if(CurrentFrame.mvpMapPoints[i2])
+ continue;
+
+ const cv::Mat &d = CurrentFrame.mDescriptors.row(i2);
+
+ const int dist = DescriptorDistance(dMP,d);
+
+ if(dist<bestDist)
+ {
+ bestDist=dist;
+ bestIdx2=i2;
+ }
+ }
+
+ if(bestDist<=ORBdist)
+ {
+ CurrentFrame.mvpMapPoints[bestIdx2]=pMP;
+ nmatches++;
+
+ if(mbCheckOrientation)
+ {
+ float rot = pKF->mvKeysUn[i].angle-CurrentFrame.mvKeysUn[bestIdx2].angle;
+ if(rot<0.0)
+ rot+=360.0f;
+ int bin = round(rot*factor);
+ if(bin==HISTO_LENGTH)
+ bin=0;
+ assert(bin>=0 && bin<HISTO_LENGTH);
+ rotHist[bin].push_back(bestIdx2);
+ }
+ }
+
+ }
+ }
+ }
+
+ if(mbCheckOrientation)
+ {
+ int ind1=-1;
+ int ind2=-1;
+ int ind3=-1;
+
+ ComputeThreeMaxima(rotHist,HISTO_LENGTH,ind1,ind2,ind3);
+
+ for(int i=0; i<HISTO_LENGTH; i++)
+ {
+ if(i!=ind1 && i!=ind2 && i!=ind3)
+ {
+ for(size_t j=0, jend=rotHist[i].size(); j<jend; j++)
+ {
+ CurrentFrame.mvpMapPoints[rotHist[i][j]]=NULL;
+ nmatches--;
+ }
+ }
+ }
+ }
+
+ return nmatches;
+}
+
+void ORBmatcher::ComputeThreeMaxima(vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3)
+{
+ int max1=0;
+ int max2=0;
+ int max3=0;
+
+ for(int i=0; i<L; i++)
+ {
+ const int s = histo[i].size();
+ if(s>max1)
+ {
+ max3=max2;
+ max2=max1;
+ max1=s;
+ ind3=ind2;
+ ind2=ind1;
+ ind1=i;
+ }
+ else if(s>max2)
+ {
+ max3=max2;
+ max2=s;
+ ind3=ind2;
+ ind2=i;
+ }
+ else if(s>max3)
+ {
+ max3=s;
+ ind3=i;
+ }
+ }
+
+ if(max2<0.1f*(float)max1)
+ {
+ ind2=-1;
+ ind3=-1;
+ }
+ else if(max3<0.1f*(float)max1)
+ {
+ ind3=-1;
+ }
+}
+
+
+// Bit set count operation from
+// http://graphics.stanford.edu/~seander/bithacks.html#CountBitsSetParallel
+int ORBmatcher::DescriptorDistance(const cv::Mat &a, const cv::Mat &b)
+{
+ const int *pa = a.ptr<int32_t>();
+ const int *pb = b.ptr<int32_t>();
+
+ int dist=0;
+
+ for(int i=0; i<8; i++, pa++, pb++)
+ {
+ unsigned int v = *pa ^ *pb;
+ v = v - ((v >> 1) & 0x55555555);
+ v = (v & 0x33333333) + ((v >> 2) & 0x33333333);
+ dist += (((v + (v >> 4)) & 0xF0F0F0F) * 0x1010101) >> 24;
+ }
+
+ return dist;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Optimizer.cc b/externals/ORB_SLAM2/src/Optimizer.cc
new file mode 100644
index 0000000000000000000000000000000000000000..83d9264546b284a174ff0817808faa83c7033376
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Optimizer.cc
@@ -0,0 +1,1244 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Optimizer.h"
+
+#include "Thirdparty/g2o/g2o/core/block_solver.h"
+#include "Thirdparty/g2o/g2o/core/optimization_algorithm_levenberg.h"
+#include "Thirdparty/g2o/g2o/solvers/linear_solver_eigen.h"
+#include "Thirdparty/g2o/g2o/types/types_six_dof_expmap.h"
+#include "Thirdparty/g2o/g2o/core/robust_kernel_impl.h"
+#include "Thirdparty/g2o/g2o/solvers/linear_solver_dense.h"
+#include "Thirdparty/g2o/g2o/types/types_seven_dof_expmap.h"
+
+#include<Eigen/StdVector>
+
+#include "Converter.h"
+
+#include<mutex>
+
+namespace ORB_SLAM2
+{
+
+
+void Optimizer::GlobalBundleAdjustemnt(Map* pMap, int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
+{
+ vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+ vector<MapPoint*> vpMP = pMap->GetAllMapPoints();
+ BundleAdjustment(vpKFs,vpMP,nIterations,pbStopFlag, nLoopKF, bRobust);
+}
+
+
+void Optimizer::BundleAdjustment(const vector<KeyFrame *> &vpKFs, const vector<MapPoint *> &vpMP,
+ int nIterations, bool* pbStopFlag, const unsigned long nLoopKF, const bool bRobust)
+{
+ vector<bool> vbNotIncludedMP;
+ vbNotIncludedMP.resize(vpMP.size());
+
+ g2o::SparseOptimizer optimizer;
+ g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
+
+ linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+ g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+ optimizer.setAlgorithm(solver);
+
+ if(pbStopFlag)
+ optimizer.setForceStopFlag(pbStopFlag);
+
+ long unsigned int maxKFid = 0;
+
+ // Set KeyFrame vertices
+ for(size_t i=0; i<vpKFs.size(); i++)
+ {
+ KeyFrame* pKF = vpKFs[i];
+ if(pKF->isBad())
+ continue;
+ g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
+ vSE3->setEstimate(Converter::toSE3Quat(pKF->GetPose()));
+ vSE3->setId(pKF->mnId);
+ vSE3->setFixed(pKF->mnId==0);
+ optimizer.addVertex(vSE3);
+ if(pKF->mnId>maxKFid)
+ maxKFid=pKF->mnId;
+ }
+
+ const float thHuber2D = sqrt(5.99);
+ const float thHuber3D = sqrt(7.815);
+
+ // Set MapPoint vertices
+ for(size_t i=0; i<vpMP.size(); i++)
+ {
+ MapPoint* pMP = vpMP[i];
+ if(pMP->isBad())
+ continue;
+ g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
+ vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
+ const int id = pMP->mnId+maxKFid+1;
+ vPoint->setId(id);
+ vPoint->setMarginalized(true);
+ optimizer.addVertex(vPoint);
+
+ const map<KeyFrame*,size_t> observations = pMP->GetObservations();
+
+ int nEdges = 0;
+ //SET EDGES
+ for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(); mit!=observations.end(); mit++)
+ {
+
+ KeyFrame* pKF = mit->first;
+ if(pKF->isBad() || pKF->mnId>maxKFid)
+ continue;
+
+ nEdges++;
+
+ const cv::KeyPoint &kpUn = pKF->mvKeysUn[mit->second];
+
+ if(pKF->mvuRight[mit->second]<0)
+ {
+ Eigen::Matrix<double,2,1> obs;
+ obs << kpUn.pt.x, kpUn.pt.y;
+
+ g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
+ e->setMeasurement(obs);
+ const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
+ e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
+
+ if(bRobust)
+ {
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(thHuber2D);
+ }
+
+ e->fx = pKF->fx;
+ e->fy = pKF->fy;
+ e->cx = pKF->cx;
+ e->cy = pKF->cy;
+
+ optimizer.addEdge(e);
+ }
+ else
+ {
+ Eigen::Matrix<double,3,1> obs;
+ const float kp_ur = pKF->mvuRight[mit->second];
+ obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
+
+ g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKF->mnId)));
+ e->setMeasurement(obs);
+ const float &invSigma2 = pKF->mvInvLevelSigma2[kpUn.octave];
+ Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
+ e->setInformation(Info);
+
+ if(bRobust)
+ {
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(thHuber3D);
+ }
+
+ e->fx = pKF->fx;
+ e->fy = pKF->fy;
+ e->cx = pKF->cx;
+ e->cy = pKF->cy;
+ e->bf = pKF->mbf;
+
+ optimizer.addEdge(e);
+ }
+ }
+
+ if(nEdges==0)
+ {
+ optimizer.removeVertex(vPoint);
+ vbNotIncludedMP[i]=true;
+ }
+ else
+ {
+ vbNotIncludedMP[i]=false;
+ }
+ }
+
+ // Optimize!
+ optimizer.initializeOptimization();
+ optimizer.optimize(nIterations);
+
+ // Recover optimized data
+
+ //Keyframes
+ for(size_t i=0; i<vpKFs.size(); i++)
+ {
+ KeyFrame* pKF = vpKFs[i];
+ if(pKF->isBad())
+ continue;
+ g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
+ g2o::SE3Quat SE3quat = vSE3->estimate();
+ if(nLoopKF==0)
+ {
+ pKF->SetPose(Converter::toCvMat(SE3quat));
+ }
+ else
+ {
+ pKF->mTcwGBA.create(4,4,CV_32F);
+ Converter::toCvMat(SE3quat).copyTo(pKF->mTcwGBA);
+ pKF->mnBAGlobalForKF = nLoopKF;
+ }
+ }
+
+ //Points
+ for(size_t i=0; i<vpMP.size(); i++)
+ {
+ if(vbNotIncludedMP[i])
+ continue;
+
+ MapPoint* pMP = vpMP[i];
+
+ if(pMP->isBad())
+ continue;
+ g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
+
+ if(nLoopKF==0)
+ {
+ pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
+ pMP->UpdateNormalAndDepth();
+ }
+ else
+ {
+ pMP->mPosGBA.create(3,1,CV_32F);
+ Converter::toCvMat(vPoint->estimate()).copyTo(pMP->mPosGBA);
+ pMP->mnBAGlobalForKF = nLoopKF;
+ }
+ }
+
+}
+
+int Optimizer::PoseOptimization(Frame *pFrame)
+{
+ g2o::SparseOptimizer optimizer;
+ g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
+
+ linearSolver = new g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+ g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+ optimizer.setAlgorithm(solver);
+
+ int nInitialCorrespondences=0;
+
+ // Set Frame vertex
+ g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
+ vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
+ vSE3->setId(0);
+ vSE3->setFixed(false);
+ optimizer.addVertex(vSE3);
+
+ // Set MapPoint vertices
+ const int N = pFrame->N;
+
+ vector<g2o::EdgeSE3ProjectXYZOnlyPose*> vpEdgesMono;
+ vector<size_t> vnIndexEdgeMono;
+ vpEdgesMono.reserve(N);
+ vnIndexEdgeMono.reserve(N);
+
+ vector<g2o::EdgeStereoSE3ProjectXYZOnlyPose*> vpEdgesStereo;
+ vector<size_t> vnIndexEdgeStereo;
+ vpEdgesStereo.reserve(N);
+ vnIndexEdgeStereo.reserve(N);
+
+ const float deltaMono = sqrt(5.991);
+ const float deltaStereo = sqrt(7.815);
+
+
+ {
+ unique_lock<mutex> lock(MapPoint::mGlobalMutex);
+
+ for(int i=0; i<N; i++)
+ {
+ MapPoint* pMP = pFrame->mvpMapPoints[i];
+ if(pMP)
+ {
+ // Monocular observation
+ if(pFrame->mvuRight[i]<0)
+ {
+ nInitialCorrespondences++;
+ pFrame->mvbOutlier[i] = false;
+
+ Eigen::Matrix<double,2,1> obs;
+ const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
+ obs << kpUn.pt.x, kpUn.pt.y;
+
+ g2o::EdgeSE3ProjectXYZOnlyPose* e = new g2o::EdgeSE3ProjectXYZOnlyPose();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
+ e->setMeasurement(obs);
+ const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
+ e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
+
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(deltaMono);
+
+ e->fx = pFrame->fx;
+ e->fy = pFrame->fy;
+ e->cx = pFrame->cx;
+ e->cy = pFrame->cy;
+ cv::Mat Xw = pMP->GetWorldPos();
+ e->Xw[0] = Xw.at<float>(0);
+ e->Xw[1] = Xw.at<float>(1);
+ e->Xw[2] = Xw.at<float>(2);
+
+ optimizer.addEdge(e);
+
+ vpEdgesMono.push_back(e);
+ vnIndexEdgeMono.push_back(i);
+ }
+ else // Stereo observation
+ {
+ nInitialCorrespondences++;
+ pFrame->mvbOutlier[i] = false;
+
+ //SET EDGE
+ Eigen::Matrix<double,3,1> obs;
+ const cv::KeyPoint &kpUn = pFrame->mvKeysUn[i];
+ const float &kp_ur = pFrame->mvuRight[i];
+ obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
+
+ g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = new g2o::EdgeStereoSE3ProjectXYZOnlyPose();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
+ e->setMeasurement(obs);
+ const float invSigma2 = pFrame->mvInvLevelSigma2[kpUn.octave];
+ Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
+ e->setInformation(Info);
+
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(deltaStereo);
+
+ e->fx = pFrame->fx;
+ e->fy = pFrame->fy;
+ e->cx = pFrame->cx;
+ e->cy = pFrame->cy;
+ e->bf = pFrame->mbf;
+ cv::Mat Xw = pMP->GetWorldPos();
+ e->Xw[0] = Xw.at<float>(0);
+ e->Xw[1] = Xw.at<float>(1);
+ e->Xw[2] = Xw.at<float>(2);
+
+ optimizer.addEdge(e);
+
+ vpEdgesStereo.push_back(e);
+ vnIndexEdgeStereo.push_back(i);
+ }
+ }
+
+ }
+ }
+
+
+ if(nInitialCorrespondences<3)
+ return 0;
+
+ // We perform 4 optimizations, after each optimization we classify observation as inlier/outlier
+ // At the next optimization, outliers are not included, but at the end they can be classified as inliers again.
+ const float chi2Mono[4]={5.991,5.991,5.991,5.991};
+ const float chi2Stereo[4]={7.815,7.815,7.815, 7.815};
+ const int its[4]={10,10,10,10};
+
+ int nBad=0;
+ for(size_t it=0; it<4; it++)
+ {
+
+ vSE3->setEstimate(Converter::toSE3Quat(pFrame->mTcw));
+ optimizer.initializeOptimization(0);
+ optimizer.optimize(its[it]);
+
+ nBad=0;
+ for(size_t i=0, iend=vpEdgesMono.size(); i<iend; i++)
+ {
+ g2o::EdgeSE3ProjectXYZOnlyPose* e = vpEdgesMono[i];
+
+ const size_t idx = vnIndexEdgeMono[i];
+
+ if(pFrame->mvbOutlier[idx])
+ {
+ e->computeError();
+ }
+
+ const float chi2 = e->chi2();
+
+ if(chi2>chi2Mono[it])
+ {
+ pFrame->mvbOutlier[idx]=true;
+ e->setLevel(1);
+ nBad++;
+ }
+ else
+ {
+ pFrame->mvbOutlier[idx]=false;
+ e->setLevel(0);
+ }
+
+ if(it==2)
+ e->setRobustKernel(0);
+ }
+
+ for(size_t i=0, iend=vpEdgesStereo.size(); i<iend; i++)
+ {
+ g2o::EdgeStereoSE3ProjectXYZOnlyPose* e = vpEdgesStereo[i];
+
+ const size_t idx = vnIndexEdgeStereo[i];
+
+ if(pFrame->mvbOutlier[idx])
+ {
+ e->computeError();
+ }
+
+ const float chi2 = e->chi2();
+
+ if(chi2>chi2Stereo[it])
+ {
+ pFrame->mvbOutlier[idx]=true;
+ e->setLevel(1);
+ nBad++;
+ }
+ else
+ {
+ e->setLevel(0);
+ pFrame->mvbOutlier[idx]=false;
+ }
+
+ if(it==2)
+ e->setRobustKernel(0);
+ }
+
+ if(optimizer.edges().size()<10)
+ break;
+ }
+
+ // Recover optimized pose and return number of inliers
+ g2o::VertexSE3Expmap* vSE3_recov = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(0));
+ g2o::SE3Quat SE3quat_recov = vSE3_recov->estimate();
+ cv::Mat pose = Converter::toCvMat(SE3quat_recov);
+ pFrame->SetPose(pose);
+
+ return nInitialCorrespondences-nBad;
+}
+
+void Optimizer::LocalBundleAdjustment(KeyFrame *pKF, bool* pbStopFlag, Map* pMap)
+{
+ // Local KeyFrames: First Breath Search from Current Keyframe
+ list<KeyFrame*> lLocalKeyFrames;
+
+ lLocalKeyFrames.push_back(pKF);
+ pKF->mnBALocalForKF = pKF->mnId;
+
+ const vector<KeyFrame*> vNeighKFs = pKF->GetVectorCovisibleKeyFrames();
+ for(int i=0, iend=vNeighKFs.size(); i<iend; i++)
+ {
+ KeyFrame* pKFi = vNeighKFs[i];
+ pKFi->mnBALocalForKF = pKF->mnId;
+ if(!pKFi->isBad())
+ lLocalKeyFrames.push_back(pKFi);
+ }
+
+ // Local MapPoints seen in Local KeyFrames
+ list<MapPoint*> lLocalMapPoints;
+ for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin() , lend=lLocalKeyFrames.end(); lit!=lend; lit++)
+ {
+ vector<MapPoint*> vpMPs = (*lit)->GetMapPointMatches();
+ for(vector<MapPoint*>::iterator vit=vpMPs.begin(), vend=vpMPs.end(); vit!=vend; vit++)
+ {
+ MapPoint* pMP = *vit;
+ if(pMP)
+ if(!pMP->isBad())
+ if(pMP->mnBALocalForKF!=pKF->mnId)
+ {
+ lLocalMapPoints.push_back(pMP);
+ pMP->mnBALocalForKF=pKF->mnId;
+ }
+ }
+ }
+
+ // Fixed Keyframes. Keyframes that see Local MapPoints but that are not Local Keyframes
+ list<KeyFrame*> lFixedCameras;
+ for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
+ {
+ map<KeyFrame*,size_t> observations = (*lit)->GetObservations();
+ for(map<KeyFrame*,size_t>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKFi = mit->first;
+
+ if(pKFi->mnBALocalForKF!=pKF->mnId && pKFi->mnBAFixedForKF!=pKF->mnId)
+ {
+ pKFi->mnBAFixedForKF=pKF->mnId;
+ if(!pKFi->isBad())
+ lFixedCameras.push_back(pKFi);
+ }
+ }
+ }
+
+ // Setup optimizer
+ g2o::SparseOptimizer optimizer;
+ g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
+
+ linearSolver = new g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>();
+
+ g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
+
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+ optimizer.setAlgorithm(solver);
+
+ if(pbStopFlag)
+ optimizer.setForceStopFlag(pbStopFlag);
+
+ unsigned long maxKFid = 0;
+
+ // Set Local KeyFrame vertices
+ for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi = *lit;
+ g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
+ vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
+ vSE3->setId(pKFi->mnId);
+ vSE3->setFixed(pKFi->mnId==0);
+ optimizer.addVertex(vSE3);
+ if(pKFi->mnId>maxKFid)
+ maxKFid=pKFi->mnId;
+ }
+
+ // Set Fixed KeyFrame vertices
+ for(list<KeyFrame*>::iterator lit=lFixedCameras.begin(), lend=lFixedCameras.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKFi = *lit;
+ g2o::VertexSE3Expmap * vSE3 = new g2o::VertexSE3Expmap();
+ vSE3->setEstimate(Converter::toSE3Quat(pKFi->GetPose()));
+ vSE3->setId(pKFi->mnId);
+ vSE3->setFixed(true);
+ optimizer.addVertex(vSE3);
+ if(pKFi->mnId>maxKFid)
+ maxKFid=pKFi->mnId;
+ }
+
+ // Set MapPoint vertices
+ const int nExpectedSize = (lLocalKeyFrames.size()+lFixedCameras.size())*lLocalMapPoints.size();
+
+ vector<g2o::EdgeSE3ProjectXYZ*> vpEdgesMono;
+ vpEdgesMono.reserve(nExpectedSize);
+
+ vector<KeyFrame*> vpEdgeKFMono;
+ vpEdgeKFMono.reserve(nExpectedSize);
+
+ vector<MapPoint*> vpMapPointEdgeMono;
+ vpMapPointEdgeMono.reserve(nExpectedSize);
+
+ vector<g2o::EdgeStereoSE3ProjectXYZ*> vpEdgesStereo;
+ vpEdgesStereo.reserve(nExpectedSize);
+
+ vector<KeyFrame*> vpEdgeKFStereo;
+ vpEdgeKFStereo.reserve(nExpectedSize);
+
+ vector<MapPoint*> vpMapPointEdgeStereo;
+ vpMapPointEdgeStereo.reserve(nExpectedSize);
+
+ const float thHuberMono = sqrt(5.991);
+ const float thHuberStereo = sqrt(7.815);
+
+ for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
+ {
+ MapPoint* pMP = *lit;
+ g2o::VertexSBAPointXYZ* vPoint = new g2o::VertexSBAPointXYZ();
+ vPoint->setEstimate(Converter::toVector3d(pMP->GetWorldPos()));
+ int id = pMP->mnId+maxKFid+1;
+ vPoint->setId(id);
+ vPoint->setMarginalized(true);
+ optimizer.addVertex(vPoint);
+
+ const map<KeyFrame*,size_t> observations = pMP->GetObservations();
+
+ //Set edges
+ for(map<KeyFrame*,size_t>::const_iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKFi = mit->first;
+
+ if(!pKFi->isBad())
+ {
+ const cv::KeyPoint &kpUn = pKFi->mvKeysUn[mit->second];
+
+ // Monocular observation
+ if(pKFi->mvuRight[mit->second]<0)
+ {
+ Eigen::Matrix<double,2,1> obs;
+ obs << kpUn.pt.x, kpUn.pt.y;
+
+ g2o::EdgeSE3ProjectXYZ* e = new g2o::EdgeSE3ProjectXYZ();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
+ e->setMeasurement(obs);
+ const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
+ e->setInformation(Eigen::Matrix2d::Identity()*invSigma2);
+
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(thHuberMono);
+
+ e->fx = pKFi->fx;
+ e->fy = pKFi->fy;
+ e->cx = pKFi->cx;
+ e->cy = pKFi->cy;
+
+ optimizer.addEdge(e);
+ vpEdgesMono.push_back(e);
+ vpEdgeKFMono.push_back(pKFi);
+ vpMapPointEdgeMono.push_back(pMP);
+ }
+ else // Stereo observation
+ {
+ Eigen::Matrix<double,3,1> obs;
+ const float kp_ur = pKFi->mvuRight[mit->second];
+ obs << kpUn.pt.x, kpUn.pt.y, kp_ur;
+
+ g2o::EdgeStereoSE3ProjectXYZ* e = new g2o::EdgeStereoSE3ProjectXYZ();
+
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id)));
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFi->mnId)));
+ e->setMeasurement(obs);
+ const float &invSigma2 = pKFi->mvInvLevelSigma2[kpUn.octave];
+ Eigen::Matrix3d Info = Eigen::Matrix3d::Identity()*invSigma2;
+ e->setInformation(Info);
+
+ g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
+ e->setRobustKernel(rk);
+ rk->setDelta(thHuberStereo);
+
+ e->fx = pKFi->fx;
+ e->fy = pKFi->fy;
+ e->cx = pKFi->cx;
+ e->cy = pKFi->cy;
+ e->bf = pKFi->mbf;
+
+ optimizer.addEdge(e);
+ vpEdgesStereo.push_back(e);
+ vpEdgeKFStereo.push_back(pKFi);
+ vpMapPointEdgeStereo.push_back(pMP);
+ }
+ }
+ }
+ }
+
+ if(pbStopFlag)
+ if(*pbStopFlag)
+ return;
+
+ optimizer.initializeOptimization();
+ optimizer.optimize(5);
+
+ bool bDoMore= true;
+
+ if(pbStopFlag)
+ if(*pbStopFlag)
+ bDoMore = false;
+
+ if(bDoMore)
+ {
+
+ // Check inlier observations
+ for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
+ {
+ g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+ MapPoint* pMP = vpMapPointEdgeMono[i];
+
+ if(pMP->isBad())
+ continue;
+
+ if(e->chi2()>5.991 || !e->isDepthPositive())
+ {
+ e->setLevel(1);
+ }
+
+ e->setRobustKernel(0);
+ }
+
+ for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
+ {
+ g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
+ MapPoint* pMP = vpMapPointEdgeStereo[i];
+
+ if(pMP->isBad())
+ continue;
+
+ if(e->chi2()>7.815 || !e->isDepthPositive())
+ {
+ e->setLevel(1);
+ }
+
+ e->setRobustKernel(0);
+ }
+
+ // Optimize again without the outliers
+
+ optimizer.initializeOptimization(0);
+ optimizer.optimize(10);
+
+ }
+
+ vector<pair<KeyFrame*,MapPoint*> > vToErase;
+ vToErase.reserve(vpEdgesMono.size()+vpEdgesStereo.size());
+
+ // Check inlier observations
+ for(size_t i=0, iend=vpEdgesMono.size(); i<iend;i++)
+ {
+ g2o::EdgeSE3ProjectXYZ* e = vpEdgesMono[i];
+ MapPoint* pMP = vpMapPointEdgeMono[i];
+
+ if(pMP->isBad())
+ continue;
+
+ if(e->chi2()>5.991 || !e->isDepthPositive())
+ {
+ KeyFrame* pKFi = vpEdgeKFMono[i];
+ vToErase.push_back(make_pair(pKFi,pMP));
+ }
+ }
+
+ for(size_t i=0, iend=vpEdgesStereo.size(); i<iend;i++)
+ {
+ g2o::EdgeStereoSE3ProjectXYZ* e = vpEdgesStereo[i];
+ MapPoint* pMP = vpMapPointEdgeStereo[i];
+
+ if(pMP->isBad())
+ continue;
+
+ if(e->chi2()>7.815 || !e->isDepthPositive())
+ {
+ KeyFrame* pKFi = vpEdgeKFStereo[i];
+ vToErase.push_back(make_pair(pKFi,pMP));
+ }
+ }
+
+ // Get Map Mutex
+ unique_lock<mutex> lock(pMap->mMutexMapUpdate);
+
+ if(!vToErase.empty())
+ {
+ for(size_t i=0;i<vToErase.size();i++)
+ {
+ KeyFrame* pKFi = vToErase[i].first;
+ MapPoint* pMPi = vToErase[i].second;
+ pKFi->EraseMapPointMatch(pMPi);
+ pMPi->EraseObservation(pKFi);
+ }
+ }
+
+ // Recover optimized data
+
+ //Keyframes
+ for(list<KeyFrame*>::iterator lit=lLocalKeyFrames.begin(), lend=lLocalKeyFrames.end(); lit!=lend; lit++)
+ {
+ KeyFrame* pKF = *lit;
+ g2o::VertexSE3Expmap* vSE3 = static_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(pKF->mnId));
+ g2o::SE3Quat SE3quat = vSE3->estimate();
+ pKF->SetPose(Converter::toCvMat(SE3quat));
+ }
+
+ //Points
+ for(list<MapPoint*>::iterator lit=lLocalMapPoints.begin(), lend=lLocalMapPoints.end(); lit!=lend; lit++)
+ {
+ MapPoint* pMP = *lit;
+ g2o::VertexSBAPointXYZ* vPoint = static_cast<g2o::VertexSBAPointXYZ*>(optimizer.vertex(pMP->mnId+maxKFid+1));
+ pMP->SetWorldPos(Converter::toCvMat(vPoint->estimate()));
+ pMP->UpdateNormalAndDepth();
+ }
+}
+
+
+void Optimizer::OptimizeEssentialGraph(Map* pMap, KeyFrame* pLoopKF, KeyFrame* pCurKF,
+ const LoopClosing::KeyFrameAndPose &NonCorrectedSim3,
+ const LoopClosing::KeyFrameAndPose &CorrectedSim3,
+ const map<KeyFrame *, set<KeyFrame *> > &LoopConnections, const bool &bFixScale)
+{
+ // Setup optimizer
+ g2o::SparseOptimizer optimizer;
+ optimizer.setVerbose(false);
+ g2o::BlockSolver_7_3::LinearSolverType * linearSolver =
+ new g2o::LinearSolverEigen<g2o::BlockSolver_7_3::PoseMatrixType>();
+ g2o::BlockSolver_7_3 * solver_ptr= new g2o::BlockSolver_7_3(linearSolver);
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+
+ solver->setUserLambdaInit(1e-16);
+ optimizer.setAlgorithm(solver);
+
+ const vector<KeyFrame*> vpKFs = pMap->GetAllKeyFrames();
+ const vector<MapPoint*> vpMPs = pMap->GetAllMapPoints();
+
+ const unsigned int nMaxKFid = pMap->GetMaxKFid();
+
+ vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vScw(nMaxKFid+1);
+ vector<g2o::Sim3,Eigen::aligned_allocator<g2o::Sim3> > vCorrectedSwc(nMaxKFid+1);
+ vector<g2o::VertexSim3Expmap*> vpVertices(nMaxKFid+1);
+
+ const int minFeat = 100;
+
+ // Set KeyFrame vertices
+ for(size_t i=0, iend=vpKFs.size(); i<iend;i++)
+ {
+ KeyFrame* pKF = vpKFs[i];
+ if(pKF->isBad())
+ continue;
+ g2o::VertexSim3Expmap* VSim3 = new g2o::VertexSim3Expmap();
+
+ const int nIDi = pKF->mnId;
+
+ LoopClosing::KeyFrameAndPose::const_iterator it = CorrectedSim3.find(pKF);
+
+ if(it!=CorrectedSim3.end())
+ {
+ vScw[nIDi] = it->second;
+ VSim3->setEstimate(it->second);
+ }
+ else
+ {
+ Eigen::Matrix<double,3,3> Rcw = Converter::toMatrix3d(pKF->GetRotation());
+ Eigen::Matrix<double,3,1> tcw = Converter::toVector3d(pKF->GetTranslation());
+ g2o::Sim3 Siw(Rcw,tcw,1.0);
+ vScw[nIDi] = Siw;
+ VSim3->setEstimate(Siw);
+ }
+
+ if(pKF==pLoopKF)
+ VSim3->setFixed(true);
+
+ VSim3->setId(nIDi);
+ VSim3->setMarginalized(false);
+ VSim3->_fix_scale = bFixScale;
+
+ optimizer.addVertex(VSim3);
+
+ vpVertices[nIDi]=VSim3;
+ }
+
+
+ set<pair<long unsigned int,long unsigned int> > sInsertedEdges;
+
+ const Eigen::Matrix<double,7,7> matLambda = Eigen::Matrix<double,7,7>::Identity();
+
+ // Set Loop edges
+ for(map<KeyFrame *, set<KeyFrame *> >::const_iterator mit = LoopConnections.begin(), mend=LoopConnections.end(); mit!=mend; mit++)
+ {
+ KeyFrame* pKF = mit->first;
+ const long unsigned int nIDi = pKF->mnId;
+ const set<KeyFrame*> &spConnections = mit->second;
+ const g2o::Sim3 Siw = vScw[nIDi];
+ const g2o::Sim3 Swi = Siw.inverse();
+
+ for(set<KeyFrame*>::const_iterator sit=spConnections.begin(), send=spConnections.end(); sit!=send; sit++)
+ {
+ const long unsigned int nIDj = (*sit)->mnId;
+ if((nIDi!=pCurKF->mnId || nIDj!=pLoopKF->mnId) && pKF->GetWeight(*sit)<minFeat)
+ continue;
+
+ const g2o::Sim3 Sjw = vScw[nIDj];
+ const g2o::Sim3 Sji = Sjw * Swi;
+
+ g2o::EdgeSim3* e = new g2o::EdgeSim3();
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
+ e->setMeasurement(Sji);
+
+ e->information() = matLambda;
+
+ optimizer.addEdge(e);
+
+ sInsertedEdges.insert(make_pair(min(nIDi,nIDj),max(nIDi,nIDj)));
+ }
+ }
+
+ // Set normal edges
+ for(size_t i=0, iend=vpKFs.size(); i<iend; i++)
+ {
+ KeyFrame* pKF = vpKFs[i];
+
+ const int nIDi = pKF->mnId;
+
+ g2o::Sim3 Swi;
+
+ LoopClosing::KeyFrameAndPose::const_iterator iti = NonCorrectedSim3.find(pKF);
+
+ if(iti!=NonCorrectedSim3.end())
+ Swi = (iti->second).inverse();
+ else
+ Swi = vScw[nIDi].inverse();
+
+ KeyFrame* pParentKF = pKF->GetParent();
+
+ // Spanning tree edge
+ if(pParentKF)
+ {
+ int nIDj = pParentKF->mnId;
+
+ g2o::Sim3 Sjw;
+
+ LoopClosing::KeyFrameAndPose::const_iterator itj = NonCorrectedSim3.find(pParentKF);
+
+ if(itj!=NonCorrectedSim3.end())
+ Sjw = itj->second;
+ else
+ Sjw = vScw[nIDj];
+
+ g2o::Sim3 Sji = Sjw * Swi;
+
+ g2o::EdgeSim3* e = new g2o::EdgeSim3();
+ e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDj)));
+ e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
+ e->setMeasurement(Sji);
+
+ e->information() = matLambda;
+ optimizer.addEdge(e);
+ }
+
+ // Loop edges
+ const set<KeyFrame*> sLoopEdges = pKF->GetLoopEdges();
+ for(set<KeyFrame*>::const_iterator sit=sLoopEdges.begin(), send=sLoopEdges.end(); sit!=send; sit++)
+ {
+ KeyFrame* pLKF = *sit;
+ if(pLKF->mnId<pKF->mnId)
+ {
+ g2o::Sim3 Slw;
+
+ LoopClosing::KeyFrameAndPose::const_iterator itl = NonCorrectedSim3.find(pLKF);
+
+ if(itl!=NonCorrectedSim3.end())
+ Slw = itl->second;
+ else
+ Slw = vScw[pLKF->mnId];
+
+ g2o::Sim3 Sli = Slw * Swi;
+ g2o::EdgeSim3* el = new g2o::EdgeSim3();
+ el->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pLKF->mnId)));
+ el->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
+ el->setMeasurement(Sli);
+ el->information() = matLambda;
+ optimizer.addEdge(el);
+ }
+ }
+
+ // Covisibility graph edges
+ const vector<KeyFrame*> vpConnectedKFs = pKF->GetCovisiblesByWeight(minFeat);
+ for(vector<KeyFrame*>::const_iterator vit=vpConnectedKFs.begin(); vit!=vpConnectedKFs.end(); vit++)
+ {
+ KeyFrame* pKFn = *vit;
+ if(pKFn && pKFn!=pParentKF && !pKF->hasChild(pKFn) && !sLoopEdges.count(pKFn))
+ {
+ if(!pKFn->isBad() && pKFn->mnId<pKF->mnId)
+ {
+ if(sInsertedEdges.count(make_pair(min(pKF->mnId,pKFn->mnId),max(pKF->mnId,pKFn->mnId))))
+ continue;
+
+ g2o::Sim3 Snw;
+
+ LoopClosing::KeyFrameAndPose::const_iterator itn = NonCorrectedSim3.find(pKFn);
+
+ if(itn!=NonCorrectedSim3.end())
+ Snw = itn->second;
+ else
+ Snw = vScw[pKFn->mnId];
+
+ g2o::Sim3 Sni = Snw * Swi;
+
+ g2o::EdgeSim3* en = new g2o::EdgeSim3();
+ en->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(pKFn->mnId)));
+ en->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(nIDi)));
+ en->setMeasurement(Sni);
+ en->information() = matLambda;
+ optimizer.addEdge(en);
+ }
+ }
+ }
+ }
+
+ // Optimize!
+ optimizer.initializeOptimization();
+ optimizer.optimize(20);
+
+ unique_lock<mutex> lock(pMap->mMutexMapUpdate);
+
+ // SE3 Pose Recovering. Sim3:[sR t;0 1] -> SE3:[R t/s;0 1]
+ for(size_t i=0;i<vpKFs.size();i++)
+ {
+ KeyFrame* pKFi = vpKFs[i];
+
+ const int nIDi = pKFi->mnId;
+
+ g2o::VertexSim3Expmap* VSim3 = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(nIDi));
+ g2o::Sim3 CorrectedSiw = VSim3->estimate();
+ vCorrectedSwc[nIDi]=CorrectedSiw.inverse();
+ Eigen::Matrix3d eigR = CorrectedSiw.rotation().toRotationMatrix();
+ Eigen::Vector3d eigt = CorrectedSiw.translation();
+ double s = CorrectedSiw.scale();
+
+ eigt *=(1./s); //[R t/s;0 1]
+
+ cv::Mat Tiw = Converter::toCvSE3(eigR,eigt);
+
+ pKFi->SetPose(Tiw);
+ }
+
+ // Correct points. Transform to "non-optimized" reference keyframe pose and transform back with optimized pose
+ for(size_t i=0, iend=vpMPs.size(); i<iend; i++)
+ {
+ MapPoint* pMP = vpMPs[i];
+
+ if(pMP->isBad())
+ continue;
+
+ int nIDr;
+ if(pMP->mnCorrectedByKF==pCurKF->mnId)
+ {
+ nIDr = pMP->mnCorrectedReference;
+ }
+ else
+ {
+ KeyFrame* pRefKF = pMP->GetReferenceKeyFrame();
+ nIDr = pRefKF->mnId;
+ }
+
+
+ g2o::Sim3 Srw = vScw[nIDr];
+ g2o::Sim3 correctedSwr = vCorrectedSwc[nIDr];
+
+ cv::Mat P3Dw = pMP->GetWorldPos();
+ Eigen::Matrix<double,3,1> eigP3Dw = Converter::toVector3d(P3Dw);
+ Eigen::Matrix<double,3,1> eigCorrectedP3Dw = correctedSwr.map(Srw.map(eigP3Dw));
+
+ cv::Mat cvCorrectedP3Dw = Converter::toCvMat(eigCorrectedP3Dw);
+ pMP->SetWorldPos(cvCorrectedP3Dw);
+
+ pMP->UpdateNormalAndDepth();
+ }
+}
+
+int Optimizer::OptimizeSim3(KeyFrame *pKF1, KeyFrame *pKF2, vector<MapPoint *> &vpMatches1, g2o::Sim3 &g2oS12, const float th2, const bool bFixScale)
+{
+ g2o::SparseOptimizer optimizer;
+ g2o::BlockSolverX::LinearSolverType * linearSolver;
+
+ linearSolver = new g2o::LinearSolverDense<g2o::BlockSolverX::PoseMatrixType>();
+
+ g2o::BlockSolverX * solver_ptr = new g2o::BlockSolverX(linearSolver);
+
+ g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
+ optimizer.setAlgorithm(solver);
+
+ // Calibration
+ const cv::Mat &K1 = pKF1->mK;
+ const cv::Mat &K2 = pKF2->mK;
+
+ // Camera poses
+ const cv::Mat R1w = pKF1->GetRotation();
+ const cv::Mat t1w = pKF1->GetTranslation();
+ const cv::Mat R2w = pKF2->GetRotation();
+ const cv::Mat t2w = pKF2->GetTranslation();
+
+ // Set Sim3 vertex
+ g2o::VertexSim3Expmap * vSim3 = new g2o::VertexSim3Expmap();
+ vSim3->_fix_scale=bFixScale;
+ vSim3->setEstimate(g2oS12);
+ vSim3->setId(0);
+ vSim3->setFixed(false);
+ vSim3->_principle_point1[0] = K1.at<float>(0,2);
+ vSim3->_principle_point1[1] = K1.at<float>(1,2);
+ vSim3->_focal_length1[0] = K1.at<float>(0,0);
+ vSim3->_focal_length1[1] = K1.at<float>(1,1);
+ vSim3->_principle_point2[0] = K2.at<float>(0,2);
+ vSim3->_principle_point2[1] = K2.at<float>(1,2);
+ vSim3->_focal_length2[0] = K2.at<float>(0,0);
+ vSim3->_focal_length2[1] = K2.at<float>(1,1);
+ optimizer.addVertex(vSim3);
+
+ // Set MapPoint vertices
+ const int N = vpMatches1.size();
+ const vector<MapPoint*> vpMapPoints1 = pKF1->GetMapPointMatches();
+ vector<g2o::EdgeSim3ProjectXYZ*> vpEdges12;
+ vector<g2o::EdgeInverseSim3ProjectXYZ*> vpEdges21;
+ vector<size_t> vnIndexEdge;
+
+ vnIndexEdge.reserve(2*N);
+ vpEdges12.reserve(2*N);
+ vpEdges21.reserve(2*N);
+
+ const float deltaHuber = sqrt(th2);
+
+ int nCorrespondences = 0;
+
+ for(int i=0; i<N; i++)
+ {
+ if(!vpMatches1[i])
+ continue;
+
+ MapPoint* pMP1 = vpMapPoints1[i];
+ MapPoint* pMP2 = vpMatches1[i];
+
+ const int id1 = 2*i+1;
+ const int id2 = 2*(i+1);
+
+ const int i2 = pMP2->GetIndexInKeyFrame(pKF2);
+
+ if(pMP1 && pMP2)
+ {
+ if(!pMP1->isBad() && !pMP2->isBad() && i2>=0)
+ {
+ g2o::VertexSBAPointXYZ* vPoint1 = new g2o::VertexSBAPointXYZ();
+ cv::Mat P3D1w = pMP1->GetWorldPos();
+ cv::Mat P3D1c = R1w*P3D1w + t1w;
+ vPoint1->setEstimate(Converter::toVector3d(P3D1c));
+ vPoint1->setId(id1);
+ vPoint1->setFixed(true);
+ optimizer.addVertex(vPoint1);
+
+ g2o::VertexSBAPointXYZ* vPoint2 = new g2o::VertexSBAPointXYZ();
+ cv::Mat P3D2w = pMP2->GetWorldPos();
+ cv::Mat P3D2c = R2w*P3D2w + t2w;
+ vPoint2->setEstimate(Converter::toVector3d(P3D2c));
+ vPoint2->setId(id2);
+ vPoint2->setFixed(true);
+ optimizer.addVertex(vPoint2);
+ }
+ else
+ continue;
+ }
+ else
+ continue;
+
+ nCorrespondences++;
+
+ // Set edge x1 = S12*X2
+ Eigen::Matrix<double,2,1> obs1;
+ const cv::KeyPoint &kpUn1 = pKF1->mvKeysUn[i];
+ obs1 << kpUn1.pt.x, kpUn1.pt.y;
+
+ g2o::EdgeSim3ProjectXYZ* e12 = new g2o::EdgeSim3ProjectXYZ();
+ e12->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id2)));
+ e12->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
+ e12->setMeasurement(obs1);
+ const float &invSigmaSquare1 = pKF1->mvInvLevelSigma2[kpUn1.octave];
+ e12->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare1);
+
+ g2o::RobustKernelHuber* rk1 = new g2o::RobustKernelHuber;
+ e12->setRobustKernel(rk1);
+ rk1->setDelta(deltaHuber);
+ optimizer.addEdge(e12);
+
+ // Set edge x2 = S21*X1
+ Eigen::Matrix<double,2,1> obs2;
+ const cv::KeyPoint &kpUn2 = pKF2->mvKeysUn[i2];
+ obs2 << kpUn2.pt.x, kpUn2.pt.y;
+
+ g2o::EdgeInverseSim3ProjectXYZ* e21 = new g2o::EdgeInverseSim3ProjectXYZ();
+
+ e21->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(id1)));
+ e21->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>(optimizer.vertex(0)));
+ e21->setMeasurement(obs2);
+ float invSigmaSquare2 = pKF2->mvInvLevelSigma2[kpUn2.octave];
+ e21->setInformation(Eigen::Matrix2d::Identity()*invSigmaSquare2);
+
+ g2o::RobustKernelHuber* rk2 = new g2o::RobustKernelHuber;
+ e21->setRobustKernel(rk2);
+ rk2->setDelta(deltaHuber);
+ optimizer.addEdge(e21);
+
+ vpEdges12.push_back(e12);
+ vpEdges21.push_back(e21);
+ vnIndexEdge.push_back(i);
+ }
+
+ // Optimize!
+ optimizer.initializeOptimization();
+ optimizer.optimize(5);
+
+ // Check inliers
+ int nBad=0;
+ for(size_t i=0; i<vpEdges12.size();i++)
+ {
+ g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+ g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+ if(!e12 || !e21)
+ continue;
+
+ if(e12->chi2()>th2 || e21->chi2()>th2)
+ {
+ size_t idx = vnIndexEdge[i];
+ vpMatches1[idx]=static_cast<MapPoint*>(NULL);
+ optimizer.removeEdge(e12);
+ optimizer.removeEdge(e21);
+ vpEdges12[i]=static_cast<g2o::EdgeSim3ProjectXYZ*>(NULL);
+ vpEdges21[i]=static_cast<g2o::EdgeInverseSim3ProjectXYZ*>(NULL);
+ nBad++;
+ }
+ }
+
+ int nMoreIterations;
+ if(nBad>0)
+ nMoreIterations=10;
+ else
+ nMoreIterations=5;
+
+ if(nCorrespondences-nBad<10)
+ return 0;
+
+ // Optimize again only with inliers
+
+ optimizer.initializeOptimization();
+ optimizer.optimize(nMoreIterations);
+
+ int nIn = 0;
+ for(size_t i=0; i<vpEdges12.size();i++)
+ {
+ g2o::EdgeSim3ProjectXYZ* e12 = vpEdges12[i];
+ g2o::EdgeInverseSim3ProjectXYZ* e21 = vpEdges21[i];
+ if(!e12 || !e21)
+ continue;
+
+ if(e12->chi2()>th2 || e21->chi2()>th2)
+ {
+ size_t idx = vnIndexEdge[i];
+ vpMatches1[idx]=static_cast<MapPoint*>(NULL);
+ }
+ else
+ nIn++;
+ }
+
+ // Recover optimized Sim3
+ g2o::VertexSim3Expmap* vSim3_recov = static_cast<g2o::VertexSim3Expmap*>(optimizer.vertex(0));
+ g2oS12= vSim3_recov->estimate();
+
+ return nIn;
+}
+
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/PnPsolver.cc b/externals/ORB_SLAM2/src/PnPsolver.cc
new file mode 100644
index 0000000000000000000000000000000000000000..842aaf2421221e06e319a3fbd5bb74bae260bd11
--- /dev/null
+++ b/externals/ORB_SLAM2/src/PnPsolver.cc
@@ -0,0 +1,1022 @@
+/**
+* This file is part of ORB-SLAM2.
+* This file is a modified version of EPnP <http://cvlab.epfl.ch/EPnP/index.php>, see FreeBSD license below.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+/**
+* Copyright (c) 2009, V. Lepetit, EPFL
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions are met:
+*
+* 1. Redistributions of source code must retain the above copyright notice, this
+* list of conditions and the following disclaimer.
+* 2. Redistributions in binary form must reproduce the above copyright notice,
+* this list of conditions and the following disclaimer in the documentation
+* and/or other materials provided with the distribution.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+* ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR
+* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*
+* The views and conclusions contained in the software and documentation are those
+* of the authors and should not be interpreted as representing official policies,
+* either expressed or implied, of the FreeBSD Project
+*/
+
+#include <iostream>
+
+#include "PnPsolver.h"
+
+#include <vector>
+#include <cmath>
+#include <opencv2/core/core.hpp>
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+#include <algorithm>
+
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+
+PnPsolver::PnPsolver(const Frame &F, const vector<MapPoint*> &vpMapPointMatches):
+ pws(0), us(0), alphas(0), pcs(0), maximum_number_of_correspondences(0), number_of_correspondences(0), mnInliersi(0),
+ mnIterations(0), mnBestInliers(0), N(0)
+{
+ mvpMapPointMatches = vpMapPointMatches;
+ mvP2D.reserve(F.mvpMapPoints.size());
+ mvSigma2.reserve(F.mvpMapPoints.size());
+ mvP3Dw.reserve(F.mvpMapPoints.size());
+ mvKeyPointIndices.reserve(F.mvpMapPoints.size());
+ mvAllIndices.reserve(F.mvpMapPoints.size());
+
+ int idx=0;
+ for(size_t i=0, iend=vpMapPointMatches.size(); i<iend; i++)
+ {
+ MapPoint* pMP = vpMapPointMatches[i];
+
+ if(pMP)
+ {
+ if(!pMP->isBad())
+ {
+ const cv::KeyPoint &kp = F.mvKeysUn[i];
+
+ mvP2D.push_back(kp.pt);
+ mvSigma2.push_back(F.mvLevelSigma2[kp.octave]);
+
+ cv::Mat Pos = pMP->GetWorldPos();
+ mvP3Dw.push_back(cv::Point3f(Pos.at<float>(0),Pos.at<float>(1), Pos.at<float>(2)));
+
+ mvKeyPointIndices.push_back(i);
+ mvAllIndices.push_back(idx);
+
+ idx++;
+ }
+ }
+ }
+
+ // Set camera calibration parameters
+ fu = F.fx;
+ fv = F.fy;
+ uc = F.cx;
+ vc = F.cy;
+
+ SetRansacParameters();
+}
+
+PnPsolver::~PnPsolver()
+{
+ delete [] pws;
+ delete [] us;
+ delete [] alphas;
+ delete [] pcs;
+}
+
+
+void PnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2)
+{
+ mRansacProb = probability;
+ mRansacMinInliers = minInliers;
+ mRansacMaxIts = maxIterations;
+ mRansacEpsilon = epsilon;
+ mRansacMinSet = minSet;
+
+ N = mvP2D.size(); // number of correspondences
+
+ mvbInliersi.resize(N);
+
+ // Adjust Parameters according to number of correspondences
+ int nMinInliers = N*mRansacEpsilon;
+ if(nMinInliers<mRansacMinInliers)
+ nMinInliers=mRansacMinInliers;
+ if(nMinInliers<minSet)
+ nMinInliers=minSet;
+ mRansacMinInliers = nMinInliers;
+
+ if(mRansacEpsilon<(float)mRansacMinInliers/N)
+ mRansacEpsilon=(float)mRansacMinInliers/N;
+
+ // Set RANSAC iterations according to probability, epsilon, and max iterations
+ int nIterations;
+
+ if(mRansacMinInliers==N)
+ nIterations=1;
+ else
+ nIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3)));
+
+ mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));
+
+ mvMaxError.resize(mvSigma2.size());
+ for(size_t i=0; i<mvSigma2.size(); i++)
+ mvMaxError[i] = mvSigma2[i]*th2;
+}
+
+cv::Mat PnPsolver::find(vector<bool> &vbInliers, int &nInliers)
+{
+ bool bFlag;
+ return iterate(mRansacMaxIts,bFlag,vbInliers,nInliers);
+}
+
+cv::Mat PnPsolver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
+{
+ bNoMore = false;
+ vbInliers.clear();
+ nInliers=0;
+
+ set_maximum_number_of_correspondences(mRansacMinSet);
+
+ if(N<mRansacMinInliers)
+ {
+ bNoMore = true;
+ return cv::Mat();
+ }
+
+ vector<size_t> vAvailableIndices;
+
+ int nCurrentIterations = 0;
+ while(mnIterations<mRansacMaxIts || nCurrentIterations<nIterations)
+ {
+ nCurrentIterations++;
+ mnIterations++;
+ reset_correspondences();
+
+ vAvailableIndices = mvAllIndices;
+
+ // Get min set of points
+ for(short i = 0; i < mRansacMinSet; ++i)
+ {
+ int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
+
+ int idx = vAvailableIndices[randi];
+
+ add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);
+
+ vAvailableIndices[randi] = vAvailableIndices.back();
+ vAvailableIndices.pop_back();
+ }
+
+ // Compute camera pose
+ compute_pose(mRi, mti);
+
+ // Check inliers
+ CheckInliers();
+
+ if(mnInliersi>=mRansacMinInliers)
+ {
+ // If it is the best solution so far, save it
+ if(mnInliersi>mnBestInliers)
+ {
+ mvbBestInliers = mvbInliersi;
+ mnBestInliers = mnInliersi;
+
+ cv::Mat Rcw(3,3,CV_64F,mRi);
+ cv::Mat tcw(3,1,CV_64F,mti);
+ Rcw.convertTo(Rcw,CV_32F);
+ tcw.convertTo(tcw,CV_32F);
+ mBestTcw = cv::Mat::eye(4,4,CV_32F);
+ Rcw.copyTo(mBestTcw.rowRange(0,3).colRange(0,3));
+ tcw.copyTo(mBestTcw.rowRange(0,3).col(3));
+ }
+
+ if(Refine())
+ {
+ nInliers = mnRefinedInliers;
+ vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
+ for(int i=0; i<N; i++)
+ {
+ if(mvbRefinedInliers[i])
+ vbInliers[mvKeyPointIndices[i]] = true;
+ }
+ return mRefinedTcw.clone();
+ }
+
+ }
+ }
+
+ if(mnIterations>=mRansacMaxIts)
+ {
+ bNoMore=true;
+ if(mnBestInliers>=mRansacMinInliers)
+ {
+ nInliers=mnBestInliers;
+ vbInliers = vector<bool>(mvpMapPointMatches.size(),false);
+ for(int i=0; i<N; i++)
+ {
+ if(mvbBestInliers[i])
+ vbInliers[mvKeyPointIndices[i]] = true;
+ }
+ return mBestTcw.clone();
+ }
+ }
+
+ return cv::Mat();
+}
+
+bool PnPsolver::Refine()
+{
+ vector<int> vIndices;
+ vIndices.reserve(mvbBestInliers.size());
+
+ for(size_t i=0; i<mvbBestInliers.size(); i++)
+ {
+ if(mvbBestInliers[i])
+ {
+ vIndices.push_back(i);
+ }
+ }
+
+ set_maximum_number_of_correspondences(vIndices.size());
+
+ reset_correspondences();
+
+ for(size_t i=0; i<vIndices.size(); i++)
+ {
+ int idx = vIndices[i];
+ add_correspondence(mvP3Dw[idx].x,mvP3Dw[idx].y,mvP3Dw[idx].z,mvP2D[idx].x,mvP2D[idx].y);
+ }
+
+ // Compute camera pose
+ compute_pose(mRi, mti);
+
+ // Check inliers
+ CheckInliers();
+
+ mnRefinedInliers =mnInliersi;
+ mvbRefinedInliers = mvbInliersi;
+
+ if(mnInliersi>mRansacMinInliers)
+ {
+ cv::Mat Rcw(3,3,CV_64F,mRi);
+ cv::Mat tcw(3,1,CV_64F,mti);
+ Rcw.convertTo(Rcw,CV_32F);
+ tcw.convertTo(tcw,CV_32F);
+ mRefinedTcw = cv::Mat::eye(4,4,CV_32F);
+ Rcw.copyTo(mRefinedTcw.rowRange(0,3).colRange(0,3));
+ tcw.copyTo(mRefinedTcw.rowRange(0,3).col(3));
+ return true;
+ }
+
+ return false;
+}
+
+
+void PnPsolver::CheckInliers()
+{
+ mnInliersi=0;
+
+ for(int i=0; i<N; i++)
+ {
+ cv::Point3f P3Dw = mvP3Dw[i];
+ cv::Point2f P2D = mvP2D[i];
+
+ float Xc = mRi[0][0]*P3Dw.x+mRi[0][1]*P3Dw.y+mRi[0][2]*P3Dw.z+mti[0];
+ float Yc = mRi[1][0]*P3Dw.x+mRi[1][1]*P3Dw.y+mRi[1][2]*P3Dw.z+mti[1];
+ float invZc = 1/(mRi[2][0]*P3Dw.x+mRi[2][1]*P3Dw.y+mRi[2][2]*P3Dw.z+mti[2]);
+
+ double ue = uc + fu * Xc * invZc;
+ double ve = vc + fv * Yc * invZc;
+
+ float distX = P2D.x-ue;
+ float distY = P2D.y-ve;
+
+ float error2 = distX*distX+distY*distY;
+
+ if(error2<mvMaxError[i])
+ {
+ mvbInliersi[i]=true;
+ mnInliersi++;
+ }
+ else
+ {
+ mvbInliersi[i]=false;
+ }
+ }
+}
+
+
+void PnPsolver::set_maximum_number_of_correspondences(int n)
+{
+ if (maximum_number_of_correspondences < n) {
+ if (pws != 0) delete [] pws;
+ if (us != 0) delete [] us;
+ if (alphas != 0) delete [] alphas;
+ if (pcs != 0) delete [] pcs;
+
+ maximum_number_of_correspondences = n;
+ pws = new double[3 * maximum_number_of_correspondences];
+ us = new double[2 * maximum_number_of_correspondences];
+ alphas = new double[4 * maximum_number_of_correspondences];
+ pcs = new double[3 * maximum_number_of_correspondences];
+ }
+}
+
+void PnPsolver::reset_correspondences(void)
+{
+ number_of_correspondences = 0;
+}
+
+void PnPsolver::add_correspondence(double X, double Y, double Z, double u, double v)
+{
+ pws[3 * number_of_correspondences ] = X;
+ pws[3 * number_of_correspondences + 1] = Y;
+ pws[3 * number_of_correspondences + 2] = Z;
+
+ us[2 * number_of_correspondences ] = u;
+ us[2 * number_of_correspondences + 1] = v;
+
+ number_of_correspondences++;
+}
+
+void PnPsolver::choose_control_points(void)
+{
+ // Take C0 as the reference points centroid:
+ cws[0][0] = cws[0][1] = cws[0][2] = 0;
+ for(int i = 0; i < number_of_correspondences; i++)
+ for(int j = 0; j < 3; j++)
+ cws[0][j] += pws[3 * i + j];
+
+ for(int j = 0; j < 3; j++)
+ cws[0][j] /= number_of_correspondences;
+
+
+ // Take C1, C2, and C3 from PCA on the reference points:
+ CvMat * PW0 = cvCreateMat(number_of_correspondences, 3, CV_64F);
+
+ double pw0tpw0[3 * 3], dc[3], uct[3 * 3];
+ CvMat PW0tPW0 = cvMat(3, 3, CV_64F, pw0tpw0);
+ CvMat DC = cvMat(3, 1, CV_64F, dc);
+ CvMat UCt = cvMat(3, 3, CV_64F, uct);
+
+ for(int i = 0; i < number_of_correspondences; i++)
+ for(int j = 0; j < 3; j++)
+ PW0->data.db[3 * i + j] = pws[3 * i + j] - cws[0][j];
+
+ cvMulTransposed(PW0, &PW0tPW0, 1);
+ cvSVD(&PW0tPW0, &DC, &UCt, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
+
+ cvReleaseMat(&PW0);
+
+ for(int i = 1; i < 4; i++) {
+ double k = sqrt(dc[i - 1] / number_of_correspondences);
+ for(int j = 0; j < 3; j++)
+ cws[i][j] = cws[0][j] + k * uct[3 * (i - 1) + j];
+ }
+}
+
+void PnPsolver::compute_barycentric_coordinates(void)
+{
+ double cc[3 * 3], cc_inv[3 * 3];
+ CvMat CC = cvMat(3, 3, CV_64F, cc);
+ CvMat CC_inv = cvMat(3, 3, CV_64F, cc_inv);
+
+ for(int i = 0; i < 3; i++)
+ for(int j = 1; j < 4; j++)
+ cc[3 * i + j - 1] = cws[j][i] - cws[0][i];
+
+ cvInvert(&CC, &CC_inv, CV_SVD);
+ double * ci = cc_inv;
+ for(int i = 0; i < number_of_correspondences; i++) {
+ double * pi = pws + 3 * i;
+ double * a = alphas + 4 * i;
+
+ for(int j = 0; j < 3; j++)
+ a[1 + j] =
+ ci[3 * j ] * (pi[0] - cws[0][0]) +
+ ci[3 * j + 1] * (pi[1] - cws[0][1]) +
+ ci[3 * j + 2] * (pi[2] - cws[0][2]);
+ a[0] = 1.0f - a[1] - a[2] - a[3];
+ }
+}
+
+void PnPsolver::fill_M(CvMat * M,
+ const int row, const double * as, const double u, const double v)
+{
+ double * M1 = M->data.db + row * 12;
+ double * M2 = M1 + 12;
+
+ for(int i = 0; i < 4; i++) {
+ M1[3 * i ] = as[i] * fu;
+ M1[3 * i + 1] = 0.0;
+ M1[3 * i + 2] = as[i] * (uc - u);
+
+ M2[3 * i ] = 0.0;
+ M2[3 * i + 1] = as[i] * fv;
+ M2[3 * i + 2] = as[i] * (vc - v);
+ }
+}
+
+void PnPsolver::compute_ccs(const double * betas, const double * ut)
+{
+ for(int i = 0; i < 4; i++)
+ ccs[i][0] = ccs[i][1] = ccs[i][2] = 0.0f;
+
+ for(int i = 0; i < 4; i++) {
+ const double * v = ut + 12 * (11 - i);
+ for(int j = 0; j < 4; j++)
+ for(int k = 0; k < 3; k++)
+ ccs[j][k] += betas[i] * v[3 * j + k];
+ }
+}
+
+void PnPsolver::compute_pcs(void)
+{
+ for(int i = 0; i < number_of_correspondences; i++) {
+ double * a = alphas + 4 * i;
+ double * pc = pcs + 3 * i;
+
+ for(int j = 0; j < 3; j++)
+ pc[j] = a[0] * ccs[0][j] + a[1] * ccs[1][j] + a[2] * ccs[2][j] + a[3] * ccs[3][j];
+ }
+}
+
+double PnPsolver::compute_pose(double R[3][3], double t[3])
+{
+ choose_control_points();
+ compute_barycentric_coordinates();
+
+ CvMat * M = cvCreateMat(2 * number_of_correspondences, 12, CV_64F);
+
+ for(int i = 0; i < number_of_correspondences; i++)
+ fill_M(M, 2 * i, alphas + 4 * i, us[2 * i], us[2 * i + 1]);
+
+ double mtm[12 * 12], d[12], ut[12 * 12];
+ CvMat MtM = cvMat(12, 12, CV_64F, mtm);
+ CvMat D = cvMat(12, 1, CV_64F, d);
+ CvMat Ut = cvMat(12, 12, CV_64F, ut);
+
+ cvMulTransposed(M, &MtM, 1);
+ cvSVD(&MtM, &D, &Ut, 0, CV_SVD_MODIFY_A | CV_SVD_U_T);
+ cvReleaseMat(&M);
+
+ double l_6x10[6 * 10], rho[6];
+ CvMat L_6x10 = cvMat(6, 10, CV_64F, l_6x10);
+ CvMat Rho = cvMat(6, 1, CV_64F, rho);
+
+ compute_L_6x10(ut, l_6x10);
+ compute_rho(rho);
+
+ double Betas[4][4], rep_errors[4];
+ double Rs[4][3][3], ts[4][3];
+
+ find_betas_approx_1(&L_6x10, &Rho, Betas[1]);
+ gauss_newton(&L_6x10, &Rho, Betas[1]);
+ rep_errors[1] = compute_R_and_t(ut, Betas[1], Rs[1], ts[1]);
+
+ find_betas_approx_2(&L_6x10, &Rho, Betas[2]);
+ gauss_newton(&L_6x10, &Rho, Betas[2]);
+ rep_errors[2] = compute_R_and_t(ut, Betas[2], Rs[2], ts[2]);
+
+ find_betas_approx_3(&L_6x10, &Rho, Betas[3]);
+ gauss_newton(&L_6x10, &Rho, Betas[3]);
+ rep_errors[3] = compute_R_and_t(ut, Betas[3], Rs[3], ts[3]);
+
+ int N = 1;
+ if (rep_errors[2] < rep_errors[1]) N = 2;
+ if (rep_errors[3] < rep_errors[N]) N = 3;
+
+ copy_R_and_t(Rs[N], ts[N], R, t);
+
+ return rep_errors[N];
+}
+
+void PnPsolver::copy_R_and_t(const double R_src[3][3], const double t_src[3],
+ double R_dst[3][3], double t_dst[3])
+{
+ for(int i = 0; i < 3; i++) {
+ for(int j = 0; j < 3; j++)
+ R_dst[i][j] = R_src[i][j];
+ t_dst[i] = t_src[i];
+ }
+}
+
+double PnPsolver::dist2(const double * p1, const double * p2)
+{
+ return
+ (p1[0] - p2[0]) * (p1[0] - p2[0]) +
+ (p1[1] - p2[1]) * (p1[1] - p2[1]) +
+ (p1[2] - p2[2]) * (p1[2] - p2[2]);
+}
+
+double PnPsolver::dot(const double * v1, const double * v2)
+{
+ return v1[0] * v2[0] + v1[1] * v2[1] + v1[2] * v2[2];
+}
+
+double PnPsolver::reprojection_error(const double R[3][3], const double t[3])
+{
+ double sum2 = 0.0;
+
+ for(int i = 0; i < number_of_correspondences; i++) {
+ double * pw = pws + 3 * i;
+ double Xc = dot(R[0], pw) + t[0];
+ double Yc = dot(R[1], pw) + t[1];
+ double inv_Zc = 1.0 / (dot(R[2], pw) + t[2]);
+ double ue = uc + fu * Xc * inv_Zc;
+ double ve = vc + fv * Yc * inv_Zc;
+ double u = us[2 * i], v = us[2 * i + 1];
+
+ sum2 += sqrt( (u - ue) * (u - ue) + (v - ve) * (v - ve) );
+ }
+
+ return sum2 / number_of_correspondences;
+}
+
+void PnPsolver::estimate_R_and_t(double R[3][3], double t[3])
+{
+ double pc0[3], pw0[3];
+
+ pc0[0] = pc0[1] = pc0[2] = 0.0;
+ pw0[0] = pw0[1] = pw0[2] = 0.0;
+
+ for(int i = 0; i < number_of_correspondences; i++) {
+ const double * pc = pcs + 3 * i;
+ const double * pw = pws + 3 * i;
+
+ for(int j = 0; j < 3; j++) {
+ pc0[j] += pc[j];
+ pw0[j] += pw[j];
+ }
+ }
+ for(int j = 0; j < 3; j++) {
+ pc0[j] /= number_of_correspondences;
+ pw0[j] /= number_of_correspondences;
+ }
+
+ double abt[3 * 3], abt_d[3], abt_u[3 * 3], abt_v[3 * 3];
+ CvMat ABt = cvMat(3, 3, CV_64F, abt);
+ CvMat ABt_D = cvMat(3, 1, CV_64F, abt_d);
+ CvMat ABt_U = cvMat(3, 3, CV_64F, abt_u);
+ CvMat ABt_V = cvMat(3, 3, CV_64F, abt_v);
+
+ cvSetZero(&ABt);
+ for(int i = 0; i < number_of_correspondences; i++) {
+ double * pc = pcs + 3 * i;
+ double * pw = pws + 3 * i;
+
+ for(int j = 0; j < 3; j++) {
+ abt[3 * j ] += (pc[j] - pc0[j]) * (pw[0] - pw0[0]);
+ abt[3 * j + 1] += (pc[j] - pc0[j]) * (pw[1] - pw0[1]);
+ abt[3 * j + 2] += (pc[j] - pc0[j]) * (pw[2] - pw0[2]);
+ }
+ }
+
+ cvSVD(&ABt, &ABt_D, &ABt_U, &ABt_V, CV_SVD_MODIFY_A);
+
+ for(int i = 0; i < 3; i++)
+ for(int j = 0; j < 3; j++)
+ R[i][j] = dot(abt_u + 3 * i, abt_v + 3 * j);
+
+ const double det =
+ R[0][0] * R[1][1] * R[2][2] + R[0][1] * R[1][2] * R[2][0] + R[0][2] * R[1][0] * R[2][1] -
+ R[0][2] * R[1][1] * R[2][0] - R[0][1] * R[1][0] * R[2][2] - R[0][0] * R[1][2] * R[2][1];
+
+ if (det < 0) {
+ R[2][0] = -R[2][0];
+ R[2][1] = -R[2][1];
+ R[2][2] = -R[2][2];
+ }
+
+ t[0] = pc0[0] - dot(R[0], pw0);
+ t[1] = pc0[1] - dot(R[1], pw0);
+ t[2] = pc0[2] - dot(R[2], pw0);
+}
+
+void PnPsolver::print_pose(const double R[3][3], const double t[3])
+{
+ cout << R[0][0] << " " << R[0][1] << " " << R[0][2] << " " << t[0] << endl;
+ cout << R[1][0] << " " << R[1][1] << " " << R[1][2] << " " << t[1] << endl;
+ cout << R[2][0] << " " << R[2][1] << " " << R[2][2] << " " << t[2] << endl;
+}
+
+void PnPsolver::solve_for_sign(void)
+{
+ if (pcs[2] < 0.0) {
+ for(int i = 0; i < 4; i++)
+ for(int j = 0; j < 3; j++)
+ ccs[i][j] = -ccs[i][j];
+
+ for(int i = 0; i < number_of_correspondences; i++) {
+ pcs[3 * i ] = -pcs[3 * i];
+ pcs[3 * i + 1] = -pcs[3 * i + 1];
+ pcs[3 * i + 2] = -pcs[3 * i + 2];
+ }
+ }
+}
+
+double PnPsolver::compute_R_and_t(const double * ut, const double * betas,
+ double R[3][3], double t[3])
+{
+ compute_ccs(betas, ut);
+ compute_pcs();
+
+ solve_for_sign();
+
+ estimate_R_and_t(R, t);
+
+ return reprojection_error(R, t);
+}
+
+// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_1 = [B11 B12 B13 B14]
+
+void PnPsolver::find_betas_approx_1(const CvMat * L_6x10, const CvMat * Rho,
+ double * betas)
+{
+ double l_6x4[6 * 4], b4[4];
+ CvMat L_6x4 = cvMat(6, 4, CV_64F, l_6x4);
+ CvMat B4 = cvMat(4, 1, CV_64F, b4);
+
+ for(int i = 0; i < 6; i++) {
+ cvmSet(&L_6x4, i, 0, cvmGet(L_6x10, i, 0));
+ cvmSet(&L_6x4, i, 1, cvmGet(L_6x10, i, 1));
+ cvmSet(&L_6x4, i, 2, cvmGet(L_6x10, i, 3));
+ cvmSet(&L_6x4, i, 3, cvmGet(L_6x10, i, 6));
+ }
+
+ cvSolve(&L_6x4, Rho, &B4, CV_SVD);
+
+ if (b4[0] < 0) {
+ betas[0] = sqrt(-b4[0]);
+ betas[1] = -b4[1] / betas[0];
+ betas[2] = -b4[2] / betas[0];
+ betas[3] = -b4[3] / betas[0];
+ } else {
+ betas[0] = sqrt(b4[0]);
+ betas[1] = b4[1] / betas[0];
+ betas[2] = b4[2] / betas[0];
+ betas[3] = b4[3] / betas[0];
+ }
+}
+
+// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_2 = [B11 B12 B22 ]
+
+void PnPsolver::find_betas_approx_2(const CvMat * L_6x10, const CvMat * Rho,
+ double * betas)
+{
+ double l_6x3[6 * 3], b3[3];
+ CvMat L_6x3 = cvMat(6, 3, CV_64F, l_6x3);
+ CvMat B3 = cvMat(3, 1, CV_64F, b3);
+
+ for(int i = 0; i < 6; i++) {
+ cvmSet(&L_6x3, i, 0, cvmGet(L_6x10, i, 0));
+ cvmSet(&L_6x3, i, 1, cvmGet(L_6x10, i, 1));
+ cvmSet(&L_6x3, i, 2, cvmGet(L_6x10, i, 2));
+ }
+
+ cvSolve(&L_6x3, Rho, &B3, CV_SVD);
+
+ if (b3[0] < 0) {
+ betas[0] = sqrt(-b3[0]);
+ betas[1] = (b3[2] < 0) ? sqrt(-b3[2]) : 0.0;
+ } else {
+ betas[0] = sqrt(b3[0]);
+ betas[1] = (b3[2] > 0) ? sqrt(b3[2]) : 0.0;
+ }
+
+ if (b3[1] < 0) betas[0] = -betas[0];
+
+ betas[2] = 0.0;
+ betas[3] = 0.0;
+}
+
+// betas10 = [B11 B12 B22 B13 B23 B33 B14 B24 B34 B44]
+// betas_approx_3 = [B11 B12 B22 B13 B23 ]
+
+void PnPsolver::find_betas_approx_3(const CvMat * L_6x10, const CvMat * Rho,
+ double * betas)
+{
+ double l_6x5[6 * 5], b5[5];
+ CvMat L_6x5 = cvMat(6, 5, CV_64F, l_6x5);
+ CvMat B5 = cvMat(5, 1, CV_64F, b5);
+
+ for(int i = 0; i < 6; i++) {
+ cvmSet(&L_6x5, i, 0, cvmGet(L_6x10, i, 0));
+ cvmSet(&L_6x5, i, 1, cvmGet(L_6x10, i, 1));
+ cvmSet(&L_6x5, i, 2, cvmGet(L_6x10, i, 2));
+ cvmSet(&L_6x5, i, 3, cvmGet(L_6x10, i, 3));
+ cvmSet(&L_6x5, i, 4, cvmGet(L_6x10, i, 4));
+ }
+
+ cvSolve(&L_6x5, Rho, &B5, CV_SVD);
+
+ if (b5[0] < 0) {
+ betas[0] = sqrt(-b5[0]);
+ betas[1] = (b5[2] < 0) ? sqrt(-b5[2]) : 0.0;
+ } else {
+ betas[0] = sqrt(b5[0]);
+ betas[1] = (b5[2] > 0) ? sqrt(b5[2]) : 0.0;
+ }
+ if (b5[1] < 0) betas[0] = -betas[0];
+ betas[2] = b5[3] / betas[0];
+ betas[3] = 0.0;
+}
+
+void PnPsolver::compute_L_6x10(const double * ut, double * l_6x10)
+{
+ const double * v[4];
+
+ v[0] = ut + 12 * 11;
+ v[1] = ut + 12 * 10;
+ v[2] = ut + 12 * 9;
+ v[3] = ut + 12 * 8;
+
+ double dv[4][6][3];
+
+ for(int i = 0; i < 4; i++) {
+ int a = 0, b = 1;
+ for(int j = 0; j < 6; j++) {
+ dv[i][j][0] = v[i][3 * a ] - v[i][3 * b];
+ dv[i][j][1] = v[i][3 * a + 1] - v[i][3 * b + 1];
+ dv[i][j][2] = v[i][3 * a + 2] - v[i][3 * b + 2];
+
+ b++;
+ if (b > 3) {
+ a++;
+ b = a + 1;
+ }
+ }
+ }
+
+ for(int i = 0; i < 6; i++) {
+ double * row = l_6x10 + 10 * i;
+
+ row[0] = dot(dv[0][i], dv[0][i]);
+ row[1] = 2.0f * dot(dv[0][i], dv[1][i]);
+ row[2] = dot(dv[1][i], dv[1][i]);
+ row[3] = 2.0f * dot(dv[0][i], dv[2][i]);
+ row[4] = 2.0f * dot(dv[1][i], dv[2][i]);
+ row[5] = dot(dv[2][i], dv[2][i]);
+ row[6] = 2.0f * dot(dv[0][i], dv[3][i]);
+ row[7] = 2.0f * dot(dv[1][i], dv[3][i]);
+ row[8] = 2.0f * dot(dv[2][i], dv[3][i]);
+ row[9] = dot(dv[3][i], dv[3][i]);
+ }
+}
+
+void PnPsolver::compute_rho(double * rho)
+{
+ rho[0] = dist2(cws[0], cws[1]);
+ rho[1] = dist2(cws[0], cws[2]);
+ rho[2] = dist2(cws[0], cws[3]);
+ rho[3] = dist2(cws[1], cws[2]);
+ rho[4] = dist2(cws[1], cws[3]);
+ rho[5] = dist2(cws[2], cws[3]);
+}
+
+void PnPsolver::compute_A_and_b_gauss_newton(const double * l_6x10, const double * rho,
+ double betas[4], CvMat * A, CvMat * b)
+{
+ for(int i = 0; i < 6; i++) {
+ const double * rowL = l_6x10 + i * 10;
+ double * rowA = A->data.db + i * 4;
+
+ rowA[0] = 2 * rowL[0] * betas[0] + rowL[1] * betas[1] + rowL[3] * betas[2] + rowL[6] * betas[3];
+ rowA[1] = rowL[1] * betas[0] + 2 * rowL[2] * betas[1] + rowL[4] * betas[2] + rowL[7] * betas[3];
+ rowA[2] = rowL[3] * betas[0] + rowL[4] * betas[1] + 2 * rowL[5] * betas[2] + rowL[8] * betas[3];
+ rowA[3] = rowL[6] * betas[0] + rowL[7] * betas[1] + rowL[8] * betas[2] + 2 * rowL[9] * betas[3];
+
+ cvmSet(b, i, 0, rho[i] -
+ (
+ rowL[0] * betas[0] * betas[0] +
+ rowL[1] * betas[0] * betas[1] +
+ rowL[2] * betas[1] * betas[1] +
+ rowL[3] * betas[0] * betas[2] +
+ rowL[4] * betas[1] * betas[2] +
+ rowL[5] * betas[2] * betas[2] +
+ rowL[6] * betas[0] * betas[3] +
+ rowL[7] * betas[1] * betas[3] +
+ rowL[8] * betas[2] * betas[3] +
+ rowL[9] * betas[3] * betas[3]
+ ));
+ }
+}
+
+void PnPsolver::gauss_newton(const CvMat * L_6x10, const CvMat * Rho,
+ double betas[4])
+{
+ const int iterations_number = 5;
+
+ double a[6*4], b[6], x[4];
+ CvMat A = cvMat(6, 4, CV_64F, a);
+ CvMat B = cvMat(6, 1, CV_64F, b);
+ CvMat X = cvMat(4, 1, CV_64F, x);
+
+ for(int k = 0; k < iterations_number; k++) {
+ compute_A_and_b_gauss_newton(L_6x10->data.db, Rho->data.db,
+ betas, &A, &B);
+ qr_solve(&A, &B, &X);
+
+ for(int i = 0; i < 4; i++)
+ betas[i] += x[i];
+ }
+}
+
+void PnPsolver::qr_solve(CvMat * A, CvMat * b, CvMat * X)
+{
+ static int max_nr = 0;
+ static double * A1, * A2;
+
+ const int nr = A->rows;
+ const int nc = A->cols;
+
+ if (max_nr != 0 && max_nr < nr) {
+ delete [] A1;
+ delete [] A2;
+ }
+ if (max_nr < nr) {
+ max_nr = nr;
+ A1 = new double[nr];
+ A2 = new double[nr];
+ }
+
+ double * pA = A->data.db, * ppAkk = pA;
+ for(int k = 0; k < nc; k++) {
+ double * ppAik = ppAkk, eta = fabs(*ppAik);
+ for(int i = k + 1; i < nr; i++) {
+ double elt = fabs(*ppAik);
+ if (eta < elt) eta = elt;
+ ppAik += nc;
+ }
+
+ if (eta == 0) {
+ A1[k] = A2[k] = 0.0;
+ cerr << "God damnit, A is singular, this shouldn't happen." << endl;
+ return;
+ } else {
+ double * ppAik = ppAkk, sum = 0.0, inv_eta = 1. / eta;
+ for(int i = k; i < nr; i++) {
+ *ppAik *= inv_eta;
+ sum += *ppAik * *ppAik;
+ ppAik += nc;
+ }
+ double sigma = sqrt(sum);
+ if (*ppAkk < 0)
+ sigma = -sigma;
+ *ppAkk += sigma;
+ A1[k] = sigma * *ppAkk;
+ A2[k] = -eta * sigma;
+ for(int j = k + 1; j < nc; j++) {
+ double * ppAik = ppAkk, sum = 0;
+ for(int i = k; i < nr; i++) {
+ sum += *ppAik * ppAik[j - k];
+ ppAik += nc;
+ }
+ double tau = sum / A1[k];
+ ppAik = ppAkk;
+ for(int i = k; i < nr; i++) {
+ ppAik[j - k] -= tau * *ppAik;
+ ppAik += nc;
+ }
+ }
+ }
+ ppAkk += nc + 1;
+ }
+
+ // b <- Qt b
+ double * ppAjj = pA, * pb = b->data.db;
+ for(int j = 0; j < nc; j++) {
+ double * ppAij = ppAjj, tau = 0;
+ for(int i = j; i < nr; i++) {
+ tau += *ppAij * pb[i];
+ ppAij += nc;
+ }
+ tau /= A1[j];
+ ppAij = ppAjj;
+ for(int i = j; i < nr; i++) {
+ pb[i] -= tau * *ppAij;
+ ppAij += nc;
+ }
+ ppAjj += nc + 1;
+ }
+
+ // X = R-1 b
+ double * pX = X->data.db;
+ pX[nc - 1] = pb[nc - 1] / A2[nc - 1];
+ for(int i = nc - 2; i >= 0; i--) {
+ double * ppAij = pA + i * nc + (i + 1), sum = 0;
+
+ for(int j = i + 1; j < nc; j++) {
+ sum += *ppAij * pX[j];
+ ppAij++;
+ }
+ pX[i] = (pb[i] - sum) / A2[i];
+ }
+}
+
+
+
+void PnPsolver::relative_error(double & rot_err, double & transl_err,
+ const double Rtrue[3][3], const double ttrue[3],
+ const double Rest[3][3], const double test[3])
+{
+ double qtrue[4], qest[4];
+
+ mat_to_quat(Rtrue, qtrue);
+ mat_to_quat(Rest, qest);
+
+ double rot_err1 = sqrt((qtrue[0] - qest[0]) * (qtrue[0] - qest[0]) +
+ (qtrue[1] - qest[1]) * (qtrue[1] - qest[1]) +
+ (qtrue[2] - qest[2]) * (qtrue[2] - qest[2]) +
+ (qtrue[3] - qest[3]) * (qtrue[3] - qest[3]) ) /
+ sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
+
+ double rot_err2 = sqrt((qtrue[0] + qest[0]) * (qtrue[0] + qest[0]) +
+ (qtrue[1] + qest[1]) * (qtrue[1] + qest[1]) +
+ (qtrue[2] + qest[2]) * (qtrue[2] + qest[2]) +
+ (qtrue[3] + qest[3]) * (qtrue[3] + qest[3]) ) /
+ sqrt(qtrue[0] * qtrue[0] + qtrue[1] * qtrue[1] + qtrue[2] * qtrue[2] + qtrue[3] * qtrue[3]);
+
+ rot_err = min(rot_err1, rot_err2);
+
+ transl_err =
+ sqrt((ttrue[0] - test[0]) * (ttrue[0] - test[0]) +
+ (ttrue[1] - test[1]) * (ttrue[1] - test[1]) +
+ (ttrue[2] - test[2]) * (ttrue[2] - test[2])) /
+ sqrt(ttrue[0] * ttrue[0] + ttrue[1] * ttrue[1] + ttrue[2] * ttrue[2]);
+}
+
+void PnPsolver::mat_to_quat(const double R[3][3], double q[4])
+{
+ double tr = R[0][0] + R[1][1] + R[2][2];
+ double n4;
+
+ if (tr > 0.0f) {
+ q[0] = R[1][2] - R[2][1];
+ q[1] = R[2][0] - R[0][2];
+ q[2] = R[0][1] - R[1][0];
+ q[3] = tr + 1.0f;
+ n4 = q[3];
+ } else if ( (R[0][0] > R[1][1]) && (R[0][0] > R[2][2]) ) {
+ q[0] = 1.0f + R[0][0] - R[1][1] - R[2][2];
+ q[1] = R[1][0] + R[0][1];
+ q[2] = R[2][0] + R[0][2];
+ q[3] = R[1][2] - R[2][1];
+ n4 = q[0];
+ } else if (R[1][1] > R[2][2]) {
+ q[0] = R[1][0] + R[0][1];
+ q[1] = 1.0f + R[1][1] - R[0][0] - R[2][2];
+ q[2] = R[2][1] + R[1][2];
+ q[3] = R[2][0] - R[0][2];
+ n4 = q[1];
+ } else {
+ q[0] = R[2][0] + R[0][2];
+ q[1] = R[2][1] + R[1][2];
+ q[2] = 1.0f + R[2][2] - R[0][0] - R[1][1];
+ q[3] = R[0][1] - R[1][0];
+ n4 = q[2];
+ }
+ double scale = 0.5f / double(sqrt(n4));
+
+ q[0] *= scale;
+ q[1] *= scale;
+ q[2] *= scale;
+ q[3] *= scale;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Sim3Solver.cc b/externals/ORB_SLAM2/src/Sim3Solver.cc
new file mode 100644
index 0000000000000000000000000000000000000000..90e8718515a1df5dfa72627904891f0464501daa
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Sim3Solver.cc
@@ -0,0 +1,425 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Sim3Solver.h"
+
+#include <vector>
+#include <cmath>
+#include <opencv2/core/core.hpp>
+
+#include "KeyFrame.h"
+#include "ORBmatcher.h"
+
+#include "Thirdparty/DBoW2/DUtils/Random.h"
+
+namespace ORB_SLAM2
+{
+
+
+Sim3Solver::Sim3Solver(KeyFrame *pKF1, KeyFrame *pKF2, const vector<MapPoint *> &vpMatched12, const bool bFixScale):
+ mnIterations(0), mnBestInliers(0), mbFixScale(bFixScale)
+{
+ mpKF1 = pKF1;
+ mpKF2 = pKF2;
+
+ vector<MapPoint*> vpKeyFrameMP1 = pKF1->GetMapPointMatches();
+
+ mN1 = vpMatched12.size();
+
+ mvpMapPoints1.reserve(mN1);
+ mvpMapPoints2.reserve(mN1);
+ mvpMatches12 = vpMatched12;
+ mvnIndices1.reserve(mN1);
+ mvX3Dc1.reserve(mN1);
+ mvX3Dc2.reserve(mN1);
+
+ cv::Mat Rcw1 = pKF1->GetRotation();
+ cv::Mat tcw1 = pKF1->GetTranslation();
+ cv::Mat Rcw2 = pKF2->GetRotation();
+ cv::Mat tcw2 = pKF2->GetTranslation();
+
+ mvAllIndices.reserve(mN1);
+
+ size_t idx=0;
+ for(int i1=0; i1<mN1; i1++)
+ {
+ if(vpMatched12[i1])
+ {
+ MapPoint* pMP1 = vpKeyFrameMP1[i1];
+ MapPoint* pMP2 = vpMatched12[i1];
+
+ if(!pMP1)
+ continue;
+
+ if(pMP1->isBad() || pMP2->isBad())
+ continue;
+
+ int indexKF1 = pMP1->GetIndexInKeyFrame(pKF1);
+ int indexKF2 = pMP2->GetIndexInKeyFrame(pKF2);
+
+ if(indexKF1<0 || indexKF2<0)
+ continue;
+
+ const cv::KeyPoint &kp1 = pKF1->mvKeysUn[indexKF1];
+ const cv::KeyPoint &kp2 = pKF2->mvKeysUn[indexKF2];
+
+ const float sigmaSquare1 = pKF1->mvLevelSigma2[kp1.octave];
+ const float sigmaSquare2 = pKF2->mvLevelSigma2[kp2.octave];
+
+ mvnMaxError1.push_back(9.210*sigmaSquare1);
+ mvnMaxError2.push_back(9.210*sigmaSquare2);
+
+ mvpMapPoints1.push_back(pMP1);
+ mvpMapPoints2.push_back(pMP2);
+ mvnIndices1.push_back(i1);
+
+ cv::Mat X3D1w = pMP1->GetWorldPos();
+ mvX3Dc1.push_back(Rcw1*X3D1w+tcw1);
+
+ cv::Mat X3D2w = pMP2->GetWorldPos();
+ mvX3Dc2.push_back(Rcw2*X3D2w+tcw2);
+
+ mvAllIndices.push_back(idx);
+ idx++;
+ }
+ }
+
+ mK1 = pKF1->mK;
+ mK2 = pKF2->mK;
+
+ FromCameraToImage(mvX3Dc1,mvP1im1,mK1);
+ FromCameraToImage(mvX3Dc2,mvP2im2,mK2);
+
+ SetRansacParameters();
+}
+
+void Sim3Solver::SetRansacParameters(double probability, int minInliers, int maxIterations)
+{
+ mRansacProb = probability;
+ mRansacMinInliers = minInliers;
+ mRansacMaxIts = maxIterations;
+
+ N = mvpMapPoints1.size(); // number of correspondences
+
+ mvbInliersi.resize(N);
+
+ // Adjust Parameters according to number of correspondences
+ float epsilon = (float)mRansacMinInliers/N;
+
+ // Set RANSAC iterations according to probability, epsilon, and max iterations
+ int nIterations;
+
+ if(mRansacMinInliers==N)
+ nIterations=1;
+ else
+ nIterations = ceil(log(1-mRansacProb)/log(1-pow(epsilon,3)));
+
+ mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts));
+
+ mnIterations = 0;
+}
+
+cv::Mat Sim3Solver::iterate(int nIterations, bool &bNoMore, vector<bool> &vbInliers, int &nInliers)
+{
+ bNoMore = false;
+ vbInliers = vector<bool>(mN1,false);
+ nInliers=0;
+
+ if(N<mRansacMinInliers)
+ {
+ bNoMore = true;
+ return cv::Mat();
+ }
+
+ vector<size_t> vAvailableIndices;
+
+ cv::Mat P3Dc1i(3,3,CV_32F);
+ cv::Mat P3Dc2i(3,3,CV_32F);
+
+ int nCurrentIterations = 0;
+ while(mnIterations<mRansacMaxIts && nCurrentIterations<nIterations)
+ {
+ nCurrentIterations++;
+ mnIterations++;
+
+ vAvailableIndices = mvAllIndices;
+
+ // Get min set of points
+ for(short i = 0; i < 3; ++i)
+ {
+ int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size()-1);
+
+ int idx = vAvailableIndices[randi];
+
+ mvX3Dc1[idx].copyTo(P3Dc1i.col(i));
+ mvX3Dc2[idx].copyTo(P3Dc2i.col(i));
+
+ vAvailableIndices[randi] = vAvailableIndices.back();
+ vAvailableIndices.pop_back();
+ }
+
+ ComputeSim3(P3Dc1i,P3Dc2i);
+
+ CheckInliers();
+
+ if(mnInliersi>=mnBestInliers)
+ {
+ mvbBestInliers = mvbInliersi;
+ mnBestInliers = mnInliersi;
+ mBestT12 = mT12i.clone();
+ mBestRotation = mR12i.clone();
+ mBestTranslation = mt12i.clone();
+ mBestScale = ms12i;
+
+ if(mnInliersi>mRansacMinInliers)
+ {
+ nInliers = mnInliersi;
+ for(int i=0; i<N; i++)
+ if(mvbInliersi[i])
+ vbInliers[mvnIndices1[i]] = true;
+ return mBestT12;
+ }
+ }
+ }
+
+ if(mnIterations>=mRansacMaxIts)
+ bNoMore=true;
+
+ return cv::Mat();
+}
+
+cv::Mat Sim3Solver::find(vector<bool> &vbInliers12, int &nInliers)
+{
+ bool bFlag;
+ return iterate(mRansacMaxIts,bFlag,vbInliers12,nInliers);
+}
+
+void Sim3Solver::ComputeCentroid(cv::Mat &P, cv::Mat &Pr, cv::Mat &C)
+{
+ cv::reduce(P,C,1,CV_REDUCE_SUM);
+ C = C/P.cols;
+
+ for(int i=0; i<P.cols; i++)
+ {
+ Pr.col(i)=P.col(i)-C;
+ }
+}
+
+void Sim3Solver::ComputeSim3(cv::Mat &P1, cv::Mat &P2)
+{
+ // Custom implementation of:
+ // Horn 1987, Closed-form solution of absolute orientataion using unit quaternions
+
+ // Step 1: Centroid and relative coordinates
+
+ cv::Mat Pr1(P1.size(),P1.type()); // Relative coordinates to centroid (set 1)
+ cv::Mat Pr2(P2.size(),P2.type()); // Relative coordinates to centroid (set 2)
+ cv::Mat O1(3,1,Pr1.type()); // Centroid of P1
+ cv::Mat O2(3,1,Pr2.type()); // Centroid of P2
+
+ ComputeCentroid(P1,Pr1,O1);
+ ComputeCentroid(P2,Pr2,O2);
+
+ // Step 2: Compute M matrix
+
+ cv::Mat M = Pr2*Pr1.t();
+
+ // Step 3: Compute N matrix
+
+ double N11, N12, N13, N14, N22, N23, N24, N33, N34, N44;
+
+ cv::Mat N(4,4,P1.type());
+
+ N11 = M.at<float>(0,0)+M.at<float>(1,1)+M.at<float>(2,2);
+ N12 = M.at<float>(1,2)-M.at<float>(2,1);
+ N13 = M.at<float>(2,0)-M.at<float>(0,2);
+ N14 = M.at<float>(0,1)-M.at<float>(1,0);
+ N22 = M.at<float>(0,0)-M.at<float>(1,1)-M.at<float>(2,2);
+ N23 = M.at<float>(0,1)+M.at<float>(1,0);
+ N24 = M.at<float>(2,0)+M.at<float>(0,2);
+ N33 = -M.at<float>(0,0)+M.at<float>(1,1)-M.at<float>(2,2);
+ N34 = M.at<float>(1,2)+M.at<float>(2,1);
+ N44 = -M.at<float>(0,0)-M.at<float>(1,1)+M.at<float>(2,2);
+
+ N = (cv::Mat_<float>(4,4) << N11, N12, N13, N14,
+ N12, N22, N23, N24,
+ N13, N23, N33, N34,
+ N14, N24, N34, N44);
+
+
+ // Step 4: Eigenvector of the highest eigenvalue
+
+ cv::Mat eval, evec;
+
+ cv::eigen(N,eval,evec); //evec[0] is the quaternion of the desired rotation
+
+ cv::Mat vec(1,3,evec.type());
+ (evec.row(0).colRange(1,4)).copyTo(vec); //extract imaginary part of the quaternion (sin*axis)
+
+ // Rotation angle. sin is the norm of the imaginary part, cos is the real part
+ double ang=atan2(norm(vec),evec.at<float>(0,0));
+
+ vec = 2*ang*vec/norm(vec); //Angle-axis representation. quaternion angle is the half
+
+ mR12i.create(3,3,P1.type());
+
+ cv::Rodrigues(vec,mR12i); // computes the rotation matrix from angle-axis
+
+ // Step 5: Rotate set 2
+
+ cv::Mat P3 = mR12i*Pr2;
+
+ // Step 6: Scale
+
+ if(!mbFixScale)
+ {
+ double nom = Pr1.dot(P3);
+ cv::Mat aux_P3(P3.size(),P3.type());
+ aux_P3=P3;
+ cv::pow(P3,2,aux_P3);
+ double den = 0;
+
+ for(int i=0; i<aux_P3.rows; i++)
+ {
+ for(int j=0; j<aux_P3.cols; j++)
+ {
+ den+=aux_P3.at<float>(i,j);
+ }
+ }
+
+ ms12i = nom/den;
+ }
+ else
+ ms12i = 1.0f;
+
+ // Step 7: Translation
+
+ mt12i.create(1,3,P1.type());
+ mt12i = O1 - ms12i*mR12i*O2;
+
+ // Step 8: Transformation
+
+ // Step 8.1 T12
+ mT12i = cv::Mat::eye(4,4,P1.type());
+
+ cv::Mat sR = ms12i*mR12i;
+
+ sR.copyTo(mT12i.rowRange(0,3).colRange(0,3));
+ mt12i.copyTo(mT12i.rowRange(0,3).col(3));
+
+ // Step 8.2 T21
+
+ mT21i = cv::Mat::eye(4,4,P1.type());
+
+ cv::Mat sRinv = (1.0/ms12i)*mR12i.t();
+
+ sRinv.copyTo(mT21i.rowRange(0,3).colRange(0,3));
+ cv::Mat tinv = -sRinv*mt12i;
+ tinv.copyTo(mT21i.rowRange(0,3).col(3));
+}
+
+
+void Sim3Solver::CheckInliers()
+{
+ vector<cv::Mat> vP1im2, vP2im1;
+ Project(mvX3Dc2,vP2im1,mT12i,mK1);
+ Project(mvX3Dc1,vP1im2,mT21i,mK2);
+
+ mnInliersi=0;
+
+ for(size_t i=0; i<mvP1im1.size(); i++)
+ {
+ cv::Mat dist1 = mvP1im1[i]-vP2im1[i];
+ cv::Mat dist2 = vP1im2[i]-mvP2im2[i];
+
+ const float err1 = dist1.dot(dist1);
+ const float err2 = dist2.dot(dist2);
+
+ if(err1<mvnMaxError1[i] && err2<mvnMaxError2[i])
+ {
+ mvbInliersi[i]=true;
+ mnInliersi++;
+ }
+ else
+ mvbInliersi[i]=false;
+ }
+}
+
+
+cv::Mat Sim3Solver::GetEstimatedRotation()
+{
+ return mBestRotation.clone();
+}
+
+cv::Mat Sim3Solver::GetEstimatedTranslation()
+{
+ return mBestTranslation.clone();
+}
+
+float Sim3Solver::GetEstimatedScale()
+{
+ return mBestScale;
+}
+
+void Sim3Solver::Project(const vector<cv::Mat> &vP3Dw, vector<cv::Mat> &vP2D, cv::Mat Tcw, cv::Mat K)
+{
+ cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
+ cv::Mat tcw = Tcw.rowRange(0,3).col(3);
+ const float &fx = K.at<float>(0,0);
+ const float &fy = K.at<float>(1,1);
+ const float &cx = K.at<float>(0,2);
+ const float &cy = K.at<float>(1,2);
+
+ vP2D.clear();
+ vP2D.reserve(vP3Dw.size());
+
+ for(size_t i=0, iend=vP3Dw.size(); i<iend; i++)
+ {
+ cv::Mat P3Dc = Rcw*vP3Dw[i]+tcw;
+ const float invz = 1/(P3Dc.at<float>(2));
+ const float x = P3Dc.at<float>(0)*invz;
+ const float y = P3Dc.at<float>(1)*invz;
+
+ vP2D.push_back((cv::Mat_<float>(2,1) << fx*x+cx, fy*y+cy));
+ }
+}
+
+void Sim3Solver::FromCameraToImage(const vector<cv::Mat> &vP3Dc, vector<cv::Mat> &vP2D, cv::Mat K)
+{
+ const float &fx = K.at<float>(0,0);
+ const float &fy = K.at<float>(1,1);
+ const float &cx = K.at<float>(0,2);
+ const float &cy = K.at<float>(1,2);
+
+ vP2D.clear();
+ vP2D.reserve(vP3Dc.size());
+
+ for(size_t i=0, iend=vP3Dc.size(); i<iend; i++)
+ {
+ const float invz = 1/(vP3Dc[i].at<float>(2));
+ const float x = vP3Dc[i].at<float>(0)*invz;
+ const float y = vP3Dc[i].at<float>(1)*invz;
+
+ vP2D.push_back((cv::Mat_<float>(2,1) << fx*x+cx, fy*y+cy));
+ }
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/System.cc b/externals/ORB_SLAM2/src/System.cc
new file mode 100644
index 0000000000000000000000000000000000000000..923b4e463df1a11c5ce84d9453dc3cb2d2765481
--- /dev/null
+++ b/externals/ORB_SLAM2/src/System.cc
@@ -0,0 +1,482 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+
+#include "System.h"
+#include "Converter.h"
+#include <thread>
+#include <pangolin/pangolin.h>
+#include <iomanip>
+
+namespace ORB_SLAM2
+{
+
+System::System(ORBVocabulary *voc, const QJsonObject settingsFile, const eSensor sensor,
+ const bool bUseViewer, const bool bLoopClosing)
+ : mSensor(sensor), mpVocabulary(voc), mpViewer(static_cast<Viewer*>(NULL)), mbReset(false),mbActivateLocalizationMode(false),
+ mbDeactivateLocalizationMode(false), mLoopClosing(bLoopClosing)
+{
+ // Output welcome message
+ cout << endl <<
+ "ORB-SLAM2 Copyright (C) 2014-2016 Raul Mur-Artal, University of Zaragoza." << endl <<
+ "This program comes with ABSOLUTELY NO WARRANTY;" << endl <<
+ "This is free software, and you are welcome to redistribute it" << endl <<
+ "under certain conditions. See LICENSE.txt." << endl << endl;
+
+ cout << "Input sensor was set to: ";
+
+ if(mSensor==MONOCULAR)
+ cout << "Monocular" << endl;
+ else if(mSensor==STEREO)
+ cout << "Stereo" << endl;
+ else if(mSensor==RGBD)
+ cout << "RGB-D" << endl;
+
+ //Check settings file
+ if (settingsFile.isEmpty())
+ {
+ cerr << "Failed to open settings file." << endl;
+ exit(-1);
+ }
+
+ //Create KeyFrame Database
+ mpKeyFrameDatabase = new KeyFrameDatabase(*mpVocabulary);
+
+ //Create the Map
+ mpMap = new Map();
+
+ //Create Drawers. These are used by the Viewer
+ mpFrameDrawer = new FrameDrawer(mpMap);
+ mpMapDrawer = new MapDrawer();
+
+ //Initialize the Tracking thread
+ //(it will live in the main thread of execution, the one that called this constructor)
+ mpTracker = new Tracking(this, mpVocabulary, mpFrameDrawer, mpMapDrawer,
+ mpMap, mpKeyFrameDatabase, settingsFile, mSensor);
+
+ //Initialize the Local Mapping thread and launch
+ mpLocalMapper = new LocalMapping(mpMap, mSensor==MONOCULAR);
+ mptLocalMapping = new thread(&ORB_SLAM2::LocalMapping::Run,mpLocalMapper);
+
+ //Initialize the Loop Closing thread and launch
+ mptLoopClosing = nullptr;
+ mpLoopCloser = new LoopClosing(mpMap, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR);
+ if (mLoopClosing) {
+ mptLoopClosing = new thread(&ORB_SLAM2::LoopClosing::Run, mpLoopCloser);
+ }
+
+ //Initialize the Viewer thread and launch
+ if(bUseViewer)
+ {
+ mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,"");
+ mptViewer = new thread(&Viewer::Run, mpViewer);
+ mpTracker->SetViewer(mpViewer);
+ }
+
+ //Set pointers between threads
+ mpTracker->SetLocalMapper(mpLocalMapper);
+ mpTracker->SetLoopClosing(mpLoopCloser);
+
+ mpLocalMapper->SetTracker(mpTracker);
+ mpLocalMapper->SetLoopCloser(mpLoopCloser);
+
+ mpLoopCloser->SetTracker(mpTracker);
+ mpLoopCloser->SetLocalMapper(mpLocalMapper);
+}
+
+cv::Mat System::TrackStereo(const cv::Mat &imLeft, const cv::Mat &imRight, const double ×tamp)
+{
+ if(mSensor!=STEREO)
+ {
+ cerr << "ERROR: you called TrackStereo but input sensor was not set to STEREO." << endl;
+ exit(-1);
+ }
+
+ // Check mode change
+ {
+ unique_lock<mutex> lock(mMutexMode);
+ if(mbActivateLocalizationMode)
+ {
+ mpLocalMapper->RequestStop();
+
+ // Wait until Local Mapping has effectively stopped
+ while(!mpLocalMapper->isStopped())
+ {
+ usleep(1000);
+ }
+
+ mpTracker->InformOnlyTracking(true);
+ mbActivateLocalizationMode = false;
+ }
+ if(mbDeactivateLocalizationMode)
+ {
+ mpTracker->InformOnlyTracking(false);
+ mpLocalMapper->Release();
+ mbDeactivateLocalizationMode = false;
+ }
+ }
+
+ // Check reset
+ {
+ unique_lock<mutex> lock(mMutexReset);
+ if(mbReset)
+ {
+ mpTracker->Reset();
+ mbReset = false;
+ }
+ }
+
+ cv::Mat Tcw = mpTracker->GrabImageStereo(imLeft,imRight,timestamp);
+
+ unique_lock<mutex> lock2(mMutexState);
+ mTrackingState = mpTracker->mState;
+ mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+ mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+ return Tcw;
+}
+
+cv::Mat System::TrackRGBD(const cv::Mat &im, const cv::Mat &depthmap, const double ×tamp)
+{
+ if(mSensor!=RGBD)
+ {
+ cerr << "ERROR: you called TrackRGBD but input sensor was not set to RGBD." << endl;
+ exit(-1);
+ }
+
+ // Check mode change
+ {
+ unique_lock<mutex> lock(mMutexMode);
+ if(mbActivateLocalizationMode)
+ {
+ mpLocalMapper->RequestStop();
+
+ // Wait until Local Mapping has effectively stopped
+ while(!mpLocalMapper->isStopped())
+ {
+ usleep(1000);
+ }
+
+ mpTracker->InformOnlyTracking(true);
+ mbActivateLocalizationMode = false;
+ }
+ if(mbDeactivateLocalizationMode)
+ {
+ mpTracker->InformOnlyTracking(false);
+ mpLocalMapper->Release();
+ mbDeactivateLocalizationMode = false;
+ }
+ }
+
+ // Check reset
+ {
+ unique_lock<mutex> lock(mMutexReset);
+ if(mbReset)
+ {
+ mpTracker->Reset();
+ mbReset = false;
+ }
+ }
+
+ cv::Mat Tcw = mpTracker->GrabImageRGBD(im,depthmap,timestamp);
+
+ unique_lock<mutex> lock2(mMutexState);
+ mTrackingState = mpTracker->mState;
+ mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+ mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+ return Tcw;
+}
+
+cv::Mat System::TrackMonocular(const cv::Mat &im, const double ×tamp)
+{
+ if(mSensor!=MONOCULAR)
+ {
+ cerr << "ERROR: you called TrackMonocular but input sensor was not set to Monocular." << endl;
+ exit(-1);
+ }
+
+ // Check mode change
+ {
+ unique_lock<mutex> lock(mMutexMode);
+ if(mbActivateLocalizationMode)
+ {
+ mpLocalMapper->RequestStop();
+
+ // Wait until Local Mapping has effectively stopped
+ while(!mpLocalMapper->isStopped())
+ {
+ usleep(1000);
+ }
+
+ mpTracker->InformOnlyTracking(true);
+ mbActivateLocalizationMode = false;
+ }
+ if(mbDeactivateLocalizationMode)
+ {
+ mpTracker->InformOnlyTracking(false);
+ mpLocalMapper->Release();
+ mbDeactivateLocalizationMode = false;
+ }
+ }
+
+ // Check reset
+ {
+ unique_lock<mutex> lock(mMutexReset);
+ if(mbReset)
+ {
+ mpTracker->Reset();
+ mbReset = false;
+ }
+ }
+
+ cv::Mat Tcw = mpTracker->GrabImageMonocular(im,timestamp);
+
+ unique_lock<mutex> lock2(mMutexState);
+ mTrackingState = mpTracker->mState;
+ mTrackedMapPoints = mpTracker->mCurrentFrame.mvpMapPoints;
+ mTrackedKeyPointsUn = mpTracker->mCurrentFrame.mvKeysUn;
+
+ return Tcw;
+}
+
+void System::ActivateLocalizationMode()
+{
+ unique_lock<mutex> lock(mMutexMode);
+ mbActivateLocalizationMode = true;
+}
+
+void System::DeactivateLocalizationMode()
+{
+ unique_lock<mutex> lock(mMutexMode);
+ mbDeactivateLocalizationMode = true;
+}
+
+bool System::MapChanged()
+{
+ static int n=0;
+ int curn = mpMap->GetLastBigChangeIdx();
+ if(n<curn)
+ {
+ n=curn;
+ return true;
+ }
+ else
+ return false;
+}
+
+void System::Reset()
+{
+ unique_lock<mutex> lock(mMutexReset);
+ mbReset = true;
+}
+
+void System::Shutdown()
+{
+ mpLocalMapper->RequestFinish();
+ if (mptLoopClosing) {
+ mpLoopCloser->RequestFinish();
+ }
+ if(mpViewer)
+ {
+ mpViewer->RequestFinish();
+ while(!mpViewer->isFinished())
+ usleep(5000);
+ }
+
+ // Wait until all thread have effectively stopped
+ while(!mpLocalMapper->isFinished() ||
+ (mptLoopClosing && !mpLoopCloser->isFinished()) ||
+ mpLoopCloser->isRunningGBA())
+ {
+ usleep(5000);
+ }
+}
+
+void System::SaveTrajectoryTUM(const string &filename)
+{
+ cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
+ if(mSensor==MONOCULAR)
+ {
+ cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
+ return;
+ }
+
+ vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
+ sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+ // Transform all keyframes so that the first keyframe is at the origin.
+ // After a loop closure the first keyframe might not be at the origin.
+ cv::Mat Two = vpKFs[0]->GetPoseInverse();
+
+ ofstream f;
+ f.open(filename.c_str());
+ f << fixed;
+
+ // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
+ // We need to get first the keyframe pose and then concatenate the relative transformation.
+ // Frames not localized (tracking failure) are not saved.
+
+ // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
+ // which is true when tracking failed (lbL).
+ list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
+ list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
+ list<bool>::iterator lbL = mpTracker->mlbLost.begin();
+ for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
+ lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
+ {
+ if(*lbL)
+ continue;
+
+ KeyFrame* pKF = *lRit;
+
+ cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
+
+ // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
+ while(pKF->isBad())
+ {
+ Trw = Trw*pKF->mTcp;
+ pKF = pKF->GetParent();
+ }
+
+ Trw = Trw*pKF->GetPose()*Two;
+
+ cv::Mat Tcw = (*lit)*Trw;
+ cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+ cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+
+ vector<float> q = Converter::toQuaternion(Rwc);
+
+ f << setprecision(6) << *lT << " " << setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+ }
+ f.close();
+ cout << endl << "trajectory saved!" << endl;
+}
+
+
+void System::SaveKeyFrameTrajectoryTUM(const string &filename)
+{
+ cout << endl << "Saving keyframe trajectory to " << filename << " ..." << endl;
+
+ vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
+ sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+ // Transform all keyframes so that the first keyframe is at the origin.
+ // After a loop closure the first keyframe might not be at the origin.
+ //cv::Mat Two = vpKFs[0]->GetPoseInverse();
+
+ ofstream f;
+ f.open(filename.c_str());
+ f << fixed;
+
+ for(size_t i=0; i<vpKFs.size(); i++)
+ {
+ KeyFrame* pKF = vpKFs[i];
+
+ // pKF->SetPose(pKF->GetPose()*Two);
+
+ if(pKF->isBad())
+ continue;
+
+ cv::Mat R = pKF->GetRotation().t();
+ vector<float> q = Converter::toQuaternion(R);
+ cv::Mat t = pKF->GetCameraCenter();
+ f << setprecision(6) << pKF->mTimeStamp << setprecision(7) << " " << t.at<float>(0) << " " << t.at<float>(1) << " " << t.at<float>(2)
+ << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
+
+ }
+
+ f.close();
+ cout << endl << "trajectory saved!" << endl;
+}
+
+void System::SaveTrajectoryKITTI(const string &filename)
+{
+ cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
+ if(mSensor==MONOCULAR)
+ {
+ cerr << "ERROR: SaveTrajectoryKITTI cannot be used for monocular." << endl;
+ return;
+ }
+
+ vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
+ sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
+
+ // Transform all keyframes so that the first keyframe is at the origin.
+ // After a loop closure the first keyframe might not be at the origin.
+ cv::Mat Two = vpKFs[0]->GetPoseInverse();
+
+ ofstream f;
+ f.open(filename.c_str());
+ f << fixed;
+
+ // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
+ // We need to get first the keyframe pose and then concatenate the relative transformation.
+ // Frames not localized (tracking failure) are not saved.
+
+ // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
+ // which is true when tracking failed (lbL).
+ list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
+ list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
+ for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(), lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++)
+ {
+ ORB_SLAM2::KeyFrame* pKF = *lRit;
+
+ cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
+
+ while(pKF->isBad())
+ {
+ // cout << "bad parent" << endl;
+ Trw = Trw*pKF->mTcp;
+ pKF = pKF->GetParent();
+ }
+
+ Trw = Trw*pKF->GetPose()*Two;
+
+ cv::Mat Tcw = (*lit)*Trw;
+ cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
+ cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
+
+ f << setprecision(9) << Rwc.at<float>(0,0) << " " << Rwc.at<float>(0,1) << " " << Rwc.at<float>(0,2) << " " << twc.at<float>(0) << " " <<
+ Rwc.at<float>(1,0) << " " << Rwc.at<float>(1,1) << " " << Rwc.at<float>(1,2) << " " << twc.at<float>(1) << " " <<
+ Rwc.at<float>(2,0) << " " << Rwc.at<float>(2,1) << " " << Rwc.at<float>(2,2) << " " << twc.at<float>(2) << endl;
+ }
+ f.close();
+ cout << endl << "trajectory saved!" << endl;
+}
+
+int System::GetTrackingState()
+{
+ unique_lock<mutex> lock(mMutexState);
+ return mTrackingState;
+}
+
+vector<MapPoint*> System::GetTrackedMapPoints()
+{
+ unique_lock<mutex> lock(mMutexState);
+ return mTrackedMapPoints;
+}
+
+vector<cv::KeyPoint> System::GetTrackedKeyPointsUn()
+{
+ unique_lock<mutex> lock(mMutexState);
+ return mTrackedKeyPointsUn;
+}
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Tracking.cc b/externals/ORB_SLAM2/src/Tracking.cc
new file mode 100644
index 0000000000000000000000000000000000000000..403351fa18fc0f89314abf366f3cc76f674e1ce4
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Tracking.cc
@@ -0,0 +1,1592 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+
+#include "Tracking.h"
+
+#include<opencv2/core/core.hpp>
+#include<opencv2/features2d/features2d.hpp>
+
+#include"ORBmatcher.h"
+#include"FrameDrawer.h"
+#include"Converter.h"
+#include"Map.h"
+#include"Initializer.h"
+
+#include"Optimizer.h"
+#include"PnPsolver.h"
+
+#include<iostream>
+
+#include<mutex>
+
+using namespace std;
+
+namespace ORB_SLAM2
+{
+
+Tracking::Tracking(System *pSys, ORBVocabulary* pVoc, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Map *pMap, KeyFrameDatabase* pKFDB, const QJsonObject settings, const int sensor):
+ mState(NO_IMAGES_YET), mSensor(sensor), mbOnlyTracking(false), mbVO(false), mpORBVocabulary(pVoc),
+ mpKeyFrameDB(pKFDB), mpInitializer(static_cast<Initializer*>(NULL)), mpSystem(pSys), mpViewer(NULL),
+ mpFrameDrawer(pFrameDrawer), mpMapDrawer(pMapDrawer), mpMap(pMap), mnLastRelocFrameId(0)
+{
+ // Load camera parameters from settings file
+
+
+ float fx = static_cast<float>(settings.value("Camera.fx").toDouble(517.306408));
+ float fy = static_cast<float>(settings.value("Camera.fy").toDouble(516.469215));
+ float cx = static_cast<float>(settings.value("Camera.cx").toDouble(318.643040));
+ float cy = static_cast<float>(settings.value("Camera.cy").toDouble(255.313989));
+
+ cv::Mat K = cv::Mat::eye(3,3,CV_32F);
+ K.at<float>(0,0) = fx;
+ K.at<float>(1,1) = fy;
+ K.at<float>(0,2) = cx;
+ K.at<float>(1,2) = cy;
+ K.copyTo(mK);
+
+ cv::Mat DistCoef(4,1,CV_32F);
+ DistCoef.at<float>(0) = static_cast<float>(settings.value("Camera.k1").toDouble(0.262383));
+ DistCoef.at<float>(1) = static_cast<float>(settings.value("Camera.k2").toDouble(-0.953104));
+ DistCoef.at<float>(2) = static_cast<float>(settings.value("Camera.p1").toDouble(-0.005358));
+ DistCoef.at<float>(3) = static_cast<float>(settings.value("Camera.p2").toDouble(0.002628));
+ const float k3 = static_cast<float>(settings.value("Camera.k3").toDouble(1.163314));
+ if(k3!=0)
+ {
+ DistCoef.resize(5);
+ DistCoef.at<float>(4) = k3;
+ }
+ DistCoef.copyTo(mDistCoef);
+
+ mbf = static_cast<float>(settings.value("Camera.bf").toDouble(0));
+
+ float fps = static_cast<float>(settings.value("Camera.fps").toDouble(30));
+ if(fps==0)
+ fps=30;
+
+ // Max/Min Frames to insert keyframes and to check relocalisation
+ mMinFrames = 0;
+ mMaxFrames = fps;
+
+ cout << endl << "Camera Parameters: " << endl;
+ cout << "- fx: " << fx << endl;
+ cout << "- fy: " << fy << endl;
+ cout << "- cx: " << cx << endl;
+ cout << "- cy: " << cy << endl;
+ cout << "- k1: " << DistCoef.at<float>(0) << endl;
+ cout << "- k2: " << DistCoef.at<float>(1) << endl;
+ if(DistCoef.rows==5)
+ cout << "- k3: " << DistCoef.at<float>(4) << endl;
+ cout << "- p1: " << DistCoef.at<float>(2) << endl;
+ cout << "- p2: " << DistCoef.at<float>(3) << endl;
+ cout << "- fps: " << fps << endl;
+
+
+ int nRGB = settings.value("Camera.RGB").toInt(1);
+ mbRGB = nRGB;
+
+ if(mbRGB)
+ cout << "- color order: RGB (ignored if grayscale)" << endl;
+ else
+ cout << "- color order: BGR (ignored if grayscale)" << endl;
+
+ // Load ORB parameters
+
+ // ORB Extractor: Number of features per image
+ int nFeatures = settings.value("ORBextractor.nFeatures").toInt(1000);
+ // ORB Extractor: Scale factor between levels in the scale pyramid
+ float fScaleFactor = static_cast<float>(settings.value("ORBextractor.scaleFactor").toDouble(1.2));
+ // ORB Extractor: Number of levels in the scale pyramid
+ int nLevels = settings.value("ORBextractor.nLevels").toInt(8);
+ // ORB Extractor: Fast threshold
+ // Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
+ // Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
+ // You can lower these values if your images have low contrast
+ int fIniThFAST = settings.value("ORBextractor.iniThFAST").toInt(20);
+ int fMinThFAST = settings.value("ORBextractor.minThFAST").toInt(7);
+
+ mpORBextractorLeft = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
+
+ if(sensor==System::STEREO)
+ mpORBextractorRight = new ORBextractor(nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
+
+ if(sensor==System::MONOCULAR)
+ mpIniORBextractor = new ORBextractor(2*nFeatures,fScaleFactor,nLevels,fIniThFAST,fMinThFAST);
+
+ cout << endl << "ORB Extractor Parameters: " << endl;
+ cout << "- Number of Features: " << nFeatures << endl;
+ cout << "- Scale Levels: " << nLevels << endl;
+ cout << "- Scale Factor: " << fScaleFactor << endl;
+ cout << "- Initial Fast Threshold: " << fIniThFAST << endl;
+ cout << "- Minimum Fast Threshold: " << fMinThFAST << endl;
+
+ if(sensor==System::STEREO || sensor==System::RGBD)
+ {
+ mThDepth = mbf*(float)settings.value("ThDepth").toDouble()/fx;
+ cout << endl << "Depth Threshold (Close/Far Points): " << mThDepth << endl;
+ }
+
+ if(sensor==System::RGBD)
+ {
+ mDepthMapFactor = settings.value("DepthMapFactor").toDouble();
+ if(fabs(mDepthMapFactor)<1e-5)
+ mDepthMapFactor=1;
+ else
+ mDepthMapFactor = 1.0f/mDepthMapFactor;
+ }
+
+}
+
+void Tracking::SetLocalMapper(LocalMapping *pLocalMapper)
+{
+ mpLocalMapper=pLocalMapper;
+}
+
+void Tracking::SetLoopClosing(LoopClosing *pLoopClosing)
+{
+ mpLoopClosing=pLoopClosing;
+}
+
+void Tracking::SetViewer(Viewer *pViewer)
+{
+ mpViewer=pViewer;
+}
+
+
+cv::Mat Tracking::GrabImageStereo(const cv::Mat &imRectLeft, const cv::Mat &imRectRight, const double ×tamp)
+{
+ mImGray = imRectLeft;
+ cv::Mat imGrayRight = imRectRight;
+
+ if(mImGray.channels()==3)
+ {
+ if(mbRGB)
+ {
+ cvtColor(mImGray,mImGray,CV_RGB2GRAY);
+ cvtColor(imGrayRight,imGrayRight,CV_RGB2GRAY);
+ }
+ else
+ {
+ cvtColor(mImGray,mImGray,CV_BGR2GRAY);
+ cvtColor(imGrayRight,imGrayRight,CV_BGR2GRAY);
+ }
+ }
+ else if(mImGray.channels()==4)
+ {
+ if(mbRGB)
+ {
+ cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
+ cvtColor(imGrayRight,imGrayRight,CV_RGBA2GRAY);
+ }
+ else
+ {
+ cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
+ cvtColor(imGrayRight,imGrayRight,CV_BGRA2GRAY);
+ }
+ }
+
+ mCurrentFrame = Frame(mImGray,imGrayRight,timestamp,mpORBextractorLeft,mpORBextractorRight,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+
+ Track();
+
+ return mCurrentFrame.mTcw.clone();
+}
+
+
+cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double ×tamp)
+{
+ mImGray = imRGB;
+ cv::Mat imDepth = imD;
+
+ if(mImGray.channels()==3)
+ {
+ if(mbRGB)
+ cvtColor(mImGray,mImGray,CV_RGB2GRAY);
+ else
+ cvtColor(mImGray,mImGray,CV_BGR2GRAY);
+ }
+ else if(mImGray.channels()==4)
+ {
+ if(mbRGB)
+ cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
+ else
+ cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
+ }
+
+ if((fabs(mDepthMapFactor-1.0f)>1e-5) || imDepth.type()!=CV_32F)
+ imDepth.convertTo(imDepth,CV_32F,mDepthMapFactor);
+
+ mCurrentFrame = Frame(mImGray,imDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+
+ Track();
+
+ return mCurrentFrame.mTcw.clone();
+}
+
+
+cv::Mat Tracking::GrabImageMonocular(const cv::Mat &im, const double ×tamp)
+{
+ mImGray = im;
+
+ if(mImGray.channels()==3)
+ {
+ if(mbRGB)
+ cvtColor(mImGray,mImGray,CV_RGB2GRAY);
+ else
+ cvtColor(mImGray,mImGray,CV_BGR2GRAY);
+ }
+ else if(mImGray.channels()==4)
+ {
+ if(mbRGB)
+ cvtColor(mImGray,mImGray,CV_RGBA2GRAY);
+ else
+ cvtColor(mImGray,mImGray,CV_BGRA2GRAY);
+ }
+
+ if(mState==NOT_INITIALIZED || mState==NO_IMAGES_YET)
+ mCurrentFrame = Frame(mImGray,timestamp,mpIniORBextractor,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+ else
+ mCurrentFrame = Frame(mImGray,timestamp,mpORBextractorLeft,mpORBVocabulary,mK,mDistCoef,mbf,mThDepth);
+
+ Track();
+
+ return mCurrentFrame.mTcw.clone();
+}
+
+void Tracking::Track()
+{
+ if(mState==NO_IMAGES_YET)
+ {
+ mState = NOT_INITIALIZED;
+ }
+
+ mLastProcessedState=mState;
+
+ // Get Map Mutex -> Map cannot be changed
+ unique_lock<mutex> lock(mpMap->mMutexMapUpdate);
+
+ if(mState==NOT_INITIALIZED)
+ {
+ if(mSensor==System::STEREO || mSensor==System::RGBD)
+ StereoInitialization();
+ else
+ MonocularInitialization();
+
+ mpFrameDrawer->Update(this);
+
+ if(mState!=OK)
+ return;
+ }
+ else
+ {
+ // System is initialized. Track Frame.
+ bool bOK;
+
+ // Initial camera pose estimation using motion model or relocalization (if tracking is lost)
+ if(!mbOnlyTracking)
+ {
+ // Local Mapping is activated. This is the normal behaviour, unless
+ // you explicitly activate the "only tracking" mode.
+
+ if(mState==OK)
+ {
+ // Local Mapping might have changed some MapPoints tracked in last frame
+ CheckReplacedInLastFrame();
+
+ if(mVelocity.empty() || mCurrentFrame.mnId<mnLastRelocFrameId+2)
+ {
+ bOK = TrackReferenceKeyFrame();
+ }
+ else
+ {
+ bOK = TrackWithMotionModel();
+ if(!bOK)
+ bOK = TrackReferenceKeyFrame();
+ }
+ }
+ else
+ {
+ bOK = Relocalization();
+ }
+ }
+ else
+ {
+ // Localization Mode: Local Mapping is deactivated
+
+ if(mState==LOST)
+ {
+ bOK = Relocalization();
+ }
+ else
+ {
+ if(!mbVO)
+ {
+ // In last frame we tracked enough MapPoints in the map
+
+ if(!mVelocity.empty())
+ {
+ bOK = TrackWithMotionModel();
+ }
+ else
+ {
+ bOK = TrackReferenceKeyFrame();
+ }
+ }
+ else
+ {
+ // In last frame we tracked mainly "visual odometry" points.
+
+ // We compute two camera poses, one from motion model and one doing relocalization.
+ // If relocalization is sucessfull we choose that solution, otherwise we retain
+ // the "visual odometry" solution.
+
+ bool bOKMM = false;
+ bool bOKReloc = false;
+ vector<MapPoint*> vpMPsMM;
+ vector<bool> vbOutMM;
+ cv::Mat TcwMM;
+ if(!mVelocity.empty())
+ {
+ bOKMM = TrackWithMotionModel();
+ vpMPsMM = mCurrentFrame.mvpMapPoints;
+ vbOutMM = mCurrentFrame.mvbOutlier;
+ TcwMM = mCurrentFrame.mTcw.clone();
+ }
+ bOKReloc = Relocalization();
+
+ if(bOKMM && !bOKReloc)
+ {
+ mCurrentFrame.SetPose(TcwMM);
+ mCurrentFrame.mvpMapPoints = vpMPsMM;
+ mCurrentFrame.mvbOutlier = vbOutMM;
+
+ if(mbVO)
+ {
+ for(int i =0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
+ {
+ mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
+ }
+ }
+ }
+ }
+ else if(bOKReloc)
+ {
+ mbVO = false;
+ }
+
+ bOK = bOKReloc || bOKMM;
+ }
+ }
+ }
+
+ mCurrentFrame.mpReferenceKF = mpReferenceKF;
+
+ // If we have an initial estimation of the camera pose and matching. Track the local map.
+ if(!mbOnlyTracking)
+ {
+ if(bOK)
+ bOK = TrackLocalMap();
+ }
+ else
+ {
+ // mbVO true means that there are few matches to MapPoints in the map. We cannot retrieve
+ // a local map and therefore we do not perform TrackLocalMap(). Once the system relocalizes
+ // the camera we will use the local map again.
+ if(bOK && !mbVO)
+ bOK = TrackLocalMap();
+ }
+
+ if(bOK)
+ mState = OK;
+ else
+ mState=LOST;
+
+ // Update drawer
+ mpFrameDrawer->Update(this);
+
+ // If tracking were good, check if we insert a keyframe
+ if(bOK)
+ {
+ // Update motion model
+ if(!mLastFrame.mTcw.empty())
+ {
+ cv::Mat LastTwc = cv::Mat::eye(4,4,CV_32F);
+ mLastFrame.GetRotationInverse().copyTo(LastTwc.rowRange(0,3).colRange(0,3));
+ mLastFrame.GetCameraCenter().copyTo(LastTwc.rowRange(0,3).col(3));
+ mVelocity = mCurrentFrame.mTcw*LastTwc;
+ }
+ else
+ mVelocity = cv::Mat();
+
+ // Clean VO matches
+ for(int i=0; i<mCurrentFrame.N; i++)
+ {
+ MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+ if(pMP)
+ if(pMP->Observations()<1)
+ {
+ mCurrentFrame.mvbOutlier[i] = false;
+ mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
+ }
+ }
+
+ // Delete temporal MapPoints
+ for(list<MapPoint*>::iterator lit = mlpTemporalPoints.begin(), lend = mlpTemporalPoints.end(); lit!=lend; lit++)
+ {
+ MapPoint* pMP = *lit;
+ delete pMP;
+ }
+ mlpTemporalPoints.clear();
+
+ // Check if we need to insert a new keyframe
+ if(NeedNewKeyFrame())
+ CreateNewKeyFrame();
+
+ // We allow points with high innovation (considererd outliers by the Huber Function)
+ // pass to the new keyframe, so that bundle adjustment will finally decide
+ // if they are outliers or not. We don't want next frame to estimate its position
+ // with those points so we discard them in the frame.
+ for(int i=0; i<mCurrentFrame.N;i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i] && mCurrentFrame.mvbOutlier[i])
+ mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
+ }
+ }
+
+ // Reset if the camera get lost soon after initialization
+ if(mState==LOST)
+ {
+ if(mpMap->KeyFramesInMap()<=5)
+ {
+ cout << "Track lost soon after initialisation, reseting..." << endl;
+ mpSystem->Reset();
+ return;
+ }
+ }
+
+ if(!mCurrentFrame.mpReferenceKF)
+ mCurrentFrame.mpReferenceKF = mpReferenceKF;
+
+ mLastFrame = Frame(mCurrentFrame);
+ }
+
+ // Store frame pose information to retrieve the complete camera trajectory afterwards.
+ if(!mCurrentFrame.mTcw.empty())
+ {
+ cv::Mat Tcr = mCurrentFrame.mTcw*mCurrentFrame.mpReferenceKF->GetPoseInverse();
+ mlRelativeFramePoses.push_back(Tcr);
+ mlpReferences.push_back(mpReferenceKF);
+ mlFrameTimes.push_back(mCurrentFrame.mTimeStamp);
+ mlbLost.push_back(mState==LOST);
+ }
+ else
+ {
+ // This can happen if tracking is lost
+ mlRelativeFramePoses.push_back(mlRelativeFramePoses.back());
+ mlpReferences.push_back(mlpReferences.back());
+ mlFrameTimes.push_back(mlFrameTimes.back());
+ mlbLost.push_back(mState==LOST);
+ }
+
+}
+
+
+void Tracking::StereoInitialization()
+{
+ if(mCurrentFrame.N>500)
+ {
+ // Set Frame pose to the origin
+ mCurrentFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
+
+ // Create KeyFrame
+ KeyFrame* pKFini = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
+
+ // Insert KeyFrame in the map
+ mpMap->AddKeyFrame(pKFini);
+
+ // Create MapPoints and asscoiate to KeyFrame
+ for(int i=0; i<mCurrentFrame.N;i++)
+ {
+ float z = mCurrentFrame.mvDepth[i];
+ if(z>0)
+ {
+ cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
+ MapPoint* pNewMP = new MapPoint(x3D,pKFini,mpMap);
+ pNewMP->AddObservation(pKFini,i);
+ pKFini->AddMapPoint(pNewMP,i);
+ pNewMP->ComputeDistinctiveDescriptors();
+ pNewMP->UpdateNormalAndDepth();
+ mpMap->AddMapPoint(pNewMP);
+
+ mCurrentFrame.mvpMapPoints[i]=pNewMP;
+ }
+ }
+
+ cout << "New map created with " << mpMap->MapPointsInMap() << " points" << endl;
+
+ mpLocalMapper->InsertKeyFrame(pKFini);
+
+ mLastFrame = Frame(mCurrentFrame);
+ mnLastKeyFrameId=mCurrentFrame.mnId;
+ mpLastKeyFrame = pKFini;
+
+ mvpLocalKeyFrames.push_back(pKFini);
+ mvpLocalMapPoints=mpMap->GetAllMapPoints();
+ mpReferenceKF = pKFini;
+ mCurrentFrame.mpReferenceKF = pKFini;
+
+ mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
+
+ mpMap->mvpKeyFrameOrigins.push_back(pKFini);
+
+ mState=OK;
+ }
+}
+
+void Tracking::MonocularInitialization()
+{
+
+ if(!mpInitializer)
+ {
+ // Set Reference Frame
+ if(mCurrentFrame.mvKeys.size()>100)
+ {
+ mInitialFrame = Frame(mCurrentFrame);
+ mLastFrame = Frame(mCurrentFrame);
+ mvbPrevMatched.resize(mCurrentFrame.mvKeysUn.size());
+ for(size_t i=0; i<mCurrentFrame.mvKeysUn.size(); i++)
+ mvbPrevMatched[i]=mCurrentFrame.mvKeysUn[i].pt;
+
+ if(mpInitializer)
+ delete mpInitializer;
+
+ mpInitializer = new Initializer(mCurrentFrame,1.0,200);
+
+ fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
+
+ return;
+ }
+ }
+ else
+ {
+ // Try to initialize
+ if((int)mCurrentFrame.mvKeys.size()<=100)
+ {
+ delete mpInitializer;
+ mpInitializer = static_cast<Initializer*>(NULL);
+ fill(mvIniMatches.begin(),mvIniMatches.end(),-1);
+ return;
+ }
+
+ // Find correspondences
+ ORBmatcher matcher(0.9,true);
+ int nmatches = matcher.SearchForInitialization(mInitialFrame,mCurrentFrame,mvbPrevMatched,mvIniMatches,100);
+
+ // Check if there are enough correspondences
+ if(nmatches<100)
+ {
+ delete mpInitializer;
+ mpInitializer = static_cast<Initializer*>(NULL);
+ return;
+ }
+
+ cv::Mat Rcw; // Current Camera Rotation
+ cv::Mat tcw; // Current Camera Translation
+ vector<bool> vbTriangulated; // Triangulated Correspondences (mvIniMatches)
+
+ if(mpInitializer->Initialize(mCurrentFrame, mvIniMatches, Rcw, tcw, mvIniP3D, vbTriangulated))
+ {
+ for(size_t i=0, iend=mvIniMatches.size(); i<iend;i++)
+ {
+ if(mvIniMatches[i]>=0 && !vbTriangulated[i])
+ {
+ mvIniMatches[i]=-1;
+ nmatches--;
+ }
+ }
+
+ // Set Frame Poses
+ mInitialFrame.SetPose(cv::Mat::eye(4,4,CV_32F));
+ cv::Mat Tcw = cv::Mat::eye(4,4,CV_32F);
+ Rcw.copyTo(Tcw.rowRange(0,3).colRange(0,3));
+ tcw.copyTo(Tcw.rowRange(0,3).col(3));
+ mCurrentFrame.SetPose(Tcw);
+
+ CreateInitialMapMonocular();
+ }
+ }
+}
+
+void Tracking::CreateInitialMapMonocular()
+{
+ // Create KeyFrames
+ KeyFrame* pKFini = new KeyFrame(mInitialFrame,mpMap,mpKeyFrameDB);
+ KeyFrame* pKFcur = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
+
+
+ pKFini->ComputeBoW();
+ pKFcur->ComputeBoW();
+
+ // Insert KFs in the map
+ mpMap->AddKeyFrame(pKFini);
+ mpMap->AddKeyFrame(pKFcur);
+
+ // Create MapPoints and asscoiate to keyframes
+ for(size_t i=0; i<mvIniMatches.size();i++)
+ {
+ if(mvIniMatches[i]<0)
+ continue;
+
+ //Create MapPoint.
+ cv::Mat worldPos(mvIniP3D[i]);
+
+ MapPoint* pMP = new MapPoint(worldPos,pKFcur,mpMap);
+
+ pKFini->AddMapPoint(pMP,i);
+ pKFcur->AddMapPoint(pMP,mvIniMatches[i]);
+
+ pMP->AddObservation(pKFini,i);
+ pMP->AddObservation(pKFcur,mvIniMatches[i]);
+
+ pMP->ComputeDistinctiveDescriptors();
+ pMP->UpdateNormalAndDepth();
+
+ //Fill Current Frame structure
+ mCurrentFrame.mvpMapPoints[mvIniMatches[i]] = pMP;
+ mCurrentFrame.mvbOutlier[mvIniMatches[i]] = false;
+
+ //Add to Map
+ mpMap->AddMapPoint(pMP);
+ }
+
+ // Update Connections
+ pKFini->UpdateConnections();
+ pKFcur->UpdateConnections();
+
+ // Bundle Adjustment
+ cout << "New Map created with " << mpMap->MapPointsInMap() << " points" << endl;
+
+ Optimizer::GlobalBundleAdjustemnt(mpMap,20);
+
+ // Set median depth to 1
+ float medianDepth = pKFini->ComputeSceneMedianDepth(2);
+ float invMedianDepth = 1.0f/medianDepth;
+
+ if(medianDepth<0 || pKFcur->TrackedMapPoints(1)<100)
+ {
+ cout << "Wrong initialization, reseting..." << endl;
+ Reset();
+ return;
+ }
+
+ // Scale initial baseline
+ cv::Mat Tc2w = pKFcur->GetPose();
+ Tc2w.col(3).rowRange(0,3) = Tc2w.col(3).rowRange(0,3)*invMedianDepth;
+ pKFcur->SetPose(Tc2w);
+
+ // Scale points
+ vector<MapPoint*> vpAllMapPoints = pKFini->GetMapPointMatches();
+ for(size_t iMP=0; iMP<vpAllMapPoints.size(); iMP++)
+ {
+ if(vpAllMapPoints[iMP])
+ {
+ MapPoint* pMP = vpAllMapPoints[iMP];
+ pMP->SetWorldPos(pMP->GetWorldPos()*invMedianDepth);
+ }
+ }
+
+ mpLocalMapper->InsertKeyFrame(pKFini);
+ mpLocalMapper->InsertKeyFrame(pKFcur);
+
+ mCurrentFrame.SetPose(pKFcur->GetPose());
+ mnLastKeyFrameId=mCurrentFrame.mnId;
+ mpLastKeyFrame = pKFcur;
+
+ mvpLocalKeyFrames.push_back(pKFcur);
+ mvpLocalKeyFrames.push_back(pKFini);
+ mvpLocalMapPoints=mpMap->GetAllMapPoints();
+ mpReferenceKF = pKFcur;
+ mCurrentFrame.mpReferenceKF = pKFcur;
+
+ mLastFrame = Frame(mCurrentFrame);
+
+ mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
+
+ mpMap->mvpKeyFrameOrigins.push_back(pKFini);
+
+ mState=OK;
+}
+
+void Tracking::CheckReplacedInLastFrame()
+{
+ for(int i =0; i<mLastFrame.N; i++)
+ {
+ MapPoint* pMP = mLastFrame.mvpMapPoints[i];
+
+ if(pMP)
+ {
+ MapPoint* pRep = pMP->GetReplaced();
+ if(pRep)
+ {
+ mLastFrame.mvpMapPoints[i] = pRep;
+ }
+ }
+ }
+}
+
+
+bool Tracking::TrackReferenceKeyFrame()
+{
+ // Compute Bag of Words vector
+ mCurrentFrame.ComputeBoW();
+
+ // We perform first an ORB matching with the reference keyframe
+ // If enough matches are found we setup a PnP solver
+ ORBmatcher matcher(0.7,true);
+ vector<MapPoint*> vpMapPointMatches;
+
+ int nmatches = matcher.SearchByBoW(mpReferenceKF,mCurrentFrame,vpMapPointMatches);
+
+ if(nmatches<15)
+ return false;
+
+ mCurrentFrame.mvpMapPoints = vpMapPointMatches;
+ mCurrentFrame.SetPose(mLastFrame.mTcw);
+
+ Optimizer::PoseOptimization(&mCurrentFrame);
+
+ // Discard outliers
+ int nmatchesMap = 0;
+ for(int i =0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i])
+ {
+ if(mCurrentFrame.mvbOutlier[i])
+ {
+ MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+
+ mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
+ mCurrentFrame.mvbOutlier[i]=false;
+ pMP->mbTrackInView = false;
+ pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+ nmatches--;
+ }
+ else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
+ nmatchesMap++;
+ }
+ }
+
+ return nmatchesMap>=10;
+}
+
+void Tracking::UpdateLastFrame()
+{
+ // Update pose according to reference keyframe
+ KeyFrame* pRef = mLastFrame.mpReferenceKF;
+ cv::Mat Tlr = mlRelativeFramePoses.back();
+
+ mLastFrame.SetPose(Tlr*pRef->GetPose());
+
+ if(mnLastKeyFrameId==mLastFrame.mnId || mSensor==System::MONOCULAR || !mbOnlyTracking)
+ return;
+
+ // Create "visual odometry" MapPoints
+ // We sort points according to their measured depth by the stereo/RGB-D sensor
+ vector<pair<float,int> > vDepthIdx;
+ vDepthIdx.reserve(mLastFrame.N);
+ for(int i=0; i<mLastFrame.N;i++)
+ {
+ float z = mLastFrame.mvDepth[i];
+ if(z>0)
+ {
+ vDepthIdx.push_back(make_pair(z,i));
+ }
+ }
+
+ if(vDepthIdx.empty())
+ return;
+
+ sort(vDepthIdx.begin(),vDepthIdx.end());
+
+ // We insert all close points (depth<mThDepth)
+ // If less than 100 close points, we insert the 100 closest ones.
+ int nPoints = 0;
+ for(size_t j=0; j<vDepthIdx.size();j++)
+ {
+ int i = vDepthIdx[j].second;
+
+ bool bCreateNew = false;
+
+ MapPoint* pMP = mLastFrame.mvpMapPoints[i];
+ if(!pMP)
+ bCreateNew = true;
+ else if(pMP->Observations()<1)
+ {
+ bCreateNew = true;
+ }
+
+ if(bCreateNew)
+ {
+ cv::Mat x3D = mLastFrame.UnprojectStereo(i);
+ MapPoint* pNewMP = new MapPoint(x3D,mpMap,&mLastFrame,i);
+
+ mLastFrame.mvpMapPoints[i]=pNewMP;
+
+ mlpTemporalPoints.push_back(pNewMP);
+ nPoints++;
+ }
+ else
+ {
+ nPoints++;
+ }
+
+ if(vDepthIdx[j].first>mThDepth && nPoints>100)
+ break;
+ }
+}
+
+bool Tracking::TrackWithMotionModel()
+{
+ ORBmatcher matcher(0.9,true);
+
+ // Update last frame pose according to its reference keyframe
+ // Create "visual odometry" points if in Localization Mode
+ UpdateLastFrame();
+
+ mCurrentFrame.SetPose(mVelocity*mLastFrame.mTcw);
+
+ fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
+
+ // Project points seen in previous frame
+ int th;
+ if(mSensor!=System::STEREO)
+ th=15;
+ else
+ th=7;
+ int nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,th,mSensor==System::MONOCULAR);
+
+ // If few matches, uses a wider window search
+ if(nmatches<20)
+ {
+ fill(mCurrentFrame.mvpMapPoints.begin(),mCurrentFrame.mvpMapPoints.end(),static_cast<MapPoint*>(NULL));
+ nmatches = matcher.SearchByProjection(mCurrentFrame,mLastFrame,2*th,mSensor==System::MONOCULAR);
+ }
+
+ if(nmatches<20)
+ return false;
+
+ // Optimize frame pose with all matches
+ Optimizer::PoseOptimization(&mCurrentFrame);
+
+ // Discard outliers
+ int nmatchesMap = 0;
+ for(int i =0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i])
+ {
+ if(mCurrentFrame.mvbOutlier[i])
+ {
+ MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+
+ mCurrentFrame.mvpMapPoints[i]=static_cast<MapPoint*>(NULL);
+ mCurrentFrame.mvbOutlier[i]=false;
+ pMP->mbTrackInView = false;
+ pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+ nmatches--;
+ }
+ else if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
+ nmatchesMap++;
+ }
+ }
+
+ if(mbOnlyTracking)
+ {
+ mbVO = nmatchesMap<10;
+ return nmatches>20;
+ }
+
+ return nmatchesMap>=10;
+}
+
+bool Tracking::TrackLocalMap()
+{
+ // We have an estimation of the camera pose and some map points tracked in the frame.
+ // We retrieve the local map and try to find matches to points in the local map.
+
+ UpdateLocalMap();
+
+ SearchLocalPoints();
+
+ // Optimize Pose
+ Optimizer::PoseOptimization(&mCurrentFrame);
+ mnMatchesInliers = 0;
+
+ // Update MapPoints Statistics
+ for(int i=0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i])
+ {
+ if(!mCurrentFrame.mvbOutlier[i])
+ {
+ mCurrentFrame.mvpMapPoints[i]->IncreaseFound();
+ if(!mbOnlyTracking)
+ {
+ if(mCurrentFrame.mvpMapPoints[i]->Observations()>0)
+ mnMatchesInliers++;
+ }
+ else
+ mnMatchesInliers++;
+ }
+ else if(mSensor==System::STEREO)
+ mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+
+ }
+ }
+
+ // Decide if the tracking was succesful
+ // More restrictive if there was a relocalization recently
+ if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && mnMatchesInliers<50)
+ return false;
+
+ if(mnMatchesInliers<30)
+ return false;
+ else
+ return true;
+}
+
+
+bool Tracking::NeedNewKeyFrame()
+{
+ if(mbOnlyTracking)
+ return false;
+
+ // If Local Mapping is freezed by a Loop Closure do not insert keyframes
+ if(mpLocalMapper->isStopped() || mpLocalMapper->stopRequested())
+ return false;
+
+ const int nKFs = mpMap->KeyFramesInMap();
+
+ // Do not insert keyframes if not enough frames have passed from last relocalisation
+ if(mCurrentFrame.mnId<mnLastRelocFrameId+mMaxFrames && nKFs>mMaxFrames)
+ return false;
+
+ // Tracked MapPoints in the reference keyframe
+ int nMinObs = 3;
+ if(nKFs<=2)
+ nMinObs=2;
+ int nRefMatches = mpReferenceKF->TrackedMapPoints(nMinObs);
+
+ // Local Mapping accept keyframes?
+ bool bLocalMappingIdle = mpLocalMapper->AcceptKeyFrames();
+
+ // Check how many "close" points are being tracked and how many could be potentially created.
+ int nNonTrackedClose = 0;
+ int nTrackedClose= 0;
+ if(mSensor!=System::MONOCULAR)
+ {
+ for(int i =0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvDepth[i]>0 && mCurrentFrame.mvDepth[i]<mThDepth)
+ {
+ if(mCurrentFrame.mvpMapPoints[i] && !mCurrentFrame.mvbOutlier[i])
+ nTrackedClose++;
+ else
+ nNonTrackedClose++;
+ }
+ }
+ }
+
+ bool bNeedToInsertClose = (nTrackedClose<100) && (nNonTrackedClose>70);
+
+ // Thresholds
+ float thRefRatio = 0.75f;
+ if(nKFs<2)
+ thRefRatio = 0.4f;
+
+ if(mSensor==System::MONOCULAR)
+ thRefRatio = 0.9f;
+
+ // Condition 1a: More than "MaxFrames" have passed from last keyframe insertion
+ const bool c1a = mCurrentFrame.mnId>=mnLastKeyFrameId+mMaxFrames;
+ // Condition 1b: More than "MinFrames" have passed and Local Mapping is idle
+ const bool c1b = (mCurrentFrame.mnId>=mnLastKeyFrameId+mMinFrames && bLocalMappingIdle);
+ //Condition 1c: tracking is weak
+ const bool c1c = mSensor!=System::MONOCULAR && (mnMatchesInliers<nRefMatches*0.25 || bNeedToInsertClose) ;
+ // Condition 2: Few tracked points compared to reference keyframe. Lots of visual odometry compared to map matches.
+ const bool c2 = ((mnMatchesInliers<nRefMatches*thRefRatio|| bNeedToInsertClose) && mnMatchesInliers>15);
+
+ if((c1a||c1b||c1c)&&c2)
+ {
+ // If the mapping accepts keyframes, insert keyframe.
+ // Otherwise send a signal to interrupt BA
+ if(bLocalMappingIdle)
+ {
+ return true;
+ }
+ else
+ {
+ mpLocalMapper->InterruptBA();
+ if(mSensor!=System::MONOCULAR)
+ {
+ if(mpLocalMapper->KeyframesInQueue()<3)
+ return true;
+ else
+ return false;
+ }
+ else
+ return false;
+ }
+ }
+ else
+ return false;
+}
+
+void Tracking::CreateNewKeyFrame()
+{
+ if(!mpLocalMapper->SetNotStop(true))
+ return;
+
+ KeyFrame* pKF = new KeyFrame(mCurrentFrame,mpMap,mpKeyFrameDB);
+
+ mpReferenceKF = pKF;
+ mCurrentFrame.mpReferenceKF = pKF;
+
+ if(mSensor!=System::MONOCULAR)
+ {
+ mCurrentFrame.UpdatePoseMatrices();
+
+ // We sort points by the measured depth by the stereo/RGBD sensor.
+ // We create all those MapPoints whose depth < mThDepth.
+ // If there are less than 100 close points we create the 100 closest.
+ vector<pair<float,int> > vDepthIdx;
+ vDepthIdx.reserve(mCurrentFrame.N);
+ for(int i=0; i<mCurrentFrame.N; i++)
+ {
+ float z = mCurrentFrame.mvDepth[i];
+ if(z>0)
+ {
+ vDepthIdx.push_back(make_pair(z,i));
+ }
+ }
+
+ if(!vDepthIdx.empty())
+ {
+ sort(vDepthIdx.begin(),vDepthIdx.end());
+
+ int nPoints = 0;
+ for(size_t j=0; j<vDepthIdx.size();j++)
+ {
+ int i = vDepthIdx[j].second;
+
+ bool bCreateNew = false;
+
+ MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+ if(!pMP)
+ bCreateNew = true;
+ else if(pMP->Observations()<1)
+ {
+ bCreateNew = true;
+ mCurrentFrame.mvpMapPoints[i] = static_cast<MapPoint*>(NULL);
+ }
+
+ if(bCreateNew)
+ {
+ cv::Mat x3D = mCurrentFrame.UnprojectStereo(i);
+ MapPoint* pNewMP = new MapPoint(x3D,pKF,mpMap);
+ pNewMP->AddObservation(pKF,i);
+ pKF->AddMapPoint(pNewMP,i);
+ pNewMP->ComputeDistinctiveDescriptors();
+ pNewMP->UpdateNormalAndDepth();
+ mpMap->AddMapPoint(pNewMP);
+
+ mCurrentFrame.mvpMapPoints[i]=pNewMP;
+ nPoints++;
+ }
+ else
+ {
+ nPoints++;
+ }
+
+ if(vDepthIdx[j].first>mThDepth && nPoints>100)
+ break;
+ }
+ }
+ }
+
+ mpLocalMapper->InsertKeyFrame(pKF);
+
+ mpLocalMapper->SetNotStop(false);
+
+ mnLastKeyFrameId = mCurrentFrame.mnId;
+ mpLastKeyFrame = pKF;
+}
+
+void Tracking::SearchLocalPoints()
+{
+ // Do not search map points already matched
+ for(vector<MapPoint*>::iterator vit=mCurrentFrame.mvpMapPoints.begin(), vend=mCurrentFrame.mvpMapPoints.end(); vit!=vend; vit++)
+ {
+ MapPoint* pMP = *vit;
+ if(pMP)
+ {
+ if(pMP->isBad())
+ {
+ *vit = static_cast<MapPoint*>(NULL);
+ }
+ else
+ {
+ pMP->IncreaseVisible();
+ pMP->mnLastFrameSeen = mCurrentFrame.mnId;
+ pMP->mbTrackInView = false;
+ }
+ }
+ }
+
+ int nToMatch=0;
+
+ // Project points in frame and check its visibility
+ for(vector<MapPoint*>::iterator vit=mvpLocalMapPoints.begin(), vend=mvpLocalMapPoints.end(); vit!=vend; vit++)
+ {
+ MapPoint* pMP = *vit;
+ if(pMP->mnLastFrameSeen == mCurrentFrame.mnId)
+ continue;
+ if(pMP->isBad())
+ continue;
+ // Project (this fills MapPoint variables for matching)
+ if(mCurrentFrame.isInFrustum(pMP,0.5))
+ {
+ pMP->IncreaseVisible();
+ nToMatch++;
+ }
+ }
+
+ if(nToMatch>0)
+ {
+ ORBmatcher matcher(0.8);
+ int th = 1;
+ if(mSensor==System::RGBD)
+ th=3;
+ // If the camera has been relocalised recently, perform a coarser search
+ if(mCurrentFrame.mnId<mnLastRelocFrameId+2)
+ th=5;
+ matcher.SearchByProjection(mCurrentFrame,mvpLocalMapPoints,th);
+ }
+}
+
+void Tracking::UpdateLocalMap()
+{
+ // This is for visualization
+ mpMap->SetReferenceMapPoints(mvpLocalMapPoints);
+
+ // Update
+ UpdateLocalKeyFrames();
+ UpdateLocalPoints();
+}
+
+void Tracking::UpdateLocalPoints()
+{
+ mvpLocalMapPoints.clear();
+
+ for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
+ {
+ KeyFrame* pKF = *itKF;
+ const vector<MapPoint*> vpMPs = pKF->GetMapPointMatches();
+
+ for(vector<MapPoint*>::const_iterator itMP=vpMPs.begin(), itEndMP=vpMPs.end(); itMP!=itEndMP; itMP++)
+ {
+ MapPoint* pMP = *itMP;
+ if(!pMP)
+ continue;
+ if(pMP->mnTrackReferenceForFrame==mCurrentFrame.mnId)
+ continue;
+ if(!pMP->isBad())
+ {
+ mvpLocalMapPoints.push_back(pMP);
+ pMP->mnTrackReferenceForFrame=mCurrentFrame.mnId;
+ }
+ }
+ }
+}
+
+
+void Tracking::UpdateLocalKeyFrames()
+{
+ // Each map point vote for the keyframes in which it has been observed
+ map<KeyFrame*,int> keyframeCounter;
+ for(int i=0; i<mCurrentFrame.N; i++)
+ {
+ if(mCurrentFrame.mvpMapPoints[i])
+ {
+ MapPoint* pMP = mCurrentFrame.mvpMapPoints[i];
+ if(!pMP->isBad())
+ {
+ const map<KeyFrame*,size_t> observations = pMP->GetObservations();
+ for(map<KeyFrame*,size_t>::const_iterator it=observations.begin(), itend=observations.end(); it!=itend; it++)
+ keyframeCounter[it->first]++;
+ }
+ else
+ {
+ mCurrentFrame.mvpMapPoints[i]=NULL;
+ }
+ }
+ }
+
+ if(keyframeCounter.empty())
+ return;
+
+ int max=0;
+ KeyFrame* pKFmax= static_cast<KeyFrame*>(NULL);
+
+ mvpLocalKeyFrames.clear();
+ mvpLocalKeyFrames.reserve(3*keyframeCounter.size());
+
+ // All keyframes that observe a map point are included in the local map. Also check which keyframe shares most points
+ for(map<KeyFrame*,int>::const_iterator it=keyframeCounter.begin(), itEnd=keyframeCounter.end(); it!=itEnd; it++)
+ {
+ KeyFrame* pKF = it->first;
+
+ if(pKF->isBad())
+ continue;
+
+ if(it->second>max)
+ {
+ max=it->second;
+ pKFmax=pKF;
+ }
+
+ mvpLocalKeyFrames.push_back(it->first);
+ pKF->mnTrackReferenceForFrame = mCurrentFrame.mnId;
+ }
+
+
+ // Include also some not-already-included keyframes that are neighbors to already-included keyframes
+ for(vector<KeyFrame*>::const_iterator itKF=mvpLocalKeyFrames.begin(), itEndKF=mvpLocalKeyFrames.end(); itKF!=itEndKF; itKF++)
+ {
+ // Limit the number of keyframes
+ if(mvpLocalKeyFrames.size()>80)
+ break;
+
+ KeyFrame* pKF = *itKF;
+
+ const vector<KeyFrame*> vNeighs = pKF->GetBestCovisibilityKeyFrames(10);
+
+ for(vector<KeyFrame*>::const_iterator itNeighKF=vNeighs.begin(), itEndNeighKF=vNeighs.end(); itNeighKF!=itEndNeighKF; itNeighKF++)
+ {
+ KeyFrame* pNeighKF = *itNeighKF;
+ if(!pNeighKF->isBad())
+ {
+ if(pNeighKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
+ {
+ mvpLocalKeyFrames.push_back(pNeighKF);
+ pNeighKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
+ break;
+ }
+ }
+ }
+
+ const set<KeyFrame*> spChilds = pKF->GetChilds();
+ for(set<KeyFrame*>::const_iterator sit=spChilds.begin(), send=spChilds.end(); sit!=send; sit++)
+ {
+ KeyFrame* pChildKF = *sit;
+ if(!pChildKF->isBad())
+ {
+ if(pChildKF->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
+ {
+ mvpLocalKeyFrames.push_back(pChildKF);
+ pChildKF->mnTrackReferenceForFrame=mCurrentFrame.mnId;
+ break;
+ }
+ }
+ }
+
+ KeyFrame* pParent = pKF->GetParent();
+ if(pParent)
+ {
+ if(pParent->mnTrackReferenceForFrame!=mCurrentFrame.mnId)
+ {
+ mvpLocalKeyFrames.push_back(pParent);
+ pParent->mnTrackReferenceForFrame=mCurrentFrame.mnId;
+ break;
+ }
+ }
+
+ }
+
+ if(pKFmax)
+ {
+ mpReferenceKF = pKFmax;
+ mCurrentFrame.mpReferenceKF = mpReferenceKF;
+ }
+}
+
+bool Tracking::Relocalization()
+{
+ // Compute Bag of Words Vector
+ mCurrentFrame.ComputeBoW();
+
+ // Relocalization is performed when tracking is lost
+ // Track Lost: Query KeyFrame Database for keyframe candidates for relocalisation
+ vector<KeyFrame*> vpCandidateKFs = mpKeyFrameDB->DetectRelocalizationCandidates(&mCurrentFrame);
+
+ if(vpCandidateKFs.empty())
+ return false;
+
+ const int nKFs = vpCandidateKFs.size();
+
+ // We perform first an ORB matching with each candidate
+ // If enough matches are found we setup a PnP solver
+ ORBmatcher matcher(0.75,true);
+
+ vector<PnPsolver*> vpPnPsolvers;
+ vpPnPsolvers.resize(nKFs);
+
+ vector<vector<MapPoint*> > vvpMapPointMatches;
+ vvpMapPointMatches.resize(nKFs);
+
+ vector<bool> vbDiscarded;
+ vbDiscarded.resize(nKFs);
+
+ int nCandidates=0;
+
+ for(int i=0; i<nKFs; i++)
+ {
+ KeyFrame* pKF = vpCandidateKFs[i];
+ if(pKF->isBad())
+ vbDiscarded[i] = true;
+ else
+ {
+ int nmatches = matcher.SearchByBoW(pKF,mCurrentFrame,vvpMapPointMatches[i]);
+ if(nmatches<15)
+ {
+ vbDiscarded[i] = true;
+ continue;
+ }
+ else
+ {
+ PnPsolver* pSolver = new PnPsolver(mCurrentFrame,vvpMapPointMatches[i]);
+ pSolver->SetRansacParameters(0.99,10,300,4,0.5,5.991);
+ vpPnPsolvers[i] = pSolver;
+ nCandidates++;
+ }
+ }
+ }
+
+ // Alternatively perform some iterations of P4P RANSAC
+ // Until we found a camera pose supported by enough inliers
+ bool bMatch = false;
+ ORBmatcher matcher2(0.9,true);
+
+ while(nCandidates>0 && !bMatch)
+ {
+ for(int i=0; i<nKFs; i++)
+ {
+ if(vbDiscarded[i])
+ continue;
+
+ // Perform 5 Ransac Iterations
+ vector<bool> vbInliers;
+ int nInliers;
+ bool bNoMore;
+
+ PnPsolver* pSolver = vpPnPsolvers[i];
+ cv::Mat Tcw = pSolver->iterate(5,bNoMore,vbInliers,nInliers);
+
+ // If Ransac reachs max. iterations discard keyframe
+ if(bNoMore)
+ {
+ vbDiscarded[i]=true;
+ nCandidates--;
+ }
+
+ // If a Camera Pose is computed, optimize
+ if(!Tcw.empty())
+ {
+ Tcw.copyTo(mCurrentFrame.mTcw);
+
+ set<MapPoint*> sFound;
+
+ const int np = vbInliers.size();
+
+ for(int j=0; j<np; j++)
+ {
+ if(vbInliers[j])
+ {
+ mCurrentFrame.mvpMapPoints[j]=vvpMapPointMatches[i][j];
+ sFound.insert(vvpMapPointMatches[i][j]);
+ }
+ else
+ mCurrentFrame.mvpMapPoints[j]=NULL;
+ }
+
+ int nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+
+ if(nGood<10)
+ continue;
+
+ for(int io =0; io<mCurrentFrame.N; io++)
+ if(mCurrentFrame.mvbOutlier[io])
+ mCurrentFrame.mvpMapPoints[io]=static_cast<MapPoint*>(NULL);
+
+ // If few inliers, search by projection in a coarse window and optimize again
+ if(nGood<50)
+ {
+ int nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,10,100);
+
+ if(nadditional+nGood>=50)
+ {
+ nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+
+ // If many inliers but still not enough, search by projection again in a narrower window
+ // the camera has been already optimized with many points
+ if(nGood>30 && nGood<50)
+ {
+ sFound.clear();
+ for(int ip =0; ip<mCurrentFrame.N; ip++)
+ if(mCurrentFrame.mvpMapPoints[ip])
+ sFound.insert(mCurrentFrame.mvpMapPoints[ip]);
+ nadditional =matcher2.SearchByProjection(mCurrentFrame,vpCandidateKFs[i],sFound,3,64);
+
+ // Final optimization
+ if(nGood+nadditional>=50)
+ {
+ nGood = Optimizer::PoseOptimization(&mCurrentFrame);
+
+ for(int io =0; io<mCurrentFrame.N; io++)
+ if(mCurrentFrame.mvbOutlier[io])
+ mCurrentFrame.mvpMapPoints[io]=NULL;
+ }
+ }
+ }
+ }
+
+
+ // If the pose is supported by enough inliers stop ransacs and continue
+ if(nGood>=50)
+ {
+ bMatch = true;
+ break;
+ }
+ }
+ }
+ }
+
+ if(!bMatch)
+ {
+ return false;
+ }
+ else
+ {
+ mnLastRelocFrameId = mCurrentFrame.mnId;
+ return true;
+ }
+
+}
+
+void Tracking::Reset()
+{
+
+ cout << "System Reseting" << endl;
+ if(mpViewer)
+ {
+ mpViewer->RequestStop();
+ while(!mpViewer->isStopped())
+ usleep(3000);
+ }
+
+ // Reset Local Mapping
+ cout << "Reseting Local Mapper...";
+ mpLocalMapper->RequestReset();
+ cout << " done" << endl;
+
+ // Reset Loop Closing
+ cout << "Reseting Loop Closing...";
+ mpLoopClosing->RequestReset();
+ cout << " done" << endl;
+
+ // Clear BoW Database
+ cout << "Reseting Database...";
+ mpKeyFrameDB->clear();
+ cout << " done" << endl;
+
+ // Clear Map (this erase MapPoints and KeyFrames)
+ mpMap->clear();
+
+ KeyFrame::nNextId = 0;
+ Frame::nNextId = 0;
+ mState = NO_IMAGES_YET;
+
+ if(mpInitializer)
+ {
+ delete mpInitializer;
+ mpInitializer = static_cast<Initializer*>(NULL);
+ }
+
+ mlRelativeFramePoses.clear();
+ mlpReferences.clear();
+ mlFrameTimes.clear();
+ mlbLost.clear();
+
+ if(mpViewer)
+ mpViewer->Release();
+}
+
+void Tracking::ChangeCalibration(const string &strSettingPath)
+{
+ cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
+ float fx = fSettings["Camera.fx"];
+ float fy = fSettings["Camera.fy"];
+ float cx = fSettings["Camera.cx"];
+ float cy = fSettings["Camera.cy"];
+
+ cv::Mat K = cv::Mat::eye(3,3,CV_32F);
+ K.at<float>(0,0) = fx;
+ K.at<float>(1,1) = fy;
+ K.at<float>(0,2) = cx;
+ K.at<float>(1,2) = cy;
+ K.copyTo(mK);
+
+ cv::Mat DistCoef(4,1,CV_32F);
+ DistCoef.at<float>(0) = fSettings["Camera.k1"];
+ DistCoef.at<float>(1) = fSettings["Camera.k2"];
+ DistCoef.at<float>(2) = fSettings["Camera.p1"];
+ DistCoef.at<float>(3) = fSettings["Camera.p2"];
+ const float k3 = fSettings["Camera.k3"];
+ if(k3!=0)
+ {
+ DistCoef.resize(5);
+ DistCoef.at<float>(4) = k3;
+ }
+ DistCoef.copyTo(mDistCoef);
+
+ mbf = fSettings["Camera.bf"];
+
+ Frame::mbInitialComputations = true;
+}
+
+void Tracking::InformOnlyTracking(const bool &flag)
+{
+ mbOnlyTracking = flag;
+}
+
+
+
+} //namespace ORB_SLAM
diff --git a/externals/ORB_SLAM2/src/Viewer.cc b/externals/ORB_SLAM2/src/Viewer.cc
new file mode 100644
index 0000000000000000000000000000000000000000..043df184645a85996e41105c1c7eb47a2df5481f
--- /dev/null
+++ b/externals/ORB_SLAM2/src/Viewer.cc
@@ -0,0 +1,119 @@
+/**
+* This file is part of ORB-SLAM2.
+*
+* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+* For more information see <https://github.com/raulmur/ORB_SLAM2>
+*
+* ORB-SLAM2 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.
+*
+* ORB-SLAM2 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 ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
+*/
+
+#include "Viewer.h"
+#include <pangolin/pangolin.h>
+
+#include <mutex>
+
+namespace ORB_SLAM2
+{
+
+Viewer::Viewer(System* pSystem, FrameDrawer *pFrameDrawer, MapDrawer *pMapDrawer, Tracking *pTracking, const string &strSettingPath):
+ mpSystem(pSystem), mpFrameDrawer(pFrameDrawer),mpMapDrawer(pMapDrawer), mpTracker(pTracking),
+ mbFinishRequested(false), mbFinished(true), mbStopped(true), mbStopRequested(false)
+{
+ cv::FileStorage fSettings(strSettingPath, cv::FileStorage::READ);
+
+ float fps = fSettings["Camera.fps"];
+ if(fps<1)
+ fps=30;
+ mT = 1e3/fps;
+
+ mImageWidth = fSettings["Camera.width"];
+ mImageHeight = fSettings["Camera.height"];
+ if(mImageWidth<1 || mImageHeight<1)
+ {
+ mImageWidth = 640;
+ mImageHeight = 480;
+ }
+
+ mViewpointX = fSettings["Viewer.ViewpointX"];
+ mViewpointY = fSettings["Viewer.ViewpointY"];
+ mViewpointZ = fSettings["Viewer.ViewpointZ"];
+ mViewpointF = fSettings["Viewer.ViewpointF"];
+}
+
+void Viewer::Run()
+{
+}
+
+void Viewer::RequestFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinishRequested = true;
+}
+
+bool Viewer::CheckFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinishRequested;
+}
+
+void Viewer::SetFinish()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ mbFinished = true;
+}
+
+bool Viewer::isFinished()
+{
+ unique_lock<mutex> lock(mMutexFinish);
+ return mbFinished;
+}
+
+void Viewer::RequestStop()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ if(!mbStopped)
+ mbStopRequested = true;
+}
+
+bool Viewer::isStopped()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ return mbStopped;
+}
+
+bool Viewer::Stop()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ unique_lock<mutex> lock2(mMutexFinish);
+
+ if(mbFinishRequested)
+ return false;
+ else if(mbStopRequested)
+ {
+ mbStopped = true;
+ mbStopRequested = false;
+ return true;
+ }
+
+ return false;
+
+}
+
+void Viewer::Release()
+{
+ unique_lock<mutex> lock(mMutexStop);
+ mbStopped = false;
+}
+
+}
diff --git a/externals/Pangolin-config/pangolin/config.h b/externals/Pangolin-config/pangolin/config.h
new file mode 100644
index 0000000000000000000000000000000000000000..fdce248f1410f53d8a5541da459fe374e811abff
--- /dev/null
+++ b/externals/Pangolin-config/pangolin/config.h
@@ -0,0 +1,72 @@
+#ifndef PANGOLIN_CONFIG_H
+#define PANGOLIN_CONFIG_H
+
+/*
+ * Configuration Header for Pangolin
+ */
+
+/// Version
+#define PANGOLIN_VERSION_MAJOR 0
+#define PANGOLIN_VERSION_MINOR 5
+#define PANGOLIN_VERSION_STRING "0.5"
+
+/// Pangolin options
+#define BUILD_PANGOLIN_GUI
+#define BUILD_PANGOLIN_VARS
+#define BUILD_PANGOLIN_VIDEO
+
+/// Configured libraries
+/* #undef HAVE_CUDA */
+/* #undef HAVE_PYTHON */
+
+#define HAVE_EIGEN
+/* #undef HAVE_TOON */
+
+#define HAVE_DC1394
+#define HAVE_V4L
+#define HAVE_OPENNI
+/* #undef HAVE_LIBREALSENSE */
+#define HAVE_OPENNI2
+/* #undef HAVE_UVC */
+/* #undef HAVE_DEPTHSENSE */
+/* #undef HAVE_TELICAM */
+/* #undef HAVE_PLEORA */
+
+#define HAVE_FFMPEG
+/* #undef HAVE_FFMPEG_MAX_ANALYZE_DURATION2 */
+#define HAVE_FFMPEG_AVFORMAT_ALLOC_OUTPUT_CONTEXT2
+#define HAVE_FFMPEG_AVPIXELFORMAT
+
+#define HAVE_GLEW
+/* #undef GLEW_STATIC */
+
+/* #undef HAVE_APPLE_OPENGL_FRAMEWORK */
+/* #undef HAVE_GLES */
+/* #undef HAVE_GLES_2 */
+/* #undef HAVE_OCULUS */
+
+#define HAVE_PNG
+#define HAVE_JPEG
+#define HAVE_TIFF
+#define HAVE_OPENEXR
+/* #undef HAVE_ZSTD */
+#define HAVE_LZ4
+
+/// Platform
+#define _UNIX_
+/* #undef _WIN_ */
+/* #undef _OSX_ */
+#define _LINUX_
+/* #undef _ANDROID_ */
+/* #undef _IOS_ */
+
+/// Compiler
+#define _GCC_
+/* #undef _CLANG_ */
+/* #undef _MSVC_ */
+
+#if (__cplusplus > 199711L) || (_MSC_VER >= 1800)
+#define CALLEE_HAS_VARIADIC_TEMPLATES
+#endif
+
+#endif //PANGOLIN_CONFIG_H
diff --git a/externals/Pangolin-config/pangolin/video_drivers.h b/externals/Pangolin-config/pangolin/video_drivers.h
new file mode 100644
index 0000000000000000000000000000000000000000..bc7d563796eab23ae224a3e4b68e01f09abf8ce1
--- /dev/null
+++ b/externals/Pangolin-config/pangolin/video_drivers.h
@@ -0,0 +1,59 @@
+// CMake generated file. Do Not Edit.
+
+#pragma once
+
+namespace pangolin {
+
+void RegisterTestVideoFactory();
+void RegisterImagesVideoFactory();
+void RegisterImagesVideoOutputFactory();
+void RegisterSplitVideoFactory();
+void RegisterTruncateVideoFactory();
+void RegisterPvnVideoFactory();
+void RegisterPangoVideoFactory();
+void RegisterPangoVideoOutputFactory();
+void RegisterDebayerVideoFactory();
+void RegisterShiftVideoFactory();
+void RegisterMirrorVideoFactory();
+void RegisterUnpackVideoFactory();
+void RegisterPackVideoFactory();
+void RegisterJoinVideoFactory();
+void RegisterMergeVideoFactory();
+void RegisterJsonVideoFactory();
+void RegisterThreadVideoFactory();
+void RegisterFirewireVideoFactory();
+void RegisterV4lVideoFactory();
+void RegisterFfmpegVideoFactory();
+void RegisterFfmpegVideoOutputFactory();
+void RegisterOpenNiVideoFactory();
+void RegisterOpenNi2VideoFactory();
+
+inline bool LoadBuiltInVideoDrivers()
+{
+ RegisterTestVideoFactory();
+ RegisterImagesVideoFactory();
+ RegisterImagesVideoOutputFactory();
+ RegisterSplitVideoFactory();
+ RegisterTruncateVideoFactory();
+ RegisterPvnVideoFactory();
+ RegisterPangoVideoFactory();
+ RegisterPangoVideoOutputFactory();
+ RegisterDebayerVideoFactory();
+ RegisterShiftVideoFactory();
+ RegisterMirrorVideoFactory();
+ RegisterUnpackVideoFactory();
+ RegisterPackVideoFactory();
+ RegisterJoinVideoFactory();
+ RegisterMergeVideoFactory();
+ RegisterJsonVideoFactory();
+ RegisterThreadVideoFactory();
+ RegisterFirewireVideoFactory();
+ RegisterV4lVideoFactory();
+ RegisterFfmpegVideoFactory();
+ RegisterFfmpegVideoOutputFactory();
+ RegisterOpenNiVideoFactory();
+ RegisterOpenNi2VideoFactory();
+ return true;
+}
+
+} // pangolin
diff --git a/externals/Pangolin-config/pangolin/window_frameworks.h b/externals/Pangolin-config/pangolin/window_frameworks.h
new file mode 100644
index 0000000000000000000000000000000000000000..d1206723db04f8df88edf9c9c4b1a12af2dea4aa
--- /dev/null
+++ b/externals/Pangolin-config/pangolin/window_frameworks.h
@@ -0,0 +1,17 @@
+// CMake generated file. Do Not Edit.
+
+#pragma once
+
+namespace pangolin {
+
+void RegisterNoneWindowFactory();
+void RegisterX11WindowFactory();
+
+inline bool LoadBuiltInWindowFrameworks()
+{
+ RegisterNoneWindowFactory();
+ RegisterX11WindowFactory();
+ return true;
+}
+
+} // pangolin
diff --git a/externals/Pangolin/LICENCE b/externals/Pangolin/LICENCE
new file mode 100644
index 0000000000000000000000000000000000000000..9b7376cdce77b67354fa903334f8aaf01a467f17
--- /dev/null
+++ b/externals/Pangolin/LICENCE
@@ -0,0 +1,22 @@
+Copyright (c) 2011 Steven Lovegrove and Richard Newcombe
+
+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.
diff --git a/externals/Pangolin/README.md b/externals/Pangolin/README.md
new file mode 100644
index 0000000000000000000000000000000000000000..254f64920cb97b991f900b63b9ffd1b2df0d797d
--- /dev/null
+++ b/externals/Pangolin/README.md
@@ -0,0 +1,172 @@
+What is Pangolin {#mainpage}
+====================================
+
+Pangolin is a lightweight portable rapid development library for managing OpenGL
+display / interaction and abstracting video input. At its heart is a simple
+OpenGl viewport manager which can help to modularise 3D visualisation without
+adding to its complexity, and offers an advanced but intuitive 3D navigation
+handler. Pangolin also provides a mechanism for manipulating program variables
+through config files and ui integration, and has a flexible real-time plotter
+for visualising graphical data.
+
+The ethos of Pangolin is to reduce the boilerplate code that normally
+gets written to visualise and interact with (typically image and 3D
+based) systems, without compromising performance. It also enables write-once
+code for a number of platforms, currently including Windows, Linux, OSX, Android
+and IOS.
+
+## Code ##
+
+Find the latest version on [Github](http://github.com/stevenlovegrove/Pangolin):
+
+```
+git clone https://github.com/stevenlovegrove/Pangolin.git
+```
+
+## Dependencies ##
+
+Optional dependencies are enabled when found, otherwise they are silently disabled.
+Check the CMake configure output for details.
+
+### Required Dependencies ###
+
+* C++11
+
+* OpenGL (Desktop / ES / ES2)
+
+* Glew
+ * (win) built automatically (assuming git is on your path)
+ * (deb) `sudo apt-get install libglew-dev`
+ * (mac) `sudo port install glew`
+
+* CMake (for build environment)
+ * (win) http://www.cmake.org/cmake/resources/software.html
+ * (deb) `sudo apt-get install cmake`
+ * (mac) `sudo port install cmake`
+
+### Recommended Dependencies ###
+
+* Python2 / Python3, for drop-down interactive console
+ * (win) http://www.python.org/downloads/windows
+ * (deb) `sudo apt-get install libpython2.7-dev`
+ * (mac) preinstalled with osx
+ * (for pybind11) `git submodule init && git submodule update`
+ * (useful modules) `sudo python -mpip install numpy pyopengl Pillow pybind11`
+
+
+### Optional Dependencies for video input ###
+
+* FFMPEG (For video decoding and image rescaling)
+ * (deb) `sudo apt-get install ffmpeg libavcodec-dev libavutil-dev libavformat-dev libswscale-dev libavdevice-dev`
+
+* DC1394 (For firewire input)
+ * (deb) `sudo apt-get install libdc1394-22-dev libraw1394-dev`
+
+* libuvc (For cross-platform webcam video input via libusb)
+ * git://github.com/ktossell/libuvc.git
+
+* libjpeg, libpng, libtiff, libopenexr (For reading still-image sequences)
+ * (deb) `sudo apt-get install libjpeg-dev libpng12-dev libtiff5-dev libopenexr-dev`
+
+* OpenNI / OpenNI2 (For Kinect / Xtrion / Primesense capture)
+
+* DepthSense SDK
+
+### Very Optional Dependencies ###
+
+* Eigen / TooN (These matrix types supported in the Pangolin API.)
+
+* CUDA Toolkit >= 3.2 (Some CUDA header-only interop utilities included)
+ * http://developer.nvidia.com/cuda-downloads
+
+* Doxygen for generating html / pdf documentation.
+
+## Building ##
+
+Pangolin uses the CMake portable pre-build tool. To checkout and build pangolin in the
+directory 'build', execute the following at a shell (or the equivelent using a GUI):
+
+```
+git clone https://github.com/stevenlovegrove/Pangolin.git
+cd Pangolin
+mkdir build
+cd build
+cmake ..
+cmake --build .
+```
+
+If you would like to build the documentation and you have Doxygen installed, you
+can execute:
+
+```
+cmake --build . --target doc
+```
+
+**On Windows**, Pangolin will attempt to download and build *glew*, *libjpeg*, *libpng* and *zlib* automatically. It does so assuming that git is available on the path - this assumption may be wrong for windows users who have downloaded Pangolin via a zip file on github. You will instead need to download and compile the dependencies manually, and set the BUILD_EXTERN_(lib) options to false for these libraries. The alternate and recommended approach is to install [gitbash](https://git-scm.com/downloads) and work from within their provided console.
+
+## Issues ##
+
+Please visit [Github Issues](https://github.com/stevenlovegrove/Pangolin/issues) to view and report problems with Pangolin. Issues and pull requests should be raised against the master branch which contains the current development version.
+
+Please note; most Pangolin dependencies are optional - to disable a dependency which may be causing trouble on your machine, set the BUILD_PANGOLIN_(option) variable to false with a cmake configuration tool (e.g. ccmake or cmake-gui).
+
+## Contributions and Continuous Integration ##
+
+For CI, Pangolin uses [travis-ci.org](https://travis-ci.org/stevenlovegrove/Pangolin) for Ubuntu, OSX and [ci.appveyor.com](https://ci.appveyor.com/project/stevenlovegrove/pangolin) for Windows.
+
+To contribute to Pangolin, I would appreciate pull requests against the master branch. This will trigger CI builds for your changes automatically, and help me to merge with confidence.
+
+## Binaries ##
+
+Binaries are available for Windows x64, as output by the Windows CI server: [Appveyor Artifacts](https://ci.appveyor.com/project/stevenlovegrove/pangolin/build/artifacts).
+
+## Bindings ##
+
+### Python ###
+
+Pangolin python bindings are enabled via [pybind11](www.pybind11.com). These bindings can be used both standalone and from within Pangolin's drop-down console (press the back-tick key, `).
+
+To enable the bindings, you must checkout the pybind submodule. To use pangolin in python, it's recommend to install a few other python packages too:
+
+```
+sudo python -mpip install numpy pyopengl Pillow pybind11
+git submodule init && git submodule update
+```
+
+The python module pypangolin must be on your python path, either through installation, or by setting it explicitly:
+
+```
+import sys
+sys.path.append('path/of/pypangolin.so')
+```
+
+## Scheme syntax for windowing and video
+
+Pangolin uses 'URI' syntax for modularising video drivers and windowing backends. The syntax follows along the lines of `module_name:[option1=value1,option2=value2,...]//module_resource_to_open`. Some examples for using this URI syntax with the VideoViewer tool is as follows:
+
+```
+VideoViewer test://
+VideoViewer uvc:[size=640x480]///dev/video0
+VideoViewer flip://debayer:[tile=rggb,method=downsample]//file://~/somefile.pango
+```
+
+Notice that for video, some modules support chaining to construct a simple filter graph. See include/pangolin/video/video.h for more examples.
+
+For windowing, you can also customize default arguments for Pangolin applications by setting the `PANGOLIN_WINDOW_URI` environment variable. For instance, on high-DPI screens (in this example on OSX), you could set:
+
+
+```
+setenv PANGOLIN_WINDOW_URI "cocoa:[HIGHRES=true]//"
+```
+
+Some window parameters that may be interesting to override are `DISPLAYNAME`, `DOUBLEBUFFER`, `SAMPLE_BUFFERS`, `SAMPLES`, `HIGHRES`. Window modules currently include `x11`, `winapi`, `cocoa`.
+
+## Acknowledgements ##
+
+I'd like to thank the growing number of kind contributors to Pangolin for helping to make it more stable and feature rich. Many features of Pangolin have been influenced by other projects such as GFlags, GLConsole, and libcvd in particular. I'd also like to thank the FOSS projects on which Pangolin depends.
+
+For a summary of those who have made code contributions, execute:
+
+```
+git shortlog -sne
+```
diff --git a/externals/Pangolin/include/experimental/optional.hpp b/externals/Pangolin/include/experimental/optional.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e3722b0c00e30afc5865aa961176f3c28a22a5d5
--- /dev/null
+++ b/externals/Pangolin/include/experimental/optional.hpp
@@ -0,0 +1,1067 @@
+// Copyright (C) 2011 - 2012 Andrzej Krzemienski.
+//
+// Use, modification, and distribution is subject to the Boost Software
+// License, Version 1.0. (See accompanying file LICENSE_1_0.txt or copy at
+// http://www.boost.org/LICENSE_1_0.txt)
+//
+// The idea and interface is based on Boost.Optional library
+// authored by Fernando Luis Cacciola Carballal
+
+# ifndef ___OPTIONAL_HPP___
+# define ___OPTIONAL_HPP___
+
+# include <utility>
+# include <type_traits>
+# include <initializer_list>
+# include <cassert>
+# include <functional>
+# include <string>
+# include <stdexcept>
+
+# define TR2_OPTIONAL_REQUIRES(...) typename enable_if<__VA_ARGS__::value, bool>::type = false
+
+# if defined __GNUC__ // NOTE: GNUC is also defined for Clang
+# if (__GNUC__ == 4) && (__GNUC_MINOR__ >= 8)
+# define TR2_OPTIONAL_GCC_4_8_AND_HIGHER___
+# elif (__GNUC__ > 4)
+# define TR2_OPTIONAL_GCC_4_8_AND_HIGHER___
+# endif
+#
+# if (__GNUC__ == 4) && (__GNUC_MINOR__ >= 7)
+# define TR2_OPTIONAL_GCC_4_7_AND_HIGHER___
+# elif (__GNUC__ > 4)
+# define TR2_OPTIONAL_GCC_4_7_AND_HIGHER___
+# endif
+#
+# if (__GNUC__ == 4) && (__GNUC_MINOR__ == 8) && (__GNUC_PATCHLEVEL__ >= 1)
+# define TR2_OPTIONAL_GCC_4_8_1_AND_HIGHER___
+# elif (__GNUC__ == 4) && (__GNUC_MINOR__ >= 9)
+# define TR2_OPTIONAL_GCC_4_8_1_AND_HIGHER___
+# elif (__GNUC__ > 4)
+# define TR2_OPTIONAL_GCC_4_8_1_AND_HIGHER___
+# endif
+# endif
+#
+# if defined __clang_major__
+# if (__clang_major__ == 3 && __clang_minor__ >= 5)
+# define TR2_OPTIONAL_CLANG_3_5_AND_HIGHTER_
+# elif (__clang_major__ > 3)
+# define TR2_OPTIONAL_CLANG_3_5_AND_HIGHTER_
+# endif
+# if defined TR2_OPTIONAL_CLANG_3_5_AND_HIGHTER_
+# define TR2_OPTIONAL_CLANG_3_4_2_AND_HIGHER_
+# elif (__clang_major__ == 3 && __clang_minor__ == 4 && __clang_patchlevel__ >= 2)
+# define TR2_OPTIONAL_CLANG_3_4_2_AND_HIGHER_
+# endif
+# endif
+#
+# if defined _MSC_VER
+# if (_MSC_VER >= 1900)
+# define TR2_OPTIONAL_MSVC_2015_AND_HIGHER___
+# endif
+# endif
+
+# if defined __clang__
+# if (__clang_major__ > 2) || (__clang_major__ == 2) && (__clang_minor__ >= 9)
+# define OPTIONAL_HAS_THIS_RVALUE_REFS 1
+# else
+# define OPTIONAL_HAS_THIS_RVALUE_REFS 0
+# endif
+# elif defined TR2_OPTIONAL_GCC_4_8_1_AND_HIGHER___
+# define OPTIONAL_HAS_THIS_RVALUE_REFS 1
+# elif defined TR2_OPTIONAL_MSVC_2015_AND_HIGHER___
+# define OPTIONAL_HAS_THIS_RVALUE_REFS 1
+# else
+# define OPTIONAL_HAS_THIS_RVALUE_REFS 0
+# endif
+
+
+# if defined TR2_OPTIONAL_GCC_4_8_1_AND_HIGHER___
+# define OPTIONAL_HAS_CONSTEXPR_INIT_LIST 1
+# define OPTIONAL_CONSTEXPR_INIT_LIST constexpr
+# else
+# define OPTIONAL_HAS_CONSTEXPR_INIT_LIST 0
+# define OPTIONAL_CONSTEXPR_INIT_LIST
+# endif
+
+# if defined TR2_OPTIONAL_CLANG_3_5_AND_HIGHTER_ && (defined __cplusplus) && (__cplusplus != 201103L)
+# define OPTIONAL_HAS_MOVE_ACCESSORS 1
+# else
+# define OPTIONAL_HAS_MOVE_ACCESSORS 0
+# endif
+
+# // In C++11 constexpr implies const, so we need to make non-const members also non-constexpr
+# if (defined __cplusplus) && (__cplusplus == 201103L)
+# define OPTIONAL_MUTABLE_CONSTEXPR
+# else
+# define OPTIONAL_MUTABLE_CONSTEXPR constexpr
+# endif
+
+namespace std{
+
+namespace experimental{
+
+// BEGIN workaround for missing is_trivially_destructible
+# if defined TR2_OPTIONAL_GCC_4_8_AND_HIGHER___
+ // leave it: it is already there
+# elif defined TR2_OPTIONAL_CLANG_3_4_2_AND_HIGHER_
+ // leave it: it is already there
+# elif defined TR2_OPTIONAL_MSVC_2015_AND_HIGHER___
+ // leave it: it is already there
+# elif defined TR2_OPTIONAL_DISABLE_EMULATION_OF_TYPE_TRAITS
+ // leave it: the user doesn't want it
+# else
+ template <typename T>
+ using is_trivially_destructible = std::has_trivial_destructor<T>;
+# endif
+// END workaround for missing is_trivially_destructible
+
+# if (defined TR2_OPTIONAL_GCC_4_7_AND_HIGHER___)
+ // leave it; our metafunctions are already defined.
+# elif defined TR2_OPTIONAL_CLANG_3_4_2_AND_HIGHER_
+ // leave it; our metafunctions are already defined.
+# elif defined TR2_OPTIONAL_MSVC_2015_AND_HIGHER___
+ // leave it: it is already there
+# elif defined TR2_OPTIONAL_DISABLE_EMULATION_OF_TYPE_TRAITS
+ // leave it: the user doesn't want it
+# else
+
+
+// workaround for missing traits in GCC and CLANG
+template <class T>
+struct is_nothrow_move_constructible
+{
+ constexpr static bool value = std::is_nothrow_constructible<T, T&&>::value;
+};
+
+
+template <class T, class U>
+struct is_assignable
+{
+ template <class X, class Y>
+ constexpr static bool has_assign(...) { return false; }
+
+ template <class X, class Y, size_t S = sizeof((std::declval<X>() = std::declval<Y>(), true)) >
+ // the comma operator is necessary for the cases where operator= returns void
+ constexpr static bool has_assign(bool) { return true; }
+
+ constexpr static bool value = has_assign<T, U>(true);
+};
+
+
+template <class T>
+struct is_nothrow_move_assignable
+{
+ template <class X, bool has_any_move_assign>
+ struct has_nothrow_move_assign {
+ constexpr static bool value = false;
+ };
+
+ template <class X>
+ struct has_nothrow_move_assign<X, true> {
+ constexpr static bool value = noexcept( std::declval<X&>() = std::declval<X&&>() );
+ };
+
+ constexpr static bool value = has_nothrow_move_assign<T, is_assignable<T&, T&&>::value>::value;
+};
+// end workaround
+
+
+# endif
+
+
+
+// 20.5.4, optional for object types
+template <class T> class optional;
+
+// 20.5.5, optional for lvalue reference types
+template <class T> class optional<T&>;
+
+
+// workaround: std utility functions aren't constexpr yet
+template <class T> inline constexpr T&& constexpr_forward(typename std::remove_reference<T>::type& t) noexcept
+{
+ return static_cast<T&&>(t);
+}
+
+template <class T> inline constexpr T&& constexpr_forward(typename std::remove_reference<T>::type&& t) noexcept
+{
+ static_assert(!std::is_lvalue_reference<T>::value, "!!");
+ return static_cast<T&&>(t);
+}
+
+template <class T> inline constexpr typename std::remove_reference<T>::type&& constexpr_move(T&& t) noexcept
+{
+ return static_cast<typename std::remove_reference<T>::type&&>(t);
+}
+
+
+#if defined NDEBUG
+# define TR2_OPTIONAL_ASSERTED_EXPRESSION(CHECK, EXPR) (EXPR)
+#else
+# define TR2_OPTIONAL_ASSERTED_EXPRESSION(CHECK, EXPR) ((CHECK) ? (EXPR) : ([]{assert(!#CHECK);}(), (EXPR)))
+#endif
+
+
+namespace detail_
+{
+
+// static_addressof: a constexpr version of addressof
+template <typename T>
+struct has_overloaded_addressof
+{
+ template <class X>
+ constexpr static bool has_overload(...) { return false; }
+
+ template <class X, size_t S = sizeof(std::declval<X&>().operator&()) >
+ constexpr static bool has_overload(bool) { return true; }
+
+ constexpr static bool value = has_overload<T>(true);
+};
+
+template <typename T, TR2_OPTIONAL_REQUIRES(!has_overloaded_addressof<T>)>
+constexpr T* static_addressof(T& ref)
+{
+ return &ref;
+}
+
+template <typename T, TR2_OPTIONAL_REQUIRES(has_overloaded_addressof<T>)>
+T* static_addressof(T& ref)
+{
+ return std::addressof(ref);
+}
+
+
+// the call to convert<A>(b) has return type A and converts b to type A iff b decltype(b) is implicitly convertible to A
+template <class U>
+constexpr U convert(U v) { return v; }
+
+
+namespace swap_ns
+{
+ using std::swap;
+
+ template <class T>
+ void adl_swap(T& t, T& u) noexcept(noexcept(swap(t, u)))
+ {
+ swap(t, u);
+ }
+
+} // namespace swap_ns
+
+} // namespace detail
+
+
+constexpr struct trivial_init_t{} trivial_init{};
+
+
+// 20.5.6, In-place construction
+constexpr struct in_place_t{} in_place{};
+
+
+// 20.5.7, Disengaged state indicator
+struct nullopt_t
+{
+ struct init{};
+ constexpr explicit nullopt_t(init){}
+};
+constexpr nullopt_t nullopt{nullopt_t::init()};
+
+
+// 20.5.8, class bad_optional_access
+class bad_optional_access : public logic_error {
+public:
+ explicit bad_optional_access(const string& what_arg) : logic_error{what_arg} {}
+ explicit bad_optional_access(const char* what_arg) : logic_error{what_arg} {}
+};
+
+
+template <class T>
+union storage_t
+{
+ unsigned char dummy_;
+ T value_;
+
+ constexpr storage_t( trivial_init_t ) noexcept : dummy_() {};
+
+ template <class... Args>
+ constexpr storage_t( Args&&... args ) : value_(constexpr_forward<Args>(args)...) {}
+
+ ~storage_t(){}
+};
+
+
+template <class T>
+union constexpr_storage_t
+{
+ unsigned char dummy_;
+ T value_;
+
+ constexpr constexpr_storage_t( trivial_init_t ) noexcept : dummy_() {};
+
+ template <class... Args>
+ constexpr constexpr_storage_t( Args&&... args ) : value_(constexpr_forward<Args>(args)...) {}
+
+ ~constexpr_storage_t() = default;
+};
+
+
+template <class T>
+struct optional_base
+{
+ bool init_;
+ storage_t<T> storage_;
+
+ constexpr optional_base() noexcept : init_(false), storage_(trivial_init) {};
+
+ explicit constexpr optional_base(const T& v) : init_(true), storage_(v) {}
+
+ explicit constexpr optional_base(T&& v) : init_(true), storage_(constexpr_move(v)) {}
+
+ template <class... Args> explicit optional_base(in_place_t, Args&&... args)
+ : init_(true), storage_(constexpr_forward<Args>(args)...) {}
+
+ template <class U, class... Args, TR2_OPTIONAL_REQUIRES(is_constructible<T, std::initializer_list<U>>)>
+ explicit optional_base(in_place_t, std::initializer_list<U> il, Args&&... args)
+ : init_(true), storage_(il, std::forward<Args>(args)...) {}
+
+ ~optional_base() { if (init_) storage_.value_.T::~T(); }
+};
+
+
+template <class T>
+struct constexpr_optional_base
+{
+ bool init_;
+ constexpr_storage_t<T> storage_;
+
+ constexpr constexpr_optional_base() noexcept : init_(false), storage_(trivial_init) {};
+
+ explicit constexpr constexpr_optional_base(const T& v) : init_(true), storage_(v) {}
+
+ explicit constexpr constexpr_optional_base(T&& v) : init_(true), storage_(constexpr_move(v)) {}
+
+ template <class... Args> explicit constexpr constexpr_optional_base(in_place_t, Args&&... args)
+ : init_(true), storage_(constexpr_forward<Args>(args)...) {}
+
+ template <class U, class... Args, TR2_OPTIONAL_REQUIRES(is_constructible<T, std::initializer_list<U>>)>
+ OPTIONAL_CONSTEXPR_INIT_LIST explicit constexpr_optional_base(in_place_t, std::initializer_list<U> il, Args&&... args)
+ : init_(true), storage_(il, std::forward<Args>(args)...) {}
+
+ ~constexpr_optional_base() = default;
+};
+
+template <class T>
+using OptionalBase = typename std::conditional<
+ is_trivially_destructible<T>::value, // if possible
+ constexpr_optional_base<typename std::remove_const<T>::type>, // use base with trivial destructor
+ optional_base<typename std::remove_const<T>::type>
+>::type;
+
+
+
+template <class T>
+class optional : private OptionalBase<T>
+{
+ static_assert( !std::is_same<typename std::decay<T>::type, nullopt_t>::value, "bad T" );
+ static_assert( !std::is_same<typename std::decay<T>::type, in_place_t>::value, "bad T" );
+
+
+ constexpr bool initialized() const noexcept { return OptionalBase<T>::init_; }
+ typename std::remove_const<T>::type* dataptr() { return std::addressof(OptionalBase<T>::storage_.value_); }
+ constexpr const T* dataptr() const { return detail_::static_addressof(OptionalBase<T>::storage_.value_); }
+
+# if OPTIONAL_HAS_THIS_RVALUE_REFS == 1
+ constexpr const T& contained_val() const& { return OptionalBase<T>::storage_.value_; }
+# if OPTIONAL_HAS_MOVE_ACCESSORS == 1
+ OPTIONAL_MUTABLE_CONSTEXPR T&& contained_val() && { return std::move(OptionalBase<T>::storage_.value_); }
+ OPTIONAL_MUTABLE_CONSTEXPR T& contained_val() & { return OptionalBase<T>::storage_.value_; }
+# else
+ T& contained_val() & { return OptionalBase<T>::storage_.value_; }
+ T&& contained_val() && { return std::move(OptionalBase<T>::storage_.value_); }
+# endif
+# else
+ constexpr const T& contained_val() const { return OptionalBase<T>::storage_.value_; }
+ T& contained_val() { return OptionalBase<T>::storage_.value_; }
+# endif
+
+ void clear() noexcept {
+ if (initialized()) dataptr()->T::~T();
+ OptionalBase<T>::init_ = false;
+ }
+
+ template <class... Args>
+ void initialize(Args&&... args) noexcept(noexcept(T(std::forward<Args>(args)...)))
+ {
+ assert(!OptionalBase<T>::init_);
+ ::new (static_cast<void*>(dataptr())) T(std::forward<Args>(args)...);
+ OptionalBase<T>::init_ = true;
+ }
+
+ template <class U, class... Args>
+ void initialize(std::initializer_list<U> il, Args&&... args) noexcept(noexcept(T(il, std::forward<Args>(args)...)))
+ {
+ assert(!OptionalBase<T>::init_);
+ ::new (static_cast<void*>(dataptr())) T(il, std::forward<Args>(args)...);
+ OptionalBase<T>::init_ = true;
+ }
+
+public:
+ typedef T value_type;
+
+ // 20.5.5.1, constructors
+ constexpr optional() noexcept : OptionalBase<T>() {};
+ constexpr optional(nullopt_t) noexcept : OptionalBase<T>() {};
+
+ optional(const optional& rhs)
+ : OptionalBase<T>()
+ {
+ if (rhs.initialized()) {
+ ::new (static_cast<void*>(dataptr())) T(*rhs);
+ OptionalBase<T>::init_ = true;
+ }
+ }
+
+ optional(optional&& rhs) noexcept(is_nothrow_move_constructible<T>::value)
+ : OptionalBase<T>()
+ {
+ if (rhs.initialized()) {
+ ::new (static_cast<void*>(dataptr())) T(std::move(*rhs));
+ OptionalBase<T>::init_ = true;
+ }
+ }
+
+ constexpr optional(const T& v) : OptionalBase<T>(v) {}
+
+ constexpr optional(T&& v) : OptionalBase<T>(constexpr_move(v)) {}
+
+ template <class... Args>
+ explicit constexpr optional(in_place_t, Args&&... args)
+ : OptionalBase<T>(in_place_t{}, constexpr_forward<Args>(args)...) {}
+
+ template <class U, class... Args, TR2_OPTIONAL_REQUIRES(is_constructible<T, std::initializer_list<U>>)>
+ OPTIONAL_CONSTEXPR_INIT_LIST explicit optional(in_place_t, std::initializer_list<U> il, Args&&... args)
+ : OptionalBase<T>(in_place_t{}, il, constexpr_forward<Args>(args)...) {}
+
+ // 20.5.4.2, Destructor
+ ~optional() = default;
+
+ // 20.5.4.3, assignment
+ optional& operator=(nullopt_t) noexcept
+ {
+ clear();
+ return *this;
+ }
+
+ optional& operator=(const optional& rhs)
+ {
+ if (initialized() == true && rhs.initialized() == false) clear();
+ else if (initialized() == false && rhs.initialized() == true) initialize(*rhs);
+ else if (initialized() == true && rhs.initialized() == true) contained_val() = *rhs;
+ return *this;
+ }
+
+ optional& operator=(optional&& rhs)
+ noexcept(is_nothrow_move_assignable<T>::value && is_nothrow_move_constructible<T>::value)
+ {
+ if (initialized() == true && rhs.initialized() == false) clear();
+ else if (initialized() == false && rhs.initialized() == true) initialize(std::move(*rhs));
+ else if (initialized() == true && rhs.initialized() == true) contained_val() = std::move(*rhs);
+ return *this;
+ }
+
+ template <class U>
+ auto operator=(U&& v)
+ -> typename enable_if
+ <
+ is_same<typename decay<U>::type, T>::value,
+ optional&
+ >::type
+ {
+ if (initialized()) { contained_val() = std::forward<U>(v); }
+ else { initialize(std::forward<U>(v)); }
+ return *this;
+ }
+
+
+ template <class... Args>
+ void emplace(Args&&... args)
+ {
+ clear();
+ initialize(std::forward<Args>(args)...);
+ }
+
+ template <class U, class... Args>
+ void emplace(initializer_list<U> il, Args&&... args)
+ {
+ clear();
+ initialize<U, Args...>(il, std::forward<Args>(args)...);
+ }
+
+ // 20.5.4.4, Swap
+ void swap(optional<T>& rhs) noexcept(is_nothrow_move_constructible<T>::value
+ && noexcept(detail_::swap_ns::adl_swap(declval<T&>(), declval<T&>())))
+ {
+ if (initialized() == true && rhs.initialized() == false) { rhs.initialize(std::move(**this)); clear(); }
+ else if (initialized() == false && rhs.initialized() == true) { initialize(std::move(*rhs)); rhs.clear(); }
+ else if (initialized() == true && rhs.initialized() == true) { using std::swap; swap(**this, *rhs); }
+ }
+
+ // 20.5.4.5, Observers
+
+ explicit constexpr operator bool() const noexcept { return initialized(); }
+ constexpr bool has_value() const noexcept { return initialized(); }
+
+ constexpr T const* operator ->() const {
+ return TR2_OPTIONAL_ASSERTED_EXPRESSION(initialized(), dataptr());
+ }
+
+# if OPTIONAL_HAS_MOVE_ACCESSORS == 1
+
+ OPTIONAL_MUTABLE_CONSTEXPR T* operator ->() {
+ assert (initialized());
+ return dataptr();
+ }
+
+ constexpr T const& operator *() const& {
+ return TR2_OPTIONAL_ASSERTED_EXPRESSION(initialized(), contained_val());
+ }
+
+ OPTIONAL_MUTABLE_CONSTEXPR T& operator *() & {
+ assert (initialized());
+ return contained_val();
+ }
+
+ OPTIONAL_MUTABLE_CONSTEXPR T&& operator *() && {
+ assert (initialized());
+ return constexpr_move(contained_val());
+ }
+
+ constexpr T const& value() const& {
+ return initialized() ? contained_val() : (throw bad_optional_access("bad optional access"), contained_val());
+ }
+
+ OPTIONAL_MUTABLE_CONSTEXPR T& value() & {
+ return initialized() ? contained_val() : (throw bad_optional_access("bad optional access"), contained_val());
+ }
+
+ OPTIONAL_MUTABLE_CONSTEXPR T&& value() && {
+ if (!initialized()) throw bad_optional_access("bad optional access");
+ return std::move(contained_val());
+ }
+
+# else
+
+ T* operator ->() {
+ assert (initialized());
+ return dataptr();
+ }
+
+ constexpr T const& operator *() const {
+ return TR2_OPTIONAL_ASSERTED_EXPRESSION(initialized(), contained_val());
+ }
+
+ T& operator *() {
+ assert (initialized());
+ return contained_val();
+ }
+
+ constexpr T const& value() const {
+ return initialized() ? contained_val() : (throw bad_optional_access("bad optional access"), contained_val());
+ }
+
+ T& value() {
+ return initialized() ? contained_val() : (throw bad_optional_access("bad optional access"), contained_val());
+ }
+
+# endif
+
+# if OPTIONAL_HAS_THIS_RVALUE_REFS == 1
+
+ template <class V>
+ constexpr T value_or(V&& v) const&
+ {
+ return *this ? **this : detail_::convert<T>(constexpr_forward<V>(v));
+ }
+
+# if OPTIONAL_HAS_MOVE_ACCESSORS == 1
+
+ template <class V>
+ OPTIONAL_MUTABLE_CONSTEXPR T value_or(V&& v) &&
+ {
+ return *this ? constexpr_move(const_cast<optional<T>&>(*this).contained_val()) : detail_::convert<T>(constexpr_forward<V>(v));
+ }
+
+# else
+
+ template <class V>
+ T value_or(V&& v) &&
+ {
+ return *this ? constexpr_move(const_cast<optional<T>&>(*this).contained_val()) : detail_::convert<T>(constexpr_forward<V>(v));
+ }
+
+# endif
+
+# else
+
+ template <class V>
+ constexpr T value_or(V&& v) const
+ {
+ return *this ? **this : detail_::convert<T>(constexpr_forward<V>(v));
+ }
+
+# endif
+
+ // 20.6.3.6, modifiers
+ void reset() noexcept { clear(); }
+};
+
+
+template <class T>
+class optional<T&>
+{
+ static_assert( !std::is_same<T, nullopt_t>::value, "bad T" );
+ static_assert( !std::is_same<T, in_place_t>::value, "bad T" );
+ T* ref;
+
+public:
+
+ // 20.5.5.1, construction/destruction
+ constexpr optional() noexcept : ref(nullptr) {}
+
+ constexpr optional(nullopt_t) noexcept : ref(nullptr) {}
+
+ constexpr optional(T& v) noexcept : ref(detail_::static_addressof(v)) {}
+
+ optional(T&&) = delete;
+
+ constexpr optional(const optional& rhs) noexcept : ref(rhs.ref) {}
+
+ explicit constexpr optional(in_place_t, T& v) noexcept : ref(detail_::static_addressof(v)) {}
+
+ explicit optional(in_place_t, T&&) = delete;
+
+ ~optional() = default;
+
+ // 20.5.5.2, mutation
+ optional& operator=(nullopt_t) noexcept {
+ ref = nullptr;
+ return *this;
+ }
+
+ // optional& operator=(const optional& rhs) noexcept {
+ // ref = rhs.ref;
+ // return *this;
+ // }
+
+ // optional& operator=(optional&& rhs) noexcept {
+ // ref = rhs.ref;
+ // return *this;
+ // }
+
+ template <typename U>
+ auto operator=(U&& rhs) noexcept
+ -> typename enable_if
+ <
+ is_same<typename decay<U>::type, optional<T&>>::value,
+ optional&
+ >::type
+ {
+ ref = rhs.ref;
+ return *this;
+ }
+
+ template <typename U>
+ auto operator=(U&& rhs) noexcept
+ -> typename enable_if
+ <
+ !is_same<typename decay<U>::type, optional<T&>>::value,
+ optional&
+ >::type
+ = delete;
+
+ void emplace(T& v) noexcept {
+ ref = detail_::static_addressof(v);
+ }
+
+ void emplace(T&&) = delete;
+
+
+ void swap(optional<T&>& rhs) noexcept
+ {
+ std::swap(ref, rhs.ref);
+ }
+
+ // 20.5.5.3, observers
+ constexpr T* operator->() const {
+ return TR2_OPTIONAL_ASSERTED_EXPRESSION(ref, ref);
+ }
+
+ constexpr T& operator*() const {
+ return TR2_OPTIONAL_ASSERTED_EXPRESSION(ref, *ref);
+ }
+
+ constexpr T& value() const {
+ return ref ? *ref : (throw bad_optional_access("bad optional access"), *ref);
+ }
+
+ explicit constexpr operator bool() const noexcept {
+ return ref != nullptr;
+ }
+
+ constexpr bool has_value() const noexcept {
+ return ref != nullptr;
+ }
+
+ template <class V>
+ constexpr typename decay<T>::type value_or(V&& v) const
+ {
+ return *this ? **this : detail_::convert<typename decay<T>::type>(constexpr_forward<V>(v));
+ }
+
+ // x.x.x.x, modifiers
+ void reset() noexcept { ref = nullptr; }
+};
+
+
+template <class T>
+class optional<T&&>
+{
+ static_assert( sizeof(T) == 0, "optional rvalue references disallowed" );
+};
+
+
+// 20.5.8, Relational operators
+template <class T> constexpr bool operator==(const optional<T>& x, const optional<T>& y)
+{
+ return bool(x) != bool(y) ? false : bool(x) == false ? true : *x == *y;
+}
+
+template <class T> constexpr bool operator!=(const optional<T>& x, const optional<T>& y)
+{
+ return !(x == y);
+}
+
+template <class T> constexpr bool operator<(const optional<T>& x, const optional<T>& y)
+{
+ return (!y) ? false : (!x) ? true : *x < *y;
+}
+
+template <class T> constexpr bool operator>(const optional<T>& x, const optional<T>& y)
+{
+ return (y < x);
+}
+
+template <class T> constexpr bool operator<=(const optional<T>& x, const optional<T>& y)
+{
+ return !(y < x);
+}
+
+template <class T> constexpr bool operator>=(const optional<T>& x, const optional<T>& y)
+{
+ return !(x < y);
+}
+
+
+// 20.5.9, Comparison with nullopt
+template <class T> constexpr bool operator==(const optional<T>& x, nullopt_t) noexcept
+{
+ return (!x);
+}
+
+template <class T> constexpr bool operator==(nullopt_t, const optional<T>& x) noexcept
+{
+ return (!x);
+}
+
+template <class T> constexpr bool operator!=(const optional<T>& x, nullopt_t) noexcept
+{
+ return bool(x);
+}
+
+template <class T> constexpr bool operator!=(nullopt_t, const optional<T>& x) noexcept
+{
+ return bool(x);
+}
+
+template <class T> constexpr bool operator<(const optional<T>&, nullopt_t) noexcept
+{
+ return false;
+}
+
+template <class T> constexpr bool operator<(nullopt_t, const optional<T>& x) noexcept
+{
+ return bool(x);
+}
+
+template <class T> constexpr bool operator<=(const optional<T>& x, nullopt_t) noexcept
+{
+ return (!x);
+}
+
+template <class T> constexpr bool operator<=(nullopt_t, const optional<T>&) noexcept
+{
+ return true;
+}
+
+template <class T> constexpr bool operator>(const optional<T>& x, nullopt_t) noexcept
+{
+ return bool(x);
+}
+
+template <class T> constexpr bool operator>(nullopt_t, const optional<T>&) noexcept
+{
+ return false;
+}
+
+template <class T> constexpr bool operator>=(const optional<T>&, nullopt_t) noexcept
+{
+ return true;
+}
+
+template <class T> constexpr bool operator>=(nullopt_t, const optional<T>& x) noexcept
+{
+ return (!x);
+}
+
+
+
+// 20.5.10, Comparison with T
+template <class T> constexpr bool operator==(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x == v : false;
+}
+
+template <class T> constexpr bool operator==(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v == *x : false;
+}
+
+template <class T> constexpr bool operator!=(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x != v : true;
+}
+
+template <class T> constexpr bool operator!=(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v != *x : true;
+}
+
+template <class T> constexpr bool operator<(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x < v : true;
+}
+
+template <class T> constexpr bool operator>(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v > *x : true;
+}
+
+template <class T> constexpr bool operator>(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x > v : false;
+}
+
+template <class T> constexpr bool operator<(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v < *x : false;
+}
+
+template <class T> constexpr bool operator>=(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x >= v : false;
+}
+
+template <class T> constexpr bool operator<=(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v <= *x : false;
+}
+
+template <class T> constexpr bool operator<=(const optional<T>& x, const T& v)
+{
+ return bool(x) ? *x <= v : true;
+}
+
+template <class T> constexpr bool operator>=(const T& v, const optional<T>& x)
+{
+ return bool(x) ? v >= *x : true;
+}
+
+
+// Comparison of optional<T&> with T
+template <class T> constexpr bool operator==(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x == v : false;
+}
+
+template <class T> constexpr bool operator==(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v == *x : false;
+}
+
+template <class T> constexpr bool operator!=(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x != v : true;
+}
+
+template <class T> constexpr bool operator!=(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v != *x : true;
+}
+
+template <class T> constexpr bool operator<(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x < v : true;
+}
+
+template <class T> constexpr bool operator>(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v > *x : true;
+}
+
+template <class T> constexpr bool operator>(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x > v : false;
+}
+
+template <class T> constexpr bool operator<(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v < *x : false;
+}
+
+template <class T> constexpr bool operator>=(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x >= v : false;
+}
+
+template <class T> constexpr bool operator<=(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v <= *x : false;
+}
+
+template <class T> constexpr bool operator<=(const optional<T&>& x, const T& v)
+{
+ return bool(x) ? *x <= v : true;
+}
+
+template <class T> constexpr bool operator>=(const T& v, const optional<T&>& x)
+{
+ return bool(x) ? v >= *x : true;
+}
+
+// Comparison of optional<T const&> with T
+template <class T> constexpr bool operator==(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x == v : false;
+}
+
+template <class T> constexpr bool operator==(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v == *x : false;
+}
+
+template <class T> constexpr bool operator!=(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x != v : true;
+}
+
+template <class T> constexpr bool operator!=(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v != *x : true;
+}
+
+template <class T> constexpr bool operator<(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x < v : true;
+}
+
+template <class T> constexpr bool operator>(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v > *x : true;
+}
+
+template <class T> constexpr bool operator>(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x > v : false;
+}
+
+template <class T> constexpr bool operator<(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v < *x : false;
+}
+
+template <class T> constexpr bool operator>=(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x >= v : false;
+}
+
+template <class T> constexpr bool operator<=(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v <= *x : false;
+}
+
+template <class T> constexpr bool operator<=(const optional<const T&>& x, const T& v)
+{
+ return bool(x) ? *x <= v : true;
+}
+
+template <class T> constexpr bool operator>=(const T& v, const optional<const T&>& x)
+{
+ return bool(x) ? v >= *x : true;
+}
+
+
+// 20.5.12, Specialized algorithms
+template <class T>
+void swap(optional<T>& x, optional<T>& y) noexcept(noexcept(x.swap(y)))
+{
+ x.swap(y);
+}
+
+
+template <class T>
+constexpr optional<typename decay<T>::type> make_optional(T&& v)
+{
+ return optional<typename decay<T>::type>(constexpr_forward<T>(v));
+}
+
+template <class X>
+constexpr optional<X&> make_optional(reference_wrapper<X> v)
+{
+ return optional<X&>(v.get());
+}
+
+
+} // namespace experimental
+} // namespace std
+
+namespace std
+{
+ template <typename T>
+ struct hash<std::experimental::optional<T>>
+ {
+ typedef typename hash<T>::result_type result_type;
+ typedef std::experimental::optional<T> argument_type;
+
+ constexpr result_type operator()(argument_type const& arg) const {
+ return arg ? std::hash<T>{}(*arg) : result_type{};
+ }
+ };
+
+ template <typename T>
+ struct hash<std::experimental::optional<T&>>
+ {
+ typedef typename hash<T>::result_type result_type;
+ typedef std::experimental::optional<T&> argument_type;
+
+ constexpr result_type operator()(argument_type const& arg) const {
+ return arg ? std::hash<T>{}(*arg) : result_type{};
+ }
+ };
+}
+
+# undef TR2_OPTIONAL_REQUIRES
+# undef TR2_OPTIONAL_ASSERTED_EXPRESSION
+
+# endif //___OPTIONAL_HPP___
+
diff --git a/externals/Pangolin/include/mpark/variant.hpp b/externals/Pangolin/include/mpark/variant.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb2cf0615ec350de45271a276dbb93bab98d4308
--- /dev/null
+++ b/externals/Pangolin/include/mpark/variant.hpp
@@ -0,0 +1,2468 @@
+// MPark.Variant
+//
+// Copyright Michael Park, 2015-2017
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
+
+#ifndef MPARK_VARIANT_HPP
+#define MPARK_VARIANT_HPP
+
+/*
+ variant synopsis
+
+namespace std {
+
+ // 20.7.2, class template variant
+ template <class... Types>
+ class variant {
+ public:
+
+ // 20.7.2.1, constructors
+ constexpr variant() noexcept(see below);
+ variant(const variant&);
+ variant(variant&&) noexcept(see below);
+
+ template <class T> constexpr variant(T&&) noexcept(see below);
+
+ template <class T, class... Args>
+ constexpr explicit variant(in_place_type_t<T>, Args&&...);
+
+ template <class T, class U, class... Args>
+ constexpr explicit variant(
+ in_place_type_t<T>, initializer_list<U>, Args&&...);
+
+ template <size_t I, class... Args>
+ constexpr explicit variant(in_place_index_t<I>, Args&&...);
+
+ template <size_t I, class U, class... Args>
+ constexpr explicit variant(
+ in_place_index_t<I>, initializer_list<U>, Args&&...);
+
+ // 20.7.2.2, destructor
+ ~variant();
+
+ // 20.7.2.3, assignment
+ variant& operator=(const variant&);
+ variant& operator=(variant&&) noexcept(see below);
+
+ template <class T> variant& operator=(T&&) noexcept(see below);
+
+ // 20.7.2.4, modifiers
+ template <class T, class... Args>
+ T& emplace(Args&&...);
+
+ template <class T, class U, class... Args>
+ T& emplace(initializer_list<U>, Args&&...);
+
+ template <size_t I, class... Args>
+ variant_alternative<I, variant>& emplace(Args&&...);
+
+ template <size_t I, class U, class... Args>
+ variant_alternative<I, variant>& emplace(initializer_list<U>, Args&&...);
+
+ // 20.7.2.5, value status
+ constexpr bool valueless_by_exception() const noexcept;
+ constexpr size_t index() const noexcept;
+
+ // 20.7.2.6, swap
+ void swap(variant&) noexcept(see below);
+ };
+
+ // 20.7.3, variant helper classes
+ template <class T> struct variant_size; // undefined
+
+ template <class T>
+ constexpr size_t variant_size_v = variant_size<T>::value;
+
+ template <class T> struct variant_size<const T>;
+ template <class T> struct variant_size<volatile T>;
+ template <class T> struct variant_size<const volatile T>;
+
+ template <class... Types>
+ struct variant_size<variant<Types...>>;
+
+ template <size_t I, class T> struct variant_alternative; // undefined
+
+ template <size_t I, class T>
+ using variant_alternative_t = typename variant_alternative<I, T>::type;
+
+ template <size_t I, class T> struct variant_alternative<I, const T>;
+ template <size_t I, class T> struct variant_alternative<I, volatile T>;
+ template <size_t I, class T> struct variant_alternative<I, const volatile T>;
+
+ template <size_t I, class... Types>
+ struct variant_alternative<I, variant<Types...>>;
+
+ constexpr size_t variant_npos = -1;
+
+ // 20.7.4, value access
+ template <class T, class... Types>
+ constexpr bool holds_alternative(const variant<Types...>&) noexcept;
+
+ template <size_t I, class... Types>
+ constexpr variant_alternative_t<I, variant<Types...>>&
+ get(variant<Types...>&);
+
+ template <size_t I, class... Types>
+ constexpr variant_alternative_t<I, variant<Types...>>&&
+ get(variant<Types...>&&);
+
+ template <size_t I, class... Types>
+ constexpr variant_alternative_t<I, variant<Types...>> const&
+ get(const variant<Types...>&);
+
+ template <size_t I, class... Types>
+ constexpr variant_alternative_t<I, variant<Types...>> const&&
+ get(const variant<Types...>&&);
+
+ template <class T, class... Types>
+ constexpr T& get(variant<Types...>&);
+
+ template <class T, class... Types>
+ constexpr T&& get(variant<Types...>&&);
+
+ template <class T, class... Types>
+ constexpr const T& get(const variant<Types...>&);
+
+ template <class T, class... Types>
+ constexpr const T&& get(const variant<Types...>&&);
+
+ template <size_t I, class... Types>
+ constexpr add_pointer_t<variant_alternative_t<I, variant<Types...>>>
+ get_if(variant<Types...>*) noexcept;
+
+ template <size_t I, class... Types>
+ constexpr add_pointer_t<const variant_alternative_t<I, variant<Types...>>>
+ get_if(const variant<Types...>*) noexcept;
+
+ template <class T, class... Types>
+ constexpr add_pointer_t<T>
+ get_if(variant<Types...>*) noexcept;
+
+ template <class T, class... Types>
+ constexpr add_pointer_t<const T>
+ get_if(const variant<Types...>*) noexcept;
+
+ // 20.7.5, relational operators
+ template <class... Types>
+ constexpr bool operator==(const variant<Types...>&, const variant<Types...>&);
+
+ template <class... Types>
+ constexpr bool operator!=(const variant<Types...>&, const variant<Types...>&);
+
+ template <class... Types>
+ constexpr bool operator<(const variant<Types...>&, const variant<Types...>&);
+
+ template <class... Types>
+ constexpr bool operator>(const variant<Types...>&, const variant<Types...>&);
+
+ template <class... Types>
+ constexpr bool operator<=(const variant<Types...>&, const variant<Types...>&);
+
+ template <class... Types>
+ constexpr bool operator>=(const variant<Types...>&, const variant<Types...>&);
+
+ // 20.7.6, visitation
+ template <class Visitor, class... Variants>
+ constexpr see below visit(Visitor&&, Variants&&...);
+
+ // 20.7.7, class monostate
+ struct monostate;
+
+ // 20.7.8, monostate relational operators
+ constexpr bool operator<(monostate, monostate) noexcept;
+ constexpr bool operator>(monostate, monostate) noexcept;
+ constexpr bool operator<=(monostate, monostate) noexcept;
+ constexpr bool operator>=(monostate, monostate) noexcept;
+ constexpr bool operator==(monostate, monostate) noexcept;
+ constexpr bool operator!=(monostate, monostate) noexcept;
+
+ // 20.7.9, specialized algorithms
+ template <class... Types>
+ void swap(variant<Types...>&, variant<Types...>&) noexcept(see below);
+
+ // 20.7.10, class bad_variant_access
+ class bad_variant_access;
+
+ // 20.7.11, hash support
+ template <class T> struct hash;
+ template <class... Types> struct hash<variant<Types...>>;
+ template <> struct hash<monostate>;
+
+} // namespace std
+
+*/
+
+#include <cstddef>
+#include <exception>
+#include <functional>
+#include <initializer_list>
+#include <new>
+#include <type_traits>
+#include <utility>
+
+// MPark.Variant
+//
+// Copyright Michael Park, 2015-2017
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
+
+#ifndef MPARK_CONFIG_HPP
+#define MPARK_CONFIG_HPP
+
+// MSVC 2015 Update 3.
+#if __cplusplus < 201103L && (!defined(_MSC_VER) || _MSC_FULL_VER < 190024210)
+#error "MPark.Variant requires C++11 support."
+#endif
+
+#ifndef __has_builtin
+#define __has_builtin(x) 0
+#endif
+
+#ifndef __has_include
+#define __has_include(x) 0
+#endif
+
+#ifndef __has_feature
+#define __has_feature(x) 0
+#endif
+
+#if __has_builtin(__builtin_addressof) || \
+ (defined(__GNUC__) && __GNUC__ >= 7) || defined(_MSC_VER)
+#define MPARK_BUILTIN_ADDRESSOF
+#endif
+
+#if __has_builtin(__builtin_unreachable)
+#define MPARK_BUILTIN_UNREACHABLE
+#endif
+
+#if __has_builtin(__type_pack_element)
+#define MPARK_TYPE_PACK_ELEMENT
+#endif
+
+#if defined(__cpp_constexpr) && __cpp_constexpr >= 201304
+#define MPARK_CPP14_CONSTEXPR
+#endif
+
+#if __has_feature(cxx_exceptions) || defined(__cpp_exceptions) || \
+ (defined(_MSC_VER) && defined(_CPPUNWIND))
+#define MPARK_EXCEPTIONS
+#endif
+
+#if defined(__cpp_generic_lambdas) || defined(_MSC_VER)
+#define MPARK_GENERIC_LAMBDAS
+#endif
+
+#if defined(__cpp_lib_integer_sequence)
+#define MPARK_INTEGER_SEQUENCE
+#endif
+
+#if defined(__cpp_return_type_deduction) || defined(_MSC_VER)
+#define MPARK_RETURN_TYPE_DEDUCTION
+#endif
+
+#if defined(__cpp_lib_transparent_operators) || defined(_MSC_VER)
+#define MPARK_TRANSPARENT_OPERATORS
+#endif
+
+#if defined(__cpp_variable_templates) || defined(_MSC_VER)
+#define MPARK_VARIABLE_TEMPLATES
+#endif
+
+#if !defined(__GLIBCXX__) || __has_include(<codecvt>) // >= libstdc++-5
+#define MPARK_TRIVIALITY_TYPE_TRAITS
+#endif
+
+#endif // MPARK_CONFIG_HPP
+
+// MPark.Variant
+//
+// Copyright Michael Park, 2015-2017
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
+
+#ifndef MPARK_IN_PLACE_HPP
+#define MPARK_IN_PLACE_HPP
+
+#include <cstddef>
+
+
+namespace mpark {
+
+ struct in_place_t { explicit in_place_t() = default; };
+
+ template <std::size_t I>
+ struct in_place_index_t { explicit in_place_index_t() = default; };
+
+ template <typename T>
+ struct in_place_type_t { explicit in_place_type_t() = default; };
+
+#ifdef MPARK_VARIABLE_TEMPLATES
+ constexpr in_place_t in_place{};
+
+ template <std::size_t I> constexpr in_place_index_t<I> in_place_index{};
+
+ template <typename T> constexpr in_place_type_t<T> in_place_type{};
+#endif
+
+} // namespace mpark
+
+#endif // MPARK_IN_PLACE_HPP
+
+// MPark.Variant
+//
+// Copyright Michael Park, 2015-2017
+//
+// Distributed under the Boost Software License, Version 1.0.
+// (See accompanying file LICENSE.md or copy at http://boost.org/LICENSE_1_0.txt)
+
+#ifndef MPARK_LIB_HPP
+#define MPARK_LIB_HPP
+
+#include <memory>
+#include <functional>
+#include <type_traits>
+#include <utility>
+
+
+#define RETURN(...) -> decltype(__VA_ARGS__) { return __VA_ARGS__; }
+
+namespace mpark {
+ namespace lib {
+ template <typename T>
+ struct identity { using type = T; };
+
+ inline namespace cpp14 {
+ template <typename T, std::size_t N>
+ struct array {
+ constexpr const T &operator[](std::size_t index) const {
+ return data[index];
+ }
+
+ T data[N == 0 ? 1 : N];
+ };
+
+ template <typename T>
+ using add_pointer_t = typename std::add_pointer<T>::type;
+
+ template <typename... Ts>
+ using common_type_t = typename std::common_type<Ts...>::type;
+
+ template <typename T>
+ using decay_t = typename std::decay<T>::type;
+
+ template <bool B, typename T = void>
+ using enable_if_t = typename std::enable_if<B, T>::type;
+
+ template <typename T>
+ using remove_const_t = typename std::remove_const<T>::type;
+
+ template <typename T>
+ using remove_reference_t = typename std::remove_reference<T>::type;
+
+ template <typename T>
+ inline constexpr T &&forward(remove_reference_t<T> &t) noexcept {
+ return static_cast<T &&>(t);
+ }
+
+ template <typename T>
+ inline constexpr T &&forward(remove_reference_t<T> &&t) noexcept {
+ static_assert(!std::is_lvalue_reference<T>::value,
+ "can not forward an rvalue as an lvalue");
+ return static_cast<T &&>(t);
+ }
+
+ template <typename T>
+ inline constexpr remove_reference_t<T> &&move(T &&t) noexcept {
+ return static_cast<remove_reference_t<T> &&>(t);
+ }
+
+#ifdef MPARK_INTEGER_SEQUENCE
+ using std::integer_sequence;
+ using std::index_sequence;
+ using std::make_index_sequence;
+ using std::index_sequence_for;
+#else
+ template <typename T, T... Is>
+ struct integer_sequence {
+ using value_type = T;
+ static constexpr std::size_t size() noexcept { return sizeof...(Is); }
+ };
+
+ template <std::size_t... Is>
+ using index_sequence = integer_sequence<std::size_t, Is...>;
+
+ template <typename Lhs, typename Rhs>
+ struct make_index_sequence_concat;
+
+ template <std::size_t... Lhs, std::size_t... Rhs>
+ struct make_index_sequence_concat<index_sequence<Lhs...>,
+ index_sequence<Rhs...>>
+ : identity<index_sequence<Lhs..., (sizeof...(Lhs) + Rhs)...>> {};
+
+ template <std::size_t N>
+ struct make_index_sequence_impl;
+
+ template <std::size_t N>
+ using make_index_sequence = typename make_index_sequence_impl<N>::type;
+
+ template <std::size_t N>
+ struct make_index_sequence_impl
+ : make_index_sequence_concat<make_index_sequence<N / 2>,
+ make_index_sequence<N - (N / 2)>> {};
+
+ template <>
+ struct make_index_sequence_impl<0> : identity<index_sequence<>> {};
+
+ template <>
+ struct make_index_sequence_impl<1> : identity<index_sequence<0>> {};
+
+ template <typename... Ts>
+ using index_sequence_for = make_index_sequence<sizeof...(Ts)>;
+#endif
+
+ // <functional>
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using equal_to = std::equal_to<>;
+#else
+ struct equal_to {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) == lib::forward<Rhs>(rhs))
+ };
+#endif
+
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using not_equal_to = std::not_equal_to<>;
+#else
+ struct not_equal_to {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) != lib::forward<Rhs>(rhs))
+ };
+#endif
+
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using less = std::less<>;
+#else
+ struct less {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) < lib::forward<Rhs>(rhs))
+ };
+#endif
+
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using greater = std::greater<>;
+#else
+ struct greater {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) > lib::forward<Rhs>(rhs))
+ };
+#endif
+
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using less_equal = std::less_equal<>;
+#else
+ struct less_equal {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) <= lib::forward<Rhs>(rhs))
+ };
+#endif
+
+#ifdef MPARK_TRANSPARENT_OPERATORS
+ using greater_equal = std::greater_equal<>;
+#else
+ struct greater_equal {
+ template <typename Lhs, typename Rhs>
+ inline constexpr auto operator()(Lhs &&lhs, Rhs &&rhs) const
+ RETURN(lib::forward<Lhs>(lhs) >= lib::forward<Rhs>(rhs))
+ };
+#endif
+ } // namespace cpp14
+
+ inline namespace cpp17 {
+
+ // <type_traits>
+ template <bool B>
+ using bool_constant = std::integral_constant<bool, B>;
+
+ template <typename...>
+ struct voider : identity<void> {};
+
+ template <typename... Ts>
+ using void_t = typename voider<Ts...>::type;
+
+ namespace detail {
+ namespace swappable {
+
+ using std::swap;
+
+ template <typename T>
+ struct is_swappable {
+ private:
+ template <typename U,
+ typename = decltype(swap(std::declval<U &>(),
+ std::declval<U &>()))>
+ inline static std::true_type test(int);
+
+ template <typename U>
+ inline static std::false_type test(...);
+
+ public:
+ static constexpr bool value = decltype(test<T>(0))::value;
+ };
+
+ template <typename T, bool = is_swappable<T>::value>
+ struct is_nothrow_swappable {
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wnoexcept"
+#endif
+ static constexpr bool value =
+ noexcept(swap(std::declval<T &>(), std::declval<T &>()));
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif
+ };
+
+ template <typename T>
+ struct is_nothrow_swappable<T, false> : std::false_type {};
+
+ } // namespace swappable
+ } // namespace detail
+
+ using detail::swappable::is_swappable;
+ using detail::swappable::is_nothrow_swappable;
+
+ // <functional>
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4100)
+#endif
+ template <typename F, typename... As>
+ inline constexpr auto invoke(F &&f, As &&... as)
+ RETURN(lib::forward<F>(f)(lib::forward<As>(as)...))
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+
+ template <typename B, typename T, typename D>
+ inline constexpr auto invoke(T B::*pmv, D &&d)
+ RETURN(lib::forward<D>(d).*pmv)
+
+ template <typename Pmv, typename Ptr>
+ inline constexpr auto invoke(Pmv pmv, Ptr &&ptr)
+ RETURN((*lib::forward<Ptr>(ptr)).*pmv)
+
+ template <typename B, typename T, typename D, typename... As>
+ inline constexpr auto invoke(T B::*pmf, D &&d, As &&... as)
+ RETURN((lib::forward<D>(d).*pmf)(lib::forward<As>(as)...))
+
+ template <typename Pmf, typename Ptr, typename... As>
+ inline constexpr auto invoke(Pmf pmf, Ptr &&ptr, As &&... as)
+ RETURN(((*lib::forward<Ptr>(ptr)).*pmf)(lib::forward<As>(as)...))
+
+ namespace detail {
+
+ template <typename Void, typename, typename...>
+ struct invoke_result {};
+
+ template <typename F, typename... Args>
+ struct invoke_result<void_t<decltype(lib::invoke(
+ std::declval<F>(), std::declval<Args>()...))>,
+ F,
+ Args...>
+ : identity<decltype(
+ lib::invoke(std::declval<F>(), std::declval<Args>()...))> {};
+
+ } // namespace detail
+
+ template <typename F, typename... Args>
+ using invoke_result = detail::invoke_result<void, F, Args...>;
+
+ template <typename F, typename... Args>
+ using invoke_result_t = typename invoke_result<F, Args...>::type;
+
+ namespace detail {
+
+ template <typename Void, typename, typename...>
+ struct is_invocable : std::false_type {};
+
+ template <typename F, typename... Args>
+ struct is_invocable<void_t<invoke_result_t<F, Args...>>, F, Args...>
+ : std::true_type {};
+
+ template <typename Void, typename, typename, typename...>
+ struct is_invocable_r : std::false_type {};
+
+ template <typename R, typename F, typename... Args>
+ struct is_invocable_r<void_t<invoke_result_t<F, Args...>>,
+ R,
+ F,
+ Args...>
+ : std::is_convertible<invoke_result_t<F, Args...>, R> {};
+
+ } // namespace detail
+
+ template <typename F, typename... Args>
+ using is_invocable = detail::is_invocable<void, F, Args...>;
+
+ template <typename R, typename F, typename... Args>
+ using is_invocable_r = detail::is_invocable_r<void, R, F, Args...>;
+
+ // <memory>
+#ifdef MPARK_BUILTIN_ADDRESSOF
+ template <typename T>
+ inline constexpr T *addressof(T &arg) noexcept {
+ return __builtin_addressof(arg);
+ }
+#else
+ namespace detail {
+
+ namespace has_addressof_impl {
+
+ struct fail;
+
+ template <typename T>
+ inline fail operator&(T &&);
+
+ template <typename T>
+ inline static constexpr bool impl() {
+ return (std::is_class<T>::value || std::is_union<T>::value) &&
+ !std::is_same<decltype(&std::declval<T &>()), fail>::value;
+ }
+
+ } // namespace has_addressof_impl
+
+ template <typename T>
+ using has_addressof = bool_constant<has_addressof_impl::impl<T>()>;
+
+ template <typename T>
+ inline constexpr T *addressof(T &arg, std::true_type) noexcept {
+ return std::addressof(arg);
+ }
+
+ template <typename T>
+ inline constexpr T *addressof(T &arg, std::false_type) noexcept {
+ return &arg;
+ }
+
+ } // namespace detail
+
+ template <typename T>
+ inline constexpr T *addressof(T &arg) noexcept {
+ return detail::addressof(arg, detail::has_addressof<T>{});
+ }
+#endif
+
+ template <typename T>
+ inline constexpr T *addressof(const T &&) = delete;
+
+ } // namespace cpp17
+
+ template <typename T>
+ struct remove_all_extents : identity<T> {};
+
+ template <typename T, std::size_t N>
+ struct remove_all_extents<array<T, N>> : remove_all_extents<T> {};
+
+ template <typename T>
+ using remove_all_extents_t = typename remove_all_extents<T>::type;
+
+ template <std::size_t N>
+ using size_constant = std::integral_constant<std::size_t, N>;
+
+ template <std::size_t I, typename T>
+ struct indexed_type : size_constant<I>, identity<T> {};
+
+ template <bool... Bs>
+ using all = std::is_same<integer_sequence<bool, true, Bs...>,
+ integer_sequence<bool, Bs..., true>>;
+
+#ifdef MPARK_TYPE_PACK_ELEMENT
+ template <std::size_t I, typename... Ts>
+ using type_pack_element_t = __type_pack_element<I, Ts...>;
+#else
+ template <std::size_t I, typename... Ts>
+ struct type_pack_element_impl {
+ private:
+ template <typename>
+ struct set;
+
+ template <std::size_t... Is>
+ struct set<index_sequence<Is...>> : indexed_type<Is, Ts>... {};
+
+ template <typename T>
+ inline static std::enable_if<true, T> impl(indexed_type<I, T>);
+
+ inline static std::enable_if<false> impl(...);
+
+ public:
+ using type = decltype(impl(set<index_sequence_for<Ts...>>{}));
+ };
+
+ template <std::size_t I, typename... Ts>
+ using type_pack_element = typename type_pack_element_impl<I, Ts...>::type;
+
+ template <std::size_t I, typename... Ts>
+ using type_pack_element_t = typename type_pack_element<I, Ts...>::type;
+#endif
+
+#ifdef MPARK_TRIVIALITY_TYPE_TRAITS
+ using std::is_trivially_copy_constructible;
+ using std::is_trivially_move_constructible;
+ using std::is_trivially_copy_assignable;
+ using std::is_trivially_move_assignable;
+#else
+ template <typename T>
+ struct is_trivially_copy_constructible
+ : bool_constant<
+ std::is_copy_constructible<T>::value && __has_trivial_copy(T)> {};
+
+ template <typename T>
+ struct is_trivially_move_constructible : bool_constant<__is_trivial(T)> {};
+
+ template <typename T>
+ struct is_trivially_copy_assignable
+ : bool_constant<
+ std::is_copy_assignable<T>::value && __has_trivial_assign(T)> {};
+
+ template <typename T>
+ struct is_trivially_move_assignable : bool_constant<__is_trivial(T)> {};
+#endif
+
+ template <typename T, bool>
+ struct dependent_type : T {};
+
+ template <typename Is, std::size_t J>
+ struct push_back;
+
+ template <typename Is, std::size_t J>
+ using push_back_t = typename push_back<Is, J>::type;
+
+ template <std::size_t... Is, std::size_t J>
+ struct push_back<index_sequence<Is...>, J> {
+ using type = index_sequence<Is..., J>;
+ };
+
+ } // namespace lib
+} // namespace mpark
+
+#undef RETURN
+
+#endif // MPARK_LIB_HPP
+
+
+namespace mpark {
+
+#ifdef MPARK_RETURN_TYPE_DEDUCTION
+
+#define AUTO auto
+#define AUTO_RETURN(...) { return __VA_ARGS__; }
+
+#define AUTO_REFREF auto &&
+#define AUTO_REFREF_RETURN(...) { return __VA_ARGS__; }
+
+#define DECLTYPE_AUTO decltype(auto)
+#define DECLTYPE_AUTO_RETURN(...) { return __VA_ARGS__; }
+
+#else
+
+#define AUTO auto
+#define AUTO_RETURN(...) \
+ -> lib::decay_t<decltype(__VA_ARGS__)> { return __VA_ARGS__; }
+
+#define AUTO_REFREF auto
+#define AUTO_REFREF_RETURN(...) \
+ -> decltype((__VA_ARGS__)) { \
+ static_assert(std::is_reference<decltype((__VA_ARGS__))>::value, ""); \
+ return __VA_ARGS__; \
+ }
+
+#define DECLTYPE_AUTO auto
+#define DECLTYPE_AUTO_RETURN(...) \
+ -> decltype(__VA_ARGS__) { return __VA_ARGS__; }
+
+#endif
+
+ class bad_variant_access : public std::exception {
+ public:
+ virtual const char *what() const noexcept { return "bad_variant_access"; }
+ };
+
+ [[noreturn]] inline void throw_bad_variant_access() {
+#ifdef MPARK_EXCEPTIONS
+ throw bad_variant_access{};
+#else
+ std::terminate();
+#ifdef MPARK_BUILTIN_UNREACHABLE
+ __builtin_unreachable();
+#endif
+#endif
+ }
+
+ template <typename... Ts>
+ class variant;
+
+ template <typename T>
+ struct variant_size;
+
+#ifdef MPARK_VARIABLE_TEMPLATES
+ template <typename T>
+ constexpr std::size_t variant_size_v = variant_size<T>::value;
+#endif
+
+ template <typename T>
+ struct variant_size<const T> : variant_size<T> {};
+
+ template <typename T>
+ struct variant_size<volatile T> : variant_size<T> {};
+
+ template <typename T>
+ struct variant_size<const volatile T> : variant_size<T> {};
+
+ template <typename... Ts>
+ struct variant_size<variant<Ts...>> : lib::size_constant<sizeof...(Ts)> {};
+
+ template <std::size_t I, typename T>
+ struct variant_alternative;
+
+ template <std::size_t I, typename T>
+ using variant_alternative_t = typename variant_alternative<I, T>::type;
+
+ template <std::size_t I, typename T>
+ struct variant_alternative<I, const T>
+ : std::add_const<variant_alternative_t<I, T>> {};
+
+ template <std::size_t I, typename T>
+ struct variant_alternative<I, volatile T>
+ : std::add_volatile<variant_alternative_t<I, T>> {};
+
+ template <std::size_t I, typename T>
+ struct variant_alternative<I, const volatile T>
+ : std::add_cv<variant_alternative_t<I, T>> {};
+
+ template <std::size_t I, typename... Ts>
+ struct variant_alternative<I, variant<Ts...>> {
+ static_assert(I < sizeof...(Ts),
+ "Index out of bounds in std::variant_alternative<>");
+ using type = lib::type_pack_element_t<I, Ts...>;
+ };
+
+ constexpr std::size_t variant_npos = static_cast<std::size_t>(-1);
+
+ namespace detail {
+
+ constexpr std::size_t not_found = static_cast<std::size_t>(-1);
+ constexpr std::size_t ambiguous = static_cast<std::size_t>(-2);
+
+#ifdef MPARK_CPP14_CONSTEXPR
+ template <typename T, typename... Ts>
+ inline constexpr std::size_t find_index() {
+ constexpr lib::array<bool, sizeof...(Ts)> matches = {
+ {std::is_same<T, Ts>::value...}
+ };
+ std::size_t result = not_found;
+ for (std::size_t i = 0; i < sizeof...(Ts); ++i) {
+ if (matches[i]) {
+ if (result != not_found) {
+ return ambiguous;
+ }
+ result = i;
+ }
+ }
+ return result;
+ }
+#else
+ inline constexpr std::size_t find_index_impl(std::size_t result,
+ std::size_t) {
+ return result;
+ }
+
+ template <typename... Bs>
+ inline constexpr std::size_t find_index_impl(std::size_t result,
+ std::size_t idx,
+ bool b,
+ Bs... bs) {
+ return b ? (result != not_found ? ambiguous
+ : find_index_impl(idx, idx + 1, bs...))
+ : find_index_impl(result, idx + 1, bs...);
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr std::size_t find_index() {
+ return find_index_impl(not_found, 0, std::is_same<T, Ts>::value...);
+ }
+#endif
+
+ template <std::size_t I>
+ using find_index_sfinae_impl =
+ lib::enable_if_t<I != not_found && I != ambiguous,
+ lib::size_constant<I>>;
+
+ template <typename T, typename... Ts>
+ using find_index_sfinae = find_index_sfinae_impl<find_index<T, Ts...>()>;
+
+ template <std::size_t I>
+ struct find_index_checked_impl : lib::size_constant<I> {
+ static_assert(I != not_found, "the specified type is not found.");
+ static_assert(I != ambiguous, "the specified type is ambiguous.");
+ };
+
+ template <typename T, typename... Ts>
+ using find_index_checked = find_index_checked_impl<find_index<T, Ts...>()>;
+
+ struct valueless_t {};
+
+ enum class Trait { TriviallyAvailable, Available, Unavailable };
+
+ template <typename T,
+ template <typename> class IsTriviallyAvailable,
+ template <typename> class IsAvailable>
+ inline constexpr Trait trait() {
+ return IsTriviallyAvailable<T>::value
+ ? Trait::TriviallyAvailable
+ : IsAvailable<T>::value ? Trait::Available
+ : Trait::Unavailable;
+ }
+
+#ifdef MPARK_CPP14_CONSTEXPR
+ template <typename... Traits>
+ inline constexpr Trait common_trait(Traits... traits) {
+ Trait result = Trait::TriviallyAvailable;
+ for (Trait t : {traits...}) {
+ if (static_cast<int>(t) > static_cast<int>(result)) {
+ result = t;
+ }
+ }
+ return result;
+ }
+#else
+ inline constexpr Trait common_trait_impl(Trait result) { return result; }
+
+ template <typename... Traits>
+ inline constexpr Trait common_trait_impl(Trait result,
+ Trait t,
+ Traits... ts) {
+ return static_cast<int>(t) > static_cast<int>(result)
+ ? common_trait_impl(t, ts...)
+ : common_trait_impl(result, ts...);
+ }
+
+ template <typename... Traits>
+ inline constexpr Trait common_trait(Traits... ts) {
+ return common_trait_impl(Trait::TriviallyAvailable, ts...);
+ }
+#endif
+
+ template <typename... Ts>
+ struct traits {
+ static constexpr Trait copy_constructible_trait =
+ common_trait(trait<Ts,
+ lib::is_trivially_copy_constructible,
+ std::is_copy_constructible>()...);
+
+ static constexpr Trait move_constructible_trait =
+ common_trait(trait<Ts,
+ lib::is_trivially_move_constructible,
+ std::is_move_constructible>()...);
+
+ static constexpr Trait copy_assignable_trait =
+ common_trait(copy_constructible_trait,
+ trait<Ts,
+ lib::is_trivially_copy_assignable,
+ std::is_copy_assignable>()...);
+
+ static constexpr Trait move_assignable_trait =
+ common_trait(move_constructible_trait,
+ trait<Ts,
+ lib::is_trivially_move_assignable,
+ std::is_move_assignable>()...);
+
+ static constexpr Trait destructible_trait =
+ common_trait(trait<Ts,
+ std::is_trivially_destructible,
+ std::is_destructible>()...);
+ };
+
+ namespace access {
+
+ struct recursive_union {
+#ifdef MPARK_RETURN_TYPE_DEDUCTION
+ template <typename V>
+ inline static constexpr auto &&get_alt(V &&v, in_place_index_t<0>) {
+ return lib::forward<V>(v).head_;
+ }
+
+ template <typename V, std::size_t I>
+ inline static constexpr auto &&get_alt(V &&v, in_place_index_t<I>) {
+ return get_alt(lib::forward<V>(v).tail_, in_place_index_t<I - 1>{});
+ }
+#else
+ template <std::size_t I, bool Dummy = true>
+ struct get_alt_impl {
+ template <typename V>
+ inline constexpr AUTO_REFREF operator()(V &&v) const
+ AUTO_REFREF_RETURN(get_alt_impl<I - 1>{}(lib::forward<V>(v).tail_))
+ };
+
+ template <bool Dummy>
+ struct get_alt_impl<0, Dummy> {
+ template <typename V>
+ inline constexpr AUTO_REFREF operator()(V &&v) const
+ AUTO_REFREF_RETURN(lib::forward<V>(v).head_)
+ };
+
+ template <typename V, std::size_t I>
+ inline static constexpr AUTO_REFREF get_alt(V &&v, in_place_index_t<I>)
+ AUTO_REFREF_RETURN(get_alt_impl<I>{}(lib::forward<V>(v)))
+#endif
+ };
+
+ struct base {
+ template <std::size_t I, typename V>
+ inline static constexpr AUTO_REFREF get_alt(V &&v)
+ AUTO_REFREF_RETURN(recursive_union::get_alt(
+ data(lib::forward<V>(v)), in_place_index_t<I>{}))
+ };
+
+ struct variant {
+ template <std::size_t I, typename V>
+ inline static constexpr AUTO_REFREF get_alt(V &&v)
+ AUTO_REFREF_RETURN(base::get_alt<I>(lib::forward<V>(v).impl_))
+ };
+
+ } // namespace access
+
+ namespace visitation {
+
+ struct base {
+ template <typename T>
+ inline static constexpr const T &at(const T &elem) noexcept {
+ return elem;
+ }
+
+ template <typename T, std::size_t N, typename... Is>
+ inline static constexpr const lib::remove_all_extents_t<T> &at(
+ const lib::array<T, N> &elems, std::size_t i, Is... is) noexcept {
+ return at(elems[i], is...);
+ }
+
+ template <typename F, typename... Fs>
+ inline static constexpr int visit_visitor_return_type_check() {
+ static_assert(lib::all<std::is_same<F, Fs>::value...>::value,
+ "`mpark::visit` requires the visitor to have a single "
+ "return type.");
+ return 0;
+ }
+
+ template <typename... Fs>
+ inline static constexpr lib::array<
+ lib::common_type_t<lib::decay_t<Fs>...>,
+ sizeof...(Fs)>
+ make_farray(Fs &&... fs) {
+ using result = lib::array<lib::common_type_t<lib::decay_t<Fs>...>,
+ sizeof...(Fs)>;
+ return visit_visitor_return_type_check<lib::decay_t<Fs>...>(),
+ result{{lib::forward<Fs>(fs)...}};
+ }
+
+ template <std::size_t... Is>
+ struct dispatcher {
+ template <typename F, typename... Vs>
+ struct impl {
+ inline static constexpr DECLTYPE_AUTO dispatch(F f, Vs... vs)
+ DECLTYPE_AUTO_RETURN(lib::invoke(
+ static_cast<F>(f),
+ access::base::get_alt<Is>(static_cast<Vs>(vs))...))
+ };
+ };
+
+ template <typename F, typename... Vs, std::size_t... Is>
+ inline static constexpr AUTO make_dispatch(lib::index_sequence<Is...>)
+ AUTO_RETURN(&dispatcher<Is...>::template impl<F, Vs...>::dispatch)
+
+ template <std::size_t I, typename F, typename... Vs>
+ inline static constexpr AUTO make_fdiagonal_impl()
+ AUTO_RETURN(make_dispatch<F, Vs...>(
+ lib::index_sequence<lib::indexed_type<I, Vs>::value...>{}))
+
+ template <typename F, typename... Vs, std::size_t... Is>
+ inline static constexpr AUTO make_fdiagonal_impl(
+ lib::index_sequence<Is...>)
+ AUTO_RETURN(make_farray(make_fdiagonal_impl<Is, F, Vs...>()...))
+
+ template <typename F, typename V, typename... Vs>
+ inline static constexpr /* auto * */ auto make_fdiagonal()
+ -> decltype(make_fdiagonal_impl<F, V, Vs...>(
+ lib::make_index_sequence<lib::decay_t<V>::size()>{})) {
+ static_assert(lib::all<(lib::decay_t<V>::size() ==
+ lib::decay_t<Vs>::size())...>::value,
+ "all of the variants must be the same size.");
+ return make_fdiagonal_impl<F, V, Vs...>(
+ lib::make_index_sequence<lib::decay_t<V>::size()>{});
+ }
+
+#ifdef MPARK_RETURN_TYPE_DEDUCTION
+ template <typename F, typename... Vs, typename Is>
+ inline static constexpr auto make_fmatrix_impl(Is is) {
+ return make_dispatch<F, Vs...>(is);
+ }
+
+ template <typename F,
+ typename... Vs,
+ typename Is,
+ std::size_t... Js,
+ typename... Ls>
+ inline static constexpr auto make_fmatrix_impl(
+ Is, lib::index_sequence<Js...>, Ls... ls) {
+ return make_farray(make_fmatrix_impl<F, Vs...>(
+ lib::push_back_t<Is, Js>{}, ls...)...);
+ }
+
+ template <typename F, typename... Vs>
+ inline static constexpr auto make_fmatrix() {
+ return make_fmatrix_impl<F, Vs...>(
+ lib::index_sequence<>{},
+ lib::make_index_sequence<lib::decay_t<Vs>::size()>{}...);
+ }
+#else
+ template <typename F, typename... Vs>
+ struct make_fmatrix_impl {
+ template <typename...>
+ struct impl;
+
+ template <typename Is>
+ struct impl<Is> {
+ inline constexpr AUTO operator()() const
+ AUTO_RETURN(make_dispatch<F, Vs...>(Is{}))
+ };
+
+ template <typename Is, std::size_t... Js, typename... Ls>
+ struct impl<Is, lib::index_sequence<Js...>, Ls...> {
+ inline constexpr AUTO operator()() const
+ AUTO_RETURN(
+ make_farray(impl<lib::push_back_t<Is, Js>, Ls...>{}()...))
+ };
+ };
+
+ template <typename F, typename... Vs>
+ inline static constexpr AUTO make_fmatrix()
+ AUTO_RETURN(
+ typename make_fmatrix_impl<F, Vs...>::template impl<
+ lib::index_sequence<>,
+ lib::make_index_sequence<lib::decay_t<Vs>::size()>...>{}())
+#endif
+ }; // namespace base
+
+ template <typename F, typename... Vs>
+ using FDiagonal = decltype(base::make_fdiagonal<F, Vs...>());
+
+ template <typename F, typename... Vs>
+ struct fdiagonal {
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4268)
+#endif
+ static constexpr FDiagonal<F, Vs...> value =
+ base::make_fdiagonal<F, Vs...>();
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+ };
+
+ template <typename F, typename... Vs>
+ constexpr FDiagonal<F, Vs...> fdiagonal<F, Vs...>::value;
+
+ template <typename F, typename... Vs>
+ using FMatrix = decltype(base::make_fmatrix<F, Vs...>());
+
+ template <typename F, typename... Vs>
+ struct fmatrix {
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4268)
+#endif
+ static constexpr FMatrix<F, Vs...> value =
+ base::make_fmatrix<F, Vs...>();
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+ };
+
+ template <typename F, typename... Vs>
+ constexpr FMatrix<F, Vs...> fmatrix<F, Vs...>::value;
+
+ struct alt {
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_alt_at(std::size_t index,
+ Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(base::at(
+ fdiagonal<Visitor &&,
+ decltype(as_base(lib::forward<Vs>(vs)))...>::value,
+ index)(lib::forward<Visitor>(visitor),
+ as_base(lib::forward<Vs>(vs))...))
+
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_alt(Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(base::at(
+ fmatrix<Visitor &&,
+ decltype(as_base(lib::forward<Vs>(vs)))...>::value,
+ vs.index()...)(lib::forward<Visitor>(visitor),
+ as_base(lib::forward<Vs>(vs))...))
+ };
+
+ struct variant {
+ private:
+ template <typename Visitor, typename... Values>
+ struct visit_exhaustive_visitor_check {
+ static_assert(
+ lib::is_invocable<Visitor, Values...>::value,
+ "`mpark::visit` requires the visitor to be exhaustive.");
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4100)
+#endif
+ inline constexpr DECLTYPE_AUTO operator()(Visitor &&visitor,
+ Values &&... values) const
+ DECLTYPE_AUTO_RETURN(lib::invoke(lib::forward<Visitor>(visitor),
+ lib::forward<Values>(values)...))
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+ };
+
+ template <typename Visitor>
+ struct value_visitor {
+ Visitor &&visitor_;
+
+ template <typename... Alts>
+ inline constexpr DECLTYPE_AUTO operator()(Alts &&... alts) const
+ DECLTYPE_AUTO_RETURN(
+ visit_exhaustive_visitor_check<
+ Visitor,
+ decltype((lib::forward<Alts>(alts).value))...>{}(
+ lib::forward<Visitor>(visitor_),
+ lib::forward<Alts>(alts).value...))
+ };
+
+ template <typename Visitor>
+ inline static constexpr AUTO make_value_visitor(Visitor &&visitor)
+ AUTO_RETURN(value_visitor<Visitor>{lib::forward<Visitor>(visitor)})
+
+ public:
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_alt_at(std::size_t index,
+ Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(
+ alt::visit_alt_at(index,
+ lib::forward<Visitor>(visitor),
+ lib::forward<Vs>(vs).impl_...))
+
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_alt(Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(alt::visit_alt(lib::forward<Visitor>(visitor),
+ lib::forward<Vs>(vs).impl_...))
+
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_value_at(std::size_t index,
+ Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(
+ visit_alt_at(index,
+ make_value_visitor(lib::forward<Visitor>(visitor)),
+ lib::forward<Vs>(vs)...))
+
+ template <typename Visitor, typename... Vs>
+ inline static constexpr DECLTYPE_AUTO visit_value(Visitor &&visitor,
+ Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(
+ visit_alt(make_value_visitor(lib::forward<Visitor>(visitor)),
+ lib::forward<Vs>(vs)...))
+ };
+
+ } // namespace visitation
+
+ template <std::size_t Index, typename T>
+ struct alt {
+ using value_type = T;
+
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4244)
+#endif
+ template <typename... Args>
+ inline explicit constexpr alt(in_place_t, Args &&... args)
+ : value(lib::forward<Args>(args)...) {}
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+
+ T value;
+ };
+
+ template <Trait DestructibleTrait, std::size_t Index, typename... Ts>
+ union recursive_union;
+
+ template <Trait DestructibleTrait, std::size_t Index>
+ union recursive_union<DestructibleTrait, Index> {};
+
+#define MPARK_VARIANT_RECURSIVE_UNION(destructible_trait, destructor) \
+ template <std::size_t Index, typename T, typename... Ts> \
+ union recursive_union<destructible_trait, Index, T, Ts...> { \
+ public: \
+ inline explicit constexpr recursive_union(valueless_t) noexcept \
+ : dummy_{} {} \
+ \
+ template <typename... Args> \
+ inline explicit constexpr recursive_union(in_place_index_t<0>, \
+ Args &&... args) \
+ : head_(in_place_t{}, lib::forward<Args>(args)...) {} \
+ \
+ template <std::size_t I, typename... Args> \
+ inline explicit constexpr recursive_union(in_place_index_t<I>, \
+ Args &&... args) \
+ : tail_(in_place_index_t<I - 1>{}, lib::forward<Args>(args)...) {} \
+ \
+ recursive_union(const recursive_union &) = default; \
+ recursive_union(recursive_union &&) = default; \
+ \
+ destructor \
+ \
+ recursive_union &operator=(const recursive_union &) = default; \
+ recursive_union &operator=(recursive_union &&) = default; \
+ \
+ private: \
+ char dummy_; \
+ alt<Index, T> head_; \
+ recursive_union<destructible_trait, Index + 1, Ts...> tail_; \
+ \
+ friend struct access::recursive_union; \
+ }
+
+ MPARK_VARIANT_RECURSIVE_UNION(Trait::TriviallyAvailable,
+ ~recursive_union() = default;);
+ MPARK_VARIANT_RECURSIVE_UNION(Trait::Available,
+ ~recursive_union() {});
+ MPARK_VARIANT_RECURSIVE_UNION(Trait::Unavailable,
+ ~recursive_union() = delete;);
+
+#undef MPARK_VARIANT_RECURSIVE_UNION
+
+ using index_t = unsigned int;
+
+ template <Trait DestructibleTrait, typename... Ts>
+ class base {
+ public:
+ inline explicit constexpr base(valueless_t tag) noexcept
+ : data_(tag), index_(static_cast<index_t>(-1)) {}
+
+ template <std::size_t I, typename... Args>
+ inline explicit constexpr base(in_place_index_t<I>, Args &&... args)
+ : data_(in_place_index_t<I>{}, lib::forward<Args>(args)...),
+ index_(I) {}
+
+ inline constexpr bool valueless_by_exception() const noexcept {
+ return index_ == static_cast<index_t>(-1);
+ }
+
+ inline constexpr std::size_t index() const noexcept {
+ return valueless_by_exception() ? variant_npos : index_;
+ }
+
+ protected:
+ using data_t = recursive_union<DestructibleTrait, 0, Ts...>;
+
+ friend inline constexpr base &as_base(base &b) { return b; }
+ friend inline constexpr const base &as_base(const base &b) { return b; }
+ friend inline constexpr base &&as_base(base &&b) { return lib::move(b); }
+ friend inline constexpr const base &&as_base(const base &&b) { return lib::move(b); }
+
+ friend inline constexpr data_t &data(base &b) { return b.data_; }
+ friend inline constexpr const data_t &data(const base &b) { return b.data_; }
+ friend inline constexpr data_t &&data(base &&b) { return lib::move(b).data_; }
+ friend inline constexpr const data_t &&data(const base &&b) { return lib::move(b).data_; }
+
+ inline static constexpr std::size_t size() { return sizeof...(Ts); }
+
+ data_t data_;
+ index_t index_;
+
+ friend struct access::base;
+ friend struct visitation::base;
+ };
+
+ struct dtor {
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4100)
+#endif
+ template <typename Alt>
+ inline void operator()(Alt &alt) const noexcept { alt.~Alt(); }
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+ };
+
+#if defined(_MSC_VER) && _MSC_VER < 1910
+#define INHERITING_CTOR(type, base) \
+ template <typename... Args> \
+ inline explicit constexpr type(Args &&... args) \
+ : base(lib::forward<Args>(args)...) {}
+#else
+#define INHERITING_CTOR(type, base) using base::base;
+#endif
+
+ template <typename Traits, Trait = Traits::destructible_trait>
+ class destructor;
+
+#define MPARK_VARIANT_DESTRUCTOR(destructible_trait, definition, destroy) \
+ template <typename... Ts> \
+ class destructor<traits<Ts...>, destructible_trait> \
+ : public base<destructible_trait, Ts...> { \
+ using super = base<destructible_trait, Ts...>; \
+ \
+ public: \
+ INHERITING_CTOR(destructor, super) \
+ using super::operator=; \
+ \
+ destructor(const destructor &) = default; \
+ destructor(destructor &&) = default; \
+ definition \
+ destructor &operator=(const destructor &) = default; \
+ destructor &operator=(destructor &&) = default; \
+ \
+ protected: \
+ destroy \
+ }
+
+ MPARK_VARIANT_DESTRUCTOR(
+ Trait::TriviallyAvailable,
+ ~destructor() = default;,
+ inline void destroy() noexcept {
+ this->index_ = static_cast<index_t>(-1);
+ });
+
+ MPARK_VARIANT_DESTRUCTOR(
+ Trait::Available,
+ ~destructor() { destroy(); },
+ inline void destroy() noexcept {
+ if (!this->valueless_by_exception()) {
+ visitation::alt::visit_alt(dtor{}, *this);
+ }
+ this->index_ = static_cast<index_t>(-1);
+ });
+
+ MPARK_VARIANT_DESTRUCTOR(
+ Trait::Unavailable,
+ ~destructor() = delete;,
+ inline void destroy() noexcept = delete;);
+
+#undef MPARK_VARIANT_DESTRUCTOR
+
+ template <typename Traits>
+ class constructor : public destructor<Traits> {
+ using super = destructor<Traits>;
+
+ public:
+ INHERITING_CTOR(constructor, super)
+ using super::operator=;
+
+ protected:
+#ifndef MPARK_GENERIC_LAMBDAS
+ struct ctor {
+ template <typename LhsAlt, typename RhsAlt>
+ inline void operator()(LhsAlt &lhs_alt, RhsAlt &&rhs_alt) const {
+ constructor::construct_alt(lhs_alt,
+ lib::forward<RhsAlt>(rhs_alt).value);
+ }
+ };
+#endif
+
+ template <std::size_t I, typename T, typename... Args>
+ inline static T &construct_alt(alt<I, T> &a, Args &&... args) {
+ ::new (static_cast<void *>(lib::addressof(a)))
+ alt<I, T>(in_place_t{}, lib::forward<Args>(args)...);
+ return a.value;
+ }
+
+ template <typename Rhs>
+ inline static void generic_construct(constructor &lhs, Rhs &&rhs) {
+ lhs.destroy();
+ if (!rhs.valueless_by_exception()) {
+ visitation::alt::visit_alt_at(
+ rhs.index(),
+#ifdef MPARK_GENERIC_LAMBDAS
+ [](auto &lhs_alt, auto &&rhs_alt) {
+ constructor::construct_alt(
+ lhs_alt, lib::forward<decltype(rhs_alt)>(rhs_alt).value);
+ }
+#else
+ ctor{}
+#endif
+ ,
+ lhs,
+ lib::forward<Rhs>(rhs));
+ lhs.index_ = rhs.index_;
+ }
+ }
+ };
+
+ template <typename Traits, Trait = Traits::move_constructible_trait>
+ class move_constructor;
+
+#define MPARK_VARIANT_MOVE_CONSTRUCTOR(move_constructible_trait, definition) \
+ template <typename... Ts> \
+ class move_constructor<traits<Ts...>, move_constructible_trait> \
+ : public constructor<traits<Ts...>> { \
+ using super = constructor<traits<Ts...>>; \
+ \
+ public: \
+ INHERITING_CTOR(move_constructor, super) \
+ using super::operator=; \
+ \
+ move_constructor(const move_constructor &) = default; \
+ definition \
+ ~move_constructor() = default; \
+ move_constructor &operator=(const move_constructor &) = default; \
+ move_constructor &operator=(move_constructor &&) = default; \
+ }
+
+ MPARK_VARIANT_MOVE_CONSTRUCTOR(
+ Trait::TriviallyAvailable,
+ move_constructor(move_constructor &&that) = default;);
+
+ MPARK_VARIANT_MOVE_CONSTRUCTOR(
+ Trait::Available,
+ move_constructor(move_constructor &&that) noexcept(
+ lib::all<std::is_nothrow_move_constructible<Ts>::value...>::value)
+ : move_constructor(valueless_t{}) {
+ this->generic_construct(*this, lib::move(that));
+ });
+
+ MPARK_VARIANT_MOVE_CONSTRUCTOR(
+ Trait::Unavailable,
+ move_constructor(move_constructor &&) = delete;);
+
+#undef MPARK_VARIANT_MOVE_CONSTRUCTOR
+
+ template <typename Traits, Trait = Traits::copy_constructible_trait>
+ class copy_constructor;
+
+#define MPARK_VARIANT_COPY_CONSTRUCTOR(copy_constructible_trait, definition) \
+ template <typename... Ts> \
+ class copy_constructor<traits<Ts...>, copy_constructible_trait> \
+ : public move_constructor<traits<Ts...>> { \
+ using super = move_constructor<traits<Ts...>>; \
+ \
+ public: \
+ INHERITING_CTOR(copy_constructor, super) \
+ using super::operator=; \
+ \
+ definition \
+ copy_constructor(copy_constructor &&) = default; \
+ ~copy_constructor() = default; \
+ copy_constructor &operator=(const copy_constructor &) = default; \
+ copy_constructor &operator=(copy_constructor &&) = default; \
+ }
+
+ MPARK_VARIANT_COPY_CONSTRUCTOR(
+ Trait::TriviallyAvailable,
+ copy_constructor(const copy_constructor &that) = default;);
+
+ MPARK_VARIANT_COPY_CONSTRUCTOR(
+ Trait::Available,
+ copy_constructor(const copy_constructor &that)
+ : copy_constructor(valueless_t{}) {
+ this->generic_construct(*this, that);
+ });
+
+ MPARK_VARIANT_COPY_CONSTRUCTOR(
+ Trait::Unavailable,
+ copy_constructor(const copy_constructor &) = delete;);
+
+#undef MPARK_VARIANT_COPY_CONSTRUCTOR
+
+ template <typename Traits>
+ class assignment : public copy_constructor<Traits> {
+ using super = copy_constructor<Traits>;
+
+ public:
+ INHERITING_CTOR(assignment, super)
+ using super::operator=;
+
+ template <std::size_t I, typename... Args>
+ inline /* auto & */ auto emplace(Args &&... args)
+ -> decltype(this->construct_alt(access::base::get_alt<I>(*this),
+ lib::forward<Args>(args)...)) {
+ this->destroy();
+ auto &result = this->construct_alt(access::base::get_alt<I>(*this),
+ lib::forward<Args>(args)...);
+ this->index_ = I;
+ return result;
+ }
+
+ protected:
+#ifndef MPARK_GENERIC_LAMBDAS
+ template <typename That>
+ struct assigner {
+ template <typename ThisAlt, typename ThatAlt>
+ inline void operator()(ThisAlt &this_alt, ThatAlt &&that_alt) const {
+ self->assign_alt(this_alt, lib::forward<ThatAlt>(that_alt).value);
+ }
+ assignment *self;
+ };
+#endif
+
+ template <std::size_t I, typename T, typename Arg>
+ inline void assign_alt(alt<I, T> &a, Arg &&arg) {
+ if (this->index() == I) {
+#ifdef _MSC_VER
+#pragma warning(push)
+#pragma warning(disable : 4244)
+#endif
+ a.value = lib::forward<Arg>(arg);
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif
+ } else {
+ struct {
+ void operator()(std::true_type) const {
+ this_->emplace<I>(lib::forward<Arg>(arg_));
+ }
+ void operator()(std::false_type) const {
+ this_->emplace<I>(T(lib::forward<Arg>(arg_)));
+ }
+ assignment *this_;
+ Arg &&arg_;
+ } impl{this, lib::forward<Arg>(arg)};
+ impl(lib::bool_constant<
+ std::is_nothrow_constructible<T, Arg>::value ||
+ !std::is_nothrow_move_constructible<T>::value>{});
+ }
+ }
+
+ template <typename That>
+ inline void generic_assign(That &&that) {
+ if (this->valueless_by_exception() && that.valueless_by_exception()) {
+ // do nothing.
+ } else if (that.valueless_by_exception()) {
+ this->destroy();
+ } else {
+ visitation::alt::visit_alt_at(
+ that.index(),
+#ifdef MPARK_GENERIC_LAMBDAS
+ [this](auto &this_alt, auto &&that_alt) {
+ this->assign_alt(
+ this_alt, lib::forward<decltype(that_alt)>(that_alt).value);
+ }
+#else
+ assigner<That>{this}
+#endif
+ ,
+ *this,
+ lib::forward<That>(that));
+ }
+ }
+ };
+
+ template <typename Traits, Trait = Traits::move_assignable_trait>
+ class move_assignment;
+
+#define MPARK_VARIANT_MOVE_ASSIGNMENT(move_assignable_trait, definition) \
+ template <typename... Ts> \
+ class move_assignment<traits<Ts...>, move_assignable_trait> \
+ : public assignment<traits<Ts...>> { \
+ using super = assignment<traits<Ts...>>; \
+ \
+ public: \
+ INHERITING_CTOR(move_assignment, super) \
+ using super::operator=; \
+ \
+ move_assignment(const move_assignment &) = default; \
+ move_assignment(move_assignment &&) = default; \
+ ~move_assignment() = default; \
+ move_assignment &operator=(const move_assignment &) = default; \
+ definition \
+ }
+
+ MPARK_VARIANT_MOVE_ASSIGNMENT(
+ Trait::TriviallyAvailable,
+ move_assignment &operator=(move_assignment &&that) = default;);
+
+ MPARK_VARIANT_MOVE_ASSIGNMENT(
+ Trait::Available,
+ move_assignment &
+ operator=(move_assignment &&that) noexcept(
+ lib::all<(std::is_nothrow_move_constructible<Ts>::value &&
+ std::is_nothrow_move_assignable<Ts>::value)...>::value) {
+ this->generic_assign(lib::move(that));
+ return *this;
+ });
+
+ MPARK_VARIANT_MOVE_ASSIGNMENT(
+ Trait::Unavailable,
+ move_assignment &operator=(move_assignment &&) = delete;);
+
+#undef MPARK_VARIANT_MOVE_ASSIGNMENT
+
+ template <typename Traits, Trait = Traits::copy_assignable_trait>
+ class copy_assignment;
+
+#define MPARK_VARIANT_COPY_ASSIGNMENT(copy_assignable_trait, definition) \
+ template <typename... Ts> \
+ class copy_assignment<traits<Ts...>, copy_assignable_trait> \
+ : public move_assignment<traits<Ts...>> { \
+ using super = move_assignment<traits<Ts...>>; \
+ \
+ public: \
+ INHERITING_CTOR(copy_assignment, super) \
+ using super::operator=; \
+ \
+ copy_assignment(const copy_assignment &) = default; \
+ copy_assignment(copy_assignment &&) = default; \
+ ~copy_assignment() = default; \
+ definition \
+ copy_assignment &operator=(copy_assignment &&) = default; \
+ }
+
+ MPARK_VARIANT_COPY_ASSIGNMENT(
+ Trait::TriviallyAvailable,
+ copy_assignment &operator=(const copy_assignment &that) = default;);
+
+ MPARK_VARIANT_COPY_ASSIGNMENT(
+ Trait::Available,
+ copy_assignment &operator=(const copy_assignment &that) {
+ this->generic_assign(that);
+ return *this;
+ });
+
+ MPARK_VARIANT_COPY_ASSIGNMENT(
+ Trait::Unavailable,
+ copy_assignment &operator=(const copy_assignment &) = delete;);
+
+#undef MPARK_VARIANT_COPY_ASSIGNMENT
+
+ template <typename... Ts>
+ class impl : public copy_assignment<traits<Ts...>> {
+ using super = copy_assignment<traits<Ts...>>;
+
+ public:
+ INHERITING_CTOR(impl, super)
+ using super::operator=;
+
+ template <std::size_t I, typename Arg>
+ inline void assign(Arg &&arg) {
+ this->assign_alt(access::base::get_alt<I>(*this),
+ lib::forward<Arg>(arg));
+ }
+
+ inline void swap(impl &that) {
+ if (this->valueless_by_exception() && that.valueless_by_exception()) {
+ // do nothing.
+ } else if (this->index() == that.index()) {
+ visitation::alt::visit_alt_at(this->index(),
+#ifdef MPARK_GENERIC_LAMBDAS
+ [](auto &this_alt, auto &that_alt) {
+ using std::swap;
+ swap(this_alt.value,
+ that_alt.value);
+ }
+#else
+ swapper{}
+#endif
+ ,
+ *this,
+ that);
+ } else {
+ impl *lhs = this;
+ impl *rhs = lib::addressof(that);
+ if (lhs->move_nothrow() && !rhs->move_nothrow()) {
+ std::swap(lhs, rhs);
+ }
+ impl tmp(lib::move(*rhs));
+#ifdef MPARK_EXCEPTIONS
+ // EXTENSION: When the move construction of `lhs` into `rhs` throws
+ // and `tmp` is nothrow move constructible then we move `tmp` back
+ // into `rhs` and provide the strong exception safety guarantee.
+ try {
+ this->generic_construct(*rhs, lib::move(*lhs));
+ } catch (...) {
+ if (tmp.move_nothrow()) {
+ this->generic_construct(*rhs, lib::move(tmp));
+ }
+ throw;
+ }
+#else
+ this->generic_construct(*rhs, lib::move(*lhs));
+#endif
+ this->generic_construct(*lhs, lib::move(tmp));
+ }
+ }
+
+ private:
+#ifndef MPARK_GENERIC_LAMBDAS
+ struct swapper {
+ template <typename ThisAlt, typename ThatAlt>
+ inline void operator()(ThisAlt &this_alt, ThatAlt &that_alt) const {
+ using std::swap;
+ swap(this_alt.value, that_alt.value);
+ }
+ };
+#endif
+
+ inline constexpr bool move_nothrow() const {
+ return this->valueless_by_exception() ||
+ lib::array<bool, sizeof...(Ts)>{
+ {std::is_nothrow_move_constructible<Ts>::value...}
+ }[this->index()];
+ }
+ };
+
+ template <std::size_t I, typename T>
+ struct overload_leaf {
+ using F = lib::size_constant<I> (*)(T);
+ operator F() const { return nullptr; }
+ };
+
+ template <typename... Ts>
+ struct overload_impl {
+ private:
+ template <typename>
+ struct impl;
+
+ template <std::size_t... Is>
+ struct impl<lib::index_sequence<Is...>> : overload_leaf<Is, Ts>... {};
+
+ public:
+ using type = impl<lib::index_sequence_for<Ts...>>;
+ };
+
+ template <typename... Ts>
+ using overload = typename overload_impl<Ts...>::type;
+
+ template <typename T, typename... Ts>
+ using best_match = lib::invoke_result_t<overload<Ts...>, T &&>;
+
+ template <typename T>
+ struct is_in_place_index : std::false_type {};
+
+ template <std::size_t I>
+ struct is_in_place_index<in_place_index_t<I>> : std::true_type {};
+
+ template <typename T>
+ struct is_in_place_type : std::false_type {};
+
+ template <typename T>
+ struct is_in_place_type<in_place_type_t<T>> : std::true_type {};
+
+ } // detail
+
+ template <typename... Ts>
+ class variant {
+ static_assert(0 < sizeof...(Ts),
+ "variant must consist of at least one alternative.");
+
+ static_assert(lib::all<!std::is_array<Ts>::value...>::value,
+ "variant can not have an array type as an alternative.");
+
+ static_assert(lib::all<!std::is_reference<Ts>::value...>::value,
+ "variant can not have a reference type as an alternative.");
+
+ static_assert(lib::all<!std::is_void<Ts>::value...>::value,
+ "variant can not have a void type as an alternative.");
+
+ public:
+ template <
+ typename Front = lib::type_pack_element_t<0, Ts...>,
+ lib::enable_if_t<std::is_default_constructible<Front>::value, int> = 0>
+ inline constexpr variant() noexcept(
+ std::is_nothrow_default_constructible<Front>::value)
+ : impl_(in_place_index_t<0>{}) {}
+
+ variant(const variant &) = default;
+ variant(variant &&) = default;
+
+ template <
+ typename Arg,
+ typename Decayed = lib::decay_t<Arg>,
+ lib::enable_if_t<!std::is_same<Decayed, variant>::value, int> = 0,
+ lib::enable_if_t<!detail::is_in_place_index<Decayed>::value, int> = 0,
+ lib::enable_if_t<!detail::is_in_place_type<Decayed>::value, int> = 0,
+ std::size_t I = detail::best_match<Arg, Ts...>::value,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<std::is_constructible<T, Arg>::value, int> = 0>
+ inline constexpr variant(Arg &&arg) noexcept(
+ std::is_nothrow_constructible<T, Arg>::value)
+ : impl_(in_place_index_t<I>{}, lib::forward<Arg>(arg)) {}
+
+ template <
+ std::size_t I,
+ typename... Args,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<std::is_constructible<T, Args...>::value, int> = 0>
+ inline explicit constexpr variant(
+ in_place_index_t<I>,
+ Args &&... args) noexcept(std::is_nothrow_constructible<T,
+ Args...>::value)
+ : impl_(in_place_index_t<I>{}, lib::forward<Args>(args)...) {}
+
+ template <
+ std::size_t I,
+ typename Up,
+ typename... Args,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<std::is_constructible<T,
+ std::initializer_list<Up> &,
+ Args...>::value,
+ int> = 0>
+ inline explicit constexpr variant(
+ in_place_index_t<I>,
+ std::initializer_list<Up> il,
+ Args &&... args) noexcept(std::
+ is_nothrow_constructible<
+ T,
+ std::initializer_list<Up> &,
+ Args...>::value)
+ : impl_(in_place_index_t<I>{}, il, lib::forward<Args>(args)...) {}
+
+ template <
+ typename T,
+ typename... Args,
+ std::size_t I = detail::find_index_sfinae<T, Ts...>::value,
+ lib::enable_if_t<std::is_constructible<T, Args...>::value, int> = 0>
+ inline explicit constexpr variant(
+ in_place_type_t<T>,
+ Args &&... args) noexcept(std::is_nothrow_constructible<T,
+ Args...>::value)
+ : impl_(in_place_index_t<I>{}, lib::forward<Args>(args)...) {}
+
+ template <
+ typename T,
+ typename Up,
+ typename... Args,
+ std::size_t I = detail::find_index_sfinae<T, Ts...>::value,
+ lib::enable_if_t<std::is_constructible<T,
+ std::initializer_list<Up> &,
+ Args...>::value,
+ int> = 0>
+ inline explicit constexpr variant(
+ in_place_type_t<T>,
+ std::initializer_list<Up> il,
+ Args &&... args) noexcept(std::
+ is_nothrow_constructible<
+ T,
+ std::initializer_list<Up> &,
+ Args...>::value)
+ : impl_(in_place_index_t<I>{}, il, lib::forward<Args>(args)...) {}
+
+ ~variant() = default;
+
+ variant &operator=(const variant &) = default;
+ variant &operator=(variant &&) = default;
+
+ template <typename Arg,
+ lib::enable_if_t<!std::is_same<lib::decay_t<Arg>, variant>::value,
+ int> = 0,
+ std::size_t I = detail::best_match<Arg, Ts...>::value,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<(std::is_assignable<T &, Arg>::value &&
+ std::is_constructible<T, Arg>::value),
+ int> = 0>
+ inline variant &operator=(Arg &&arg) noexcept(
+ (std::is_nothrow_assignable<T &, Arg>::value &&
+ std::is_nothrow_constructible<T, Arg>::value)) {
+ impl_.template assign<I>(lib::forward<Arg>(arg));
+ return *this;
+ }
+
+ template <
+ std::size_t I,
+ typename... Args,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<std::is_constructible<T, Args...>::value, int> = 0>
+ inline T &emplace(Args &&... args) {
+ return impl_.template emplace<I>(lib::forward<Args>(args)...);
+ }
+
+ template <
+ std::size_t I,
+ typename Up,
+ typename... Args,
+ typename T = lib::type_pack_element_t<I, Ts...>,
+ lib::enable_if_t<std::is_constructible<T,
+ std::initializer_list<Up> &,
+ Args...>::value,
+ int> = 0>
+ inline T &emplace(std::initializer_list<Up> il, Args &&... args) {
+ return impl_.template emplace<I>(il, lib::forward<Args>(args)...);
+ }
+
+ template <
+ typename T,
+ typename... Args,
+ std::size_t I = detail::find_index_sfinae<T, Ts...>::value,
+ lib::enable_if_t<std::is_constructible<T, Args...>::value, int> = 0>
+ inline T &emplace(Args &&... args) {
+ return impl_.template emplace<I>(lib::forward<Args>(args)...);
+ }
+
+ template <
+ typename T,
+ typename Up,
+ typename... Args,
+ std::size_t I = detail::find_index_sfinae<T, Ts...>::value,
+ lib::enable_if_t<std::is_constructible<T,
+ std::initializer_list<Up> &,
+ Args...>::value,
+ int> = 0>
+ inline T &emplace(std::initializer_list<Up> il, Args &&... args) {
+ return impl_.template emplace<I>(il, lib::forward<Args>(args)...);
+ }
+
+ inline constexpr bool valueless_by_exception() const noexcept {
+ return impl_.valueless_by_exception();
+ }
+
+ inline constexpr std::size_t index() const noexcept {
+ return impl_.index();
+ }
+
+ template <bool Dummy = true,
+ lib::enable_if_t<
+ lib::all<Dummy,
+ (lib::dependent_type<std::is_move_constructible<Ts>,
+ Dummy>::value &&
+ lib::dependent_type<lib::is_swappable<Ts>,
+ Dummy>::value)...>::value,
+ int> = 0>
+ inline void swap(variant &that) noexcept(
+ lib::all<(std::is_nothrow_move_constructible<Ts>::value &&
+ lib::is_nothrow_swappable<Ts>::value)...>::value) {
+ impl_.swap(that.impl_);
+ }
+
+ private:
+ detail::impl<Ts...> impl_;
+
+ friend struct detail::access::variant;
+ friend struct detail::visitation::variant;
+ };
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr bool holds_alternative(const variant<Ts...> &v) noexcept {
+ return v.index() == I;
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr bool holds_alternative(const variant<Ts...> &v) noexcept {
+ return holds_alternative<detail::find_index_checked<T, Ts...>::value>(v);
+ }
+
+ namespace detail {
+ template <std::size_t I, typename V>
+ struct generic_get_impl {
+ constexpr generic_get_impl(int) noexcept {}
+
+ constexpr AUTO_REFREF operator()(V &&v) const
+ AUTO_REFREF_RETURN(
+ access::variant::get_alt<I>(lib::forward<V>(v)).value)
+ };
+
+ template <std::size_t I, typename V>
+ inline constexpr AUTO_REFREF generic_get(V &&v)
+ AUTO_REFREF_RETURN(generic_get_impl<I, V>(
+ holds_alternative<I>(v) ? 0 : (throw_bad_variant_access(), 0))(
+ lib::forward<V>(v)))
+ } // namespace detail
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr variant_alternative_t<I, variant<Ts...>> &get(
+ variant<Ts...> &v) {
+ return detail::generic_get<I>(v);
+ }
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr variant_alternative_t<I, variant<Ts...>> &&get(
+ variant<Ts...> &&v) {
+ return detail::generic_get<I>(lib::move(v));
+ }
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr const variant_alternative_t<I, variant<Ts...>> &get(
+ const variant<Ts...> &v) {
+ return detail::generic_get<I>(v);
+ }
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr const variant_alternative_t<I, variant<Ts...>> &&get(
+ const variant<Ts...> &&v) {
+ return detail::generic_get<I>(lib::move(v));
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr T &get(variant<Ts...> &v) {
+ return get<detail::find_index_checked<T, Ts...>::value>(v);
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr T &&get(variant<Ts...> &&v) {
+ return get<detail::find_index_checked<T, Ts...>::value>(lib::move(v));
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr const T &get(const variant<Ts...> &v) {
+ return get<detail::find_index_checked<T, Ts...>::value>(v);
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr const T &&get(const variant<Ts...> &&v) {
+ return get<detail::find_index_checked<T, Ts...>::value>(lib::move(v));
+ }
+
+ namespace detail {
+
+ template <std::size_t I, typename V>
+ inline constexpr /* auto * */ AUTO generic_get_if(V *v) noexcept
+ AUTO_RETURN(v && holds_alternative<I>(*v)
+ ? lib::addressof(access::variant::get_alt<I>(*v).value)
+ : nullptr)
+
+ } // namespace detail
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr lib::add_pointer_t<variant_alternative_t<I, variant<Ts...>>>
+ get_if(variant<Ts...> *v) noexcept {
+ return detail::generic_get_if<I>(v);
+ }
+
+ template <std::size_t I, typename... Ts>
+ inline constexpr lib::add_pointer_t<
+ const variant_alternative_t<I, variant<Ts...>>>
+ get_if(const variant<Ts...> *v) noexcept {
+ return detail::generic_get_if<I>(v);
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr lib::add_pointer_t<T>
+ get_if(variant<Ts...> *v) noexcept {
+ return get_if<detail::find_index_checked<T, Ts...>::value>(v);
+ }
+
+ template <typename T, typename... Ts>
+ inline constexpr lib::add_pointer_t<const T>
+ get_if(const variant<Ts...> *v) noexcept {
+ return get_if<detail::find_index_checked<T, Ts...>::value>(v);
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator==(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::equal_to;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (lhs.index() != rhs.index()) return false;
+ if (lhs.valueless_by_exception()) return true;
+ return variant::visit_value_at(lhs.index(), equal_to{}, lhs, rhs);
+#else
+ return lhs.index() == rhs.index() &&
+ (lhs.valueless_by_exception() ||
+ variant::visit_value_at(lhs.index(), equal_to{}, lhs, rhs));
+#endif
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator!=(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::not_equal_to;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (lhs.index() != rhs.index()) return true;
+ if (lhs.valueless_by_exception()) return false;
+ return variant::visit_value_at(lhs.index(), not_equal_to{}, lhs, rhs);
+#else
+ return lhs.index() != rhs.index() ||
+ (!lhs.valueless_by_exception() &&
+ variant::visit_value_at(lhs.index(), not_equal_to{}, lhs, rhs));
+#endif
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator<(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::less;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (rhs.valueless_by_exception()) return false;
+ if (lhs.valueless_by_exception()) return true;
+ if (lhs.index() < rhs.index()) return true;
+ if (lhs.index() > rhs.index()) return false;
+ return variant::visit_value_at(lhs.index(), less{}, lhs, rhs);
+#else
+ return !rhs.valueless_by_exception() &&
+ (lhs.valueless_by_exception() || lhs.index() < rhs.index() ||
+ (lhs.index() == rhs.index() &&
+ variant::visit_value_at(lhs.index(), less{}, lhs, rhs)));
+#endif
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator>(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::greater;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (lhs.valueless_by_exception()) return false;
+ if (rhs.valueless_by_exception()) return true;
+ if (lhs.index() > rhs.index()) return true;
+ if (lhs.index() < rhs.index()) return false;
+ return variant::visit_value_at(lhs.index(), greater{}, lhs, rhs);
+#else
+ return !lhs.valueless_by_exception() &&
+ (rhs.valueless_by_exception() || lhs.index() > rhs.index() ||
+ (lhs.index() == rhs.index() &&
+ variant::visit_value_at(lhs.index(), greater{}, lhs, rhs)));
+#endif
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator<=(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::less_equal;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (lhs.valueless_by_exception()) return true;
+ if (rhs.valueless_by_exception()) return false;
+ if (lhs.index() < rhs.index()) return true;
+ if (lhs.index() > rhs.index()) return false;
+ return variant::visit_value_at(lhs.index(), less_equal{}, lhs, rhs);
+#else
+ return lhs.valueless_by_exception() ||
+ (!rhs.valueless_by_exception() &&
+ (lhs.index() < rhs.index() ||
+ (lhs.index() == rhs.index() &&
+ variant::visit_value_at(lhs.index(), less_equal{}, lhs, rhs))));
+#endif
+ }
+
+ template <typename... Ts>
+ inline constexpr bool operator>=(const variant<Ts...> &lhs,
+ const variant<Ts...> &rhs) {
+ using detail::visitation::variant;
+ using lib::greater_equal;
+#ifdef MPARK_CPP14_CONSTEXPR
+ if (rhs.valueless_by_exception()) return true;
+ if (lhs.valueless_by_exception()) return false;
+ if (lhs.index() > rhs.index()) return true;
+ if (lhs.index() < rhs.index()) return false;
+ return variant::visit_value_at(lhs.index(), greater_equal{}, lhs, rhs);
+#else
+ return rhs.valueless_by_exception() ||
+ (!lhs.valueless_by_exception() &&
+ (lhs.index() > rhs.index() ||
+ (lhs.index() == rhs.index() &&
+ variant::visit_value_at(
+ lhs.index(), greater_equal{}, lhs, rhs))));
+#endif
+ }
+
+ struct monostate {};
+
+ inline constexpr bool operator<(monostate, monostate) noexcept {
+ return false;
+ }
+
+ inline constexpr bool operator>(monostate, monostate) noexcept {
+ return false;
+ }
+
+ inline constexpr bool operator<=(monostate, monostate) noexcept {
+ return true;
+ }
+
+ inline constexpr bool operator>=(monostate, monostate) noexcept {
+ return true;
+ }
+
+ inline constexpr bool operator==(monostate, monostate) noexcept {
+ return true;
+ }
+
+ inline constexpr bool operator!=(monostate, monostate) noexcept {
+ return false;
+ }
+
+#ifdef MPARK_CPP14_CONSTEXPR
+ namespace detail {
+
+ inline constexpr bool all(std::initializer_list<bool> bs) {
+ for (bool b : bs) {
+ if (!b) {
+ return false;
+ }
+ }
+ return true;
+ }
+
+ } // namespace detail
+
+ template <typename Visitor, typename... Vs>
+ inline constexpr decltype(auto) visit(Visitor &&visitor, Vs &&... vs) {
+ return (detail::all({!vs.valueless_by_exception()...})
+ ? (void)0
+ : throw_bad_variant_access()),
+ detail::visitation::variant::visit_value(
+ lib::forward<Visitor>(visitor), lib::forward<Vs>(vs)...);
+ }
+#else
+ namespace detail {
+
+ template <std::size_t N>
+ inline constexpr bool all_impl(const lib::array<bool, N> &bs,
+ std::size_t idx) {
+ return idx >= N || (bs[idx] && all_impl(bs, idx + 1));
+ }
+
+ template <std::size_t N>
+ inline constexpr bool all(const lib::array<bool, N> &bs) {
+ return all_impl(bs, 0);
+ }
+
+ } // namespace detail
+
+ template <typename Visitor, typename... Vs>
+ inline constexpr DECLTYPE_AUTO visit(Visitor &&visitor, Vs &&... vs)
+ DECLTYPE_AUTO_RETURN(
+ (detail::all(
+ lib::array<bool, sizeof...(Vs)>{{!vs.valueless_by_exception()...}})
+ ? (void)0
+ : throw_bad_variant_access()),
+ detail::visitation::variant::visit_value(lib::forward<Visitor>(visitor),
+ lib::forward<Vs>(vs)...))
+#endif
+
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wnoexcept"
+#endif
+ template <typename... Ts>
+ inline auto swap(variant<Ts...> &lhs,
+ variant<Ts...> &rhs) noexcept(noexcept(lhs.swap(rhs)))
+ -> decltype(lhs.swap(rhs)) {
+ lhs.swap(rhs);
+ }
+#if defined(__GNUC__) && !defined(__clang__)
+#pragma GCC diagnostic pop
+#endif
+
+ namespace detail {
+
+ template <typename T, typename...>
+ using enabled_type = T;
+
+ namespace hash {
+
+ template <typename H, typename K>
+ constexpr bool meets_requirements() noexcept {
+ return std::is_copy_constructible<H>::value &&
+ std::is_move_constructible<H>::value &&
+ lib::is_invocable_r<std::size_t, H, const K &>::value;
+ }
+
+ template <typename K>
+ constexpr bool is_enabled() noexcept {
+ using H = std::hash<K>;
+ return meets_requirements<H, K>() &&
+ std::is_default_constructible<H>::value &&
+ std::is_copy_assignable<H>::value &&
+ std::is_move_assignable<H>::value;
+ }
+
+ } // namespace hash
+
+ } // namespace detail
+
+#undef AUTO
+#undef AUTO_RETURN
+
+#undef AUTO_REFREF
+#undef AUTO_REFREF_RETURN
+
+#undef DECLTYPE_AUTO
+#undef DECLTYPE_AUTO_RETURN
+
+} // namespace mpark
+
+namespace std {
+
+ template <typename... Ts>
+ struct hash<mpark::detail::enabled_type<
+ mpark::variant<Ts...>,
+ mpark::lib::enable_if_t<mpark::lib::all<mpark::detail::hash::is_enabled<
+ mpark::lib::remove_const_t<Ts>>()...>::value>>> {
+ using argument_type = mpark::variant<Ts...>;
+ using result_type = std::size_t;
+
+ inline result_type operator()(const argument_type &v) const {
+ using mpark::detail::visitation::variant;
+ std::size_t result =
+ v.valueless_by_exception()
+ ? 299792458 // Random value chosen by the universe upon creation
+ : variant::visit_alt(
+#ifdef MPARK_GENERIC_LAMBDAS
+ [](const auto &alt) {
+ using alt_type = mpark::lib::decay_t<decltype(alt)>;
+ using value_type = mpark::lib::remove_const_t<
+ typename alt_type::value_type>;
+ return hash<value_type>{}(alt.value);
+ }
+#else
+ hasher{}
+#endif
+ ,
+ v);
+ return hash_combine(result, hash<std::size_t>{}(v.index()));
+ }
+
+ private:
+#ifndef MPARK_GENERIC_LAMBDAS
+ struct hasher {
+ template <typename Alt>
+ inline std::size_t operator()(const Alt &alt) const {
+ using alt_type = mpark::lib::decay_t<Alt>;
+ using value_type =
+ mpark::lib::remove_const_t<typename alt_type::value_type>;
+ return hash<value_type>{}(alt.value);
+ }
+ };
+#endif
+
+ static std::size_t hash_combine(std::size_t lhs, std::size_t rhs) {
+ return lhs ^= rhs + 0x9e3779b9 + (lhs << 6) + (lhs >> 2);
+ }
+ };
+
+ template <>
+ struct hash<mpark::monostate> {
+ using argument_type = mpark::monostate;
+ using result_type = std::size_t;
+
+ inline result_type operator()(const argument_type &) const noexcept {
+ return 66740831; // return a fundamentally attractive random value.
+ }
+ };
+
+} // namespace std
+
+#endif // MPARK_VARIANT_HPP
diff --git a/externals/Pangolin/include/pangolin/compat/glutbitmap.h b/externals/Pangolin/include/pangolin/compat/glutbitmap.h
new file mode 100644
index 0000000000000000000000000000000000000000..b4661b07f6022c9be97888818b606792664c3445
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/compat/glutbitmap.h
@@ -0,0 +1,92 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/gl/glfont.h>
+
+#ifdef HAVE_GLES
+GLfloat g_raster_pos[4];
+
+inline void glRasterPos3f(GLfloat x, GLfloat y, GLfloat z)
+{
+ // find object point (x,y,z)' in pixel coords
+ GLdouble projection[16];
+ GLdouble modelview[16];
+ GLint view[4];
+
+#ifdef HAVE_GLES_2
+ std::copy(pangolin::glEngine().projection.top().m, pangolin::glEngine().projection.top().m+16, projection);
+ std::copy(pangolin::glEngine().modelview.top().m, pangolin::glEngine().modelview.top().m+16, modelview);
+#else
+ glGetDoublev(GL_PROJECTION_MATRIX, projection );
+ glGetDoublev(GL_MODELVIEW_MATRIX, modelview );
+#endif
+ glGetIntegerv(GL_VIEWPORT, view );
+
+ pangolin::glProject(x, y, z, modelview, projection, view,
+ g_raster_pos, g_raster_pos + 1, g_raster_pos + 2);
+}
+
+inline void glRasterPos2f(GLfloat x, GLfloat y)
+{
+ glRasterPos3f(x,y,1.0f);
+}
+
+inline void glRasterPos2i(GLint x, GLint y)
+{
+ glRasterPos3f((GLfloat)x, (GLfloat)y, 1.0f );
+}
+
+inline void glRasterPos3fv(const GLfloat *v){
+ glRasterPos3f(v[0],v[1],v[2]);
+}
+
+inline void glRasterPos2fv(const GLfloat *v){
+ glRasterPos3f(v[0],v[1],1.0f);
+}
+#endif // HAVE_GLES
+
+inline void glutBitmapString(void * /*font*/, const unsigned char *str)
+{
+#ifndef HAVE_GLES
+ float g_raster_pos[4];
+ glGetFloatv(GL_CURRENT_RASTER_POSITION, g_raster_pos);
+#endif
+
+ pangolin::GlFont::I().Text( (const char *)str ).DrawWindow(
+ g_raster_pos[0], g_raster_pos[1], g_raster_pos[2]
+ );
+}
+
+inline int glutBitmapLength(void * /*font*/, const unsigned char *str)
+{
+ return (int)(pangolin::GlFont::I().Text((const char *)str).Width());
+}
+
+#define GLUT_BITMAP_HELVETICA_12 0;
diff --git a/externals/Pangolin/include/pangolin/compat/optional.h b/externals/Pangolin/include/pangolin/compat/optional.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d021d7cf0b84c1c6451a0cf06dba23aafc11843
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/compat/optional.h
@@ -0,0 +1,20 @@
+#pragma once
+
+#include <pangolin/platform.h>
+
+// Use either C++17 optional, or the standalone backwards compatible version
+#if (__cplusplus >= 201703L)
+# include <optional>
+#else
+# include <experimental/optional.hpp>
+#endif
+
+namespace pangolin {
+#if (__cplusplus >= 201703L)
+template <typename T>
+using optional = std::optional<T>;
+#else
+template <typename T>
+using optional = std::experimental::optional<T>;
+#endif
+}
diff --git a/externals/Pangolin/include/pangolin/compat/type_traits.h b/externals/Pangolin/include/pangolin/compat/type_traits.h
new file mode 100644
index 0000000000000000000000000000000000000000..30bec5ba52bda78e380287c42c242e28e53fdcda
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/compat/type_traits.h
@@ -0,0 +1,49 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <typeinfo>
+
+#include <type_traits>
+
+// enable_if From Boost
+namespace pangolin
+{
+ template <bool B, class T = void>
+ struct enable_if_c {
+ typedef T type;
+ };
+
+ template <class T>
+ struct enable_if_c<false, T> {};
+
+ template <class Cond, class T = void>
+ struct enable_if : public enable_if_c<Cond::value, T> {};
+}
diff --git a/externals/Pangolin/include/pangolin/compat/variant.h b/externals/Pangolin/include/pangolin/compat/variant.h
new file mode 100644
index 0000000000000000000000000000000000000000..84ee74b01856c5c8ecfffb930ac4e1e0eefdc3db
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/compat/variant.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <pangolin/platform.h>
+
+// Use either C++17 variant, or the standalone backwards compatible version
+// of M. Park.
+#if (__cplusplus >= 201703L)
+# include <variant>
+#else
+# include <mpark/variant.hpp>
+#endif
+
+namespace pangolin {
+#if (__cplusplus >= 201703L)
+using std::variant;
+using std::get;
+using std::get_if;
+using std::visit;
+#else
+using mpark::variant;
+using mpark::get;
+using mpark::get_if;
+using mpark::visit;
+#endif
+}
diff --git a/externals/Pangolin/include/pangolin/console/ConsoleInterpreter.h b/externals/Pangolin/include/pangolin/console/ConsoleInterpreter.h
new file mode 100644
index 0000000000000000000000000000000000000000..31d1e1087d4b22e8c9041658c2fd1d8997cf8574
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/console/ConsoleInterpreter.h
@@ -0,0 +1,80 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <vector>
+#include <string>
+
+namespace pangolin
+{
+
+enum ConsoleLineType
+{
+ ConsoleLineTypeCmd,
+ ConsoleLineTypeCmdOptions,
+ ConsoleLineTypeStdout,
+ ConsoleLineTypeStderr,
+ ConsoleLineTypeOutput,
+ ConsoleLineTypeHelp,
+};
+
+class ConsoleLine
+{
+public:
+ inline ConsoleLine()
+ : linetype(ConsoleLineTypeCmd)
+ {
+ }
+
+ inline ConsoleLine(std::string text, ConsoleLineType linetype = ConsoleLineTypeOutput)
+ : text(text), linetype(linetype)
+ {
+ }
+
+ std::string text;
+ ConsoleLineType linetype;
+};
+
+class ConsoleInterpreter
+{
+public:
+ inline virtual ~ConsoleInterpreter()
+ {
+ }
+
+ virtual void PushCommand(const std::string& cmd) = 0;
+
+ virtual bool PullLine(ConsoleLine& line) = 0;
+
+ virtual std::vector<std::string> Complete(
+ const std::string& cmd, int max_options = 20
+ ) = 0;
+
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/console/ConsoleView.h b/externals/Pangolin/include/pangolin/console/ConsoleView.h
new file mode 100644
index 0000000000000000000000000000000000000000..e45feab8497e05665bd549c8c170a998415e26d6
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/console/ConsoleView.h
@@ -0,0 +1,109 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <deque>
+
+#include <pangolin/platform.h>
+#include <pangolin/gl/glfont.h>
+#include <pangolin/var/var.h>
+#include <pangolin/display/view.h>
+#include <pangolin/handler/handler.h>
+#include <pangolin/gl/colour.h>
+
+#include <pangolin/console/ConsoleInterpreter.h>
+
+namespace pangolin
+{
+
+class ConsoleView : public pangolin::View, pangolin::Handler
+{
+public:
+ struct Line
+ {
+ Line()
+ : linetype(ConsoleLineTypeCmd)
+ {
+ }
+
+ Line(const GlText& text, ConsoleLineType linetype = ConsoleLineTypeCmd )
+ : text(text), linetype(linetype)
+ {
+ }
+
+ GlText text;
+ ConsoleLineType linetype;
+ };
+
+
+ // Construct with interpreter (and take ownership)
+ ConsoleView(ConsoleInterpreter* interpreter);
+
+ ~ConsoleView();
+
+ View& ShowWithoutAnimation(bool show=true);
+
+ // Replace implementation in View to account for hiding animation
+ View& Show(bool show=true);
+
+ // Replace implementation in View to account for hiding animation
+ void ToggleShow();
+
+ // Replace implementation in View to account for hiding animation
+ bool IsShown() const;
+
+ void Render() override;
+
+ void Keyboard(View&, unsigned char key, int x, int y, bool pressed) override;
+
+private:
+ void DrawLine(const ConsoleView::Line& l, int carat);
+
+ void ProcessOutputLines();
+
+ void AddLine(const std::string& text, ConsoleLineType linetype = ConsoleLineTypeCmd);
+
+ Line* GetLine(int id, ConsoleLineType line_type, const std::string& prefix = "");
+
+ ConsoleInterpreter* interpreter;
+
+ GlFont& font;
+
+ int carat;
+ Line current_line;
+ std::deque<Line> line_buffer;
+
+ bool hiding;
+ GLfloat bottom;
+
+ Colour background_colour;
+ std::map<ConsoleLineType,pangolin::Colour> line_colours;
+ float animation_speed;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/attach.h b/externals/Pangolin/include/pangolin/display/attach.h
new file mode 100644
index 0000000000000000000000000000000000000000..1bc776ca2cf7b2467faa5d507cbb5fed50ad4b49
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/attach.h
@@ -0,0 +1,86 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <iostream>
+#include <string>
+#include <cmath>
+
+namespace pangolin
+{
+
+/// Units for measuring screen distances.
+enum Unit {
+ Fraction,
+ Pixel,
+ ReversePixel
+};
+
+/// Defines absolute or relative position from parent viewport edge.
+/// Constructors distinguised by whole pixels, or floating
+/// fraction in interval [0,1]
+struct PANGOLIN_EXPORT Attach {
+ /// Attach to Left/Bottom edge
+ Attach() : unit(Fraction), p(0) {}
+
+ /// General constructor
+ Attach(Unit unit, GLfloat p) : unit(unit), p(p) {}
+
+ /// Specify relative position in range [0,1].
+ /// 0 represents leftmost / bottom-most edge,
+ /// 1 represents rightmost / topmost edge
+ Attach(GLfloat p) : unit(Fraction), p(p) {
+ // Allow for numerical imprecision when checking usage.
+ if( p < -1E-3 || 1.001 < p ) {
+ std::cerr << "Pangolin API Change: Display::SetBounds must be used with Attach::Pix or Attach::ReversePix to specify pixel bounds relative to an edge. See the code samples for details." << std::endl;
+ throw std::exception();
+ }
+ }
+
+ /// Specify absolute position from leftmost / bottom-most edge.
+ static Attach Pix(int p) {
+ return Attach(p >=0 ? Pixel : ReversePixel, std::abs((float)p));
+ }
+
+ /// Specify absolute position from rightmost / topmost edge.
+ static Attach ReversePix(int p) {
+ return Attach(ReversePixel, (GLfloat)p);
+ }
+
+ /// Specify relative position in range [0,1].
+ /// 0 represents leftmost / bottom-most edge,
+ /// 1 represents rightmost / topmost edge
+ static Attach Frac(float frac) {
+ return Attach(frac);
+ }
+
+ Unit unit;
+ GLfloat p;
+};
+
+} // namespace pangolin
diff --git a/externals/Pangolin/include/pangolin/display/device/OsxWindow.h b/externals/Pangolin/include/pangolin/display/device/OsxWindow.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d1be5df70d60372e2f6fb2bc934764ac5790596
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/OsxWindow.h
@@ -0,0 +1,68 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/display/display_internal.h>
+
+#include <pangolin/display/device/PangolinNSApplication.h>
+#include <pangolin/display/device/PangolinNSGLView.h>
+
+namespace pangolin
+{
+
+struct OsxWindow : public PangolinGl
+{
+ OsxWindow(const std::string& title, int width, int height, bool USE_RETINA);
+
+ ~OsxWindow();
+
+ void StartFullScreen();
+
+ void StopFullScreen();
+
+ void ToggleFullscreen() override;
+
+ void Move(int x, int y) override;
+
+ void Resize(unsigned int w, unsigned int h) override;
+
+ void MakeCurrent() override;
+
+ void RemoveCurrent() override;
+
+ void SwapBuffers() override;
+
+ void ProcessEvents() override;
+
+private:
+ NSWindow* _window;
+ PangolinNSGLView *view;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/device/PangolinNSApplication.h b/externals/Pangolin/include/pangolin/display/device/PangolinNSApplication.h
new file mode 100644
index 0000000000000000000000000000000000000000..799f3750d9f2987ac765c569a31d2371f2cf4197
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/PangolinNSApplication.h
@@ -0,0 +1,59 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#import <Carbon/Carbon.h>
+#import <Cocoa/Cocoa.h>
+
+////////////////////////////////////////////////////////////////////
+// PangolinNSApplication
+////////////////////////////////////////////////////////////////////
+
+@interface PangolinNSApplication : NSObject {
+}
+
++ (void)run_pre;
++ (void)run_step;
+
+@end
+
+////////////////////////////////////////////////////////////////////
+// PangolinWindowDelegate
+////////////////////////////////////////////////////////////////////
+
+@interface PangolinWindowDelegate : NSObject <NSWindowDelegate>
+
+@end
+
+////////////////////////////////////////////////////////////////////
+// PangolinAppDelegate
+////////////////////////////////////////////////////////////////////
+
+@interface PangolinAppDelegate : NSObject <NSApplicationDelegate>
+
+@end
diff --git a/externals/Pangolin/include/pangolin/display/device/PangolinNSGLView.h b/externals/Pangolin/include/pangolin/display/device/PangolinNSGLView.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b8cef58fa24c868fe3354b64217c0d009b41f01
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/PangolinNSGLView.h
@@ -0,0 +1,45 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#import <Carbon/Carbon.h>
+#import <Cocoa/Cocoa.h>
+
+#include <pangolin/display/display_internal.h>
+
+////////////////////////////////////////////////////////////////////
+// PangolinNSGLView
+////////////////////////////////////////////////////////////////////
+
+@interface PangolinNSGLView : NSOpenGLView
+{
+ pangolin::PangolinGl* context;
+ float backing_scale;
+}
+@end
+
diff --git a/externals/Pangolin/include/pangolin/display/device/WinWindow.h b/externals/Pangolin/include/pangolin/display/device/WinWindow.h
new file mode 100644
index 0000000000000000000000000000000000000000..35e85adcfd67cbad1d886c9eadcece52423ed569
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/WinWindow.h
@@ -0,0 +1,89 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/display/display_internal.h>
+
+#include <string>
+
+#include <windowsx.h>
+
+namespace pangolin
+{
+
+struct WinWindow : public PangolinGl
+{
+ WinWindow(
+ const std::string& title, int width, int height
+ );
+
+ ~WinWindow();
+
+ void StartFullScreen();
+
+ void StopFullScreen();
+
+ void ToggleFullscreen() override;
+
+ void Move(int x, int y) override;
+
+ void Resize(unsigned int w, unsigned int h) override;
+
+ void MakeCurrent() override;
+
+ void RemoveCurrent() override;
+
+ void SwapBuffers() override;
+
+ void ProcessEvents() override;
+
+ HGLRC GetGLRenderContext()
+ {
+ return hGLRC;
+ }
+
+private:
+ static LRESULT APIENTRY WndProc(HWND hWnd, UINT message, WPARAM wParam, LPARAM lParam);
+
+ LRESULT HandleWinMessages(UINT message, WPARAM wParam, LPARAM lParam);
+
+ void RegisterThisClass(HMODULE hCurrentInst);
+
+ void SetupPixelFormat(HDC hdc);
+
+ void SetupPalette(HDC hDC);
+
+ // Owns the Window
+ HWND hWnd;
+ HDC hDC;
+ HGLRC hGLRC;
+ HPALETTE hPalette;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/device/X11GlContext.h b/externals/Pangolin/include/pangolin/display/device/X11GlContext.h
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/externals/Pangolin/include/pangolin/display/device/X11Window.h b/externals/Pangolin/include/pangolin/display/device/X11Window.h
new file mode 100644
index 0000000000000000000000000000000000000000..c34c724b8a08c2afacfd2a7f9db605daa528bc06
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/X11Window.h
@@ -0,0 +1,109 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/display/display_internal.h>
+
+#include <stdexcept>
+#include <string>
+#include <X11/Xlib.h>
+#include <X11/Xutil.h>
+#include <GL/glx.h>
+
+namespace pangolin
+{
+
+struct X11Display
+{
+ X11Display(const char* name = 0) {
+ XInitThreads();
+ display = XOpenDisplay(name);
+ if (!display) {
+ throw std::runtime_error("Pangolin X11: Failed to open X display");
+ }
+ }
+
+ ~X11Display() {
+ XCloseDisplay(display);
+ }
+
+ // Owns the display
+ ::Display* display;
+};
+
+struct X11GlContext : public GlContextInterface
+{
+ X11GlContext(std::shared_ptr<X11Display> &d, ::GLXFBConfig chosenFbc, std::shared_ptr<X11GlContext> shared_context = std::shared_ptr<X11GlContext>() );
+ ~X11GlContext();
+
+ std::shared_ptr<X11Display> display;
+
+ std::shared_ptr<X11GlContext> shared_context;
+
+ // Owns the OpenGl Context
+ ::GLXContext glcontext;
+};
+
+struct X11Window : public PangolinGl
+{
+ X11Window(
+ const std::string& title, int width, int height,
+ std::shared_ptr<X11Display>& display, ::GLXFBConfig chosenFbc
+ );
+
+ ~X11Window();
+
+ void ToggleFullscreen() override;
+
+ void Move(int x, int y) override;
+
+ void Resize(unsigned int w, unsigned int h) override;
+
+ void MakeCurrent(GLXContext ctx);
+
+ void MakeCurrent() override;
+
+ void RemoveCurrent() override;
+
+ void SwapBuffers() override;
+
+ void ProcessEvents() override;
+
+ // References the X11 display and context.
+ std::shared_ptr<X11Display> display;
+ std::shared_ptr<X11GlContext> glcontext;
+
+ // Owns the X11 Window and Colourmap
+ ::Window win;
+ ::Colormap cmap;
+
+ Atom delete_message;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/device/display_android.h b/externals/Pangolin/include/pangolin/display/device/display_android.h
new file mode 100644
index 0000000000000000000000000000000000000000..b98a9b85d3422fce40eb61492576f9751f168b21
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/device/display_android.h
@@ -0,0 +1,333 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <pthread.h>
+#include <sched.h>
+
+#include <android/configuration.h>
+#include <android/looper.h>
+#include <android/native_activity.h>
+#include <android/log.h>
+#include <string>
+
+#include <pangolin/utils/type_convert.h>
+
+#define LOGI(...) ((void)__android_log_print(ANDROID_LOG_INFO, "pango", __VA_ARGS__))
+#define LOGW(...) ((void)__android_log_print(ANDROID_LOG_WARN, "pango", __VA_ARGS__))
+#define LOGE(...) ((void)__android_log_print(ANDROID_LOG_ERROR, "pango", __VA_ARGS__))
+
+/* For debug builds, always enable the debug traces in this library */
+#undef NDEBUG
+#ifndef NDEBUG
+# define LOGV(...) ((void)__android_log_print(ANDROID_LOG_VERBOSE, "pango", __VA_ARGS__))
+#else
+# define LOGV(...) ((void)0)
+#endif
+
+template<typename T> inline
+void Log(T v) {
+ const std::string sv = pangolin::Convert<std::string,T>::Do(v);
+ LOGI(sv.c_str());
+}
+
+template<typename T> inline
+void Log(const std::string& str, T v)
+{
+ const std::string sv = pangolin::Convert<std::string,T>::Do(v);
+ LOGI((str + ":" + sv).c_str());
+}
+
+namespace pangolin
+{
+ void CreateAndroidWindowAndBind(std::string name);
+ void ProcessAndroidEvents();
+ void FinishAndroidFrame();
+}
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+struct android_app;
+
+/**
+ * Data associated with an ALooper fd that will be returned as the "outData"
+ * when that source has data ready.
+ */
+struct android_poll_source {
+ // The identifier of this source. May be LOOPER_ID_MAIN or
+ // LOOPER_ID_INPUT.
+ int32_t id;
+
+ // The android_app this ident is associated with.
+ struct android_app* app;
+
+ // Function to call to perform the standard processing of data from
+ // this source.
+ void (*process)(struct android_app* app, struct android_poll_source* source);
+};
+
+/**
+ * This is the interface for the standard glue code of a threaded
+ * application. In this model, the application's code is running
+ * in its own thread separate from the main thread of the process.
+ * It is not required that this thread be associated with the Java
+ * VM, although it will need to be in order to make JNI calls any
+ * Java objects.
+ */
+struct android_app {
+ // The application can place a pointer to its own state object
+ // here if it likes.
+ void* userData;
+
+ // Fill this in with the function to process main app commands (APP_CMD_*)
+ void (*onAppCmd)(struct android_app* app, int32_t cmd);
+
+ // Fill this in with the function to process input events. At this point
+ // the event has already been pre-dispatched, and it will be finished upon
+ // return. Return 1 if you have handled the event, 0 for any default
+ // dispatching.
+ int32_t (*onInputEvent)(struct android_app* app, AInputEvent* event);
+
+ // The ANativeActivity object instance that this app is running in.
+ ANativeActivity* activity;
+
+ // The current configuration the app is running in.
+ AConfiguration* config;
+
+ // This is the last instance's saved state, as provided at creation time.
+ // It is NULL if there was no state. You can use this as you need; the
+ // memory will remain around until you call android_app_exec_cmd() for
+ // APP_CMD_RESUME, at which point it will be freed and savedState set to NULL.
+ // These variables should only be changed when processing a APP_CMD_SAVE_STATE,
+ // at which point they will be initialized to NULL and you can malloc your
+ // state and place the information here. In that case the memory will be
+ // freed for you later.
+ void* savedState;
+ size_t savedStateSize;
+
+ // The ALooper associated with the app's thread.
+ ALooper* looper;
+
+ // When non-NULL, this is the input queue from which the app will
+ // receive user input events.
+ AInputQueue* inputQueue;
+
+ // When non-NULL, this is the window surface that the app can draw in.
+ ANativeWindow* window;
+
+ // Current content rectangle of the window; this is the area where the
+ // window's content should be placed to be seen by the user.
+ ARect contentRect;
+
+ // Current state of the app's activity. May be either APP_CMD_START,
+ // APP_CMD_RESUME, APP_CMD_PAUSE, or APP_CMD_STOP; see below.
+ int activityState;
+
+ // This is non-zero when the application's NativeActivity is being
+ // destroyed and waiting for the app thread to complete.
+ int destroyRequested;
+
+ // -------------------------------------------------
+ // Below are "private" implementation of the glue code.
+
+ pthread_mutex_t mutex;
+ pthread_cond_t cond;
+
+ int msgread;
+ int msgwrite;
+
+ pthread_t thread;
+
+ struct android_poll_source cmdPollSource;
+ struct android_poll_source inputPollSource;
+
+ int running;
+ int stateSaved;
+ int destroyed;
+ int redrawNeeded;
+ AInputQueue* pendingInputQueue;
+ ANativeWindow* pendingWindow;
+ ARect pendingContentRect;
+
+ const char* application_so;
+};
+
+enum {
+ /**
+ * Looper data ID of commands coming from the app's main thread, which
+ * is returned as an identifier from ALooper_pollOnce(). The data for this
+ * identifier is a pointer to an android_poll_source structure.
+ * These can be retrieved and processed with android_app_read_cmd()
+ * and android_app_exec_cmd().
+ */
+ LOOPER_ID_MAIN = 1,
+
+ /**
+ * Looper data ID of events coming from the AInputQueue of the
+ * application's window, which is returned as an identifier from
+ * ALooper_pollOnce(). The data for this identifier is a pointer to an
+ * android_poll_source structure. These can be read via the inputQueue
+ * object of android_app.
+ */
+ LOOPER_ID_INPUT = 2,
+
+ /**
+ * Start of user-defined ALooper identifiers.
+ */
+ LOOPER_ID_USER = 3,
+};
+
+enum {
+ /**
+ * Command from main thread: the AInputQueue has changed. Upon processing
+ * this command, android_app->inputQueue will be updated to the new queue
+ * (or NULL).
+ */
+ APP_CMD_INPUT_CHANGED,
+
+ /**
+ * Command from main thread: a new ANativeWindow is ready for use. Upon
+ * receiving this command, android_app->window will contain the new window
+ * surface.
+ */
+ APP_CMD_INIT_WINDOW,
+
+ /**
+ * Command from main thread: the existing ANativeWindow needs to be
+ * terminated. Upon receiving this command, android_app->window still
+ * contains the existing window; after calling android_app_exec_cmd
+ * it will be set to NULL.
+ */
+ APP_CMD_TERM_WINDOW,
+
+ /**
+ * Command from main thread: the current ANativeWindow has been resized.
+ * Please redraw with its new size.
+ */
+ APP_CMD_WINDOW_RESIZED,
+
+ /**
+ * Command from main thread: the system needs that the current ANativeWindow
+ * be redrawn. You should redraw the window before handing this to
+ * android_app_exec_cmd() in order to avoid transient drawing glitches.
+ */
+ APP_CMD_WINDOW_REDRAW_NEEDED,
+
+ /**
+ * Command from main thread: the content area of the window has changed,
+ * such as from the soft input window being shown or hidden. You can
+ * find the new content rect in android_app::contentRect.
+ */
+ APP_CMD_CONTENT_RECT_CHANGED,
+
+ /**
+ * Command from main thread: the app's activity window has gained
+ * input focus.
+ */
+ APP_CMD_GAINED_FOCUS,
+
+ /**
+ * Command from main thread: the app's activity window has lost
+ * input focus.
+ */
+ APP_CMD_LOST_FOCUS,
+
+ /**
+ * Command from main thread: the current device configuration has changed.
+ */
+ APP_CMD_CONFIG_CHANGED,
+
+ /**
+ * Command from main thread: the system is running low on memory.
+ * Try to reduce your memory use.
+ */
+ APP_CMD_LOW_MEMORY,
+
+ /**
+ * Command from main thread: the app's activity has been started.
+ */
+ APP_CMD_START,
+
+ /**
+ * Command from main thread: the app's activity has been resumed.
+ */
+ APP_CMD_RESUME,
+
+ /**
+ * Command from main thread: the app should generate a new saved state
+ * for itself, to restore from later if needed. If you have saved state,
+ * allocate it with malloc and place it in android_app.savedState with
+ * the size in android_app.savedStateSize. The will be freed for you
+ * later.
+ */
+ APP_CMD_SAVE_STATE,
+
+ /**
+ * Command from main thread: the app's activity has been paused.
+ */
+ APP_CMD_PAUSE,
+
+ /**
+ * Command from main thread: the app's activity has been stopped.
+ */
+ APP_CMD_STOP,
+
+ /**
+ * Command from main thread: the app's activity is being destroyed,
+ * and waiting for the app thread to clean up and exit before proceeding.
+ */
+ APP_CMD_DESTROY,
+};
+
+/**
+ * Call when ALooper_pollAll() returns LOOPER_ID_MAIN, reading the next
+ * app command message.
+ */
+int8_t android_app_read_cmd(struct android_app* android_app);
+
+/**
+ * Call with the command returned by android_app_read_cmd() to do the
+ * initial pre-processing of the given command. You can perform your own
+ * actions for the command after calling this function.
+ */
+void android_app_pre_exec_cmd(struct android_app* android_app, int8_t cmd);
+
+/**
+ * Call with the command returned by android_app_read_cmd() to do the
+ * final post-processing of the given command. You must have done your own
+ * actions for the command before calling this function.
+ */
+void android_app_post_exec_cmd(struct android_app* android_app, int8_t cmd);
+
+#ifdef __cplusplus
+}
+#endif
diff --git a/externals/Pangolin/include/pangolin/display/display.h b/externals/Pangolin/include/pangolin/display/display.h
new file mode 100644
index 0000000000000000000000000000000000000000..987b90b14f94a1ddf6f9c04f76ee236546825563
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/display.h
@@ -0,0 +1,219 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove, Richard Newcombe
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/handler/handler_enums.h>
+#include <pangolin/utils/params.h>
+#include <pangolin/display/window.h>
+
+#include <functional>
+#include <string>
+#include <memory>
+
+/*! \file display.h
+ * This file contains a number of global methods for creating and
+ * querying window state as well as handling user input.
+ */
+
+namespace pangolin
+{
+
+ // CreateWindowAndBind parameter key names.
+ // X11 Window options:
+ extern const char* PARAM_DISPLAYNAME; // std::string
+ extern const char* PARAM_DOUBLEBUFFER; // bool
+ extern const char* PARAM_SAMPLE_BUFFERS; // int
+ extern const char* PARAM_SAMPLES; // int
+ extern const char* PARAM_HIGHRES; // bool - Apple Retina screens only
+
+ // Forward Declarations
+ struct View;
+ struct Viewport;
+ class UserApp;
+
+ /// Give this OpenGL context a name or switch contexts.
+ /// This is required to initialise Pangolin for use with an
+ /// externally defined OpenGL context. You needn't call it
+ /// if you have used CreateWindowAndBind() to create a window
+ /// or launched a pangolin::UserApp
+ PANGOLIN_EXPORT
+ WindowInterface& BindToContext(std::string name);
+
+ /// Initialise OpenGL window (determined by platform) and bind context.
+ /// This method will choose an available windowing system if one is present.
+ PANGOLIN_EXPORT
+ WindowInterface& CreateWindowAndBind(std::string window_title, int w = 640, int h = 480, const Params& params = Params());
+
+ /// Return pointer to current Pangolin Window context, or nullptr if none bound.
+ PANGOLIN_EXPORT
+ WindowInterface* GetBoundWindow();
+
+ PANGOLIN_EXPORT
+ void DestroyWindow(const std::string& window_title);
+
+ /// Launch users derived UserApp, controlling OpenGL event loop.
+ /// This method will block until the application exits, calling app's
+ /// Init() method to start and Render() method subsequently to draw each frame.
+ /// @return exit code for use when returning from main. Currently always 0.
+ PANGOLIN_EXPORT
+ int LaunchUserApp(UserApp& app);
+
+ /// Perform any post rendering, event processing and frame swapping.
+ PANGOLIN_EXPORT
+ void FinishFrame();
+
+ /// Request that the window close.
+ PANGOLIN_EXPORT
+ void Quit();
+
+ /// Request that all windows close.
+ PANGOLIN_EXPORT
+ void QuitAll();
+
+ /// Returns true if user has requested to close OpenGL window.
+ PANGOLIN_EXPORT
+ bool ShouldQuit();
+
+ /// Returns true if user has interacted with the window since this was last called.
+ PANGOLIN_EXPORT
+ bool HadInput();
+
+ /// Returns true if user has resized the window.
+ PANGOLIN_EXPORT
+ bool HasResized();
+
+ /// Renders any views with default draw methods.
+ PANGOLIN_EXPORT
+ void RenderViews();
+
+ /// Perform any post render events, such as screen recording.
+ PANGOLIN_EXPORT
+ void PostRender();
+
+ /// Request to be notified via functor when key is pressed.
+ /// Functor may take one parameter which will equal the key pressed
+ PANGOLIN_EXPORT
+ void RegisterKeyPressCallback(int key, std::function<void(void)> func);
+
+ /// Save window contents to image.
+ PANGOLIN_EXPORT
+ void SaveWindowOnRender(std::string filename_prefix);
+
+ PANGOLIN_EXPORT
+ void SaveFramebuffer(std::string prefix, const Viewport& v);
+
+ namespace process
+ {
+ /// Tell pangolin to process input to drive display.
+ /// You will need to call this manually if you haven't let
+ /// Pangolin register callbacks from your windowing system
+ PANGOLIN_EXPORT
+ void Keyboard( unsigned char key, int x, int y);
+
+ PANGOLIN_EXPORT
+ void KeyboardUp(unsigned char key, int x, int y);
+
+ PANGOLIN_EXPORT
+ void SpecialFunc(int key, int x, int y);
+
+ PANGOLIN_EXPORT
+ void SpecialFuncUp(int key, int x, int y);
+
+ /// Tell pangolin base window size has changed
+ /// You will need to call this manually if you haven't let
+ /// Pangolin register callbacks from your windowing system
+ PANGOLIN_EXPORT
+ void Resize(int width, int height);
+
+ /// Event based rendering entry point. Not currently supported.
+ PANGOLIN_EXPORT
+ void Display();
+
+ PANGOLIN_EXPORT
+ void Mouse( int button, int state, int x, int y);
+
+ PANGOLIN_EXPORT
+ void MouseMotion( int x, int y);
+
+ PANGOLIN_EXPORT
+ void PassiveMouseMotion(int x, int y);
+
+ PANGOLIN_EXPORT
+ void Scroll(float x, float y);
+
+ PANGOLIN_EXPORT
+ void Zoom(float m);
+
+ PANGOLIN_EXPORT
+ void Rotate(float r);
+
+ PANGOLIN_EXPORT
+ void SubpixMotion(float x, float y, float pressure, float rotation, float tiltx, float tilty);
+
+ PANGOLIN_EXPORT
+ void SpecialInput(InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4);
+
+ }
+
+ /// Retrieve 'base' display, corresponding to entire window.
+ PANGOLIN_EXPORT
+ View& DisplayBase();
+
+ /// Create or retrieve named display managed by pangolin (automatically deleted).
+ PANGOLIN_EXPORT
+ View& Display(const std::string& name);
+
+ /// Create unnamed display managed by pangolin (automatically deleted).
+ PANGOLIN_EXPORT
+ View& CreateDisplay();
+
+ /// Switch between windowed and fullscreen mode.
+ PANGOLIN_EXPORT
+ void ToggleFullscreen();
+
+ /// Switch windows/fullscreenmode = fullscreen.
+ PANGOLIN_EXPORT
+ void SetFullscreen(bool fullscreen = true);
+
+ /// Toggle display of Pangolin console
+ PANGOLIN_EXPORT
+ void ToggleConsole();
+
+ /// Convenience functor for toggling pangolin::View.
+ /// Use with RegisterKeyPressCallback for example
+ struct ToggleViewFunctor {
+ inline ToggleViewFunctor(View& view);
+ inline ToggleViewFunctor(const std::string& name);
+ void operator()();
+ View& view;
+ };
+
+}
+
diff --git a/externals/Pangolin/include/pangolin/display/display_internal.h b/externals/Pangolin/include/pangolin/display/display_internal.h
new file mode 100644
index 0000000000000000000000000000000000000000..a39dea56fa0924a7cfee7192a3f49d2658a16236
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/display_internal.h
@@ -0,0 +1,138 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/display/window.h>
+
+#include <pangolin/display/view.h>
+#include <pangolin/display/user_app.h>
+#include <functional>
+#include <memory>
+
+#include <map>
+#include <queue>
+
+#ifdef BUILD_PANGOLIN_VIDEO
+# include <pangolin/video/video_output.h>
+#endif // BUILD_PANGOLIN_VIDEO
+
+
+namespace pangolin
+{
+
+// Forward Declarations
+#ifdef HAVE_PYTHON
+class ConsoleView;
+#endif // HAVE_PYTHON
+class GlFont;
+
+typedef std::map<const std::string,View*> ViewMap;
+typedef std::map<int,std::function<void(void)> > KeyhookMap;
+
+struct PANGOLIN_EXPORT PangolinGl : public WindowInterface
+{
+ PangolinGl();
+ ~PangolinGl();
+
+ // Base container for displays
+ View base;
+
+ // Named views which are managed by pangolin (i.e. created / deleted by pangolin)
+ ViewMap named_managed_views;
+
+ // Optional user app
+ UserApp* user_app;
+
+ // Global keypress hooks
+ KeyhookMap keypress_hooks;
+
+ // Manage fullscreen (ToggleFullscreen is quite new)
+ bool is_double_buffered;
+ bool is_fullscreen;
+ GLint windowed_size[2];
+ bool is_high_res;
+
+ // State relating to interactivity
+ bool quit;
+ int had_input;
+ int has_resized;
+ int mouse_state;
+ View* activeDisplay;
+
+ std::queue<std::pair<std::string,Viewport> > screen_capture;
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ View* record_view;
+ VideoOutput recorder;
+#endif
+
+#ifdef HAVE_PYTHON
+ ConsoleView* console_view;
+#endif
+
+ std::shared_ptr<GlFont> font;
+
+ virtual void ToggleFullscreen() override {
+ pango_print_warn("ToggleFullscreen: Not available with non-pangolin window.\n");
+ }
+
+ virtual void ProcessEvents() override {
+ pango_print_warn("ProcessEvents: Not available with non-pangolin window.\n");
+ }
+
+ virtual void SwapBuffers() override {
+ pango_print_warn("SwapBuffers: Not available with non-pangolin window.\n");
+ }
+
+ virtual void MakeCurrent() override {
+ pango_print_warn("MakeCurrent: Not available with non-pangolin window.\n");
+ }
+
+ virtual void RemoveCurrent() override {
+ pango_print_warn("RemoveCurrent: Not available with non-pangolin window.\n");
+ }
+
+ virtual void Move(int /*x*/, int /*y*/) override {
+ pango_print_warn("Move: Not available with non-pangolin window.\n");
+ }
+
+ virtual void Resize(unsigned int /*w*/, unsigned int /*h*/) override {
+ pango_print_warn("Resize: Not available with non-pangolin window.\n");
+ }
+
+
+};
+
+PangolinGl* GetCurrentContext();
+void RegisterNewContext(const std::string& name, std::shared_ptr<PangolinGl> newcontext);
+void DeleteContext(const std::string& name);
+PangolinGl *FindContext(const std::string& name);
+
+}
+
diff --git a/externals/Pangolin/include/pangolin/display/image_view.h b/externals/Pangolin/include/pangolin/display/image_view.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce53cad200445a79ceed494204de5a564978a016
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/image_view.h
@@ -0,0 +1,74 @@
+#pragma once
+
+#include <pangolin/display/display.h>
+#include <pangolin/gl/glpixformat.h>
+#include <pangolin/gl/glformattraits.h>
+#include <pangolin/gl/glsl.h>
+#include <pangolin/handler/handler_image.h>
+#include <pangolin/image/image_utils.h>
+
+#include <mutex>
+
+namespace pangolin
+{
+
+class ImageView : public pangolin::View, public pangolin::ImageViewHandler
+{
+ public:
+ ImageView();
+
+ ~ImageView();
+
+ void Render() override;
+
+ void Mouse(View& view, pangolin::MouseButton button, int x, int y, bool pressed, int button_state) override;
+
+ void Keyboard(View& view, unsigned char key, int x, int y, bool pressed) override;
+
+ pangolin::GlTexture& Tex();
+
+ ImageView& SetImage(void* ptr, size_t w, size_t h, size_t pitch, pangolin::GlPixFormat img_fmt, bool delayed_upload = false);
+
+ ImageView& SetImage(const pangolin::Image<unsigned char>& img, const pangolin::GlPixFormat& glfmt, bool delayed_upload = false);
+
+ template<typename T> inline
+ ImageView& SetImage(const pangolin::Image<T>& img, bool delayed_upload = false)
+ {
+ return SetImage(img.template UnsafeReinterpret<unsigned char>(), GlPixFormat::FromType<T>(), delayed_upload);
+ }
+
+ ImageView& SetImage(const pangolin::TypedImage& img, bool delayed_upload = false);
+
+ ImageView& SetImage(const pangolin::GlTexture& texture);
+
+ void LoadPending();
+
+ ImageView& Clear();
+
+ std::pair<float, float>& GetOffsetScale();
+
+ bool MouseReleased() const;
+
+ bool MousePressed() const;
+
+ void SetRenderOverlay(const bool& val);
+
+// private:
+ // img_to_load contains image data that should be uploaded to the texture on
+ // the next render cycle. The data is owned by this object and should be
+ // freed after use.
+ pangolin::ManagedImage<unsigned char> img_to_load;
+ pangolin::GlPixFormat img_fmt_to_load;
+
+ std::pair<float, float> offset_scale;
+ pangolin::GlPixFormat fmt;
+ pangolin::GlTexture tex;
+ bool lastPressed;
+ bool mouseReleased;
+ bool mousePressed;
+ bool overlayRender;
+
+ std::mutex texlock;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/opengl_render_state.h b/externals/Pangolin/include/pangolin/display/opengl_render_state.h
new file mode 100644
index 0000000000000000000000000000000000000000..60de1b9e5ece9f39715c5c120a3423bf2ad85b2e
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/opengl_render_state.h
@@ -0,0 +1,446 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/utils/simple_math.h>
+
+#include <vector>
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#endif
+
+#ifdef HAVE_TOON
+#include <cstring>
+#include <TooN/TooN.h>
+#include <TooN/se3.h>
+#endif
+
+#ifdef HAVE_OCULUS
+#include <pangolin/compat/ovr.h>
+#endif
+
+namespace pangolin {
+
+#ifdef HAVE_GLES
+ typedef float GLprecision;
+#else
+ typedef double GLprecision;
+#endif
+
+/// Capture OpenGL matrix types in enum to typing.
+enum OpenGlStack {
+ GlModelViewStack = 0x1700, // GL_MODELVIEW
+ GlProjectionStack = 0x1701, // GL_PROJECTION
+ GlTextureStack = 0x1702 // GL_TEXTURE
+};
+
+enum AxisDirection
+{
+ AxisNone,
+ AxisNegX, AxisX,
+ AxisNegY, AxisY,
+ AxisNegZ, AxisZ
+};
+
+struct CameraSpec {
+ GLprecision forward[3];
+ GLprecision up[3];
+ GLprecision right[3];
+ GLprecision img_up[2];
+ GLprecision img_right[2];
+};
+
+const static CameraSpec CameraSpecOpenGl = {{0,0,-1},{0,1,0},{1,0,0},{0,1},{1,0}};
+
+const static CameraSpec CameraSpecYDownZForward = {{0,0,1},{0,-1,0},{1,0,0},{0,-1},{1,0}};
+
+/// Direction vector for each AxisDirection enum
+const static GLprecision AxisDirectionVector[7][3] = {
+ {0,0,0},
+ {-1,0,0}, {1,0,0},
+ {0,-1,0}, {0,1,0},
+ {0,0,-1}, {0,0,1}
+};
+
+/// Object representing OpenGl Matrix.
+struct PANGOLIN_EXPORT OpenGlMatrix {
+ static OpenGlMatrix Translate(GLprecision x, GLprecision y, GLprecision z);
+ static OpenGlMatrix Scale(GLprecision x, GLprecision y, GLprecision z);
+ static OpenGlMatrix RotateX(GLprecision theta_rad);
+ static OpenGlMatrix RotateY(GLprecision theta_rad);
+ static OpenGlMatrix RotateZ(GLprecision theta_rad);
+
+
+ template<typename P>
+ static OpenGlMatrix ColMajor4x4(const P* col_major_4x4);
+
+ OpenGlMatrix();
+
+#ifdef USE_EIGEN
+ template<typename P>
+ OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat);
+
+ template<typename P>
+ OpenGlMatrix(const Eigen::Transform<P,3,Eigen::Affine>& mat) : OpenGlMatrix(mat.matrix()) { }
+
+ template<typename P>
+ operator Eigen::Matrix<P,4,4>() const;
+
+ template<typename P>
+ operator Eigen::Transform<P,3,Eigen::Affine>() const;
+#endif // USE_EIGEN
+
+#ifdef HAVE_TOON
+ OpenGlMatrix(const TooN::SE3<>& T);
+ OpenGlMatrix(const TooN::Matrix<4,4>& M);
+ operator const TooN::SE3<>() const;
+ operator const TooN::Matrix<4,4>() const;
+#endif // HAVE_TOON
+
+#ifdef HAVE_OCULUS
+ OpenGlMatrix(const OVR::Matrix4f& M);
+ operator const OVR::Matrix4f() const;
+#endif // HAVE_OCULUS
+
+ // Load matrix on to OpenGl stack
+ void Load() const;
+
+ void Multiply() const;
+
+ void SetIdentity();
+
+ OpenGlMatrix Transpose() const;
+
+ OpenGlMatrix Inverse() const;
+
+ GLprecision& operator()(int r, int c) {
+ return m[4*c +r];
+ }
+
+ GLprecision operator()(int r, int c) const {
+ return m[4 * c + r];
+ }
+
+ // Column major Internal buffer
+ GLprecision m[16];
+};
+
+PANGOLIN_EXPORT
+OpenGlMatrix operator*(const OpenGlMatrix& lhs, const OpenGlMatrix& rhs);
+
+PANGOLIN_EXPORT
+std::ostream& operator<<(std::ostream& os, const OpenGlMatrix& mat);
+
+/// Deprecated.
+struct PANGOLIN_EXPORT OpenGlMatrixSpec : public OpenGlMatrix {
+ // Specify which stack this refers to
+ OpenGlStack type;
+};
+
+/// Object representing attached OpenGl Matrices / transforms.
+class PANGOLIN_EXPORT OpenGlRenderState
+{
+public:
+ OpenGlRenderState();
+ OpenGlRenderState(const OpenGlMatrix& projection_matrix);
+ OpenGlRenderState(const OpenGlMatrix& projection_matrix, const OpenGlMatrix& modelview_matrix);
+
+ static void ApplyIdentity();
+
+ void Apply() const;
+ OpenGlRenderState& SetProjectionMatrix(OpenGlMatrix m);
+ OpenGlRenderState& SetModelViewMatrix(OpenGlMatrix m);
+
+ OpenGlMatrix& GetProjectionMatrix();
+ OpenGlMatrix GetProjectionMatrix() const;
+
+ OpenGlMatrix& GetModelViewMatrix();
+ OpenGlMatrix GetModelViewMatrix() const;
+
+ OpenGlMatrix GetProjectionModelViewMatrix() const;
+ OpenGlMatrix GetProjectiveTextureMatrix() const;
+
+ void EnableProjectiveTexturing() const;
+ void DisableProjectiveTexturing() const;
+
+ //! Seemlessly move OpenGl camera relative to changes in T_wc,
+ //! whilst still enabling interaction
+ void Follow(const OpenGlMatrix& T_wc, bool follow = true);
+ void Unfollow();
+
+ // Experimental - subject to change
+ OpenGlMatrix& GetProjectionMatrix(unsigned int view);
+ OpenGlMatrix GetProjectionMatrix(unsigned int view) const;
+ OpenGlMatrix& GetViewOffset(unsigned int view);
+ OpenGlMatrix GetViewOffset(unsigned int view) const;
+ OpenGlMatrix GetModelViewMatrix(int i) const;
+ void ApplyNView(int view) const;
+
+ PANGOLIN_DEPRECATED
+ OpenGlRenderState& Set(OpenGlMatrixSpec spec);
+
+protected:
+ OpenGlMatrix modelview;
+ std::vector<OpenGlMatrix> projection;
+ std::vector<OpenGlMatrix> modelview_premult;
+ OpenGlMatrix T_cw;
+ bool follow;
+};
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRUB_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRUB_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRDF_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRDF_TopRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRDF_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixRDF_BottomRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+//! Use OpenGl's default frame RUB_BottomLeft
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrix(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar );
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec ProjectionMatrixOrthographic(GLprecision l, GLprecision r, GLprecision b, GLprecision t, GLprecision n, GLprecision f );
+
+
+//! Generate glulookat style model view matrix, looking at (lx,ly,lz)
+//! X-Right, Y-Up, Z-Back
+PANGOLIN_EXPORT
+OpenGlMatrix ModelViewLookAtRUB(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
+
+//! Generate glulookat style model view matrix, looking at (lx,ly,lz)
+//! X-Right, Y-Down, Z-Forward
+PANGOLIN_EXPORT
+OpenGlMatrix ModelViewLookAtRDF(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
+
+//! Generate glulookat style model view matrix, OpenGL Default camera convention (XYZ=RUB), looking at (lx,ly,lz)
+PANGOLIN_EXPORT
+OpenGlMatrix ModelViewLookAt(GLprecision x, GLprecision y, GLprecision z, GLprecision lx, GLprecision ly, GLprecision lz, AxisDirection up);
+
+PANGOLIN_EXPORT
+OpenGlMatrix ModelViewLookAt(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz);
+
+
+PANGOLIN_EXPORT
+OpenGlMatrix IdentityMatrix();
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec IdentityMatrix(OpenGlStack type);
+
+PANGOLIN_EXPORT
+OpenGlMatrixSpec negIdentityMatrix(OpenGlStack type);
+
+#ifdef HAVE_TOON
+OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw);
+OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M);
+TooN::Matrix<4,4> ToTooN(const OpenGlMatrixSpec& ms);
+TooN::SE3<> ToTooN_SE3(const OpenGlMatrixSpec& ms);
+#endif
+
+#ifdef HAVE_EIGEN
+template<typename P>
+Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms);
+#endif
+
+}
+
+// Inline definitions
+namespace pangolin
+{
+
+template<typename P>
+inline OpenGlMatrix OpenGlMatrix::ColMajor4x4(const P* col_major_4x4)
+{
+ OpenGlMatrix mat;
+ std::copy(col_major_4x4, col_major_4x4 + 16, mat.m);
+ return mat;
+}
+
+inline OpenGlMatrix::OpenGlMatrix() {
+}
+
+#ifdef USE_EIGEN
+template<typename P> inline
+OpenGlMatrix::OpenGlMatrix(const Eigen::Matrix<P,4,4>& mat)
+{
+ for(int r=0; r<4; ++r ) {
+ for(int c=0; c<4; ++c ) {
+ m[c*4+r] = mat(r,c);
+ }
+ }
+}
+
+template<typename P>
+OpenGlMatrix::operator Eigen::Matrix<P,4,4>() const
+{
+ return ToEigen<P>(*this);
+}
+
+template<typename P>
+OpenGlMatrix::operator Eigen::Transform<P,3,Eigen::Affine>() const
+{
+ return Eigen::Transform<P,3,Eigen::Affine>(ToEigen<P>(*this));
+}
+
+template<typename P> inline
+Eigen::Matrix<P,4,4> ToEigen(const OpenGlMatrix& ms)
+{
+ Eigen::Matrix<P,4,4> mat;
+ for(int r=0; r<4; ++r ) {
+ for(int c=0; c<4; ++c ) {
+ mat(r,c) = (P)ms.m[c*4+r];
+ }
+ }
+ return mat;
+}
+
+#endif // USE_EIGEN
+
+#ifdef HAVE_TOON
+inline OpenGlMatrix::OpenGlMatrix(const TooN::SE3<>& T)
+{
+ TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
+ M.slice<0,0,3,3>() = T.get_rotation().get_matrix();
+ M.T()[3].slice<0,3>() = T.get_translation();
+ M[3] = TooN::makeVector(0,0,0,1);
+ std::memcpy(m, &(M[0][0]),16*sizeof(GLprecision));
+}
+
+inline OpenGlMatrix::OpenGlMatrix(const TooN::Matrix<4,4>& M)
+{
+ // Read in remembering col-major convension for our matrices
+ int el = 0;
+ for(int c=0; c<4; ++c)
+ for(int r=0; r<4; ++r)
+ m[el++] = M[r][c];
+}
+
+inline OpenGlMatrix::operator const TooN::SE3<>() const
+{
+ const TooN::Matrix<4,4> m = *this;
+ const TooN::SO3<> R(m.slice<0,0,3,3>());
+ const TooN::Vector<3> t = m.T()[3].slice<0,3>();
+ return TooN::SE3<>(R,t);
+}
+
+inline OpenGlMatrix::operator const TooN::Matrix<4,4>() const
+{
+ TooN::Matrix<4,4> M;
+ int el = 0;
+ for( int c=0; c<4; ++c )
+ for( int r=0; r<4; ++r )
+ M(r,c) = m[el++];
+ return M;
+}
+
+PANGOLIN_DEPRECATED
+inline OpenGlMatrixSpec FromTooN(const TooN::SE3<>& T_cw)
+{
+ TooN::Matrix<4,4,GLprecision,TooN::ColMajor> M;
+ M.slice<0,0,3,3>() = T_cw.get_rotation().get_matrix();
+ M.T()[3].slice<0,3>() = T_cw.get_translation();
+ M[3] = TooN::makeVector(0,0,0,1);
+
+ OpenGlMatrixSpec P;
+ P.type = GlModelViewStack;
+ std::memcpy(P.m, &(M[0][0]),16*sizeof(GLprecision));
+ return P;
+}
+
+PANGOLIN_DEPRECATED
+inline OpenGlMatrixSpec FromTooN(OpenGlStack type, const TooN::Matrix<4,4>& M)
+{
+ // Read in remembering col-major convension for our matrices
+ OpenGlMatrixSpec P;
+ P.type = type;
+ int el = 0;
+ for(int c=0; c<4; ++c)
+ for(int r=0; r<4; ++r)
+ P.m[el++] = M[r][c];
+ return P;
+}
+
+PANGOLIN_DEPRECATED
+inline TooN::Matrix<4,4> ToTooN(const OpenGlMatrix& ms)
+{
+ TooN::Matrix<4,4> m;
+ int el = 0;
+ for( int c=0; c<4; ++c )
+ for( int r=0; r<4; ++r )
+ m(r,c) = ms.m[el++];
+ return m;
+}
+
+PANGOLIN_DEPRECATED
+inline TooN::SE3<> ToTooN_SE3(const OpenGlMatrix& ms)
+{
+ TooN::Matrix<4,4> m = ms;
+ const TooN::SO3<> R(m.slice<0,0,3,3>());
+ const TooN::Vector<3> t = m.T()[3].slice<0,3>();
+ return TooN::SE3<>(R,t);
+}
+
+#endif // HAVE_TOON
+
+#ifdef HAVE_OCULUS
+inline OpenGlMatrix::OpenGlMatrix(const OVR::Matrix4f& mat)
+{
+ for(int r=0; r<4; ++r )
+ for(int c=0; c<4; ++c )
+ m[c*4+r] = mat.M[r][c];
+}
+
+inline OpenGlMatrix::operator const OVR::Matrix4f() const
+{
+ OVR::Matrix4f mat;
+ for(int r=0; r<4; ++r )
+ for(int c=0; c<4; ++c )
+ mat.M[r][c] = m[c*4+r];
+ return mat;
+}
+#endif // HAVE_OCULUS
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/user_app.h b/externals/Pangolin/include/pangolin/display/user_app.h
new file mode 100644
index 0000000000000000000000000000000000000000..013ad26d23194283a002f582819d6877776ca37e
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/user_app.h
@@ -0,0 +1,43 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT UserApp
+{
+public:
+ virtual ~UserApp() {}
+ virtual void Init() {}
+ virtual void Render() = 0;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/view.h b/externals/Pangolin/include/pangolin/display/view.h
new file mode 100644
index 0000000000000000000000000000000000000000..7bc7a0d8c8c55efd4ad1c46b2204cbe2bb7133ef
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/view.h
@@ -0,0 +1,233 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <functional>
+#include <vector>
+
+#include <pangolin/display/viewport.h>
+#include <pangolin/display/attach.h>
+
+namespace pangolin
+{
+
+enum Layout
+{
+ LayoutOverlay,
+ LayoutVertical,
+ LayoutHorizontal,
+ LayoutEqual,
+ LayoutEqualVertical,
+ LayoutEqualHorizontal
+};
+
+enum Lock {
+ LockLeft = 0,
+ LockBottom = 0,
+ LockCenter = 1,
+ LockRight = 2,
+ LockTop = 2
+};
+
+// Forward declarations
+struct Handler;
+
+class OpenGlRenderState;
+
+/// A Display manages the location and resizing of an OpenGl viewport.
+struct PANGOLIN_EXPORT View
+{
+ View(double aspect=0.0)
+ : aspect(aspect), top(1.0),left(0.0),right(1.0),bottom(0.0), hlock(LockCenter),vlock(LockCenter),
+ layout(LayoutOverlay), scroll_offset(0), show(1), zorder(0), handler(0) {}
+
+ virtual ~View() {}
+
+ //! Activate Displays viewport for drawing within this area
+ void Activate() const;
+
+ //! Activate Displays and set State Matrices
+ void Activate(const OpenGlRenderState& state ) const;
+
+ //! Activate Displays viewport and Scissor for drawing within this area
+ void ActivateAndScissor() const;
+
+ //! Activate Displays viewport and Scissor for drawing within this area
+ void ActivateScissorAndClear() const;
+
+ //! Activate Display and set State Matrices
+ void ActivateAndScissor(const OpenGlRenderState& state ) const;
+
+ //! Activate Display and set State Matrices
+ void ActivateScissorAndClear(const OpenGlRenderState& state ) const;
+
+ //! Activate Display and setup coordinate system for 2d pixel View coordinates
+ void ActivatePixelOrthographic() const;
+
+ //! Activate Display and reset coordinate system to OpenGL default
+ void ActivateIdentity() const;
+
+ //! Return closest depth buffer value within radius of window (winx,winy)
+ GLfloat GetClosestDepth(int winx, int winy, int radius) const;
+
+ //! Obtain camera space coordinates of scene at pixel (winx, winy, winzdepth)
+ //! winzdepth can be obtained from GetClosestDepth
+ void GetCamCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const;
+
+ //! Obtain object space coordinates of scene at pixel (winx, winy, winzdepth)
+ //! winzdepth can be obtained from GetClosestDepth
+ void GetObjectCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const;
+
+ //! Given the specification of Display, compute viewport
+ virtual void Resize(const Viewport& parent);
+
+ //! Instruct all children to resize
+ virtual void ResizeChildren();
+
+ //! Perform any automatic rendering for this View.
+ //! Default implementation simply instructs children to render themselves.
+ virtual void Render();
+
+ //! Instruct all children to render themselves if appropriate
+ virtual void RenderChildren();
+
+ //! Set this view as the active View to receive input
+ View& SetFocus();
+
+ //! Returns true iff this view currently has focus and will receive user input
+ bool HasFocus() const;
+
+ //! Set bounds for the View using mixed fractional / pixel coordinates (OpenGl view coordinates)
+ View& SetBounds(Attach bottom, Attach top, Attach left, Attach right);
+
+ //! Set bounds for the View using mixed fractional / pixel coordinates (OpenGl view coordinates)
+ View& SetBounds(Attach bottom, Attach top, Attach left, Attach right, bool keep_aspect);
+
+ //! Set bounds for the View using mixed fractional / pixel coordinates (OpenGl view coordinates)
+ View& SetBounds(Attach bottom, Attach top, Attach left, Attach right, double aspect);
+
+ //! Designate handler for accepting mouse / keyboard input.
+ View& SetHandler(Handler* handler);
+
+ //! Set drawFunc as the drawing function for this view
+ View& SetDrawFunction(const std::function<void(View&)>& drawFunc);
+
+ //! Force this view to have the given aspect, whilst fitting snuggly
+ //! within the parent. A negative value with 'over-draw', fitting the
+ //! smaller side of the parent.
+ View& SetAspect(double aspect);
+
+ //! Set how this view should be positioned relative to its parent
+ View& SetLock(Lock horizontal, Lock vertical );
+
+ //! Set layout policy for this view
+ View& SetLayout(Layout layout);
+
+ //! Add view as child
+ View& AddDisplay(View& view);
+
+ //! Show / hide this view
+ View& Show(bool show=true);
+
+ //! Toggle this views visibility
+ void ToggleShow();
+
+ //! Return whether this view should be shown.
+ //! This method should be checked if drawing manually
+ bool IsShown() const;
+
+ //! Returns viewport reflecting space that will actually get drawn
+ //! The minimum of vp and v
+ Viewport GetBounds() const;
+
+ //! Specify that this views region in the framebuffer should be saved to
+ //! a file just before the buffer is flipped.
+ void SaveOnRender(const std::string& filename_prefix);
+
+ //! Specify that this views region in the framebuffer should be saved to
+ //! a video just before the buffer is flipped
+ void RecordOnRender(const std::string& record_uri);
+
+ //! Uses the views default render method to draw into an FBO 'scale' times
+ //! the size of the view and save to a file.
+ void SaveRenderNow(const std::string& filename_prefix, float scale = 1);
+
+ //! Return number of child views attached to this view
+ size_t NumChildren() const;
+
+ //! Return (i)th child of this view
+ View& operator[](size_t i);
+
+ //! Return number of visible child views attached to this view.
+ size_t NumVisibleChildren() const;
+
+ //! Return visible child by index.
+ View& VisibleChild(size_t i);
+
+ //! Return visible child at window coords x,y
+ View* FindChild(int x, int y);
+
+ // Desired width / height aspect (0 if dynamic)
+ double aspect;
+
+ // Bounds to fit display within
+ Attach top, left, right, bottom;
+ Lock hlock;
+ Lock vlock;
+ Layout layout;
+
+ int scroll_offset;
+
+ // Cached client area (space allocated from parent)
+ Viewport vp;
+
+ // Cached absolute viewport (recomputed on resize - respects aspect)
+ Viewport v;
+
+ // Should this view be displayed?
+ bool show;
+
+ // Child views are rendered in order of low to high z-order
+ // Views default to 0 z-order
+ int zorder;
+
+ // Input event handler (if any)
+ Handler* handler;
+
+ // Map for sub-displays (if any)
+ std::vector<View*> views;
+
+ // External draw function
+ std::function<void(View&)> extern_draw_function;
+
+private:
+ // Private copy constructor
+ View(View&) { /* Do Not copy - take reference instead*/ }
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/viewport.h b/externals/Pangolin/include/pangolin/display/viewport.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3d75f1a1d7e3b9f9f6ce2e49fca6196dc6cc724
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/viewport.h
@@ -0,0 +1,65 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/opengl_render_state.h>
+
+namespace pangolin
+{
+
+/// Encapsulates OpenGl Viewport.
+struct PANGOLIN_EXPORT Viewport
+{
+ Viewport() : l(0),b(0),w(0),h(0) {}
+ Viewport(GLint l,GLint b,GLint w,GLint h) : l(l),b(b),w(w),h(h) {}
+
+ void Activate() const;
+ void ActivateIdentity() const;
+ void ActivatePixelOrthographic() const;
+
+ void Scissor() const;
+ void ActivateAndScissor() const;
+
+ bool Contains(int x, int y) const;
+
+ Viewport Inset(int i) const;
+ Viewport Inset(int horiz, int vert) const;
+ Viewport Intersect(const Viewport& vp) const;
+
+ void GetCamCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const;
+
+ static void DisableScissor();
+
+ GLint r() const { return l+w;}
+ GLint t() const { return b+h;}
+ GLfloat aspect() const { return (GLfloat)w / (GLfloat)h; }
+ GLint l,b,w,h;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/widgets/widgets.h b/externals/Pangolin/include/pangolin/display/widgets/widgets.h
new file mode 100644
index 0000000000000000000000000000000000000000..1991ee807b2f4cd6417c31ce66bca35a889deff3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/widgets/widgets.h
@@ -0,0 +1,141 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/display/view.h>
+#include <pangolin/var/var.h>
+#include <pangolin/handler/handler.h>
+#include <pangolin/gl/glfont.h>
+
+#include <functional>
+
+namespace pangolin
+{
+
+PANGOLIN_EXPORT
+View& CreatePanel(const std::string& name);
+
+struct PANGOLIN_EXPORT Panel : public View
+{
+ Panel();
+ Panel(const std::string& auto_register_var_prefix);
+ void Render();
+ void ResizeChildren();
+ static void AddVariable(void* data, const std::string& name, VarValueGeneric& var, bool brand_new);
+};
+
+template<typename T>
+struct Widget : public View, Handler, Var<T>
+{
+ Widget(std::string title, VarValueGeneric& tv)
+ : Var<T>(tv), title(title)
+ {
+ handler = this;
+ }
+
+ std::string title;
+};
+
+struct PANGOLIN_EXPORT Button : public Widget<bool>
+{
+ Button(std::string title, VarValueGeneric& tv);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void Render();
+
+ //Cache params on resize
+ void ResizeChildren();
+ GlText gltext;
+ GLfloat raster[2];
+ bool down;
+};
+
+struct PANGOLIN_EXPORT FunctionButton : public Widget<std::function<void(void)> >
+{
+ FunctionButton(std::string title, VarValueGeneric& tv);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void Render();
+
+ //Cache params on resize
+ void ResizeChildren();
+ GlText gltext;
+ GLfloat raster[2];
+ bool down;
+};
+
+struct PANGOLIN_EXPORT Checkbox : public Widget<bool>
+{
+ Checkbox(std::string title, VarValueGeneric& tv);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void Render();
+
+ //Cache params on resize
+ void ResizeChildren();
+ GlText gltext;
+ GLfloat raster[2];
+ Viewport vcb;
+};
+
+struct PANGOLIN_EXPORT Slider : public Widget<double>
+{
+ Slider(std::string title, VarValueGeneric& tv);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void MouseMotion(View&, int x, int y, int mouse_state);
+ void Keyboard(View&, unsigned char key, int x, int y, bool pressed);
+ void Render();
+
+ //Cache params on resize
+ void ResizeChildren();
+ GlText gltext;
+ GLfloat raster[2];
+ bool lock_bounds;
+ bool logscale;
+ bool is_integral_type;
+};
+
+struct PANGOLIN_EXPORT TextInput : public Widget<std::string>
+{
+ TextInput(std::string title, VarValueGeneric& tv);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void MouseMotion(View&, int x, int y, int mouse_state);
+ void Keyboard(View&, unsigned char key, int x, int y, bool pressed);
+ void Render();
+
+ std::string edit;
+ GlText gledit;
+
+ //Cache params on resize
+ void ResizeChildren();
+ GlText gltext;
+ GLfloat raster[2];
+ bool can_edit;
+ bool do_edit;
+ int sel[2];
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/display/window.h b/externals/Pangolin/include/pangolin/display/window.h
new file mode 100644
index 0000000000000000000000000000000000000000..7e2dd598e26f4289198af1cde579059d8f2a228b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/display/window.h
@@ -0,0 +1,90 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2016 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+#include <exception>
+#include <pangolin/platform.h>
+#include <string>
+
+namespace pangolin
+{
+
+class GlContextInterface
+{
+public:
+ virtual ~GlContextInterface() {}
+};
+
+class WindowInterface
+{
+public:
+ virtual ~WindowInterface() {}
+
+ virtual void ToggleFullscreen() = 0;
+
+ virtual void Move(int x, int y) = 0;
+
+ virtual void Resize(unsigned int w, unsigned int h) = 0;
+
+ /**
+ * @brief MakeCurrent set the current context
+ * to be called in a thread before accessing OpenGL
+ */
+ virtual void MakeCurrent() = 0;
+
+ /**
+ * @brief RemoveCurrent remove the current context
+ * to be called at the end of a thread
+ */
+ virtual void RemoveCurrent() = 0;
+
+ virtual void ProcessEvents() = 0;
+
+ virtual void SwapBuffers() = 0;
+};
+
+
+struct PANGOLIN_EXPORT WindowException : std::exception
+{
+ WindowException(std::string str) : desc(str) {}
+ WindowException(std::string str, std::string detail) {
+ desc = str + "\n\t" + detail;
+ }
+ ~WindowException() throw() {}
+ const char* what() const throw() { return desc.c_str(); }
+ std::string desc;
+};
+
+struct PANGOLIN_EXPORT WindowExceptionNoKnownHandler : public WindowException
+{
+ WindowExceptionNoKnownHandler(const std::string& scheme)
+ : WindowException("No known window handler for URI '" + scheme + "'")
+ {
+ }
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/factory/factory_registry.h b/externals/Pangolin/include/pangolin/factory/factory_registry.h
new file mode 100644
index 0000000000000000000000000000000000000000..1ca8382dbf7adfb487c3a442cbb69d05b59f9365
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/factory/factory_registry.h
@@ -0,0 +1,114 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <memory>
+#include <vector>
+#include <algorithm>
+
+#include <pangolin/utils/uri.h>
+
+namespace pangolin
+{
+
+template<typename T>
+struct FactoryInterface
+{
+ typedef T FactoryItem;
+
+ virtual ~FactoryInterface() = default;
+ virtual std::unique_ptr<T> Open(const Uri& uri) = 0;
+};
+
+template<typename T>
+class FactoryRegistry
+{
+public:
+ // IMPORTANT: Implement for each templated instantiation within a seperate compilation unit.
+ static FactoryRegistry<T>& I();
+
+ ~FactoryRegistry()
+ {
+ }
+
+ void RegisterFactory(std::shared_ptr<FactoryInterface<T>> factory, uint32_t precedence, const std::string& scheme_name )
+ {
+ FactoryItem item = {precedence, scheme_name, factory};
+ factories.push_back( item );
+ std::sort(factories.begin(), factories.end());
+ }
+
+ void UnregisterFactory(FactoryInterface<T>* factory)
+ {
+ for( auto i = factories.end()-1; i != factories.begin(); --i)
+ {
+ if( i->factory.get() == factory ) {
+ factories.erase(i);
+ }
+ }
+ }
+
+ void UnregisterAllFactories()
+ {
+ factories.clear();
+ }
+
+ std::unique_ptr<T> Open(const Uri& uri)
+ {
+ // Iterate over all registered factories in order of precedence.
+ for(auto& item : factories) {
+ if( item.scheme == uri.scheme) {
+ std::unique_ptr<T> video = item.factory->Open(uri);
+ if(video) {
+ return video;
+ }
+ }
+ }
+
+ return std::unique_ptr<T>();
+ }
+
+private:
+ struct FactoryItem
+ {
+ uint32_t precedence;
+ std::string scheme;
+ std::shared_ptr<FactoryInterface<T>> factory;
+
+ bool operator<(const FactoryItem& rhs) const {
+ return precedence < rhs.precedence;
+ }
+ };
+
+ // Priority, Factory tuple
+ std::vector<FactoryItem> factories;
+};
+
+#define PANGOLIN_REGISTER_FACTORY(x) void Register ## x ## Factory()
+
+}
diff --git a/externals/Pangolin/include/pangolin/geometry/geometry.h b/externals/Pangolin/include/pangolin/geometry/geometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..4c6e78f80f48f2c5f1cdcbf8692488c78c17cd2b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/geometry/geometry.h
@@ -0,0 +1,96 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#ifndef PANGOLIN_GEOMETRY_H
+#define PANGOLIN_GEOMETRY_H
+
+#include <map>
+#include <unordered_map>
+#include <vector>
+#include <pangolin/image/typed_image.h>
+#include <pangolin/compat/variant.h>
+
+#ifdef HAVE_EIGEN
+#include <Eigen/Geometry>
+#endif
+
+namespace pangolin
+{
+
+struct Geometry
+{
+ struct Element : public ManagedImage<uint8_t> {
+ Element() = default;
+ Element(Element&&) = default;
+ Element& operator=(Element&&) = default;
+ Element& operator=(const Element&) = default;
+
+ Element(size_t stride_bytes, size_t num_elements)
+ : ManagedImage<uint8_t>(stride_bytes, num_elements)
+ {}
+
+ using Attribute = variant<Image<float>,Image<uint32_t>,Image<uint16_t>,Image<uint8_t>>;
+ // "vertex", "rgb", "normal", "uv", "tris", "quads", ...
+ std::map<std::string, Attribute> attributes;
+ };
+
+ // Store vertices and attributes
+ std::map<std::string, Element> buffers;
+ // Stores index buffers for each sub-object
+ std::multimap<std::string, Element> objects;
+ // Stores pixmaps
+ std::map<std::string, TypedImage> textures;
+};
+
+pangolin::Geometry::Element::Attribute MakeAttribute(uint32_t gldatatype, size_t num_items, size_t count_per_item, void* ptr, size_t pitch_bytes);
+
+pangolin::Geometry LoadGeometry(const std::string& filename);
+
+#ifdef HAVE_EIGEN
+inline Eigen::AlignedBox3f GetAxisAlignedBox(const Geometry& geom)
+{
+ Eigen::AlignedBox3f box;
+ box.setEmpty();
+
+ for(const auto& b : geom.buffers) {
+ const auto& it_vert = b.second.attributes.find("vertex");
+ if(it_vert != b.second.attributes.end()) {
+ const Image<float>& vs = get<Image<float>>(it_vert->second);
+ for(size_t i=0; i < vs.h; ++i) {
+ const Eigen::Map<const Eigen::Vector3f> v(vs.RowPtr(i));
+ box.extend(v);
+ }
+ }
+ }
+
+ return box;
+}
+#endif
+
+}
+
+#endif // PANGOLIN_GEOMETRY_H
diff --git a/externals/Pangolin/include/pangolin/geometry/geometry_obj.h b/externals/Pangolin/include/pangolin/geometry/geometry_obj.h
new file mode 100644
index 0000000000000000000000000000000000000000..9e2570bc1a2375425c35bb58e614019566b6d3f1
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/geometry/geometry_obj.h
@@ -0,0 +1,34 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/geometry/geometry.h>
+
+namespace pangolin {
+
+pangolin::Geometry LoadGeometryObj(const std::string& filename);
+
+}
diff --git a/externals/Pangolin/include/pangolin/geometry/geometry_ply.h b/externals/Pangolin/include/pangolin/geometry/geometry_ply.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c3f1c1ca58739d5d9bfae3acc7d7c5349e7c519
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/geometry/geometry_ply.h
@@ -0,0 +1,157 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/platform.h>
+#include <pangolin/compat/variant.h>
+#include <pangolin/geometry/geometry.h>
+
+#include <fstream>
+#include <vector>
+#include <algorithm>
+
+namespace pangolin
+{
+
+#define PLY_GROUP_LIST(m) m(PlyHeader) m(PlyFormat) m(PlyType)
+#define PLY_HEADER_LIST(m) m(ply) m(format) m(comment) m(property) m(element) m(end_header)
+#define PLY_FORMAT_LIST(m) m(ascii) m(binary_big_endian) m(binary_little_endian)
+#define PLY_TYPE_LIST(m) m(char) m(uchar) m(short) m(ushort) m(int) m(uint) m(float) m(double) m(list)
+
+// Define Enums / strings
+enum PlyHeader {
+#define FORMAT_ENUM(x) PlyHeader_##x,
+ PLY_HEADER_LIST(FORMAT_ENUM)
+ PlyHeaderSize
+#undef FORMAT_ENUM
+};
+
+enum PlyFormat {
+#define FORMAT_ENUM(x) PlyFormat_##x,
+ PLY_FORMAT_LIST(FORMAT_ENUM)
+ PlyFormatSize
+#undef FORMAT_ENUM
+};
+
+enum PlyType {
+#define FORMAT_ENUM(x) PlyType_##x,
+ PLY_TYPE_LIST(FORMAT_ENUM)
+ PlyTypeSize
+#undef FORMAT_ENUM
+};
+const size_t PlyTypeGl[] = {
+// GL_BYTE, GL_UNSIGNED_BYTE,
+ 0x1400, 0x1401,
+// GL_SHORT, GL_UNSIGNED_SHORT,
+ 0x1402, 0x1403,
+// GL_INT, GL_UNSIGNED_INT,
+ 0x1404, 0x1405,
+// GL_FLOAT, GL_DOUBLE,
+ 0x1406, 0x140A,
+// GL_NONE,
+ 0
+};
+
+#undef FORMAT_ENUM
+
+struct PlyPropertyDetails
+{
+ std::string name;
+
+ // Type of property (GLenum)
+ size_t type;
+
+ // Type of list index if a list, or 0 otherwise. (GLenum)
+ size_t list_index_type;
+
+ // Offset from element start
+ size_t offset_bytes;
+
+ // Number of items in the list. 1 if not a list. -1 if unknown.
+ int num_items;
+
+ bool isList() const {
+ return list_index_type > 0;
+ }
+};
+
+struct PlyElementDetails
+{
+ std::string name;
+ int num_items;
+ int stride_bytes;
+ std::vector<PlyPropertyDetails> properties;
+
+ inline std::vector<PlyPropertyDetails>::iterator FindProperty(const std::string& name)
+ {
+ return std::find_if(properties.begin(), properties.end(),
+ [&name](const PlyPropertyDetails& p){ return p.name == name;}
+ );
+ }
+};
+
+struct PlyHeaderDetails
+{
+ PlyFormat format;
+ std::string version;
+ std::vector<PlyElementDetails> elements;
+
+ inline std::vector<PlyElementDetails>::iterator FindElement(const std::string& name)
+ {
+ return std::find_if(elements.begin(), elements.end(),
+ [&name](const PlyElementDetails& el){ return el.name == name;}
+ );
+ }
+};
+
+void ParsePlyHeader(PlyHeaderDetails& ply, std::istream& is);
+
+struct PlyBuffer
+{
+ size_t index_size_bytes;
+ size_t element_item_size_bytes;
+ std::vector<unsigned char> data;
+};
+
+void ParsePlyAscii(pangolin::Geometry& /*geom*/, const PlyHeaderDetails& /*ply*/, std::istream& /*is*/);
+
+// Convert Seperate "x","y","z" attributes into a single "vertex" attribute
+void StandardizeXyzToVertex(pangolin::Geometry& geom);
+
+// The Artec scanner saves with these attributes, for example
+void StandardizeMultiTextureFaceToXyzuv(pangolin::Geometry& geom);
+
+void Standardize(pangolin::Geometry& geom);
+
+void ParsePlyLE(pangolin::Geometry& geom, PlyHeaderDetails& ply, std::istream& is);
+
+void ParsePlyBE(pangolin::Geometry& /*geom*/, const PlyHeaderDetails& /*ply*/, std::istream& /*is*/);
+
+void AttachAssociatedTexturesPly(pangolin::Geometry& geom, const std::string& filename);
+
+pangolin::Geometry LoadGeometryPly(const std::string& filename);
+
+}
diff --git a/externals/Pangolin/include/pangolin/geometry/glgeometry.h b/externals/Pangolin/include/pangolin/geometry/glgeometry.h
new file mode 100644
index 0000000000000000000000000000000000000000..a034b0ada2887c6908b7bf336d0f5b1f20cbddba
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/geometry/glgeometry.h
@@ -0,0 +1,87 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/geometry/geometry.h>
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glsl.h>
+
+namespace pangolin {
+
+struct GlGeometry
+{
+ GlGeometry() = default;
+ GlGeometry(GlGeometry&&) = default;
+ GlGeometry& operator=(GlGeometry&&) = default;
+
+ struct Element : public GlBufferData
+ {
+ Element() = default;
+ Element(Element&&) = default;
+ Element& operator=(Element&&) = default;
+
+ Element(GlBufferType buffer_type, size_t size_bytes, GLenum gluse, uint8_t* data)
+ : GlBufferData(buffer_type, size_bytes, gluse, data)
+ {}
+
+ inline bool HasAttribute(const std::string& name) const {
+ return attributes.find(name) != attributes.end();
+ }
+
+ struct Attribute {
+ // Stuff needed by glVertexAttribPointer
+ GLenum gltype;
+ size_t count_per_element;
+ size_t num_elements;
+ size_t offset;
+ size_t stride_bytes;
+ };
+ std::map<std::string, Attribute> attributes;
+ };
+
+ inline bool HasAttribute(const std::string& name) const
+ {
+ for(const auto& b : buffers) if(b.second.HasAttribute(name)) return true;
+ return false;
+ }
+
+ // Store vertices and attributes
+ std::map<std::string, Element> buffers;
+ // Stores index buffers for each sub-object
+ std::multimap<std::string, Element> objects;
+ // Stores pixmaps
+ std::map<std::string, GlTexture> textures;
+};
+
+GlGeometry::Element ToGlGeometry(const Geometry::Element& el, GlBufferType buffertype);
+
+GlGeometry ToGlGeometry(const Geometry& geom);
+
+void GlDraw(GlSlProgram& prog, const GlGeometry& geom, const GlTexture *matcap);
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/cg.h b/externals/Pangolin/include/pangolin/gl/cg.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd83901dc97247a016ec55a10decdb9fb2cddc23
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/cg.h
@@ -0,0 +1,283 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <sstream>
+#include <algorithm>
+
+// Cg includes
+#include <Cg/cg.h>
+#include <Cg/cgGL.h>
+
+#include "gl.h"
+
+#ifdef HAVE_TOON
+#include <TooN/TooN.h>
+#endif // HAVE_TOON
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Interface
+////////////////////////////////////////////////
+
+/// Lightweight object wrapper for NVidia Cg Shader program objects.
+class CgProgram
+{
+ friend class CgLoader;
+public:
+ void SetUniform(const std::string& name, GlTexture& tex);
+ void SetUniform(const std::string& name, float f);
+ void SetUniform(const std::string& name, float v0, float v1);
+ void SetUniform(const std::string& name, float v0, float v1, float v2, float v3);
+
+#ifdef HAVE_TOON
+ void SetUniform(const std::string& name, const TooN::Vector<2>& v );
+ void SetUniform(const std::string& name, const TooN::Vector<3>& v );
+
+ template <int R, int C>
+ void SetUniform(const std::string& name, const TooN::Matrix<R,C>& M );
+#endif
+
+ void UpdateParams();
+
+protected:
+ CGprogram mProg;
+ CGcontext mContext;
+ CGprofile mProfile;
+};
+
+class CgLoader
+{
+public:
+ CgLoader();
+ ~CgLoader();
+
+ // Call AFTER glewInit (or similar)
+ void Initialise();
+
+ CgProgram LoadProgramFromFile(const std::string& file, const std::string& function, bool isVertexShader );
+
+ void EnableProgram(CgProgram program);
+ void DisablePrograms();
+
+ void RenderDummyQuad();
+ void RenderDummyQuadWithTexCoords(int w, int h);
+
+protected:
+ CGcontext mContext;
+ CGprofile mFragmentProfile;
+ CGprofile mVertexProfile;
+};
+
+
+
+
+////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////
+
+inline bool cgOkay()
+{
+ CGerror error;
+ const char *string = cgGetLastErrorString(&error);
+
+ if (error != CG_NO_ERROR) {
+ std::cout << "CG Error: " << string << std::endl;
+ // assert(0);
+ return false;
+ }
+ return true;
+}
+
+inline CgLoader::CgLoader()
+ :mContext(0)
+{
+}
+
+inline CgLoader::~CgLoader()
+{
+ if(mContext)
+ {
+ // Destroying context destroys all programs associated with it
+ cgDestroyContext(mContext);
+ }
+}
+
+
+inline void CgLoader::Initialise()
+{
+ mContext = cgCreateContext();
+ cgSetParameterSettingMode(mContext, CG_DEFERRED_PARAMETER_SETTING);
+ cgOkay();
+
+ mFragmentProfile = cgGLGetLatestProfile(CG_GL_FRAGMENT);
+ cgGLSetOptimalOptions(mFragmentProfile);
+ cgOkay();
+
+ mVertexProfile = cgGLGetLatestProfile(CG_GL_VERTEX);
+ cgGLSetOptimalOptions(mVertexProfile);
+ cgOkay();
+}
+
+inline CgProgram CgLoader::LoadProgramFromFile(const std::string& file, const std::string& function, bool isVertexShader )
+{
+ if( !mContext ) {
+ Initialise();
+ }
+
+ CgProgram prog;
+
+ prog.mContext = mContext;
+ prog.mProfile = isVertexShader ? mVertexProfile : mFragmentProfile;
+ prog.mProg = cgCreateProgramFromFile( prog.mContext, CG_SOURCE, file.c_str(), prog.mProfile, function.c_str(), NULL);
+
+ if( !cgOkay() )
+ {
+ std::cout << cgGetLastListing(mContext) << std::endl;
+ assert(0);
+ }
+
+ cgGLLoadProgram(prog.mProg);
+ if( !cgOkay() )
+ {
+ const char* err = cgGetProgramString( prog.mProg, CG_COMPILED_PROGRAM );
+ int pos;
+ glGetIntegerv(GL_PROGRAM_ERROR_POSITION_ARB, &pos);
+ std::cout << err << std::endl;
+ std::cout << "@ " << pos << std::endl;
+ assert(0);
+ }
+ return prog;
+}
+
+inline void CgLoader::EnableProgram(CgProgram program)
+{
+ cgGLBindProgram(program.mProg);
+ cgGLEnableProfile(program.mProfile);
+ cgOkay();
+}
+
+inline void CgLoader::DisablePrograms()
+{
+ cgGLDisableProfile(mFragmentProfile);
+ cgGLDisableProfile(mVertexProfile);
+}
+
+inline void CgLoader::RenderDummyQuad()
+{
+ glBegin(GL_QUADS);
+ glVertex2d(-1,1);
+ glVertex2d(1,1);
+ glVertex2d(1,-1);
+ glVertex2d(-1,-1);
+ glEnd();
+}
+
+inline void CgLoader::RenderDummyQuadWithTexCoords(int w, int h)
+{
+ glBegin(GL_QUADS);
+ glTexCoord2f(0, 0);
+ glVertex2d(-1,-1);
+ glTexCoord2f(w, 0);
+ glVertex2d(1,-1);
+ glTexCoord2f(w, h);
+ glVertex2d(1,1);
+ glTexCoord2f(0, h);
+ glVertex2d(-1,1);
+ glEnd();
+}
+
+void CgProgram::SetUniform(const std::string& name, float f)
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgSetParameter1f( p, f );
+ cgUpdateProgramParameters(mProg);
+}
+
+void CgProgram::SetUniform(const std::string& name, GlTexture& tex)
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgGLSetTextureParameter(p, tex.tid );
+ cgGLEnableTextureParameter(p);
+ cgUpdateProgramParameters(mProg);
+}
+
+void CgProgram::SetUniform(const std::string& name, float v0, float v1, float v2, float v3)
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgGLSetParameter4f(p, v0,v1,v2,v3);
+ cgUpdateProgramParameters(mProg);
+}
+
+void CgProgram::SetUniform(const std::string& name, float v0, float v1)
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgGLSetParameter2f(p, v0,v1);
+ cgUpdateProgramParameters(mProg);
+}
+
+#ifdef HAVE_TOON
+void CgProgram::SetUniform(const std::string& name, const TooN::Vector<2>& v )
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgGLSetParameter2f(p, v[0],v[1] );
+ cgUpdateProgramParameters(mProg);
+}
+
+void CgProgram::SetUniform(const std::string& name, const TooN::Vector<3>& v )
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ cgGLSetParameter3f(p, v[0],v[1],v[2] );
+ cgUpdateProgramParameters(mProg);
+}
+
+template <int R, int C>
+void CgProgram::SetUniform(const std::string& name, const TooN::Matrix<R,C>& M )
+{
+ CGparameter p = cgGetNamedParameter( mProg, name.c_str());
+ float Mdata[R*C];
+
+ int i=0;
+ for( int r=0; r<R; ++r )
+ for( int c=0; c<C; ++c )
+ Mdata[i++] = (float)(M[r][c]);
+
+ cgGLSetMatrixParameterfr(p, Mdata );
+ cgUpdateProgramParameters(mProg);
+}
+#endif
+
+void CgProgram::UpdateParams()
+{
+ cgUpdateProgramParameters(mProg);
+}
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/colour.h b/externals/Pangolin/include/pangolin/gl/colour.h
new file mode 100644
index 0000000000000000000000000000000000000000..077c75b10d915108b37342ca7e42619ad6710ccd
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/colour.h
@@ -0,0 +1,173 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <cmath>
+
+#include <stdexcept>
+
+namespace pangolin
+{
+
+/// Represent OpenGL floating point colour: Red, Green and Blue with alpha.
+struct Colour
+{
+ inline static Colour White() {
+ return Colour(1.0f,1.0f,1.0f,1.0f);
+ }
+ inline static Colour Black() {
+ return Colour(0.0f,0.0f,0.0f,1.0f);
+ }
+ inline static Colour Red() {
+ return Colour(1.0f,0.0f,0.0f,1.0f);
+ }
+ inline static Colour Green() {
+ return Colour(0.0f,1.0f,0.0f,1.0f);
+ }
+ inline static Colour Blue() {
+ return Colour(0.0f,0.0f,1.0f,1.0f);
+ }
+ inline static Colour Unspecified() {
+ return Colour(
+ std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN(),
+ std::numeric_limits<float>::quiet_NaN(), std::numeric_limits<float>::quiet_NaN()
+ );
+ }
+
+ /// Default constructs white.
+ inline Colour()
+ : red(1.0f), green(1.0f), blue(1.0f), alpha(1.0f)
+ {
+ }
+
+ /// Construct from component values
+ inline Colour(const float red, const float green, const float blue, const float alpha = 1.0f)
+ : red(red), green(green), blue(blue), alpha(alpha)
+ {
+ }
+
+ /// Construct from rgba array.
+ inline Colour(const float rgba[4])
+ {
+ r = rgba[0];
+ g = rgba[1];
+ b = rgba[2];
+ a = rgba[3];
+ }
+
+ /// Return pointer to OpenGL compatible RGBA array.
+ inline float* Get()
+ {
+ return c;
+ }
+
+ /// Return this colour with alpha adjusted.
+ inline Colour WithAlpha(const float alpha)
+ {
+ return Colour(r,g,b,alpha);
+ }
+
+ /// Construct from HSV Colour
+ /// @param hue Colour hue in range [0,1]
+ /// @param sat Saturation in range [0,1]
+ /// @param val Value / Brightness in range [0,1].
+ static inline Colour Hsv(const float hue, const float sat = 1.0f, const float val = 1.0f, const float alpha = 1.0f)
+ {
+ const float h = 6.0f * hue;
+ const int i = (int)floor(h);
+ const float f = (i%2 == 0) ? 1-(h-i) : h-i;
+ const float m = val * (1-sat);
+ const float n = val * (1-sat*f);
+
+ switch(i)
+ {
+ case 0: return Colour(val,n,m,alpha);
+ case 1: return Colour(n,val,m,alpha);
+ case 2: return Colour(m,val,n,alpha);
+ case 3: return Colour(m,n,val,alpha);
+ case 4: return Colour(n,m,val,alpha);
+ case 5: return Colour(val,m,n,alpha);
+ default:
+ throw std::runtime_error("Found extra colour in rainbow.");
+ }
+ }
+
+ union {
+ struct {
+ float red;
+ float green;
+ float blue;
+ float alpha;
+ };
+ struct {
+ float r;
+ float g;
+ float b;
+ float a;
+ };
+ float c[4];
+ };
+
+};
+
+/// A ColourWheel is like a continuous colour palate that can be sampled.
+/// In the future, different ColourWheels will be supported, but this one
+/// is based on sampling hues in HSV colourspace. An indefinite number of
+/// unique colours are sampled using the golden angle.
+class ColourWheel
+{
+public:
+ /// Construct ColourWheel with Saturation, Value and Alpha constant.
+ inline ColourWheel(float saturation = 0.5f, float value = 1.0f, float alpha = 1.0f)
+ : unique_colours(0), sat(saturation), val(value), alpha(alpha)
+ {
+
+ }
+
+ /// Use Golden ratio (/angle) to pick well spaced colours.
+ inline Colour GetColourBin(int i) const
+ {
+ float hue = i * 0.5f * (3.0f - sqrt(5.0f));
+ hue -= (int)hue;
+ return Colour::Hsv(hue,sat,val,alpha);
+ }
+
+ /// Return next unique colour from ColourWheel.
+ inline Colour GetUniqueColour()
+ {
+ return GetColourBin(unique_colours++);
+ }
+
+protected:
+ int unique_colours;
+ float sat;
+ float val;
+ float alpha;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/compat/gl2engine.h b/externals/Pangolin/include/pangolin/gl/compat/gl2engine.h
new file mode 100644
index 0000000000000000000000000000000000000000..184aa60cd027e390dead2efdde327b6e5dae771f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/compat/gl2engine.h
@@ -0,0 +1,320 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <stack>
+
+#include <pangolin/opengl_render_state.h>
+#include <pangolin/glsl.h>
+
+namespace pangolin {
+
+class GlEngine
+{
+public:
+ const char* vert =
+ "attribute vec4 a_position;\n"
+ "attribute vec4 a_color;\n"
+ "attribute vec3 a_normal;\n"
+ "attribute vec2 a_texcoord;\n"
+ "uniform vec4 u_color;\n"
+ "uniform mat4 u_modelViewMatrix;\n"
+ "uniform mat4 u_modelViewProjectionMatrix;\n"
+ "varying vec4 v_frontColor;\n"
+ "varying vec2 v_texcoord;\n"
+ "void main() {\n"
+ " gl_Position = u_modelViewProjectionMatrix * a_position;\n"
+ " v_frontColor = u_color;\n"
+ " v_texcoord = a_texcoord;\n"
+ "}\n";
+
+ const char* frag =
+ #ifdef HAVE_GLES_2
+ "precision mediump float;\n"
+ #endif // HAVE_GLES_2
+ "varying vec4 v_frontColor;\n"
+ "varying vec2 v_texcoord;\n"
+ "uniform sampler2D u_texture;\n"
+ "uniform bool u_textureEnable;\n"
+ "void main() {\n"
+ " gl_FragColor = v_frontColor;\n"
+ " if(u_textureEnable) {\n"
+ " gl_FragColor *= texture2D(u_texture, v_texcoord);\n"
+ " }\n"
+ "}\n";
+
+ GlEngine()
+ {
+ // Initialise default state
+ projection.push(IdentityMatrix());
+ modelview.push(IdentityMatrix());
+ currentmatrix = &modelview;
+
+ // Set GL_TEXTURE0 as default active texture
+ glActiveTexture(GL_TEXTURE0);
+
+ // Compile and link shaders
+ prog_fixed.AddShader(GlSlVertexShader, vert);
+ prog_fixed.AddShader(GlSlFragmentShader, frag);
+ prog_fixed.BindPangolinDefaultAttribLocationsAndLink();
+
+ // Save locations of uniforms
+ u_color = prog_fixed.GetUniformHandle("u_color");
+ u_modelViewMatrix = prog_fixed.GetUniformHandle("u_modelViewMatrix");
+ u_modelViewProjectionMatrix = prog_fixed.GetUniformHandle("u_modelViewProjectionMatrix");
+ u_texture = prog_fixed.GetUniformHandle("u_texture");
+ u_textureEnable = prog_fixed.GetUniformHandle("u_textureEnable");
+
+ // Initialise default uniform values
+ UpdateMatrices();
+ SetColor(1.0,1.0,1.0,1.0);
+ }
+
+ void UpdateMatrices()
+ {
+ OpenGlMatrix pmv = projection.top() * modelview.top();
+ prog_fixed.SaveBind();
+ glUniformMatrix4fv( u_modelViewMatrix, 1, false, modelview.top().m );
+ glUniformMatrix4fv( u_modelViewProjectionMatrix, 1, false, pmv.m );
+ prog_fixed.Unbind();
+ }
+
+ void SetColor(float r, float g, float b, float a)
+ {
+ prog_fixed.SaveBind();
+ glUniform4f( u_color, r, g, b, a);
+ prog_fixed.Unbind();
+ }
+
+ void EnableTexturing(GLboolean v)
+ {
+ prog_fixed.SaveBind();
+ glUniform1i( u_textureEnable, v);
+ prog_fixed.Unbind();
+ }
+
+//protected:
+ std::stack<OpenGlMatrix> projection;
+ std::stack<OpenGlMatrix> modelview;
+ std::stack<OpenGlMatrix>* currentmatrix;
+
+ GLenum matrixmode;
+
+ float color[4];
+
+ GlSlProgram prog_fixed;
+
+ GLint u_color;
+ GLint u_modelViewMatrix;
+ GLint u_modelViewProjectionMatrix;
+ GLint u_texture;
+ GLint u_textureEnable;
+};
+
+GlEngine& glEngine();
+
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// OpenGL 1.0 compatibility - Emulate fixed pipeline
+///////////////////////////////////////////////////////////////////////////////
+
+// Missing defines that we'll be using
+#define GL_MODELVIEW 0x1700
+#define GL_PROJECTION 0x1701
+#define GL_SHADE_MODEL 0x0B54
+#define GL_POINT_SIZE 0x0B11
+
+#define GL_MULTISAMPLE 0x809D
+
+#define GL_LIGHTING 0x0B50
+#define GL_POINT_SMOOTH 0x0B10
+#define GL_LINE_SMOOTH 0x0B20
+#define GL_SCISSOR_TEST 0x0C11
+#define GL_COLOR_MATERIAL 0x0B57
+
+#define GL_FLAT 0x1D00
+#define GL_SMOOTH 0x1D01
+
+#define GL_MODULATE 0x2100
+#define GL_DECAL 0x2101
+#define GL_ADD 0x0104
+#define GL_TEXTURE_ENV_MODE 0x2200
+#define GL_TEXTURE_ENV_COLOR 0x2201
+#define GL_TEXTURE_ENV 0x2300
+
+#define GL_PERSPECTIVE_CORRECTION_HINT 0x0C50
+#define GL_POINT_SMOOTH_HINT 0x0C51
+#define GL_LINE_SMOOTH_HINT 0x0C52
+
+#define GL_VERTEX_ARRAY 0x8074
+#define GL_NORMAL_ARRAY 0x8075
+#define GL_COLOR_ARRAY 0x8076
+#define GL_TEXTURE_COORD_ARRAY 0x8078
+
+inline void glEnableClientState(GLenum cap)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ if(cap == GL_VERTEX_ARRAY) {
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_POSITION);
+ }else if(cap == GL_COLOR_ARRAY) {
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_COLOUR);
+ }else if(cap == GL_NORMAL_ARRAY) {
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_NORMAL);
+ }else if(cap == GL_TEXTURE_COORD_ARRAY) {
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_TEXCOORD);
+ gl.EnableTexturing(true);
+ }else{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+ }
+}
+
+inline void glDisableClientState(GLenum cap)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ if(cap == GL_VERTEX_ARRAY) {
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_POSITION);
+ }else if(cap == GL_COLOR_ARRAY) {
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_COLOUR);
+ }else if(cap == GL_NORMAL_ARRAY) {
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_NORMAL);
+ }else if(cap == GL_TEXTURE_COORD_ARRAY) {
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_TEXCOORD);
+ gl.EnableTexturing(false);
+ }else{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+ }
+}
+
+inline void glVertexPointer( GLint size, GLenum type, GLsizei stride, const GLvoid * pointer)
+{
+ glVertexAttribPointer(pangolin::DEFAULT_LOCATION_POSITION, size, type, GL_FALSE, stride, pointer);
+}
+
+inline void glTexCoordPointer( GLint size, GLenum type, GLsizei stride, const GLvoid * pointer)
+{
+ glVertexAttribPointer(pangolin::DEFAULT_LOCATION_TEXCOORD, size, type, GL_FALSE, stride, pointer);
+}
+
+inline void glMatrixMode(GLenum mode)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ gl.currentmatrix = (mode == pangolin::GlProjectionStack) ? &gl.projection : &gl.modelview;
+}
+
+inline void glLoadIdentity()
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ gl.currentmatrix->top() = pangolin::IdentityMatrix();
+ gl.UpdateMatrices();
+}
+
+inline void glLoadMatrixf(const GLfloat* m)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ pangolin::GLprecision* cm = gl.currentmatrix->top().m;
+ for(int i=0; i<16; ++i) cm[i] = (pangolin::GLprecision)m[i];
+ gl.UpdateMatrices();
+}
+
+inline void glLoadMatrixd(const GLdouble* m)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ pangolin::GLprecision* cm = gl.currentmatrix->top().m;
+ for(int i=0; i<16; ++i) cm[i] = (pangolin::GLprecision)m[i];
+ gl.UpdateMatrices();
+}
+
+inline void glMultMatrixf(const GLfloat* m)
+{
+// pangolin::GlEngine& gl = pangolin::glEngine();
+// float res[16];
+// pangolin::MatMul<4,4,4,float>(res, m, gl.currentmatrix->m );
+// std::memcpy(gl.currentmatrix->m, res, sizeof(float) * 16 );
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+}
+
+inline void glMultMatrixd(const GLdouble* m)
+{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+}
+
+inline void glPushMatrix(void)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ gl.currentmatrix->push(gl.currentmatrix->top());
+}
+
+inline void glPopMatrix(void)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ gl.currentmatrix->pop();
+ gl.UpdateMatrices();
+}
+
+inline void glTranslatef(GLfloat x, GLfloat y, GLfloat z )
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ pangolin::GLprecision* cm = gl.currentmatrix->top().m;
+ cm[12] += x;
+ cm[13] += y;
+ cm[14] += z;
+ gl.UpdateMatrices();
+}
+
+inline void glOrtho(
+ GLdouble l, GLdouble r,
+ GLdouble b, GLdouble t,
+ GLdouble n, GLdouble f)
+{
+ pangolin::GlEngine& gl = pangolin::glEngine();
+ gl.currentmatrix->top() = pangolin::ProjectionMatrixOrthographic(l,r,b,t,n,f);
+ gl.UpdateMatrices();
+}
+
+inline void glColor4f( GLfloat red, GLfloat green, GLfloat blue, GLfloat alpha)
+{
+ pangolin::glEngine().SetColor(red,green,blue,alpha);
+}
+
+inline void glShadeModel( GLenum mode)
+{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+}
+
+inline void glPointSize(GLfloat size)
+{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+}
+
+inline void glTexEnvf( GLenum target,
+ GLenum pname,
+ GLfloat param)
+{
+ pango_print_error("Not Implemented: %s, %s, %d", __FUNCTION__, __FILE__, __LINE__);
+}
diff --git a/externals/Pangolin/include/pangolin/gl/compat/gl_es_compat.h b/externals/Pangolin/include/pangolin/gl/compat/gl_es_compat.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c1fdb3bbd670dc10ee9bb98d1aadb9957fb2dcf
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/compat/gl_es_compat.h
@@ -0,0 +1,60 @@
+#pragma once
+
+#include <pangolin/platform.h>
+
+#define GLdouble GLfloat
+#define glClearDepth glClearDepthf
+#define glFrustum glFrustumf
+
+#define glColor4fv(a) glColor4f(a[0], a[1], a[2], a[3])
+#define glColor3fv(a) glColor4f(a[0], a[1], a[2], 1.0f)
+#define glColor3f(a,b,c) glColor4f(a, b, c, 1.0f)
+
+#define GL_CLAMP GL_CLAMP_TO_EDGE
+
+#ifdef HAVE_GLES_2
+ #define glGenFramebuffersEXT glGenFramebuffers
+ #define glDeleteFramebuffersEXT glDeleteFramebuffers
+ #define glBindFramebufferEXT glBindFramebuffer
+ #define glDrawBuffers glDrawBuffers
+ #define glFramebufferTexture2DEXT glFramebufferTexture2D
+ #define GL_FRAMEBUFFER_EXT GL_FRAMEBUFFER
+ #define GL_DEPTH_COMPONENT24 GL_DEPTH_COMPONENT16 // <----
+ #define GL_COLOR_ATTACHMENT0_EXT GL_COLOR_ATTACHMENT0
+ #define GL_DEPTH_ATTACHMENT_EXT GL_DEPTH_ATTACHMENT
+#else
+ #define glOrtho glOrthof
+ #define glGenFramebuffersEXT glGenFramebuffersOES
+ #define glDeleteFramebuffersEXT glDeleteFramebuffersOES
+ #define glBindFramebufferEXT glBindFramebufferOES
+ #define glDrawBuffers glDrawBuffersOES
+ #define glFramebufferTexture2DEXT glFramebufferTexture2DOES
+ #define GL_FRAMEBUFFER_EXT GL_FRAMEBUFFER_OES
+ #define GL_DEPTH_COMPONENT24 GL_DEPTH_COMPONENT24_OES
+ #define GL_COLOR_ATTACHMENT0_EXT GL_COLOR_ATTACHMENT0_OES
+#endif
+
+#define glGetDoublev glGetFloatv
+
+#ifdef HAVE_GLES_2
+#include <pangolin/gl2engine.h>
+#endif
+
+inline void glRectf(GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2)
+{
+ GLfloat verts[] = { x1,y1, x2,y1, x2,y2, x1,y2 };
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, verts);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisableClientState(GL_VERTEX_ARRAY);
+}
+
+inline void glRecti(int x1, int y1, int x2, int y2)
+{
+ GLfloat verts[] = { (float)x1,(float)y1, (float)x2,(float)y1,
+ (float)x2,(float)y2, (float)x1,(float)y2 };
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, verts);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisableClientState(GL_VERTEX_ARRAY);
+}
diff --git a/externals/Pangolin/include/pangolin/gl/gl.h b/externals/Pangolin/include/pangolin/gl/gl.h
new file mode 100644
index 0000000000000000000000000000000000000000..4330e73c6725869ac8b36546215598b85679c76e
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/gl.h
@@ -0,0 +1,273 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/display/viewport.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/image/image_io.h>
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#endif
+
+#include <cstdlib>
+#include <iostream>
+#include <math.h>
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Interface
+////////////////////////////////////////////////
+
+class PANGOLIN_EXPORT GlTexture
+{
+public:
+ //! internal_format normally one of GL_RGBA8, GL_LUMINANCE8, GL_INTENSITY16
+ GlTexture(GLint width, GLint height, GLint internal_format = GL_RGBA8, bool sampling_linear = true, int border = 0, GLenum glformat = GL_RGBA, GLenum gltype = GL_UNSIGNED_BYTE, GLvoid* data = NULL );
+
+ // Construct this texture from a CPU image
+ GlTexture(const TypedImage& img, bool sampling_linear=true);
+
+ //! Move Constructor / asignment
+ GlTexture(GlTexture&& tex);
+ GlTexture& operator=(GlTexture&& tex);
+
+ //! Default constructor represents 'no texture'
+ GlTexture();
+ virtual ~GlTexture();
+
+ bool IsValid() const;
+
+ //! Delete OpenGL resources and fall back to representing 'no texture'
+ void Delete();
+
+ //! Reinitialise teture width / height / format
+ virtual void Reinitialise(GLsizei width, GLsizei height, GLint internal_format = GL_RGBA8, bool sampling_linear = true, int border = 0, GLenum glformat = GL_RGBA, GLenum gltype = GL_UNSIGNED_BYTE, GLvoid* data = NULL );
+
+ void Bind() const;
+ void Unbind() const;
+
+ //! data_layout normally one of GL_LUMINANCE, GL_RGB, ...
+ //! data_type normally one of GL_UNSIGNED_BYTE, GL_UNSIGNED_SHORT, GL_FLOAT
+ void Upload(const void* image, GLenum data_format = GL_LUMINANCE, GLenum data_type = GL_FLOAT);
+
+ //! Upload data to texture, overwriting a sub-region of it.
+ //! data ptr contains packed data_w x data_h of pixel data.
+ void Upload(const void* data,
+ GLsizei tex_x_offset, GLsizei tex_y_offset,
+ GLsizei data_w, GLsizei data_h,
+ GLenum data_format, GLenum data_type
+ );
+
+ void Load(const TypedImage& image, bool sampling_linear = true);
+
+ void LoadFromFile(const std::string& filename, bool sampling_linear = true);
+
+ void Download(void* image, GLenum data_layout = GL_LUMINANCE, GLenum data_type = GL_FLOAT) const;
+
+ void Download(TypedImage& image) const;
+
+ void CopyFrom(const GlTexture& tex);
+
+ void Save(const std::string& filename, bool top_line_first = true);
+
+ void SetLinear();
+ void SetNearestNeighbour();
+
+ void RenderToViewport(const bool flip) const;
+ void RenderToViewport() const;
+ void RenderToViewport(Viewport tex_vp, bool flipx=false, bool flipy=false) const;
+ void RenderToViewportFlipY() const;
+ void RenderToViewportFlipXFlipY() const;
+
+ GLint internal_format;
+ GLuint tid;
+ GLint width;
+ GLint height;
+
+private:
+ // Private copy constructor
+ GlTexture(const GlTexture&) {}
+};
+
+struct PANGOLIN_EXPORT GlRenderBuffer
+{
+ GlRenderBuffer();
+ GlRenderBuffer(GLint width, GLint height, GLint internal_format = GL_DEPTH_COMPONENT24);
+
+ void Reinitialise(GLint width, GLint height, GLint internal_format = GL_DEPTH_COMPONENT24);
+
+ //! Move Constructor
+ GlRenderBuffer(GlRenderBuffer&& tex);
+
+ ~GlRenderBuffer();
+
+ GLint width;
+ GLint height;
+ GLuint rbid;
+
+private:
+ // Private copy constructor
+ GlRenderBuffer(const GlRenderBuffer&) {}
+};
+
+struct PANGOLIN_EXPORT GlFramebuffer
+{
+ GlFramebuffer();
+ ~GlFramebuffer();
+
+ GlFramebuffer(GlTexture& colour, GlRenderBuffer& depth);
+ GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlRenderBuffer& depth);
+ GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlTexture& colour2, GlRenderBuffer& depth);
+ GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlTexture& colour2, GlTexture& colour3, GlRenderBuffer& depth);
+
+ void Bind() const;
+ void Unbind() const;
+
+ void Reinitialise();
+
+ // Attach Colour texture to frame buffer
+ // Return attachment texture is bound to (e.g. GL_COLOR_ATTACHMENT0_EXT)
+ GLenum AttachColour(GlTexture& tex);
+
+ // Attach Depth render buffer to frame buffer
+ void AttachDepth(GlRenderBuffer& rb);
+
+ GLuint fbid;
+ unsigned attachments;
+};
+
+enum GlBufferType
+{
+ GlUndefined = 0,
+ GlArrayBuffer = GL_ARRAY_BUFFER, // VBO's, CBO's, NBO's
+ GlElementArrayBuffer = GL_ELEMENT_ARRAY_BUFFER, // IBO's
+#ifndef HAVE_GLES
+ GlPixelPackBuffer = GL_PIXEL_PACK_BUFFER, // PBO's
+ GlPixelUnpackBuffer = GL_PIXEL_UNPACK_BUFFER,
+ GlShaderStorageBuffer = GL_SHADER_STORAGE_BUFFER
+#endif
+};
+
+// This encapsulates a GL Buffer object.
+struct PANGOLIN_EXPORT GlBufferData
+{
+ //! Default constructor represents 'no buffer'
+ GlBufferData();
+ GlBufferData(GlBufferType buffer_type, GLuint size_bytes, GLenum gluse = GL_DYNAMIC_DRAW, const unsigned char* data = 0 );
+ virtual ~GlBufferData();
+ void Free();
+
+ //! Move Constructor
+ GlBufferData(GlBufferData&& tex);
+ GlBufferData& operator=(GlBufferData&& tex);
+
+ bool IsValid() const;
+
+ size_t SizeBytes() const;
+
+ void Reinitialise(GlBufferType buffer_type, GLuint size_bytes, GLenum gluse = GL_DYNAMIC_DRAW, const unsigned char* data = 0 );
+
+ void Bind() const;
+ void Unbind() const;
+ void Upload(const GLvoid* data, GLsizeiptr size_bytes, GLintptr offset = 0);
+ void Download(GLvoid* ptr, GLsizeiptr size_bytes, GLintptr offset = 0) const;
+
+ GLuint bo;
+ GlBufferType buffer_type;
+ GLenum gluse;
+ GLuint size_bytes;
+
+private:
+ GlBufferData(const GlBufferData&) {}
+};
+
+// This encapsulates a GL Buffer object, also storing information about its contents.
+// You should try to use GlBufferData instead.
+struct PANGOLIN_EXPORT GlBuffer : public GlBufferData
+{
+ //! Default constructor represents 'no buffer'
+ GlBuffer();
+ GlBuffer(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse = GL_DYNAMIC_DRAW );
+ GlBuffer(const GlBuffer&) = delete;
+
+ //! Move Constructor
+ GlBuffer(GlBuffer&& tex);
+ GlBuffer& operator=(GlBuffer&& tex);
+
+ void Reinitialise(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse, const unsigned char* data = nullptr );
+ void Reinitialise(GlBuffer const& other );
+ void Resize(GLuint num_elements);
+
+ GLenum datatype;
+ GLuint num_elements;
+ GLuint count_per_element;
+};
+
+class PANGOLIN_EXPORT GlSizeableBuffer
+ : public pangolin::GlBuffer
+{
+public:
+ GlSizeableBuffer(pangolin::GlBufferType buffer_type, GLuint initial_num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse = GL_DYNAMIC_DRAW );
+
+ void Clear();
+
+#ifdef USE_EIGEN
+ template<typename Derived>
+ void Add(const Eigen::DenseBase<Derived>& vec);
+
+ template<typename Derived>
+ void Update(const Eigen::DenseBase<Derived>& vec, size_t position = 0);
+#endif
+
+ size_t start() const;
+
+ size_t size() const;
+
+protected:
+ void CheckResize(size_t num_verts);
+
+ size_t NextSize(size_t min_size) const;
+
+ size_t m_num_verts;
+};
+
+size_t GlFormatChannels(GLenum data_layout);
+
+size_t GlDataTypeBytes(GLenum type);
+
+}
+
+// Include implementation
+#include <pangolin/gl/gl.hpp>
diff --git a/externals/Pangolin/include/pangolin/gl/gl.hpp b/externals/Pangolin/include/pangolin/gl/gl.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..914d7ee2a6f7cfe8d463eb2ea99b204d97470f02
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/gl.hpp
@@ -0,0 +1,864 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glpixformat.h>
+#include <pangolin/display/display.h>
+#include <pangolin/image/image_io.h>
+#include <algorithm>
+#include <stdexcept>
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Implementation of gl.h
+////////////////////////////////////////////////
+
+#ifndef HAVE_GLES
+const int MAX_ATTACHMENTS = 8;
+const static GLuint attachment_buffers[] = {
+ GL_COLOR_ATTACHMENT0_EXT,
+ GL_COLOR_ATTACHMENT1_EXT,
+ GL_COLOR_ATTACHMENT2_EXT,
+ GL_COLOR_ATTACHMENT3_EXT,
+ GL_COLOR_ATTACHMENT4_EXT,
+ GL_COLOR_ATTACHMENT5_EXT,
+ GL_COLOR_ATTACHMENT6_EXT,
+ GL_COLOR_ATTACHMENT7_EXT
+};
+#else // HAVE_GLES
+const int MAX_ATTACHMENTS = 1;
+const static GLuint attachment_buffers[] = {
+ GL_COLOR_ATTACHMENT0_EXT
+};
+#endif // HAVE_GLES
+
+const static size_t datatype_bytes[] = {
+ 1, // #define GL_BYTE 0x1400
+ 1, // #define GL_UNSIGNED_BYTE 0x1401
+ 2, // #define GL_SHORT 0x1402
+ 2, // #define GL_UNSIGNED_SHORT 0x1403
+ 4, // #define GL_INT 0x1404
+ 4, // #define GL_UNSIGNED_INT 0x1405
+ 4, // #define GL_FLOAT 0x1406
+ 2, // #define GL_2_BYTES 0x1407
+ 3, // #define GL_3_BYTES 0x1408
+ 4, // #define GL_4_BYTES 0x1409
+ 8 // #define GL_DOUBLE 0x140A
+};
+
+const static size_t format_channels[] = {
+ 1, // #define GL_RED 0x1903
+ 1, // #define GL_GREEN 0x1904
+ 1, // #define GL_BLUE 0x1905
+ 1, // #define GL_ALPHA 0x1906
+ 3, // #define GL_RGB 0x1907
+ 4, // #define GL_RGBA 0x1908
+ 1, // #define GL_LUMINANCE 0x1909
+ 2 // #define GL_LUMINANCE_ALPHA 0x190A
+};
+
+inline size_t GlDataTypeBytes(GLenum type)
+{
+ return datatype_bytes[type - GL_BYTE];
+}
+
+inline size_t GlFormatChannels(GLenum data_layout)
+{
+ return format_channels[data_layout - GL_RED];
+}
+
+//template<typename T>
+//struct GlDataTypeTrait {};
+//template<> struct GlDataTypeTrait<float>{ static const GLenum type = GL_FLOAT; };
+//template<> struct GlDataTypeTrait<int>{ static const GLenum type = GL_INT; };
+//template<> struct GlDataTypeTrait<unsigned char>{ static const GLenum type = GL_UNSIGNED_BYTE; };
+
+inline GlTexture::GlTexture()
+ : internal_format(0), tid(0), width(0), height(0)
+{
+ // Not a texture constructor
+}
+
+inline GlTexture::GlTexture(GLint width, GLint height, GLint internal_format, bool sampling_linear, int border, GLenum glformat, GLenum gltype, GLvoid* data )
+ : internal_format(0), tid(0)
+{
+ Reinitialise(width,height,internal_format,sampling_linear,border,glformat,gltype,data);
+}
+
+inline GlTexture::GlTexture(const TypedImage& img, bool sampling_linear)
+{
+ this->Load(img, sampling_linear);
+}
+
+inline GlTexture::GlTexture(GlTexture&& tex)
+{
+ *this = std::move(tex);
+}
+
+inline GlTexture& GlTexture::operator=(GlTexture&& tex)
+{
+ if (&tex != this) {
+ internal_format = tex.internal_format;
+ tid = tex.tid;
+ width = tex.width;
+ height = tex.height;
+
+ tex.internal_format = 0;
+ tex.tid = 0;
+ }
+ return *this;
+}
+
+inline bool GlTexture::IsValid() const
+{
+ return tid != 0;
+}
+
+inline void GlTexture::Delete()
+{
+ // We have no GL context whilst exiting.
+ if(internal_format!=0 && !pangolin::ShouldQuit() ) {
+ glDeleteTextures(1,&tid);
+ internal_format = 0;
+ tid = 0;
+ width = 0;
+ height = 0;
+ }
+}
+
+inline GlTexture::~GlTexture()
+{
+ // We have no GL context whilst exiting.
+ if(internal_format!=0 && !pangolin::ShouldQuit() ) {
+ glDeleteTextures(1,&tid);
+ }
+}
+
+inline void GlTexture::Bind() const
+{
+ glBindTexture(GL_TEXTURE_2D, tid);
+}
+
+inline void GlTexture::Unbind() const
+{
+ glBindTexture(GL_TEXTURE_2D, 0);
+}
+
+inline void GlTexture::Reinitialise(GLsizei w, GLsizei h, GLint int_format, bool sampling_linear, int border, GLenum glformat, GLenum gltype, GLvoid* data )
+{
+ if(tid!=0) {
+ glDeleteTextures(1,&tid);
+ }
+
+ internal_format = int_format;
+ width = w;
+ height = h;
+
+ glGenTextures(1,&tid);
+ Bind();
+
+ // GL_LUMINANCE and GL_FLOAT don't seem to actually affect buffer, but some values are required
+ // for call to succeed.
+ glTexImage2D(GL_TEXTURE_2D, 0, internal_format, width, height, border, glformat, gltype, data);
+
+ if(sampling_linear) {
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ }else{
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ }
+
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+
+ CheckGlDieOnError();
+}
+
+inline void GlTexture::Upload(
+ const void* data,
+ GLenum data_format, GLenum data_type
+) {
+ Bind();
+ glTexSubImage2D(GL_TEXTURE_2D,0,0,0,width,height,data_format,data_type,data);
+ // CheckGlDieOnError();
+}
+
+inline void GlTexture::Upload(
+ const void* data,
+ GLsizei tex_x_offset, GLsizei tex_y_offset,
+ GLsizei data_w, GLsizei data_h,
+ GLenum data_format, GLenum data_type )
+{
+ Bind();
+ glTexSubImage2D(GL_TEXTURE_2D,0,tex_x_offset,tex_y_offset,data_w,data_h,data_format,data_type,data);
+ CheckGlDieOnError();
+}
+
+inline void GlTexture::Load(const TypedImage& image, bool sampling_linear)
+{
+ GlPixFormat fmt(image.fmt);
+ Reinitialise((GLint)image.w, (GLint)image.h, GL_RGBA32F, sampling_linear, 0, fmt.glformat, fmt.gltype, image.ptr );
+}
+
+inline void GlTexture::LoadFromFile(const std::string& filename, bool sampling_linear)
+{
+ TypedImage image = LoadImage(filename);
+ Load(image, sampling_linear);
+}
+
+#ifndef HAVE_GLES
+inline void GlTexture::Download(void* image, GLenum data_layout, GLenum data_type) const
+{
+ Bind();
+ glGetTexImage(GL_TEXTURE_2D, 0, data_layout, data_type, image);
+ Unbind();
+}
+
+inline void GlTexture::Download(TypedImage& image) const
+{
+ switch (internal_format)
+ {
+ case GL_LUMINANCE8:
+ image.Reinitialise(width, height, PixelFormatFromString("GRAY8") );
+ Download(image.ptr, GL_LUMINANCE, GL_UNSIGNED_BYTE);
+ break;
+ case GL_LUMINANCE16:
+ image.Reinitialise(width, height, PixelFormatFromString("GRAY16LE") );
+ Download(image.ptr, GL_LUMINANCE, GL_UNSIGNED_SHORT);
+ break;
+ case GL_RGB8:
+ image.Reinitialise(width, height, PixelFormatFromString("RGB24"));
+ Download(image.ptr, GL_RGB, GL_UNSIGNED_BYTE);
+ break;
+ case GL_RGBA8:
+ image.Reinitialise(width, height, PixelFormatFromString("RGBA32"));
+ Download(image.ptr, GL_RGBA, GL_UNSIGNED_BYTE);
+ break;
+ case GL_RGB16:
+ image.Reinitialise(width, height, PixelFormatFromString("RGB48"));
+ Download(image.ptr, GL_RGB, GL_UNSIGNED_SHORT);
+ break;
+ case GL_RGBA16:
+ image.Reinitialise(width, height, PixelFormatFromString("RGBA64"));
+ Download(image.ptr, GL_RGBA, GL_UNSIGNED_SHORT);
+ break;
+ case GL_LUMINANCE:
+ case GL_LUMINANCE32F_ARB:
+ image.Reinitialise(width, height, PixelFormatFromString("GRAY32F"));
+ Download(image.ptr, GL_LUMINANCE, GL_FLOAT);
+ break;
+ case GL_RGB:
+ case GL_RGB32F:
+ image.Reinitialise(width, height, PixelFormatFromString("RGB96F"));
+ Download(image.ptr, GL_RGB, GL_FLOAT);
+ break;
+ case GL_RGBA:
+ case GL_RGBA32F:
+ image.Reinitialise(width, height, PixelFormatFromString("RGBA128F"));
+ Download(image.ptr, GL_RGBA, GL_FLOAT);
+ break;
+ default:
+ throw std::runtime_error(
+ "GlTexture::Download - Unknown internal format (" +
+ pangolin::Convert<std::string,GLint>::Do(internal_format) +
+ ")"
+ );
+ }
+
+}
+
+inline void GlTexture::CopyFrom(const GlTexture& tex)
+{
+ if(!tid || width != tex.width || height != tex.height ||
+ internal_format != tex.internal_format)
+ {
+ Reinitialise(tex.width, tex.height, tex.internal_format, true);
+ }
+
+ glCopyImageSubData(tex.tid, GL_TEXTURE_2D, 0, 0, 0, 0,
+ tid, GL_TEXTURE_2D, 0, 0, 0, 0,
+ width, height, 1);
+ CheckGlDieOnError();
+}
+
+inline void GlTexture::Save(const std::string& filename, bool top_line_first)
+{
+ TypedImage image;
+ Download(image);
+ pangolin::SaveImage(image, filename, top_line_first);
+}
+#endif // HAVE_GLES
+
+inline void GlTexture::SetLinear()
+{
+ Bind();
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+ Unbind();
+}
+
+inline void GlTexture::SetNearestNeighbour()
+{
+ Bind();
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+ Unbind();
+}
+
+inline void GlTexture::RenderToViewport(const bool flip) const
+{
+ if(flip) {
+ RenderToViewportFlipY();
+ }else{
+ RenderToViewport();
+ }
+}
+
+inline void GlTexture::RenderToViewport() const
+{
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ GLfloat sq_tex[] = { 0,0, 1,0, 1,1, 0,1 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glEnable(GL_TEXTURE_2D);
+ Bind();
+
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(GL_TEXTURE_2D);
+}
+
+inline void GlTexture::RenderToViewport(Viewport tex_vp, bool flipx, bool flipy) const
+{
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ GLfloat l = tex_vp.l / (float)(width);
+ GLfloat b = tex_vp.b / (float)(height);
+ GLfloat r = (tex_vp.l+tex_vp.w) / (float)(width);
+ GLfloat t = (tex_vp.b+tex_vp.h) / (float)(height);
+
+ if(flipx) std::swap(l,r);
+ if(flipy) std::swap(b,t);
+
+ GLfloat sq_tex[] = { l,b, r,b, r,t, l,t };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glEnable(GL_TEXTURE_2D);
+ Bind();
+
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(GL_TEXTURE_2D);
+}
+
+inline void GlTexture::RenderToViewportFlipY() const
+{
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ GLfloat sq_tex[] = { 0,1, 1,1, 1,0, 0,0 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glEnable(GL_TEXTURE_2D);
+ Bind();
+
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(GL_TEXTURE_2D);
+}
+
+inline void GlTexture::RenderToViewportFlipXFlipY() const
+{
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ GLfloat sq_vert[] = { 1,1, -1,1, -1,-1, 1,-1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ GLfloat sq_tex[] = { 0,0, 1,0, 1,1, 0,1 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glEnable(GL_TEXTURE_2D);
+ Bind();
+
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(GL_TEXTURE_2D);
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+inline GlRenderBuffer::GlRenderBuffer()
+ : width(0), height(0), rbid(0)
+{
+}
+
+inline GlRenderBuffer::GlRenderBuffer(GLint width, GLint height, GLint internal_format )
+ : width(0), height(0), rbid(0)
+{
+ Reinitialise(width,height,internal_format);
+}
+
+#ifndef HAVE_GLES
+inline void GlRenderBuffer::Reinitialise(GLint width, GLint height, GLint internal_format)
+{
+ if( this->width != 0 ) {
+ glDeleteRenderbuffersEXT(1, &rbid);
+ }
+
+ this->width = width;
+ this->height = height;
+ glGenRenderbuffersEXT(1, &rbid);
+ glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, rbid);
+ glRenderbufferStorageEXT(GL_RENDERBUFFER_EXT, internal_format, width, height);
+ glBindRenderbufferEXT(GL_RENDERBUFFER_EXT, 0);
+}
+
+inline GlRenderBuffer::~GlRenderBuffer()
+{
+ // We have no GL context whilst exiting.
+ if( width!=0 && !pangolin::ShouldQuit() ) {
+ glDeleteRenderbuffersEXT(1, &rbid);
+ }
+}
+#else
+inline void GlRenderBuffer::Reinitialise(GLint width, GLint height, GLint internal_format)
+{
+ if( width!=0 ) {
+ glDeleteTextures(1, &rbid);
+ }
+
+ // Use a texture instead...
+ glGenTextures(1, &rbid);
+ glBindTexture(GL_TEXTURE_2D, rbid);
+
+ glTexImage2D(GL_TEXTURE_2D, 0, internal_format,
+ width, height,
+ 0, internal_format, GL_UNSIGNED_SHORT, NULL);
+
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+}
+
+inline GlRenderBuffer::~GlRenderBuffer()
+{
+ // We have no GL context whilst exiting.
+ if( width!=0 && !pangolin::ShouldQuit() ) {
+ glDeleteTextures(1, &rbid);
+ }
+}
+#endif // HAVE_GLES
+
+inline GlRenderBuffer::GlRenderBuffer(GlRenderBuffer&& tex)
+ : width(tex.width), height(tex.height), rbid(tex.rbid)
+{
+ tex.rbid = tex.width = tex.height = 0;
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+inline GlFramebuffer::GlFramebuffer()
+ : fbid(0), attachments(0)
+{
+}
+
+inline GlFramebuffer::~GlFramebuffer()
+{
+ if(fbid) {
+ glDeleteFramebuffersEXT(1, &fbid);
+ }
+}
+
+inline GlFramebuffer::GlFramebuffer(GlTexture& colour, GlRenderBuffer& depth)
+ : attachments(0)
+{
+ glGenFramebuffersEXT(1, &fbid);
+ AttachColour(colour);
+ AttachDepth(depth);
+ CheckGlDieOnError();
+}
+
+inline GlFramebuffer::GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlRenderBuffer& depth)
+ : attachments(0)
+{
+ glGenFramebuffersEXT(1, &fbid);
+ AttachColour(colour0);
+ AttachColour(colour1);
+ AttachDepth(depth);
+ CheckGlDieOnError();
+}
+
+inline GlFramebuffer::GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlTexture& colour2, GlRenderBuffer& depth)
+ : attachments(0)
+{
+ glGenFramebuffersEXT(1, &fbid);
+ AttachColour(colour0);
+ AttachColour(colour1);
+ AttachColour(colour2);
+ AttachDepth(depth);
+ CheckGlDieOnError();
+}
+
+inline GlFramebuffer::GlFramebuffer(GlTexture& colour0, GlTexture& colour1, GlTexture& colour2, GlTexture& colour3, GlRenderBuffer& depth)
+ : attachments(0)
+{
+ glGenFramebuffersEXT(1, &fbid);
+ AttachColour(colour0);
+ AttachColour(colour1);
+ AttachColour(colour2);
+ AttachColour(colour3);
+ AttachDepth(depth);
+ CheckGlDieOnError();
+}
+
+inline void GlFramebuffer::Bind() const
+{
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fbid);
+#ifndef HAVE_GLES
+ glDrawBuffers( attachments, attachment_buffers );
+#endif
+}
+
+inline void GlFramebuffer::Reinitialise()
+{
+ if(fbid) {
+ glDeleteFramebuffersEXT(1, &fbid);
+ }
+ glGenFramebuffersEXT(1, &fbid);
+}
+
+inline void GlFramebuffer::Unbind() const
+{
+#ifndef HAVE_GLES
+ glDrawBuffers( 1, attachment_buffers );
+#endif
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+}
+
+inline GLenum GlFramebuffer::AttachColour(GlTexture& tex )
+{
+ if(!fbid) Reinitialise();
+
+ const GLenum color_attachment = GL_COLOR_ATTACHMENT0_EXT + attachments;
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fbid);
+ glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, color_attachment, GL_TEXTURE_2D, tex.tid, 0);
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+ attachments++;
+ CheckGlDieOnError();
+ return color_attachment;
+}
+
+inline void GlFramebuffer::AttachDepth(GlRenderBuffer& rb )
+{
+ if(!fbid) Reinitialise();
+
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, fbid);
+#if !defined(HAVE_GLES)
+ glFramebufferRenderbufferEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_RENDERBUFFER_EXT, rb.rbid);
+#elif defined(HAVE_GLES_2)
+ glFramebufferTexture2DEXT(GL_FRAMEBUFFER_EXT, GL_DEPTH_ATTACHMENT_EXT, GL_TEXTURE_2D, rb.rbid, 0);
+#else
+ throw std::exception();
+#endif
+ glBindFramebufferEXT(GL_FRAMEBUFFER_EXT, 0);
+ CheckGlDieOnError();
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+inline GlBufferData::GlBufferData()
+ : bo(0)
+{
+}
+
+inline GlBufferData::GlBufferData(GlBufferType buffer_type, GLuint size_bytes, GLenum gluse, const unsigned char* data )
+ : bo(0)
+{
+ Reinitialise(buffer_type, size_bytes, gluse, data );
+}
+
+//! Move Constructor
+inline GlBufferData::GlBufferData(GlBufferData&& tex)
+ : bo(0)
+{
+ *this = std::move(tex);
+}
+inline GlBufferData& GlBufferData::operator=(GlBufferData&& tex)
+{
+ Free();
+ this->bo = tex.bo;
+ this->buffer_type = tex.buffer_type;
+ this->gluse = tex.gluse;
+ this->size_bytes = tex.size_bytes;
+ tex.bo = 0;
+ return *this;
+}
+
+inline GlBufferData::~GlBufferData()
+{
+ Free();
+}
+
+inline void GlBufferData::Free()
+{
+ if(bo!=0) {
+ glDeleteBuffers(1, &bo);
+ }
+}
+
+inline bool GlBufferData::IsValid() const
+{
+ return bo != 0;
+}
+
+inline size_t GlBufferData::SizeBytes() const
+{
+ return size_bytes;
+}
+
+inline void GlBufferData::Reinitialise(GlBufferType buffer_type, GLuint size_bytes, GLenum gluse, const unsigned char* data )
+{
+ if(!bo) {
+ glGenBuffers(1, &bo);
+ }
+
+ this->buffer_type = buffer_type;
+ this->gluse = gluse;
+ this->size_bytes = size_bytes;
+
+ Bind();
+ glBufferData(buffer_type, size_bytes, data, gluse);
+ Unbind();
+}
+
+inline void GlBufferData::Bind() const
+{
+ glBindBuffer(buffer_type, bo);
+}
+
+inline void GlBufferData::Unbind() const
+{
+ glBindBuffer(buffer_type, 0);
+}
+
+inline void GlBufferData::Upload(const GLvoid* data, GLsizeiptr size_bytes, GLintptr offset)
+{
+ if(offset + size_bytes > this->size_bytes) {
+ throw std::runtime_error("GlBufferData: Trying to upload past capacity.");
+ }
+
+ Bind();
+ glBufferSubData(buffer_type,offset,size_bytes,data);
+ Unbind();
+}
+
+inline void GlBufferData::Download(GLvoid* data, GLsizeiptr size_bytes, GLintptr offset) const
+{
+ Bind();
+ glGetBufferSubData(buffer_type, offset, size_bytes, data);
+ Unbind();
+}
+
+////////////////////////////////////////////////////////////////////////////
+
+inline GlBuffer::GlBuffer()
+ : GlBufferData(), num_elements(0)
+{
+}
+
+inline GlBuffer::GlBuffer(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse )
+ : GlBufferData(buffer_type, num_elements * count_per_element * GlDataTypeBytes(datatype), gluse),
+ datatype(datatype), num_elements(num_elements), count_per_element(count_per_element)
+{
+}
+
+
+inline GlBuffer::GlBuffer(GlBuffer&& o)
+ : GlBufferData()
+{
+ *this = std::move(o);
+}
+
+inline GlBuffer& GlBuffer::operator=(GlBuffer&& o)
+{
+ datatype = o.datatype;
+ num_elements = o.num_elements;
+ count_per_element = o.count_per_element;
+ GlBufferData::operator =(std::move(o));
+ return *this;
+}
+
+inline void GlBuffer::Reinitialise(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse, const unsigned char* data )
+{
+ this->datatype = datatype;
+ this->num_elements = num_elements;
+ this->count_per_element = count_per_element;
+ const GLuint size_bytes = num_elements * count_per_element * GlDataTypeBytes(datatype);
+ GlBufferData::Reinitialise(buffer_type, size_bytes, gluse, data);
+}
+
+inline void GlBuffer::Reinitialise(GlBuffer const& other )
+{
+ Reinitialise(other.buffer_type, other.num_elements, other.datatype, other.count_per_element, other.gluse);
+}
+
+inline void GlBuffer::Resize(GLuint new_num_elements)
+{
+ if(bo!=0) {
+#ifndef HAVE_GLES
+ // Backup current data, reinit memory, restore old data
+ const size_t backup_elements = std::min(new_num_elements,num_elements);
+ const size_t backup_size_bytes = backup_elements*GlDataTypeBytes(datatype)*count_per_element;
+ unsigned char* backup = new unsigned char[backup_size_bytes];
+ Bind();
+ glGetBufferSubData(buffer_type, 0, backup_size_bytes, backup);
+ glBufferData(buffer_type, new_num_elements*GlDataTypeBytes(datatype)*count_per_element, 0, gluse);
+ glBufferSubData(buffer_type, 0, backup_size_bytes, backup);
+ Unbind();
+ delete[] backup;
+#else
+ throw std::exception();
+#endif
+ }else{
+ Reinitialise(buffer_type, new_num_elements, datatype, count_per_element, gluse);
+ }
+ num_elements = new_num_elements;
+}
+
+
+////////////////////////////////////////////////////////////////////////////
+
+inline GlSizeableBuffer::GlSizeableBuffer(GlBufferType buffer_type, GLuint initial_num_elements, GLenum datatype, GLuint count_per_element, GLenum gluse )
+ : GlBuffer(buffer_type, initial_num_elements, datatype, count_per_element, gluse), m_num_verts(0)
+{
+
+}
+
+inline void GlSizeableBuffer::Clear()
+{
+ m_num_verts = 0;
+}
+
+#ifdef USE_EIGEN
+template<typename Derived> inline
+void GlSizeableBuffer::Add(const Eigen::DenseBase<Derived>& vec)
+{
+ typedef typename Eigen::DenseBase<Derived>::Scalar Scalar;
+ assert(vec.rows()==GlBuffer::count_per_element);
+ CheckResize(m_num_verts + 1);
+ // TODO: taking address of first element is really dodgey. Need to work out
+ // when this is okay!
+ Upload(&vec(0,0), sizeof(Scalar)*vec.rows()*vec.cols(), sizeof(Scalar)*vec.rows()*m_num_verts);
+ m_num_verts += vec.cols();
+}
+
+template<typename Derived> inline
+void GlSizeableBuffer::Update(const Eigen::DenseBase<Derived>& vec, size_t position )
+{
+ typedef typename Eigen::DenseBase<Derived>::Scalar Scalar;
+ assert(vec.rows()==GlBuffer::count_per_element);
+ CheckResize(position + vec.cols() );
+ // TODO: taking address of first element is really dodgey. Need to work out
+ // when this is okay!
+ Upload(&vec(0,0), sizeof(Scalar)*vec.rows()*vec.cols(), sizeof(Scalar)*vec.rows()*position );
+ m_num_verts = std::max(position+vec.cols(), m_num_verts);
+}
+#endif
+
+inline size_t GlSizeableBuffer::start() const {
+ return 0;
+}
+
+inline size_t GlSizeableBuffer::size() const {
+ return m_num_verts;
+}
+
+inline void GlSizeableBuffer::CheckResize(size_t num_verts)
+{
+ if( num_verts > GlBuffer::num_elements) {
+ const size_t new_size = NextSize(num_verts);
+ GlBuffer::Resize((GLuint)new_size);
+ }
+}
+
+inline size_t GlSizeableBuffer::NextSize(size_t min_size) const
+{
+ size_t new_size = std::max(GlBuffer::num_elements, 1u);
+ while(new_size < min_size) {
+ new_size *= 2;
+ }
+ return new_size;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glchar.h b/externals/Pangolin/include/pangolin/gl/glchar.h
new file mode 100644
index 0000000000000000000000000000000000000000..000d0da23d83d69499e1f8bcfc07ac5f6309cc28
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glchar.h
@@ -0,0 +1,78 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+#include <map>
+
+namespace pangolin {
+
+struct PANGOLIN_EXPORT XYUV
+{
+ XYUV() {}
+ XYUV(GLfloat x, GLfloat y, GLfloat tu, GLfloat tv)
+ : x(x), y(y), tu(tu), tv(tv) {}
+
+ XYUV operator+(float dx) const {
+ return XYUV(x+dx,y,tu,tv);
+ }
+
+ GLfloat x, y, tu, tv;
+};
+
+class PANGOLIN_EXPORT GlChar
+{
+public:
+ GlChar();
+ GlChar(int tw, int th, int x, int y, int w, int h, GLfloat x_step, GLfloat ox, GLfloat oy);
+
+ inline const XYUV& GetVert(size_t i) const {
+ return vs[i];
+ }
+
+ inline GLfloat StepX() const {
+ return x_step;
+ }
+
+ inline GLfloat YMin() const {
+ return y_min;
+ }
+
+ inline GLfloat YMax() const {
+ return y_max;
+ }
+
+ void Draw() const;
+
+protected:
+ XYUV vs[4];
+ GLfloat x_step;
+ GLfloat y_min, y_max;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glcuda.h b/externals/Pangolin/include/pangolin/gl/glcuda.h
new file mode 100644
index 0000000000000000000000000000000000000000..9fbf14116a982f5c15537497384689c0153888f1
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glcuda.h
@@ -0,0 +1,258 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <algorithm>
+#include <cuda_runtime.h>
+#include <cuda_gl_interop.h>
+
+#include "gl.h"
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Interface
+////////////////////////////////////////////////
+
+struct GlBufferCudaPtr : public GlBuffer
+{
+ //! Default constructor represents 'no buffer'
+ GlBufferCudaPtr();
+
+ GlBufferCudaPtr(GlBufferType buffer_type, GLuint size_bytes, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ );
+ GlBufferCudaPtr(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ );
+
+ PANGOLIN_DEPRECATED
+ GlBufferCudaPtr(GlBufferType buffer_type, GLuint width, GLuint height, GLenum datatype, GLuint count_per_element, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ );
+
+ ~GlBufferCudaPtr();
+
+ void Reinitialise(GlBufferType buffer_type, GLuint size_bytes, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ );
+ void Reinitialise(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ );
+
+ /**
+ * Use parameters from another @c GlBufferCudaPtr to initialize this buffer.
+ */
+ void Reinitialise(const GlBufferCudaPtr& other);
+
+ unsigned int cuda_use;
+ cudaGraphicsResource* cuda_res;
+};
+
+struct GlTextureCudaArray : GlTexture
+{
+ GlTextureCudaArray();
+ // Some internal_formats aren't accepted. I have trouble with GL_RGB8
+ GlTextureCudaArray(int width, int height, GLint internal_format, bool sampling_linear = true, int border = 0, GLenum glformat = GL_RGBA, GLenum gltype = GL_UNSIGNED_BYTE, GLvoid* data = NULL);
+ ~GlTextureCudaArray();
+
+ void Reinitialise(int width, int height, GLint internal_format, bool sampling_linear = true, int border = 0, GLenum glformat = GL_RGBA, GLenum gltype = GL_UNSIGNED_BYTE, GLvoid* data = NULL) override;
+ cudaGraphicsResource* cuda_res;
+};
+
+struct CudaScopedMappedPtr
+{
+ CudaScopedMappedPtr(const GlBufferCudaPtr& buffer);
+ ~CudaScopedMappedPtr();
+ void* operator*();
+ cudaGraphicsResource* res;
+
+private:
+ CudaScopedMappedPtr(const CudaScopedMappedPtr&) {}
+};
+
+struct CudaScopedMappedArray
+{
+ CudaScopedMappedArray(const GlTextureCudaArray& tex);
+ ~CudaScopedMappedArray();
+ cudaArray* operator*();
+ cudaGraphicsResource* res;
+
+private:
+ CudaScopedMappedArray(const CudaScopedMappedArray&) {}
+};
+
+void CopyPboToTex(GlBufferCudaPtr& buffer, GlTexture& tex);
+
+void swap(GlBufferCudaPtr& a, GlBufferCudaPtr& b);
+
+////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////
+
+inline GlBufferCudaPtr::GlBufferCudaPtr()
+ : cuda_res(0)
+{
+}
+
+inline GlBufferCudaPtr::GlBufferCudaPtr(GlBufferType buffer_type, GLuint size_bytes, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ )
+ : cuda_res(0)
+{
+ Reinitialise(buffer_type, size_bytes, cudause, gluse);
+}
+
+inline GlBufferCudaPtr::GlBufferCudaPtr(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, unsigned int cudause, GLenum gluse )
+ : cuda_res(0)
+{
+ Reinitialise(buffer_type, num_elements, datatype, count_per_element, cudause, gluse);
+}
+
+inline GlBufferCudaPtr::GlBufferCudaPtr(GlBufferType buffer_type, GLuint width, GLuint height, GLenum datatype, GLuint count_per_element, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ )
+ : cuda_res(0)
+{
+ Reinitialise(buffer_type, width*height, datatype, count_per_element, cudause, gluse);
+}
+
+inline GlBufferCudaPtr::~GlBufferCudaPtr()
+{
+ if(cuda_res) {
+ cudaGraphicsUnregisterResource(cuda_res);
+ }
+}
+
+inline void GlBufferCudaPtr::Reinitialise(GlBufferType buffer_type, GLuint size_bytes, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ )
+{
+ GlBufferCudaPtr::Reinitialise(buffer_type, size_bytes, GL_BYTE, 1, cudause, gluse);
+}
+
+inline void GlBufferCudaPtr::Reinitialise(GlBufferType buffer_type, GLuint num_elements, GLenum datatype, GLuint count_per_element, unsigned int cudause /*= cudaGraphicsMapFlagsNone*/, GLenum gluse /*= GL_DYNAMIC_DRAW*/ )
+{
+ if(cuda_res) {
+ cudaGraphicsUnregisterResource(cuda_res);
+ }
+ GlBuffer::Reinitialise(buffer_type, num_elements, datatype, count_per_element, gluse);
+
+ cuda_use = cudause;
+ cudaGraphicsGLRegisterBuffer( &cuda_res, bo, cudause );
+}
+
+inline void GlBufferCudaPtr::Reinitialise(const GlBufferCudaPtr& other)
+{
+ Reinitialise(other.buffer_type, other.num_elements, other.datatype, other.count_per_element, other.cuda_use, other.gluse);
+}
+
+inline GlTextureCudaArray::GlTextureCudaArray()
+ : GlTexture(), cuda_res(0)
+{
+ // Not a texture
+}
+
+inline GlTextureCudaArray::GlTextureCudaArray(int width, int height, GLint internal_format, bool sampling_linear, int border, GLenum glformat, GLenum gltype, GLvoid *data)
+ :GlTexture(width,height,internal_format, sampling_linear, border, glformat, gltype, data)
+{
+ // TODO: specify flags too
+ const cudaError_t err = cudaGraphicsGLRegisterImage(&cuda_res, tid, GL_TEXTURE_2D, cudaGraphicsMapFlagsNone);
+ if( err != cudaSuccess ) {
+ std::cout << "cudaGraphicsGLRegisterImage failed: " << err << std::endl;
+ }
+}
+
+inline GlTextureCudaArray::~GlTextureCudaArray()
+{
+ if(cuda_res) {
+ cudaGraphicsUnregisterResource(cuda_res);
+ }
+}
+
+inline void GlTextureCudaArray::Reinitialise(int width, int height, GLint internal_format, bool sampling_linear, int border, GLenum glformat, GLenum gltype, GLvoid* data)
+{
+ if(cuda_res) {
+ cudaGraphicsUnregisterResource(cuda_res);
+ }
+
+ GlTexture::Reinitialise(width, height, internal_format, sampling_linear, border, glformat, gltype, data);
+
+ const cudaError_t err = cudaGraphicsGLRegisterImage(&cuda_res, tid, GL_TEXTURE_2D, cudaGraphicsMapFlagsNone);
+ if( err != cudaSuccess ) {
+ std::cout << "cudaGraphicsGLRegisterImage failed: " << err << std::endl;
+ }
+}
+
+inline CudaScopedMappedPtr::CudaScopedMappedPtr(const GlBufferCudaPtr& buffer)
+ : res(buffer.cuda_res)
+{
+ cudaGraphicsMapResources(1, &res, 0);
+}
+
+inline CudaScopedMappedPtr::~CudaScopedMappedPtr()
+{
+ cudaGraphicsUnmapResources(1, &res, 0);
+}
+
+inline void* CudaScopedMappedPtr::operator*()
+{
+ size_t num_bytes;
+ void* d_ptr;
+ cudaGraphicsResourceGetMappedPointer(&d_ptr,&num_bytes,res);
+ return d_ptr;
+}
+
+inline CudaScopedMappedArray::CudaScopedMappedArray(const GlTextureCudaArray& tex)
+ : res(tex.cuda_res)
+{
+ cudaGraphicsMapResources(1, &res);
+}
+
+inline CudaScopedMappedArray::~CudaScopedMappedArray()
+{
+ cudaGraphicsUnmapResources(1, &res);
+}
+
+inline cudaArray* CudaScopedMappedArray::operator*()
+{
+ cudaArray* array;
+ cudaGraphicsSubResourceGetMappedArray(&array, res, 0, 0);
+ return array;
+}
+
+inline void CopyPboToTex(const GlBufferCudaPtr& buffer, GlTexture& tex, GLenum buffer_layout, GLenum buffer_data_type )
+{
+ buffer.Bind();
+ tex.Bind();
+ glTexImage2D(GL_TEXTURE_2D, 0, tex.internal_format, tex.width, tex.height, 0, buffer_layout, buffer_data_type, 0);
+ buffer.Unbind();
+ tex.Unbind();
+}
+
+template<typename T>
+inline void CopyDevMemtoTex(T* d_img, size_t pitch, GlTextureCudaArray& tex )
+{
+ CudaScopedMappedArray arr_tex(tex);
+ cudaMemcpy2DToArray(*arr_tex, 0, 0, d_img, pitch, tex.width*sizeof(T), tex.height, cudaMemcpyDeviceToDevice );
+}
+
+inline void swap(GlBufferCudaPtr& a, GlBufferCudaPtr& b)
+{
+ std::swap(a.bo, b.bo);
+ std::swap(a.cuda_res, b.cuda_res);
+ std::swap(a.buffer_type, b.buffer_type);
+}
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/gldraw.h b/externals/Pangolin/include/pangolin/gl/gldraw.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ac0be7187b8ed1ff7b386d76be94bf0adbc95c9
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/gldraw.h
@@ -0,0 +1,518 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/gl/glformattraits.h>
+#include <pangolin/display/opengl_render_state.h>
+
+#include <vector>
+#include <math.h>
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#include <Eigen/src/Geometry/AlignedBox.h>
+#endif // USE_EIGEN
+
+namespace pangolin
+{
+
+// h [0,360)
+// s [0,1]
+// v [0,1]
+inline void glColorHSV( GLfloat hue, GLfloat s=1.0f, GLfloat v=1.0f )
+{
+ const GLfloat h = hue / 60.0f;
+ const int i = (int)floor(h);
+ const GLfloat f = (i%2 == 0) ? 1-(h-i) : h-i;
+ const GLfloat m = v * (1-s);
+ const GLfloat n = v * (1-s*f);
+ switch(i)
+ {
+ case 0: glColor4f(v,n,m,1); break;
+ case 1: glColor4f(n,v,m,1); break;
+ case 2: glColor4f(m,v,n,1); break;
+ case 3: glColor4f(m,n,v,1); break;
+ case 4: glColor4f(n,m,v,1); break;
+ case 5: glColor4f(v,m,n,1); break;
+ default:
+ break;
+ }
+}
+
+inline void glColorBin( int bin, int max_bins, GLfloat sat=1.0f, GLfloat val=1.0f )
+{
+ if( bin >= 0 ) {
+ const GLfloat hue = (GLfloat)(bin%max_bins) * 360.0f / (GLfloat)max_bins;
+ glColorHSV(hue,sat,val);
+ }else{
+ glColor4f(1,1,1,1);
+ }
+}
+
+template<typename T>
+inline void glDrawVertices(
+ size_t num_vertices, const T* const vertex_ptr, GLenum mode,
+ size_t elements_per_vertex = GlFormatTraits<T>::components,
+ size_t vertex_stride_bytes = 0 )
+{
+ if(num_vertices > 0)
+ {
+ PANGO_ENSURE(vertex_ptr != nullptr);
+ PANGO_ENSURE(mode != GL_LINES || num_vertices % 2 == 0, "number of vertices (%) must be even in GL_LINES mode", num_vertices );
+
+ glVertexPointer(elements_per_vertex, GlFormatTraits<T>::gltype, vertex_stride_bytes, vertex_ptr);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glDrawArrays(mode, 0, num_vertices);
+ glDisableClientState(GL_VERTEX_ARRAY);
+ }
+}
+
+template<typename TV, typename TC>
+inline void glDrawColoredVertices(
+ size_t num_vertices, const TV* const vertex_ptr, const TC* const color_ptr, GLenum mode,
+ size_t elements_per_vertex = GlFormatTraits<TV>::components,
+ size_t elements_per_color = GlFormatTraits<TC>::components,
+ size_t vertex_stride_bytes = 0,
+ size_t color_stride_bytes = 0
+) {
+ if(color_ptr) {
+ glColorPointer(elements_per_color, GlFormatTraits<TC>::gltype, color_stride_bytes, color_ptr);
+ glEnableClientState(GL_COLOR_ARRAY);
+ glDrawVertices<TV>(num_vertices, vertex_ptr, mode, elements_per_vertex, vertex_stride_bytes);
+ glDisableClientState(GL_COLOR_ARRAY);
+ }else{
+ glDrawVertices<TV>(num_vertices, vertex_ptr, mode, elements_per_vertex, vertex_stride_bytes);
+ }
+}
+
+inline void glDrawLine( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2 )
+{
+ const GLfloat verts[] = { x1,y1, x2,y2 };
+ glDrawVertices<float>(2, verts, GL_LINES, 2);
+}
+
+inline void glDrawLine( GLfloat x1, GLfloat y1, GLfloat z1, GLfloat x2, GLfloat y2, GLfloat z2)
+{
+ const GLfloat verts[] = { x1,y1,z1, x2,y2,z2 };
+ glDrawVertices<float>(2, verts, GL_LINES, 3);
+}
+
+inline void glDrawCross( GLfloat x, GLfloat y, GLfloat rad )
+{
+ const GLfloat verts[] = { x-rad,y, x+rad, y, x,y-rad, x, y+rad};
+ glDrawVertices<float>(4, verts, GL_LINES, 2);
+}
+
+inline void glDrawCross( GLfloat x, GLfloat y, GLfloat z, GLfloat rad )
+{
+ const GLfloat verts[] = { x-rad,y,z, x+rad,y,z, x,y-rad,z, x,y+rad,z, x,y,z-rad, x,y,z+rad };
+ glDrawVertices<float>(6, verts, GL_LINES, 3);
+}
+
+inline void glDrawAxis(float s)
+{
+ const GLfloat cols[] = { 1,0,0, 1,0,0, 0,1,0, 0,1,0, 0,0,1, 0,0,1 };
+ const GLfloat verts[] = { 0,0,0, s,0,0, 0,0,0, 0,s,0, 0,0,0, 0,0,s };
+ glDrawColoredVertices<float,float>(6, verts, cols, GL_LINES, 3, 3);
+}
+
+inline void glDrawRect( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2, GLenum mode = GL_TRIANGLE_FAN )
+{
+ const GLfloat verts[] = { x1,y1, x2,y1, x2,y2, x1,y2 };
+ glDrawVertices<float>(4, verts, mode, 2);
+}
+
+inline void glDrawRectPerimeter( GLfloat x1, GLfloat y1, GLfloat x2, GLfloat y2 )
+{
+ glDrawRect(x1,y1, x2,y2, GL_LINE_LOOP);
+}
+
+inline void glDrawCirclePerimeter( float x, float y, float rad )
+{
+ const int N = 50;
+ GLfloat verts[N*2];
+
+ const float TAU_DIV_N = 2*(float)M_PI/N;
+ for(int i = 0; i < N*2; i+=2) {
+ verts[i] = x + rad * cos(i*TAU_DIV_N);
+ verts[i+1] = y + rad * sin(i*TAU_DIV_N);
+ }
+
+ glDrawVertices<float>(N, verts, GL_LINES, 2);
+}
+
+inline void glDrawCircle( GLfloat x, GLfloat y, GLfloat rad )
+{
+ const int N = 50;
+ GLfloat verts[N*2];
+
+ // Draw vertices anticlockwise for front face
+ const float TAU_DIV_N = 2*(float)M_PI/N;
+ for(int i = 0; i < N*2; i+=2) {
+ verts[i] = x + rad * cos(-i*TAU_DIV_N);
+ verts[i+1] = y + rad * sin(-i*TAU_DIV_N);
+ }
+
+ // Render filled shape and outline (to make it look smooth)
+ glVertexPointer(2, GL_FLOAT, 0, verts);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, N);
+ glDrawArrays(GL_LINE_STRIP, 0, N);
+ glDisableClientState(GL_VERTEX_ARRAY);
+}
+
+inline void glDrawColouredCube(GLfloat axis_min=-0.5f, GLfloat axis_max = +0.5f)
+{
+ const GLfloat l = axis_min;
+ const GLfloat h = axis_max;
+
+ const GLfloat verts[] = {
+ l,l,h, h,l,h, l,h,h, h,h,h, // FRONT
+ l,l,l, l,h,l, h,l,l, h,h,l, // BACK
+ l,l,h, l,h,h, l,l,l, l,h,l, // LEFT
+ h,l,l, h,h,l, h,l,h, h,h,h, // RIGHT
+ l,h,h, h,h,h, l,h,l, h,h,l, // TOP
+ l,l,h, l,l,l, h,l,h, h,l,l // BOTTOM
+ };
+
+ glVertexPointer(3, GL_FLOAT, 0, verts);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ glColor4f(1.0f, 0.0f, 0.0f, 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+ glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
+
+ glColor4f(0.0f, 1.0f, 0.0f, 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 8, 4);
+ glDrawArrays(GL_TRIANGLE_STRIP, 12, 4);
+
+ glColor4f(0.0f, 0.0f, 1.0f, 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 16, 4);
+ glDrawArrays(GL_TRIANGLE_STRIP, 20, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+}
+
+inline void glDraw_x0(GLfloat scale, int grid)
+{
+ const GLfloat maxord = grid*scale;
+ for (int i = -grid; i <= grid; ++i) {
+ glDrawLine(0.0, i*scale, -maxord, 0.0, i*scale, +maxord);
+ glDrawLine(0.0, -maxord, i*scale, 0.0, +maxord, i*scale);
+ }
+}
+
+inline void glDraw_y0(GLfloat scale, int grid)
+{
+ const GLfloat maxord = grid*scale;
+ for (int i = -grid; i <= grid; ++i) {
+ glDrawLine(i*scale, 0.0, -maxord, i*scale, 0.0, +maxord);
+ glDrawLine(-maxord, 0.0, i*scale, +maxord, 0.0, i*scale);
+ }
+}
+
+inline void glDraw_z0(GLfloat scale, int grid)
+{
+ const GLfloat maxord = grid*scale;
+ for(int i=-grid; i<=grid; ++i ) {
+ glDrawLine(i*scale,-maxord, i*scale,+maxord);
+ glDrawLine(-maxord, i*scale, +maxord, i*scale);
+ }
+}
+
+inline void glDrawFrustum( GLfloat u0, GLfloat v0, GLfloat fu, GLfloat fv, int w, int h, GLfloat scale )
+{
+ const GLfloat xl = scale * u0;
+ const GLfloat xh = scale * (w*fu + u0);
+ const GLfloat yl = scale * v0;
+ const GLfloat yh = scale * (h*fv + v0);
+
+ const GLfloat verts[] = {
+ xl,yl,scale, xh,yl,scale,
+ xh,yh,scale, xl,yh,scale,
+ xl,yl,scale, 0,0,0,
+ xh,yl,scale, 0,0,0,
+ xl,yh,scale, 0,0,0,
+ xh,yh,scale
+ };
+
+ glDrawVertices(11, verts, GL_LINE_STRIP, 3);
+}
+
+inline void glDrawTexture(GLenum target, GLuint texid)
+{
+ glBindTexture(target, texid);
+ glEnable(target);
+
+ const GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ const GLfloat sq_tex[] = { 0,0, 1,0, 1,1, 0,1 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glColor4f(1,1,1,1);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(target);
+}
+
+inline void glDrawTextureFlipY(GLenum target, GLuint texid)
+{
+ glBindTexture(target, texid);
+ glEnable(target);
+
+ const GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ const GLfloat sq_tex[] = { 0,1, 1,1, 1,0, 0,0 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glColor4f(1,1,1,1);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(target);
+}
+
+
+#ifdef USE_EIGEN
+
+#ifndef HAVE_GLES
+inline void glVertex( const Eigen::Vector3d& p )
+{
+ glVertex3dv(p.data());
+}
+#endif
+
+inline void glDrawLine( const Eigen::Vector2d& p1, const Eigen::Vector2d& p2 )
+{
+ glDrawLine((GLfloat)p1(0), (GLfloat)p1(1), (GLfloat)p2(0), (GLfloat)p2(1));
+}
+
+// Draws a vector of 2d or 3d vertices using provided ``mode``.
+//
+// Preconditions:
+// - ``mode`` must be GL_POINTS, GL_LINES, GL_LINE_STRIP, GL_LINE_LOOP, etc
+// - If ``mode == GL_LINES``, then ``vertices.size()`` must be a multiple of 2.
+//
+template<typename P, int N, class Allocator>
+void glDrawVertices(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices, GLenum mode)
+{
+ glDrawVertices(vertices.size(), vertices.data(), mode);
+}
+
+// Draws a vector of 2d or 3d points.
+//
+template<typename P, int N, class Allocator>
+void glDrawPoints(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
+{
+ glDrawVertices(vertices, GL_POINTS);
+}
+
+// Draws a vector of 2d or 3d lines.
+//
+// Precondition: ``vertices.size()`` must be a multiple of 2.
+//
+template<typename P, int N, class Allocator>
+void glDrawLines(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
+{
+ glDrawVertices(vertices, GL_LINES);
+}
+
+// Draws a 2d or 3d line strip.
+//
+template<typename P, int N, class Allocator>
+void glDrawLineStrip(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
+{
+ glDrawVertices(vertices, GL_LINE_STRIP);
+}
+
+// Draws a 2d or 3d line loop.
+//
+template<typename P, int N, class Allocator>
+void glDrawLineLoop(const std::vector<Eigen::Matrix<P, N, 1>, Allocator>& vertices)
+{
+ glDrawVertices(vertices, GL_LINE_LOOP);
+}
+
+inline void glDrawCross( const Eigen::Vector2d& p, double r = 5.0 )
+{
+ glDrawCross((GLfloat)p(0), (GLfloat)p(1), (GLfloat)r);
+}
+
+inline void glDrawCross( const Eigen::Vector3d& p, double r = 5.0 )
+{
+ glDrawCross((GLfloat)p(0), (GLfloat)p(1), (GLfloat)p(2), (GLfloat)r);
+}
+
+inline void glDrawCircle( const Eigen::Vector2d& p, double radius = 5.0 )
+{
+ glDrawCircle((GLfloat)p(0), (GLfloat)p(1), (GLfloat)radius);
+}
+
+inline void glDrawCirclePerimeter( const Eigen::Vector2d& p, double radius = 5.0 )
+{
+ glDrawCirclePerimeter((GLfloat)p(0), (GLfloat)p(1), (GLfloat)radius);
+}
+
+inline void glSetFrameOfReference( const Eigen::Matrix4f& T_wf )
+{
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+ glMultMatrixf( T_wf.data() );
+}
+
+inline void glSetFrameOfReference( const Eigen::Matrix4d& T_wf )
+{
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+#ifndef HAVE_GLES
+ glMultMatrixd( T_wf.data() );
+#else
+ const Eigen::Matrix4f fT_wf = T_wf.cast<GLfloat>();
+ glMultMatrixf( fT_wf.data() );
+#endif
+}
+
+inline void glSetFrameOfReference( const pangolin::OpenGlMatrix& T_wf )
+{
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+ glMultMatrixd( T_wf.m );
+}
+
+inline void glUnsetFrameOfReference()
+{
+ glPopMatrix();
+}
+
+template<typename T, typename S>
+inline void glDrawAxis( const T& T_wf, S scale )
+{
+ glSetFrameOfReference(T_wf);
+ glDrawAxis(scale);
+ glUnsetFrameOfReference();
+}
+
+template<typename T>
+inline void glDrawFrustum( const Eigen::Matrix<T,3,3>& Kinv, int w, int h, GLfloat scale )
+{
+ glDrawFrustum((GLfloat)Kinv(0,2), (GLfloat)Kinv(1,2), (GLfloat)Kinv(0,0), (GLfloat)Kinv(1,1), w, h, scale);
+}
+
+template<typename T>
+inline void glDrawFrustum( const Eigen::Matrix<T,3,3>& Kinv, int w, int h, const Eigen::Matrix<T,4,4>& T_wf, T scale )
+{
+ glSetFrameOfReference(T_wf);
+ glDrawFrustum(Kinv,w,h,scale);
+ glUnsetFrameOfReference();
+}
+
+template<typename T>
+inline void glDrawAlignedBox( const Eigen::AlignedBox<T,2>& box, GLenum mode = GL_TRIANGLE_FAN )
+{
+ const Eigen::Matrix<float,2,1> l = box.min().template cast<float>();
+ const Eigen::Matrix<float,2,1> h = box.max().template cast<float>();
+
+ GLfloat verts[] = {
+ l[0], l[1],
+ h[0], l[1],
+ h[0], h[1],
+ l[0], h[1]
+ };
+
+ glDrawVertices(4, verts, mode, 2);
+}
+
+template<typename T>
+inline void glDrawAlignedBoxPerimeter( const Eigen::AlignedBox<T,2>& box)
+{
+ glDrawAlignedBox<T>(box, GL_LINE_LOOP);
+}
+
+template<typename T>
+inline void glDrawAlignedBox( const Eigen::AlignedBox<T,3>& box)
+{
+ const Eigen::Matrix<float,3,1> l = box.min().template cast<float>();
+ const Eigen::Matrix<float,3,1> h = box.max().template cast<float>();
+
+ GLfloat verts[] = {
+ l[0], l[1], l[2],
+ l[0], l[1], h[2],
+ h[0], l[1], h[2],
+ h[0], l[1], l[2],
+ l[0], l[1], l[2],
+ l[0], h[1], l[2],
+ h[0], h[1], l[2],
+ h[0], l[1], l[2],
+ h[0], h[1], l[2],
+ h[0], h[1], h[2],
+ l[0], h[1], h[2],
+ l[0], h[1], l[2],
+ l[0], h[1], h[2],
+ l[0], l[1], h[2],
+ h[0], l[1], h[2],
+ h[0], h[1], h[2]
+ };
+
+ glDrawVertices(16, verts, GL_LINE_STRIP, 3);
+}
+
+#endif // USE_EIGEN
+
+#ifndef HAVE_GLES
+inline void glPixelTransferScale( float r, float g, float b )
+{
+ glPixelTransferf(GL_RED_SCALE,r);
+ glPixelTransferf(GL_GREEN_SCALE,g);
+ glPixelTransferf(GL_BLUE_SCALE,b);
+}
+
+inline void glPixelTransferScale( float scale )
+{
+ glPixelTransferScale(scale,scale,scale);
+}
+#endif
+
+void glRecordGraphic(float x, float y, float radius);
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glfont.h b/externals/Pangolin/include/pangolin/gl/glfont.h
new file mode 100644
index 0000000000000000000000000000000000000000..2304c2f3bdc0776f85ca20481cd85c5e85e39459
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glfont.h
@@ -0,0 +1,78 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/gltext.h>
+
+#include <cstdio>
+#include <cstdarg>
+
+namespace pangolin {
+
+class PANGOLIN_EXPORT GlFont
+{
+public:
+ // Singleton instance if requested.
+ static GlFont& I();
+
+ // Load GL Font data. Delay uploading as texture until first use.
+ GlFont(const unsigned char* ttf_buffer, float pixel_height, int tex_w=512, int tex_h=512);
+ GlFont(const std::string& filename, float pixel_height, int tex_w=512, int tex_h=512);
+
+ virtual ~GlFont();
+
+ // Generate renderable GlText object from this font.
+ GlText Text( const char* fmt, ... );
+
+ GlText Text( const std::string& str );
+
+ inline float Height() const {
+ return font_height_px;
+ }
+
+protected:
+ void InitialiseFont(const unsigned char* ttf_buffer, float pixel_height, int tex_w, int tex_h);
+
+ // This can only be called once GL context is initialised
+ void InitialiseGlTexture();
+
+ const static int FIRST_CHAR = 32;
+ const static int NUM_CHARS = 96;
+
+ float font_height_px;
+
+ int tex_w;
+ int tex_h;
+ unsigned char* font_bitmap;
+ GlTexture mTex;
+
+ GlChar chardata[NUM_CHARS];
+ GLfloat kern_table[NUM_CHARS*NUM_CHARS];
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glformattraits.h b/externals/Pangolin/include/pangolin/gl/glformattraits.h
new file mode 100644
index 0000000000000000000000000000000000000000..7c1dc588d148e906b4cc2082b4461d0b7925ac16
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glformattraits.h
@@ -0,0 +1,214 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+
+#ifdef HAVE_EIGEN
+# include <Eigen/Core>
+#endif
+
+namespace pangolin
+{
+
+template<typename T>
+struct GlFormatTraits;
+//{
+// static const GLint glinternalformat = 0;
+// static const GLenum glformat = 0;
+// static const GLenum gltype = 0;
+// static const T glmin = 0;
+// static const T glmax = 0;
+//};
+
+template<>
+struct GlFormatTraits<unsigned char>
+{
+ static const GLint glinternalformat = GL_LUMINANCE8;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_UNSIGNED_BYTE;
+ static const size_t components = 1;
+};
+
+template<>
+struct GlFormatTraits<unsigned short>
+{
+ static const GLint glinternalformat = GL_LUMINANCE16;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_UNSIGNED_SHORT;
+ static const size_t components = 1;
+};
+
+template<>
+struct GlFormatTraits<unsigned int>
+{
+ static const GLint glinternalformat = GL_LUMINANCE32I_EXT;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_UNSIGNED_INT;
+ static const size_t components = 1;
+};
+
+template<>
+struct GlFormatTraits<int>
+{
+ static const GLint glinternalformat = GL_LUMINANCE32I_EXT;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_INT;
+ static const size_t components = 1;
+};
+
+template<>
+struct GlFormatTraits<float>
+{
+ static const GLint glinternalformat = GL_LUMINANCE32F_ARB;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_FLOAT;
+ static const size_t components = 1;
+};
+
+template<>
+struct GlFormatTraits<double>
+{
+ static const GLint glinternalformat = GL_LUMINANCE32F_ARB;
+ static const GLenum glformat = GL_LUMINANCE;
+ static const GLenum gltype = GL_DOUBLE;
+ static const size_t components = 1;
+};
+
+
+
+#ifdef HAVE_EIGEN
+
+//////////////////////////////////////////////////////////////////
+
+template <>
+struct GlFormatTraits<Eigen::Vector2i>
+{
+ static const GLint glinternalformat = GL_RG32I;
+ static const GLenum glformat = GL_RG;
+ static const GLenum gltype = GL_INT;
+ static const size_t components = 2;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector2f>
+{
+ static const GLint glinternalformat = GL_RG32F;
+ static const GLenum glformat = GL_RG;
+ static const GLenum gltype = GL_FLOAT;
+ static const size_t components = 2;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector2d>
+{
+ static const GLint glinternalformat = GL_RG32F;
+ static const GLenum glformat = GL_RG;
+ static const GLenum gltype = GL_DOUBLE;
+ static const size_t components = 2;
+};
+
+//////////////////////////////////////////////////////////////////
+
+template <>
+struct GlFormatTraits<Eigen::Matrix<unsigned char,3,1>>
+{
+ static const GLint glinternalformat = GL_RGB8;
+ static const GLenum glformat = GL_RGB;
+ static const GLenum gltype = GL_UNSIGNED_BYTE;
+ static const size_t components = 3;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Matrix<unsigned short,3,1>>
+{
+ static const GLint glinternalformat = GL_RGBA16;
+ static const GLenum glformat = GL_RGB;
+ static const GLenum gltype = GL_UNSIGNED_SHORT;
+ static const size_t components = 3;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector3f>
+{
+ static const GLint glinternalformat = GL_RGB32F;
+ static const GLenum glformat = GL_RGB;
+ static const GLenum gltype = GL_FLOAT;
+ static const size_t components = 3;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector3d>
+{
+ static const GLint glinternalformat = GL_RGB32F;
+ static const GLenum glformat = GL_RGB;
+ static const GLenum gltype = GL_DOUBLE;
+ static const size_t components = 3;
+};
+
+//////////////////////////////////////////////////////////////////
+
+template <>
+struct GlFormatTraits<Eigen::Matrix<unsigned char,4,1>>
+{
+ static const GLint glinternalformat = GL_RGBA8;
+ static const GLenum glformat = GL_RGBA;
+ static const GLenum gltype = GL_UNSIGNED_BYTE;
+ static const size_t components = 4;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Matrix<unsigned short,4,1>>
+{
+ static const GLint glinternalformat = GL_RGBA16;
+ static const GLenum glformat = GL_RGBA;
+ static const GLenum gltype = GL_UNSIGNED_SHORT;
+ static const size_t components = 4;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector4f>
+{
+ static const GLint glinternalformat = GL_RGBA32F;
+ static const GLenum glformat = GL_RGBA;
+ static const GLenum gltype = GL_FLOAT;
+ static const size_t components = 4;
+};
+
+template <>
+struct GlFormatTraits<Eigen::Vector4d>
+{
+ static const GLint glinternalformat = GL_RGBA32F;
+ static const GLenum glformat = GL_RGBA;
+ static const GLenum gltype = GL_DOUBLE;
+ static const size_t components = 4;
+};
+
+#endif // HAVE_EIGEN
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glinclude.h b/externals/Pangolin/include/pangolin/gl/glinclude.h
new file mode 100644
index 0000000000000000000000000000000000000000..2174b8f094ac54850e0b4ce5e3a19faf64f65ae0
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glinclude.h
@@ -0,0 +1,46 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove, Richard Newcombe
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+
+#ifdef HAVE_GLES
+#include <pangolin/gl/compat/gl_es_compat.h>
+#endif
+
+#define CheckGlDieOnError() pangolin::_CheckGlDieOnError( __FILE__, __LINE__ );
+namespace pangolin {
+inline void _CheckGlDieOnError( const char *sFile, const int nLine )
+{
+ GLenum glError = glGetError();
+ if( glError != GL_NO_ERROR ) {
+ pango_print_error( "OpenGL Error: %s (%x)\n", glErrorString(glError), glError );
+ pango_print_error("In: %s, line %d\n", sFile, nLine);
+ }
+}
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glpangoglu.h b/externals/Pangolin/include/pangolin/gl/glpangoglu.h
new file mode 100644
index 0000000000000000000000000000000000000000..c6f5dae689b809496be3d1c2eb6d458de8dbe807
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glpangoglu.h
@@ -0,0 +1,80 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+
+namespace pangolin {
+
+/// Clone of gluProject
+PANGOLIN_EXPORT
+const GLubyte* glErrorString(GLenum error);
+
+/// Clone of gluProject
+PANGOLIN_EXPORT
+GLint glProject(
+ float objx, float objy, float objz,
+ const float modelMatrix[16],
+ const float projMatrix[16],
+ const GLint viewport[4],
+ float* winx, float* winy, float* winz
+);
+
+
+/// Clone of gluUnProject
+PANGOLIN_EXPORT
+GLint glUnProject(
+ float winx, float winy, float winz,
+ const float modelMatrix[16],
+ const float projMatrix[16],
+ const GLint viewport[4],
+ float* objx, float* objy, float* objz
+);
+
+/// Clone of gluProject
+PANGOLIN_EXPORT
+GLint glProject(
+ double objx, double objy, double objz,
+ const double modelMatrix[16],
+ const double projMatrix[16],
+ const GLint viewport[4],
+ double* winx, double* winy, double* winz
+);
+
+
+/// Clone of gluUnProject
+PANGOLIN_EXPORT
+GLint glUnProject(
+ double winx, double winy, double winz,
+ const double modelMatrix[16],
+ const double projMatrix[16],
+ const GLint viewport[4],
+ double* objx, double* objy, double* objz
+);
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glpixformat.h b/externals/Pangolin/include/pangolin/gl/glpixformat.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ceebd5bef0f418b6220fb05d08af923c5511c5c
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glpixformat.h
@@ -0,0 +1,95 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/gl/glformattraits.h>
+#include <pangolin/image/pixel_format.h>
+#include <stdexcept>
+
+namespace pangolin {
+
+// This class may dissapear in the future
+struct GlPixFormat
+{
+ GlPixFormat() {}
+
+ GlPixFormat(const PixelFormat& fmt)
+ {
+ switch( fmt.channels) {
+ case 1: glformat = GL_LUMINANCE; break;
+ case 3: glformat = (fmt.format == "BGR24" || fmt.format == "BGR48") ? GL_BGR : GL_RGB; break;
+ case 4: glformat = (fmt.format == "BGRA24" || fmt.format == "BGRA32" || fmt.format == "BGRA48") ? GL_BGRA : GL_RGBA; break;
+ default: throw std::runtime_error("Unable to form OpenGL format from video format: '" + fmt.format + "'.");
+ }
+
+ const bool is_integral = fmt.format.find('F') == std::string::npos;
+
+ switch (fmt.channel_bits[0]) {
+ case 8: gltype = GL_UNSIGNED_BYTE; break;
+ case 16: gltype = GL_UNSIGNED_SHORT; break;
+ case 32: gltype = (is_integral ? GL_UNSIGNED_INT : GL_FLOAT); break;
+ case 64: gltype = (is_integral ? GL_UNSIGNED_INT64_NV : GL_DOUBLE); break;
+ default: throw std::runtime_error("Unknown OpenGL data type for video format: '" + fmt.format + "'.");
+ }
+
+ if(glformat == GL_LUMINANCE) {
+ if(gltype == GL_UNSIGNED_BYTE) {
+ scalable_internal_format = GL_LUMINANCE8;
+ }else if(gltype == GL_UNSIGNED_SHORT){
+ scalable_internal_format = GL_LUMINANCE16;
+ }else{
+ scalable_internal_format = GL_LUMINANCE32F_ARB;
+ }
+ }else{
+ if(gltype == GL_UNSIGNED_BYTE) {
+ scalable_internal_format = GL_RGBA8;
+ }else if(gltype == GL_UNSIGNED_SHORT) {
+ scalable_internal_format = GL_RGBA16;
+ }else{
+ scalable_internal_format = GL_RGBA32F;
+ }
+ }
+ }
+
+ template<typename T>
+ static GlPixFormat FromType()
+ {
+ GlPixFormat fmt;
+ fmt.glformat = GlFormatTraits<T>::glformat;
+ fmt.gltype = GlFormatTraits<T>::gltype;
+ fmt.scalable_internal_format = GlFormatTraits<T>::glinternalformat;
+ return fmt;
+ }
+
+ GLint glformat;
+ GLenum gltype;
+ GLint scalable_internal_format;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glplatform.h b/externals/Pangolin/include/pangolin/gl/glplatform.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e05e131397467b8adf8c9377f481a831001ff97
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glplatform.h
@@ -0,0 +1,85 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+//////////////////////////////////////////////////////////
+// Attempt to portably include Necessary OpenGL headers
+//////////////////////////////////////////////////////////
+
+#include <pangolin/platform.h>
+
+#ifdef _WIN_
+ // Define maths quantities when using <cmath> to match posix systems
+ #ifndef _USE_MATH_DEFINES
+ # define _USE_MATH_DEFINES
+ #endif
+
+ // Don't define min / max macros in windows.h or other unnecessary macros
+ #ifndef NOMINMAX
+ # define NOMINMAX
+ #endif
+ #ifndef WIN32_LEAN_AND_MEAN
+ # define WIN32_LEAN_AND_MEAN
+ #endif
+ #include <Windows.h>
+
+ // Undef nuisance Windows.h macros which interfere with our methods
+ #undef LoadImage
+ #undef near
+ #undef far
+ #undef ERROR
+#endif
+
+#ifdef HAVE_GLEW
+ #include <GL/glew.h>
+#endif
+
+#ifdef HAVE_GLES
+ #if defined(_ANDROID_)
+ #include <EGL/egl.h>
+ #ifdef HAVE_GLES_2
+ #include <GLES2/gl2.h>
+ #include <GLES2/gl2ext.h>
+ #else
+ #include <GLES/gl.h>
+ #define GL_GLEXT_PROTOTYPES
+ #include <GLES/glext.h>
+ #endif
+ #elif defined(_APPLE_IOS_)
+ #include <OpenGLES/ES2/gl.h>
+ #include <OpenGLES/ES2/glext.h>
+ #endif
+#else
+ #ifdef _OSX_
+ #include <OpenGL/gl.h>
+ #else
+ #include <GL/gl.h>
+ #endif
+#endif // HAVE_GLES
+
+#include <pangolin/gl/glpangoglu.h>
diff --git a/externals/Pangolin/include/pangolin/gl/glsl.h b/externals/Pangolin/include/pangolin/gl/glsl.h
new file mode 100644
index 0000000000000000000000000000000000000000..beea7b3361d8036375f6eee5396156087824e2ec
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glsl.h
@@ -0,0 +1,738 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <sstream>
+#include <fstream>
+#include <algorithm>
+#include <vector>
+#include <map>
+#include <cctype>
+
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/gl/colour.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/display/opengl_render_state.h>
+
+#ifdef HAVE_GLES
+ #define GLhandleARB GLuint
+#endif
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#endif // USE_EIGEN
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Standard attribute locations
+////////////////////////////////////////////////
+
+const GLuint DEFAULT_LOCATION_POSITION = 0;
+const GLuint DEFAULT_LOCATION_COLOUR = 1;
+const GLuint DEFAULT_LOCATION_NORMAL = 2;
+const GLuint DEFAULT_LOCATION_TEXCOORD = 3;
+
+const char DEFAULT_NAME_POSITION[] = "a_position";
+const char DEFAULT_NAME_COLOUR[] = "a_color";
+const char DEFAULT_NAME_NORMAL[] = "a_normal";
+const char DEFAULT_NAME_TEXCOORD[] = "a_texcoord";
+
+////////////////////////////////////////////////
+// Interface
+////////////////////////////////////////////////
+
+enum GlSlShaderType
+{
+ GlSlAnnotatedShader = 0,
+ GlSlFragmentShader = GL_FRAGMENT_SHADER,
+ GlSlVertexShader = GL_VERTEX_SHADER,
+ GlSlGeometryShader = 0x8DD9 /*GL_GEOMETRY_SHADER*/,
+ GlSlComputeShader = 0x91B9 /*GL_COMPUTE_SHADER*/
+};
+
+class GlSlProgram
+{
+public:
+ GlSlProgram();
+
+ //! Move Constructor
+ GlSlProgram(GlSlProgram&& tex);
+
+ ~GlSlProgram();
+
+ bool AddShader(
+ GlSlShaderType shader_type,
+ const std::string& source_code,
+ const std::map<std::string,std::string>& program_defines = std::map<std::string,std::string>(),
+ const std::vector<std::string>& search_path = std::vector<std::string>()
+ );
+
+ bool AddShaderFromFile(
+ GlSlShaderType shader_type,
+ const std::string& filename,
+ const std::map<std::string,std::string>& program_defines = std::map<std::string,std::string>(),
+ const std::vector<std::string>& search_path = std::vector<std::string>()
+ );
+
+ bool Link();
+
+ // Remove all shaders from this program, and reload from files.
+ bool ReloadShaderFiles();
+
+ GLint GetAttributeHandle(const std::string& name);
+ GLint GetUniformHandle(const std::string& name);
+
+ // Before setting uniforms, be sure to Bind() the GlSl program first.
+ void SetUniform(const std::string& name, int x);
+ void SetUniform(const std::string& name, int x1, int x2);
+ void SetUniform(const std::string& name, int x1, int x2, int x3);
+ void SetUniform(const std::string& name, int x1, int x2, int x3, int x4);
+
+ void SetUniform(const std::string& name, float f);
+ void SetUniform(const std::string& name, float f1, float f2);
+ void SetUniform(const std::string& name, float f1, float f2, float f3);
+ void SetUniform(const std::string& name, float f1, float f2, float f3, float f4);
+
+ void SetUniform(const std::string& name, Colour c);
+
+ void SetUniform(const std::string& name, const OpenGlMatrix& m);
+
+#ifdef HAVE_EIGEN
+ void SetUniform(const std::string& name, const Eigen::Matrix3f& m);
+ void SetUniform(const std::string& name, const Eigen::Matrix4f& m);
+ void SetUniform(const std::string& name, const Eigen::Matrix3d& m);
+ void SetUniform(const std::string& name, const Eigen::Matrix4d& m);
+#endif
+
+#if GL_VERSION_4_3
+ GLint GetProgramResourceIndex(const std::string& name);
+ void SetShaderStorageBlock(const std::string& name, const int& bindingIndex);
+#endif
+
+ void Bind();
+ void SaveBind();
+ void Unbind();
+
+
+ void BindPangolinDefaultAttribLocationsAndLink();
+
+ // Unlink all shaders from program
+ void ClearShaders();
+
+ GLint ProgramId() const {
+ return prog;
+ }
+
+ bool Valid() const {
+ return ProgramId() != 0;
+ }
+
+protected:
+ struct ShaderFileOrCode
+ {
+ GlSlShaderType shader_type;
+ std::string filename;
+ std::string code;
+ std::map<std::string,std::string> program_defines;
+ std::vector<std::string> search_path;
+ };
+
+
+ // Convenience method to load shader description
+ bool AddShaderFile(const ShaderFileOrCode &shader_file);
+
+ std::string ParseIncludeFilename(
+ const std::string& location
+ );
+
+ std::string SearchIncludePath(
+ const std::string& filename,
+ const std::vector<std::string>& search_path,
+ const std::string& current_path
+ );
+
+ bool AddPreprocessedShader(
+ GlSlShaderType shader_type,
+ const std::string& source_code,
+ const std::string& name_for_errors
+ );
+
+ void PreprocessGLSL(
+ std::istream& input,
+ std::ostream& output,
+ const std::map<std::string,std::string>& program_defines,
+ const std::vector<std::string>& search_path,
+ const std::string& current_path
+ );
+
+ // Split 'code' into several code blocks per shader type
+ // shader blocks in 'code' must be annotated with:
+ // @start vertex, @start fragment, @start geometry or @start compute
+ static std::map<GlSlShaderType,std::string>
+ SplitAnnotatedShaders(const std::string& code);
+
+ bool linked;
+ std::vector<GLhandleARB> shaders;
+ GLenum prog;
+ GLint prev_prog;
+ std::vector<ShaderFileOrCode> shader_files;
+};
+
+class GlSlUtilities
+{
+public:
+ inline static GlSlProgram& OffsetAndScale(float offset, float scale) {
+ GlSlProgram& prog = Instance().prog_offsetscale;
+ prog.Bind();
+ prog.SetUniform("offset", offset);
+ prog.SetUniform("scale", scale);
+ return prog;
+ }
+
+ inline static GlSlProgram& Scale(float scale, float bias = 0.0f) {
+ GlSlProgram& prog = Instance().prog_scale;
+ prog.Bind();
+ prog.SetUniform("scale", scale);
+ prog.SetUniform("bias", bias);
+ return prog;
+ }
+
+ inline static void UseNone()
+ {
+ glUseProgram(0);
+ }
+
+protected:
+ static GlSlUtilities& Instance() {
+ // TODO: BUG: The GlSLUtilities instance needs to be tied
+ // to the OpenGL context, not the thread, or globally.
+#ifndef PANGO_NO_THREADLOCAL
+ thread_local
+#else
+ static
+#endif
+ GlSlUtilities instance;
+ return instance;
+ }
+
+ // protected constructor
+ GlSlUtilities() {
+ const char* source_scale =
+ "uniform float scale;"
+ "uniform float bias;"
+ "uniform sampler2D tex;"
+ "void main() {"
+ " vec2 uv = gl_TexCoord[0].st;"
+ " if(0.0 <= uv.x && uv.x <= 1.0 && 0.0 <= uv.y && uv.y <= 1.0) {"
+ " gl_FragColor = texture2D(tex,uv);"
+ " gl_FragColor.xyz *= scale;"
+ " gl_FragColor.xyz += vec3(bias,bias,bias);"
+ " }else{"
+ " float v = 0.1;"
+ " gl_FragColor.xyz = vec3(v,v,v);"
+ " }"
+ "}";
+ prog_scale.AddShader(GlSlFragmentShader, source_scale);
+ prog_scale.Link();
+
+ const char* source_offsetscale =
+ "uniform float offset;"
+ "uniform float scale;"
+ "uniform sampler2D tex;"
+ "void main() {"
+ " vec2 uv = gl_TexCoord[0].st;"
+ " if(0.0 <= uv.x && uv.x <= 1.0 && 0.0 <= uv.y && uv.y <= 1.0) {"
+ " gl_FragColor = texture2D(tex,gl_TexCoord[0].st);"
+ " gl_FragColor.xyz += vec3(offset,offset,offset);"
+ " gl_FragColor.xyz *= scale;"
+ " }else{"
+ " float v = 0.1;"
+ " gl_FragColor.xyz = vec3(v,v,v);"
+ " }"
+ "}";
+ prog_offsetscale.AddShader(GlSlFragmentShader, source_offsetscale);
+ prog_offsetscale.Link();
+ }
+
+ GlSlProgram prog_scale;
+ GlSlProgram prog_offsetscale;
+};
+
+
+////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////
+
+inline bool IsLinkSuccessPrintLog(GLhandleARB prog)
+{
+ GLint status;
+ glGetProgramiv(prog, GL_LINK_STATUS, &status);
+ if(status != GL_TRUE) {
+ pango_print_error("GLSL Program link failed: ");
+ const int PROGRAM_LOG_MAX_LEN = 10240;
+ char infolog[PROGRAM_LOG_MAX_LEN];
+ GLsizei len;
+ glGetProgramInfoLog(prog, PROGRAM_LOG_MAX_LEN, &len, infolog);
+ if(len) {
+ pango_print_error("%s\n",infolog);
+ }else{
+ pango_print_error("No details provided.\n");
+ }
+ return false;
+ }
+ return true;
+}
+
+inline bool IsCompileSuccessPrintLog(GLhandleARB shader, const std::string& name_for_errors)
+{
+ GLint status;
+ glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
+ if(status != GL_TRUE) {
+ pango_print_error("GLSL Shader compilation failed: ");
+ const int SHADER_LOG_MAX_LEN = 10240;
+ char infolog[SHADER_LOG_MAX_LEN];
+ GLsizei len;
+ glGetShaderInfoLog(shader, SHADER_LOG_MAX_LEN, &len, infolog);
+ if(len) {
+ pango_print_error("%s:\n%s\n",name_for_errors.c_str(), infolog);
+ }else{
+ pango_print_error("%s:\nNo details provided.\n",name_for_errors.c_str());
+ }
+ return false;
+ }
+ return true;
+}
+
+inline GlSlProgram::GlSlProgram()
+ : linked(false), prog(0), prev_prog(0)
+{
+}
+
+//! Move Constructor
+inline GlSlProgram::GlSlProgram(GlSlProgram&& o)
+ : linked(o.linked), shaders(o.shaders), prog(o.prog), prev_prog(o.prev_prog)
+{
+ o.prog = 0;
+}
+
+inline GlSlProgram::~GlSlProgram()
+{
+ if(prog) {
+ ClearShaders();
+ glDeleteProgram(prog);
+ }
+}
+
+inline void PrintSourceCode(const std::string& src)
+{
+ std::stringstream ss(src);
+ std::string line;
+
+ for(int linenum=1; std::getline(ss,line,'\n'); ++linenum) {
+ std::cout << linenum << ":\t" << line << std::endl;
+ }
+}
+
+inline bool GlSlProgram::AddPreprocessedShader(
+ GlSlShaderType shader_type,
+ const std::string& source_code,
+ const std::string& name_for_errors
+) {
+ if(!prog) {
+ prog = glCreateProgram();
+ }
+
+// PrintSourceCode(source_code);
+
+ GLhandleARB shader = glCreateShader(shader_type);
+ const char* source = source_code.c_str();
+ glShaderSource(shader, 1, &source, NULL);
+ glCompileShader(shader);
+ bool success = IsCompileSuccessPrintLog(shader, name_for_errors);
+ if(success) {
+ glAttachShader(prog, shader);
+ shaders.push_back(shader);
+ linked = false;
+ }
+ return success;
+}
+
+inline std::string GlSlProgram::ParseIncludeFilename(const std::string& location)
+{
+ size_t start = location.find_first_of("\"<");
+ if(start != std::string::npos) {
+ size_t end = location.find_first_of("\">", start+1);
+ if(end != std::string::npos) {
+ return location.substr(start+1, end - start - 1);
+ }
+ }
+ throw std::runtime_error("GLSL Parser: Unable to parse include location " + location );
+}
+
+inline std::string GlSlProgram::SearchIncludePath(
+ const std::string& filename,
+ const std::vector<std::string>& search_path,
+ const std::string& current_path
+) {
+ if(FileExists(current_path + "/" + filename)) {
+ return current_path + "/" + filename;
+ }else{
+ for(size_t i=0; i < search_path.size(); ++i) {
+ const std::string hypoth = search_path[i] + "/" + filename;
+ if( FileExists(hypoth) ) {
+ return hypoth;
+ }
+ }
+ }
+ return "";
+}
+
+inline void GlSlProgram::PreprocessGLSL(
+ std::istream& input, std::ostream& output,
+ const std::map<std::string,std::string>& program_defines,
+ const std::vector<std::string> &search_path,
+ const std::string ¤t_path
+) {
+ const size_t MAXLINESIZE = 10240;
+ char line[MAXLINESIZE];
+
+ while(!input.eof()) {
+ // Take like from source
+ input.getline(line,MAXLINESIZE);
+
+ // Transform
+ if( !strncmp(line, "#include", 8 ) ) {
+ // C++ / G3D style include directive
+ const std::string import_file = ParseIncludeFilename(line+8);
+ const std::string resolved_file = SearchIncludePath(import_file, search_path, current_path);
+
+ std::ifstream ifs(resolved_file.c_str());
+ if(ifs.good()) {
+ const std::string file_path = pangolin::PathParent(resolved_file);
+ PreprocessGLSL(ifs, output, program_defines, search_path, file_path);
+ }else{
+ throw std::runtime_error("GLSL Parser: Unable to open " + import_file );
+ }
+ }else if( !strncmp(line, "#expect", 7) ) {
+ // G3D style 'expect' directive, annotating expected preprocessor
+ // definition with document string
+
+ // Consume whitespace before token
+ size_t token_start = 7;
+ while( std::isspace(line[token_start]) ) ++token_start;
+
+ // Iterate over contigous charecters until \0 or whitespace
+ size_t token_end = token_start;
+ while( line[token_end] && !std::isspace(line[token_end]) ) ++token_end;
+
+ std::string token(line+token_start, line+token_end);
+ std::map<std::string,std::string>::const_iterator it = program_defines.find(token);
+ if( it == program_defines.end() ) {
+ pango_print_warn("Expected define missing (defaulting to 0): '%s'\n%s\n", token.c_str(), line + token_end );
+ output << "#define " << token << " 0" << std::endl;
+ }else{
+ output << "#define " << token << " " << it->second << std::endl;
+ }
+ }else{
+ // Output directly
+ output << line << std::endl;
+ }
+ }
+}
+
+inline void GlSlProgram::ClearShaders()
+{
+ // Remove and delete each shader
+ for(size_t i=0; i<shaders.size(); ++i ) {
+ glDetachShader(prog, shaders[i]);
+ glDeleteShader(shaders[i]);
+ }
+ shaders.clear();
+}
+
+inline bool GlSlProgram::AddShaderFile(const ShaderFileOrCode& shader_file)
+{
+ std::stringstream buffer;
+
+ if(shader_file.code.empty()) {
+ std::ifstream ifs(shader_file.filename.c_str());
+ if(ifs.is_open()) {
+ PreprocessGLSL(ifs, buffer, shader_file.program_defines, shader_file.search_path, ".");
+ }else{
+ throw std::runtime_error(FormatString("Unable to open shader file '%'", shader_file.filename));
+ }
+ }else{
+ std::istringstream iss(shader_file.code);
+ PreprocessGLSL(iss, buffer, shader_file.program_defines, shader_file.search_path, ".");
+ }
+
+ const std::string code = buffer.str();
+ const std::string input_name = !shader_file.filename.empty() ? shader_file.filename : "<string>";
+
+ if(shader_file.shader_type == GlSlAnnotatedShader) {
+ const std::map<GlSlShaderType,std::string> split_progs = SplitAnnotatedShaders(code);
+ for(const auto& type_code : split_progs) {
+ if(!AddPreprocessedShader(type_code.first, type_code.second, input_name )) {
+ return false;
+ }
+ }
+ return true;
+ }else{
+ return AddPreprocessedShader(shader_file.shader_type, code, input_name);
+ }
+}
+
+inline bool GlSlProgram::AddShaderFromFile(
+ GlSlShaderType shader_type,
+ const std::string& filename,
+ const std::map<std::string,std::string>& program_defines,
+ const std::vector<std::string>& search_path
+) {
+ ShaderFileOrCode shader_file = {
+ shader_type,
+ pangolin::PathExpand(filename),
+ std::string(),
+ program_defines,
+ search_path
+ };
+ shader_files.push_back(shader_file);
+ return AddShaderFile(shader_file);
+}
+
+inline bool GlSlProgram::AddShader(
+ GlSlShaderType shader_type,
+ const std::string& source_code,
+ const std::map<std::string,std::string>& program_defines,
+ const std::vector<std::string>& search_path
+) {
+ ShaderFileOrCode shader_file = {
+ shader_type,
+ std::string(),
+ source_code,
+ program_defines,
+ search_path
+ };
+ shader_files.push_back(shader_file);
+ return AddShaderFile(shader_file);
+}
+
+inline bool GlSlProgram::ReloadShaderFiles()
+{
+ ClearShaders();
+
+ for(const auto& sf : shader_files) {
+ if(!AddShaderFile(sf)) {
+ return false;
+ }
+ }
+
+ Link();
+ return true;
+}
+
+inline std::map<GlSlShaderType,std::string>
+GlSlProgram::SplitAnnotatedShaders(const std::string& code)
+{
+ std::map<GlSlShaderType,std::string> ret;
+
+ std::stringstream input(code);
+ std::stringstream output;
+
+ const size_t MAXLINESIZE = 10240;
+ char line[MAXLINESIZE];
+
+ GlSlShaderType current_type = GlSlAnnotatedShader;
+ auto finish_block = [&](GlSlShaderType type){
+ if(current_type != GlSlAnnotatedShader) {
+ ret[current_type] = output.str();
+ }
+ output.str(std::string());
+ current_type = type;
+ };
+
+ while(!input.eof()) {
+ // Take like from source
+ input.getline(line,MAXLINESIZE);
+
+ // Transform
+ if( !strncmp(line, "@start", 6 ) ) {
+ const std::string str_shader_type = pangolin::Trim(std::string(line).substr(6));
+ if(str_shader_type == "vertex") {
+ finish_block(GlSlVertexShader);
+ }else if(str_shader_type == "fragment") {
+ finish_block(GlSlFragmentShader);
+ }else if(str_shader_type == "geometry") {
+ finish_block(GlSlGeometryShader);
+ }else if(str_shader_type == "compute") {
+ finish_block(GlSlComputeShader);
+ }
+ }else{
+ output << line << std::endl;
+ }
+ }
+
+ finish_block(GlSlAnnotatedShader);
+
+ return ret;
+}
+
+inline bool GlSlProgram::Link()
+{
+ glLinkProgram(prog);
+ return IsLinkSuccessPrintLog(prog);
+}
+
+inline void GlSlProgram::Bind()
+{
+ prev_prog = 0;
+ glUseProgram(prog);
+}
+
+inline void GlSlProgram::SaveBind()
+{
+ glGetIntegerv(GL_CURRENT_PROGRAM, &prev_prog);
+ glUseProgram(prog);
+}
+
+inline void GlSlProgram::Unbind()
+{
+ glUseProgram(prev_prog);
+}
+
+inline GLint GlSlProgram::GetAttributeHandle(const std::string& name)
+{
+ return glGetAttribLocation(prog, name.c_str());
+}
+
+inline GLint GlSlProgram::GetUniformHandle(const std::string& name)
+{
+ return glGetUniformLocation(prog, name.c_str());
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, int x)
+{
+ glUniform1i( GetUniformHandle(name), x);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, int x1, int x2)
+{
+ glUniform2i( GetUniformHandle(name), x1, x2);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, int x1, int x2, int x3)
+{
+ glUniform3i( GetUniformHandle(name), x1, x2, x3);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, int x1, int x2, int x3, int x4)
+{
+ glUniform4i( GetUniformHandle(name), x1, x2, x3, x4);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, float f)
+{
+ glUniform1f( GetUniformHandle(name), f);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, float f1, float f2)
+{
+ glUniform2f( GetUniformHandle(name), f1,f2);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, float f1, float f2, float f3)
+{
+ glUniform3f( GetUniformHandle(name), f1,f2,f3);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, float f1, float f2, float f3, float f4)
+{
+ glUniform4f( GetUniformHandle(name), f1,f2,f3,f4);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, Colour c)
+{
+ glUniform4f( GetUniformHandle(name), c.r, c.g, c.b, c.a);
+}
+
+inline void GlSlProgram::SetUniform(const std::string& name, const OpenGlMatrix& mat)
+{
+ // glUniformMatrix4dv seems to be crashing...
+ float m[16];
+ for (int i = 0; i < 16; ++i) {
+ m[i] = (float)mat.m[i];
+ }
+ glUniformMatrix4fv( GetUniformHandle(name), 1, GL_FALSE, m);
+}
+
+#ifdef HAVE_EIGEN
+inline void GlSlProgram::SetUniform(const std::string& name, const Eigen::Matrix3f& m)
+{
+ glUniformMatrix3fv( GetUniformHandle(name), 1, GL_FALSE, m.data());
+}
+inline void GlSlProgram::SetUniform(const std::string& name, const Eigen::Matrix4f& m)
+{
+ glUniformMatrix4fv( GetUniformHandle(name), 1, GL_FALSE, m.data());
+}
+inline void GlSlProgram::SetUniform(const std::string& name, const Eigen::Matrix3d& m)
+{
+ glUniformMatrix3dv( GetUniformHandle(name), 1, GL_FALSE, m.data());
+}
+inline void GlSlProgram::SetUniform(const std::string& name, const Eigen::Matrix4d& m)
+{
+ glUniformMatrix4dv( GetUniformHandle(name), 1, GL_FALSE, m.data());
+}
+#endif
+
+inline void GlSlProgram::BindPangolinDefaultAttribLocationsAndLink()
+{
+ glBindAttribLocation(prog, DEFAULT_LOCATION_POSITION, DEFAULT_NAME_POSITION);
+ glBindAttribLocation(prog, DEFAULT_LOCATION_COLOUR, DEFAULT_NAME_COLOUR);
+ glBindAttribLocation(prog, DEFAULT_LOCATION_NORMAL, DEFAULT_NAME_NORMAL);
+ glBindAttribLocation(prog, DEFAULT_LOCATION_TEXCOORD, DEFAULT_NAME_TEXCOORD);
+ Link();
+}
+
+#if GL_VERSION_4_3
+inline GLint GlSlProgram::GetProgramResourceIndex(const std::string& name)
+{
+ return glGetProgramResourceIndex(prog, GL_SHADER_STORAGE_BLOCK, name.c_str());
+}
+
+inline void GlSlProgram::SetShaderStorageBlock(const std::string& name, const int& bindingIndex)
+{
+ glShaderStorageBlockBinding(prog, GetProgramResourceIndex(name), bindingIndex);
+}
+#endif
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glstate.h b/externals/Pangolin/include/pangolin/gl/glstate.h
new file mode 100644
index 0000000000000000000000000000000000000000..0757ba22d48d5bc4bab9e67cd4a6c081f2e51ed2
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glstate.h
@@ -0,0 +1,220 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Vincent Mamo, Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glinclude.h>
+#include <stack>
+
+namespace pangolin
+{
+
+class GlState {
+
+ class CapabilityState {
+ public:
+
+ CapabilityState(GLenum cap, GLboolean enable)
+ : m_cap(cap), m_enable(enable)
+ {
+
+ }
+
+ void Apply() {
+ if(m_enable) {
+ ::glEnable(m_cap);
+ }else{
+ ::glDisable(m_cap);
+ }
+ }
+
+ void UnApply() {
+ if(m_enable) {
+ ::glDisable(m_cap);
+ }else{
+ ::glEnable(m_cap);
+ }
+ }
+
+ protected:
+ GLenum m_cap;
+ GLboolean m_enable;
+ };
+
+public:
+ GlState()
+ : m_DepthMaskCalled(false),
+ m_ShadeModelCalled(false),
+ m_CullFaceCalled(false),
+ m_PointSizeCalled(false),
+ m_LineWidthCalled(false),
+ m_ColorMaskCalled(false),
+ m_ViewportCalled(false)
+ {
+ }
+
+ ~GlState() {
+ // Restore original state
+ while (!m_history.empty()) {
+ m_history.top().UnApply();
+ m_history.pop();
+ }
+
+ if (m_DepthMaskCalled) {
+ ::glDepthMask(m_OriginalDepthMask);
+ }
+
+ if (m_ShadeModelCalled) {
+ ::glShadeModel(m_OriginalShadeModel);
+ }
+
+ if (m_CullFaceCalled) {
+ ::glCullFace(m_OriginalCullFace);
+ }
+
+ if(m_PointSizeCalled) {
+ ::glPointSize(m_OriginalPointSize);
+ }
+
+ if(m_LineWidthCalled) {
+ ::glLineWidth(m_OriginalLineWidth);
+ }
+
+ if (m_ColorMaskCalled) {
+ ::glColorMask(m_OriginalColorMask[0], m_OriginalColorMask[1], m_OriginalColorMask[2], m_OriginalColorMask[3]);
+ }
+
+ if (m_ViewportCalled) {
+ ::glViewport(m_OriginalViewport[0], m_OriginalViewport[1], m_OriginalViewport[2], m_OriginalViewport[3]);
+ }
+ }
+
+ static inline GLboolean IsEnabled(GLenum cap)
+ {
+ GLboolean curVal;
+ glGetBooleanv(cap, &curVal);
+ return curVal;
+ }
+
+ inline void glEnable(GLenum cap)
+ {
+ if(!IsEnabled(cap)) {
+ m_history.push(CapabilityState(cap,true));
+ ::glEnable(cap);
+ }
+ }
+
+ inline void glDisable(GLenum cap)
+ {
+ if(IsEnabled(cap)) {
+ m_history.push(CapabilityState(cap,false));
+ ::glDisable(cap);
+ }
+ }
+
+ bool m_DepthMaskCalled;
+ GLboolean m_OriginalDepthMask;
+ inline void glDepthMask(GLboolean flag)
+ {
+ if(!m_DepthMaskCalled) {
+ m_DepthMaskCalled = true;
+ glGetBooleanv(GL_DEPTH_WRITEMASK, &m_OriginalDepthMask);
+ }
+ ::glDepthMask(flag);
+ }
+
+ bool m_ShadeModelCalled;
+ GLint m_OriginalShadeModel;
+ inline void glShadeModel(GLint mode)
+ {
+ if(!m_ShadeModelCalled) {
+ m_ShadeModelCalled = true;
+ glGetIntegerv(GL_SHADE_MODEL, &m_OriginalShadeModel);
+ }
+ ::glShadeModel(mode);
+ }
+
+ bool m_CullFaceCalled;
+ GLint m_OriginalCullFace;
+ void glCullFace(GLenum mode)
+ {
+ if(!m_ShadeModelCalled) {
+ m_ShadeModelCalled = true;
+ glGetIntegerv(GL_CULL_FACE_MODE, &m_OriginalCullFace);
+ }
+ ::glCullFace(mode);
+ }
+
+ bool m_PointSizeCalled;
+ GLfloat m_OriginalPointSize;
+ void glPointSize(GLfloat size)
+ {
+ if(!m_PointSizeCalled) {
+ m_PointSizeCalled = true;
+ glGetFloatv(GL_POINT_SIZE, &m_OriginalPointSize);
+ }
+ ::glPointSize(size);
+ }
+
+ bool m_LineWidthCalled;
+ GLfloat m_OriginalLineWidth;
+ void glLineWidth(GLfloat width)
+ {
+ if(!m_LineWidthCalled) {
+ m_LineWidthCalled = true;
+ glGetFloatv(GL_LINE_WIDTH, &m_OriginalLineWidth);
+ }
+ ::glLineWidth(width);
+
+ }
+
+ bool m_ColorMaskCalled;
+ GLboolean m_OriginalColorMask[4];
+ inline void glColorMask(GLboolean red, GLboolean green, GLboolean blue, GLboolean alpha)
+ {
+ if(!m_ColorMaskCalled) {
+ m_ColorMaskCalled = true;
+ glGetBooleanv(GL_COLOR_WRITEMASK, m_OriginalColorMask);
+ }
+ ::glColorMask(red, green, blue, alpha);
+ }
+
+ bool m_ViewportCalled;
+ GLint m_OriginalViewport[4];
+ inline void glViewport(GLint x, GLint y, GLsizei width, GLsizei height)
+ {
+ if(!m_ViewportCalled) {
+ m_ViewportCalled = true;
+ glGetIntegerv(GL_VIEWPORT, m_OriginalViewport);
+ }
+ ::glViewport(x, y, width, height);
+ }
+
+ std::stack<CapabilityState> m_history;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/gltext.h b/externals/Pangolin/include/pangolin/gl/gltext.h
new file mode 100644
index 0000000000000000000000000000000000000000..1c8f0b278eb693a676fa06fcd6a81dbd5c6dc35d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/gltext.h
@@ -0,0 +1,98 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glchar.h>
+
+#include <vector>
+#include <string>
+
+namespace pangolin {
+
+class PANGOLIN_EXPORT GlText
+{
+public:
+ GlText();
+
+ GlText(const GlText& txt);
+
+ GlText(const GlTexture& font_tex);
+
+ void AddSpace(GLfloat s);
+
+ // Add specified charector to this string.
+ void Add(unsigned char c, const GlChar& glc);
+
+ // Clear text
+ void Clear();
+
+ // Render without transform in text-centric pixel coordinates
+ void Draw() const;
+ void DrawGlSl() const;
+
+#ifdef BUILD_PANGOLIN_GUI
+ // Render at (x,y,z)' in object coordinates,
+ // keeping text size and orientation constant
+ void Draw(GLfloat x, GLfloat y, GLfloat z = 0.0f) const;
+
+ // Render at (x,y,z)' in window coordinates.
+ void DrawWindow(GLfloat x, GLfloat y, GLfloat z = 0.0f) const;
+#endif // BUILD_PANGOLIN_GUI
+
+ // Return text that this object signifies.
+ const std::string& Text() const {
+ return str;
+ }
+
+ // Return width in pixels of this text.
+ GLfloat Width() const {
+ return width;
+ }
+
+ // Return height in pixels of this text.
+ GLfloat Height() const {
+ return ymax;
+ }
+
+ // Return height in pixels of this text, including under baseline
+ GLfloat FullHeight() const {
+ return ymax - ymin;
+ }
+
+//protected:
+ const GlTexture* tex;
+ std::string str;
+ GLfloat width;
+ GLfloat ymin;
+ GLfloat ymax;
+
+ std::vector<XYUV> vs;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/gltexturecache.h b/externals/Pangolin/include/pangolin/gl/gltexturecache.h
new file mode 100644
index 0000000000000000000000000000000000000000..f3ad30307cae9dd0aacbf94db6a87605bf9d2905
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/gltexturecache.h
@@ -0,0 +1,116 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glformattraits.h>
+#include <pangolin/gl/glpixformat.h>
+#include <pangolin/image/image.h>
+
+#include <memory>
+#include <map>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT TextureCache
+{
+public:
+ static TextureCache& I();
+
+ GlTexture& GlTex(GLsizei w, GLsizei h, GLint internal_format, GLint glformat, GLenum gltype)
+ {
+ const long key =
+ (((long)internal_format)<<20) ^
+ (((long)glformat)<<10) ^ gltype;
+
+ // Lookup texture
+ std::shared_ptr<GlTexture>& ptex = texture_map[key];
+ if(!ptex) {
+ ptex = std::shared_ptr<GlTexture>(new GlTexture());
+ }
+ GlTexture& tex = *ptex;
+
+ // Initialise if it didn't already exist or the size was too small
+ if(!tex.tid || tex.width < w || tex.height < h) {
+ tex.Reinitialise(
+ std::max(tex.width,w), std::max(tex.height,h),
+ internal_format, default_sampling_linear, 0,
+ glformat, gltype
+ );
+ }
+
+ return tex;
+ }
+
+ template<typename T>
+ GlTexture& GlTex(GLsizei w, GLsizei h)
+ {
+ return GlTex( w, h,
+ GlFormatTraits<T>::glinternalformat,
+ GlFormatTraits<T>::glformat,
+ GlFormatTraits<T>::gltype
+ );
+ }
+
+protected:
+ bool default_sampling_linear;
+ std::map<long, std::shared_ptr<GlTexture> > texture_map;
+
+ // Protected constructor
+ TextureCache()
+ : default_sampling_linear(true)
+ {
+ }
+};
+
+template<typename T>
+void RenderToViewport(Image<T>& image, bool flipx=false, bool flipy=false)
+{
+ // Retrieve texture that is at least as large as image and of appropriate type.
+ GlTexture& tex = TextureCache::I().GlTex<T>(image.w, image.h);
+ tex.Upload(image.ptr,0,0, image.w, image.h, GlFormatTraits<T>::glformat, GlFormatTraits<T>::gltype);
+ tex.RenderToViewport(Viewport(0,0,image.w, image.h), flipx, flipy);
+}
+
+// This method may dissapear in the future
+inline void RenderToViewport(
+ Image<unsigned char>& image,
+ const pangolin::GlPixFormat& fmt,
+ bool flipx=false, bool flipy=false,
+ bool linear_sampling = true
+) {
+ pangolin::GlTexture& tex = pangolin::TextureCache::I().GlTex((GLsizei)image.w, (GLsizei)image.h, fmt.scalable_internal_format, fmt.glformat, fmt.gltype);
+ tex.Bind();
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, linear_sampling ? GL_LINEAR : GL_NEAREST);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, linear_sampling ? GL_LINEAR : GL_NEAREST);
+ tex.Upload(image.ptr,0,0, (GLsizei)image.w, (GLsizei)image.h, fmt.glformat, fmt.gltype);
+ tex.RenderToViewport(pangolin::Viewport(0,0,(GLint)image.w, (GLint)image.h), flipx, flipy);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/gl/glvbo.h b/externals/Pangolin/include/pangolin/gl/glvbo.h
new file mode 100644
index 0000000000000000000000000000000000000000..546f6d594acd985aa8064034843407683316645d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/gl/glvbo.h
@@ -0,0 +1,225 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/gl.h>
+
+namespace pangolin
+{
+
+////////////////////////////////////////////////
+// Interface
+////////////////////////////////////////////////
+
+void MakeTriangleStripIboForVbo(GlBuffer& ibo, int w, int h);
+
+GlBuffer MakeTriangleStripIboForVbo(int w, int h);
+
+void RenderVbo(GlBuffer& vbo, GLenum mode = GL_POINTS);
+
+void RenderVboCbo(GlBuffer& vbo, GlBuffer& cbo, bool draw_color = true, GLenum mode = GL_POINTS);
+
+void RenderVboIbo(GlBuffer& vbo, GlBuffer& ibo, bool draw_mesh = true);
+
+void RenderVboIboCbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& cbo, bool draw_mesh = true, bool draw_color = true);
+
+void RenderVboIboNbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& nbo, bool draw_mesh = true, bool draw_normals = true);
+
+void RenderVboIboCboNbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& cbo, GlBuffer& nbo, bool draw_mesh = true, bool draw_color = true, bool draw_normals = true);
+
+////////////////////////////////////////////////
+// Implementation
+////////////////////////////////////////////////
+
+inline void MakeTriangleStripIboForVbo(GlBuffer& ibo, int w, int h)
+{
+ const int num_elements = w*(h-1)*2;
+ unsigned int* buffer = new unsigned int[num_elements];
+ unsigned int* ptr = buffer;
+
+ for(int y=0; y < (h-1);)
+ {
+ for(int x=0; x<w; ++x) {
+ (*ptr++) = y*w+x;
+ (*ptr++) = (y+1)*w+x;
+ }
+ ++y;
+
+ if(y>=(h-1)) break;
+ for(int x=w-1; x>=0; --x) {
+ (*ptr++) = y*w+x;
+ (*ptr++) = (y+1)*w+x;
+ }
+ ++y;
+ }
+
+ ibo.Reinitialise(GlElementArrayBuffer, num_elements, GL_UNSIGNED_INT, 1, GL_STATIC_DRAW );
+ ibo.Upload(buffer, sizeof(unsigned int)*num_elements );
+
+ delete[] buffer;
+}
+
+inline GlBuffer MakeTriangleStripIboForVbo(int w, int h)
+{
+ GlBuffer ibo;
+ MakeTriangleStripIboForVbo(ibo,w,h);
+ return ibo;
+}
+
+inline void RenderVbo(GlBuffer& vbo, GLenum mode)
+{
+ vbo.Bind();
+ glVertexPointer(vbo.count_per_element, vbo.datatype, 0, 0);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ glDrawArrays(mode, 0, vbo.num_elements);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ vbo.Unbind();
+}
+
+inline void RenderVboCbo(GlBuffer& vbo, GlBuffer& cbo, bool draw_color, GLenum mode )
+{
+ if(draw_color) {
+ cbo.Bind();
+ glColorPointer(cbo.count_per_element, cbo.datatype, 0, 0);
+ glEnableClientState(GL_COLOR_ARRAY);
+ }
+
+ RenderVbo(vbo,mode);
+
+ if(draw_color) {
+ glDisableClientState(GL_COLOR_ARRAY);
+ cbo.Unbind();
+ }
+}
+
+inline void RenderVboIbo(GlBuffer& vbo, GlBuffer& ibo, bool draw_mesh)
+{
+ vbo.Bind();
+ glVertexPointer(vbo.count_per_element, vbo.datatype, 0, 0);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ if(draw_mesh) {
+ ibo.Bind();
+ glDrawElements(GL_TRIANGLE_STRIP,ibo.num_elements, ibo.datatype, 0);
+ ibo.Unbind();
+ }else{
+ glDrawArrays(GL_POINTS, 0, vbo.num_elements);
+ }
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ vbo.Unbind();
+}
+
+inline void RenderVboIboCbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& cbo, bool draw_mesh, bool draw_color )
+{
+ if(draw_color) {
+ cbo.Bind();
+ glColorPointer(cbo.count_per_element, cbo.datatype, 0, 0);
+ glEnableClientState(GL_COLOR_ARRAY);
+ }
+
+ RenderVboIbo(vbo,ibo,draw_mesh);
+
+ if(draw_color) {
+ glDisableClientState(GL_COLOR_ARRAY);
+ cbo.Unbind();
+ }
+}
+
+inline void RenderVboIboCboNbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& cbo, GlBuffer& nbo, bool draw_mesh, bool draw_color, bool draw_normals)
+{
+ if(draw_color) {
+ cbo.Bind();
+ glColorPointer(cbo.count_per_element, cbo.datatype, 0, 0);
+ glEnableClientState(GL_COLOR_ARRAY);
+ }
+
+ if(draw_normals) {
+ nbo.Bind();
+ glNormalPointer(nbo.datatype, (GLsizei)(nbo.count_per_element * GlDataTypeBytes(nbo.datatype)),0);
+ glEnableClientState(GL_NORMAL_ARRAY);
+ }
+
+ vbo.Bind();
+ glVertexPointer(vbo.count_per_element, vbo.datatype, 0, 0);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ if(draw_mesh) {
+ ibo.Bind();
+ glDrawElements(GL_TRIANGLE_STRIP,ibo.num_elements, ibo.datatype, 0);
+ ibo.Unbind();
+ }else{
+ glDrawArrays(GL_POINTS, 0, vbo.num_elements);
+ }
+
+ if(draw_color) {
+ glDisableClientState(GL_COLOR_ARRAY);
+ cbo.Unbind();
+ }
+
+ if(draw_normals) {
+ glDisableClientState(GL_NORMAL_ARRAY);
+ nbo.Unbind();
+ }
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ vbo.Unbind();
+}
+
+inline void RenderVboIboNbo(GlBuffer& vbo, GlBuffer& ibo, GlBuffer& nbo, bool draw_mesh, bool draw_normals)
+{
+ vbo.Bind();
+ glVertexPointer(vbo.count_per_element, vbo.datatype, 0, 0);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ if(draw_normals) {
+ nbo.Bind();
+ glNormalPointer(nbo.datatype, (GLsizei)(nbo.count_per_element * GlDataTypeBytes(nbo.datatype)), 0);
+ glEnableClientState(GL_NORMAL_ARRAY);
+ }
+
+ if(draw_mesh) {
+ ibo.Bind();
+ glDrawElements(GL_TRIANGLE_STRIP,ibo.num_elements, ibo.datatype, 0);
+ ibo.Unbind();
+ }else{
+ glDrawArrays(GL_POINTS, 0, vbo.num_elements);
+ }
+
+ if(draw_normals) {
+ glDisableClientState(GL_NORMAL_ARRAY);
+ nbo.Unbind();
+ }
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ vbo.Unbind();
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/handler/handler.h b/externals/Pangolin/include/pangolin/handler/handler.h
new file mode 100644
index 0000000000000000000000000000000000000000..4e1b3267f17bcec796c7035609d2ac747c28acac
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/handler/handler.h
@@ -0,0 +1,116 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/handler/handler_enums.h>
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#endif
+
+#ifdef _OSX_
+#define PANGO_DFLT_HANDLER3D_ZF (1.0f/50.0f)
+#else
+#define PANGO_DFLT_HANDLER3D_ZF (1.0f/10.0f)
+#endif
+
+namespace pangolin
+{
+
+// Forward declarations
+struct View;
+
+/// Input Handler base class.
+/// Virtual methods which recurse into sub-displays.
+struct PANGOLIN_EXPORT Handler
+{
+ virtual ~Handler() {}
+ virtual void Keyboard(View&, unsigned char key, int x, int y, bool pressed);
+ virtual void Mouse(View&, MouseButton button, int x, int y, bool pressed, int button_state);
+ virtual void MouseMotion(View&, int x, int y, int button_state);
+ virtual void PassiveMouseMotion(View&, int x, int y, int button_state);
+ virtual void Special(View&, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state);
+};
+
+struct PANGOLIN_EXPORT HandlerScroll : Handler
+{
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int button_state);
+ void Special(View&, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state);
+};
+
+struct PANGOLIN_EXPORT Handler3D : Handler
+{
+ Handler3D(OpenGlRenderState& cam_state, AxisDirection enforce_up=AxisNone, float trans_scale=0.01f, float zoom_fraction= PANGO_DFLT_HANDLER3D_ZF);
+
+ virtual bool ValidWinDepth(GLprecision depth);
+ virtual void PixelUnproject( View& view, GLprecision winx, GLprecision winy, GLprecision winz, GLprecision Pc[3]);
+ virtual void GetPosNormal(View& view, int x, int y, GLprecision p[3], GLprecision Pw[3], GLprecision Pc[3], GLprecision nw[3], GLprecision default_z = 1.0);
+
+ void Keyboard(View&, unsigned char key, int x, int y, bool pressed);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int button_state);
+ void MouseMotion(View&, int x, int y, int button_state);
+ void Special(View&, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state);
+
+#ifdef USE_EIGEN
+ // Return selected point in world coordinates
+ inline Eigen::Vector3d Selected_P_w() const {
+ return Eigen::Map<const Eigen::Matrix<GLprecision,3,1>>(Pw).cast<double>();
+ }
+#endif
+ inline int KeyState() const{
+ return funcKeyState;
+ }
+
+protected:
+ OpenGlRenderState* cam_state;
+ const static int hwin = 8;
+ AxisDirection enforce_up;
+ float tf; // translation factor
+ float zf; // zoom fraction
+ CameraSpec cameraspec;
+ GLprecision last_z;
+ float last_pos[2];
+ GLprecision rot_center[3];
+
+ GLprecision p[3];
+ GLprecision Pw[3];
+ GLprecision Pc[3];
+ GLprecision n[3];
+
+ int funcKeyState;
+};
+
+static Handler StaticHandler;
+static HandlerScroll StaticHandlerScroll;
+
+}
diff --git a/externals/Pangolin/include/pangolin/handler/handler_enums.h b/externals/Pangolin/include/pangolin/handler/handler_enums.h
new file mode 100644
index 0000000000000000000000000000000000000000..389040ff31233ca80709acc9477985ef454a7422
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/handler/handler_enums.h
@@ -0,0 +1,94 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+namespace pangolin
+{
+
+// Supported Key modifiers for GlobalKeyPressCallback.
+// e.g. PANGO_CTRL + 'r', PANGO_SPECIAL + PANGO_KEY_RIGHT, etc.
+const int PANGO_SPECIAL = 128;
+const int PANGO_CTRL = -96;
+const int PANGO_OPTN = 132;
+
+// Ordinary keys
+const int PANGO_KEY_TAB = 9;
+const int PANGO_KEY_ESCAPE = 27;
+
+// Special Keys (same as GLUT_ defines)
+const int PANGO_KEY_F1 = 1;
+const int PANGO_KEY_F2 = 2;
+const int PANGO_KEY_F3 = 3;
+const int PANGO_KEY_F4 = 4;
+const int PANGO_KEY_F5 = 5;
+const int PANGO_KEY_F6 = 6;
+const int PANGO_KEY_F7 = 7;
+const int PANGO_KEY_F8 = 8;
+const int PANGO_KEY_F9 = 9;
+const int PANGO_KEY_F10 = 10;
+const int PANGO_KEY_F11 = 11;
+const int PANGO_KEY_F12 = 12;
+const int PANGO_KEY_LEFT = 100;
+const int PANGO_KEY_UP = 101;
+const int PANGO_KEY_RIGHT = 102;
+const int PANGO_KEY_DOWN = 103;
+const int PANGO_KEY_PAGE_UP = 104;
+const int PANGO_KEY_PAGE_DOWN = 105;
+const int PANGO_KEY_HOME = 106;
+const int PANGO_KEY_END = 107;
+const int PANGO_KEY_INSERT = 108;
+
+enum MouseButton
+{
+ MouseButtonLeft = 1,
+ MouseButtonMiddle = 2,
+ MouseButtonRight = 4,
+ MouseWheelUp = 8,
+ MouseWheelDown = 16,
+ MouseWheelRight = 32,
+ MouseWheelLeft = 64,
+};
+
+enum KeyModifier
+{
+ KeyModifierShift = 1<<16,
+ KeyModifierCtrl = 1<<17,
+ KeyModifierAlt = 1<<18,
+ KeyModifierCmd = 1<<19,
+ KeyModifierFnc = 1<<20
+};
+
+enum InputSpecial
+{
+ InputSpecialScroll,
+ InputSpecialZoom,
+ InputSpecialRotate,
+ InputSpecialTablet
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/handler/handler_glbuffer.h b/externals/Pangolin/include/pangolin/handler/handler_glbuffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..3d984f1ac9f5e60542f039c9b9691bebc607d718
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/handler/handler_glbuffer.h
@@ -0,0 +1,48 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#ifndef HANDLER_GLBUFFER_H
+#define HANDLER_GLBUFFER_H
+
+#include <pangolin/handler/handler.h>
+#include <pangolin/gl/gl.h>
+
+namespace pangolin
+{
+
+struct Handler3DFramebuffer : public pangolin::Handler3D
+{
+ Handler3DFramebuffer(GlFramebuffer& fb, pangolin::OpenGlRenderState& cam_state, pangolin::AxisDirection enforce_up=pangolin::AxisNone, float trans_scale=0.01f);
+ void GetPosNormal(pangolin::View& view, int x, int y, GLprecision p[3], GLprecision Pw[3], GLprecision Pc[3], GLprecision /*n*/[3], GLprecision default_z);
+
+protected:
+ GlFramebuffer& fb;
+};
+
+}
+
+#endif // HANDLER_GLBUFFER_H
diff --git a/externals/Pangolin/include/pangolin/handler/handler_image.h b/externals/Pangolin/include/pangolin/handler/handler_image.h
new file mode 100644
index 0000000000000000000000000000000000000000..00412788f34a1b7f3c81b1335ac708427930c891
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/handler/handler_image.h
@@ -0,0 +1,162 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/image/image_utils.h>
+#include <pangolin/display/viewport.h>
+#include <pangolin/display/view.h>
+#include <pangolin/handler/handler.h>
+#include <pangolin/plot/range.h>
+#include <pangolin/gl/gl.h>
+
+#include <functional>
+
+namespace pangolin
+{
+
+class ImageViewHandler : public Handler
+{
+public:
+ struct EventData {
+ EventData(View& v, ImageViewHandler& h) : view(v), handler(h) {}
+ View& view;
+ ImageViewHandler& handler;
+ };
+
+ struct OnSelectionEventData : public EventData {
+ OnSelectionEventData(View& v, ImageViewHandler& h, bool dragging)
+ : EventData(v,h), dragging(dragging) {}
+ bool dragging;
+ };
+
+ typedef std::function<void(OnSelectionEventData)> OnSelectionCallbackFn;
+
+ // Default constructor: User must call SetDimensions() once image dimensions are known.
+ // Default range is [0,1] in x and y.
+ ImageViewHandler();
+
+ // View ranges store extremes of image (boundary of pixels)
+ // in 'discrete' coords, where 0,0 is center of top-left pixel.
+ ImageViewHandler(size_t w, size_t h);
+
+ void SetDimensions(size_t w, size_t h);
+
+ void UpdateView();
+
+ void glSetViewOrtho();
+
+ void glRenderTexture(pangolin::GlTexture& tex);
+ void glRenderTexture(GLuint tex, GLint width, GLint height);
+
+ void glRenderOverlay();
+
+ void ScreenToImage(Viewport& v, float xpix, float ypix, float& ximg, float& yimg);
+
+ void ImageToScreen(Viewport& v, float ximg, float yimg, float& xpix, float& ypix);
+
+ bool UseNN() const;
+
+ bool& UseNN();
+
+ bool& FlipTextureX();
+
+ bool& FlipTextureY();
+
+ pangolin::XYRangef& GetViewToRender();
+
+ float GetViewScale();
+
+ pangolin::XYRangef& GetView();
+
+ pangolin::XYRangef& GetDefaultView();
+
+ pangolin::XYRangef& GetSelection();
+
+ void GetHover(float& x, float& y);
+
+ void SetView(const pangolin::XYRangef& range);
+
+ void SetViewSmooth(const pangolin::XYRangef& range);
+
+ void ScrollView(float x, float y);
+
+ void ScrollViewSmooth(float x, float y);
+
+ void ScaleView(float x, float y, float cx, float cy);
+
+ void ScaleViewSmooth(float x, float y, float cx, float cy);
+
+ void ResetView();
+
+ ///////////////////////////////////////////////////////
+ /// pangolin::Handler
+ ///////////////////////////////////////////////////////
+
+ void Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed) override;
+
+ void Mouse(View& view, pangolin::MouseButton button, int x, int y, bool pressed, int button_state) override;
+
+ void MouseMotion(View& view, int x, int y, int button_state) override;
+
+ void PassiveMouseMotion(View&, int /*x*/, int /*y*/, int /*button_state*/) override;
+
+ void Special(View& view, pangolin::InputSpecial inType, float x, float y, float p1, float p2, float /*p3*/, float /*p4*/, int /*button_state*/) override;
+
+ ///////////////////////////////////////////////////////
+ /// Callbacks
+ ///////////////////////////////////////////////////////
+
+ OnSelectionCallbackFn OnSelectionCallback;
+
+protected:
+ void FixSelection(XYRangef& sel);
+
+ void AdjustScale();
+
+ void AdjustTranslation();
+
+ static ImageViewHandler* to_link;
+ static float animate_factor;
+
+ ImageViewHandler* linked_view_handler;
+
+ pangolin::XYRangef rview_default;
+ pangolin::XYRangef rview_max;
+ pangolin::XYRangef rview;
+ pangolin::XYRangef target;
+ pangolin::XYRangef selection;
+
+ float hover_img[2];
+ int last_mouse_pos[2];
+
+ bool use_nn;
+ bool flipTextureX;
+ bool flipTextureY;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/copy.h b/externals/Pangolin/include/pangolin/image/copy.h
new file mode 100644
index 0000000000000000000000000000000000000000..160fa6b9888db870f7b5f71a019aabce2c31d178
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/copy.h
@@ -0,0 +1,45 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+namespace pangolin {
+
+// Hold a reference to an object to be copied
+template<typename T>
+struct CopyObject {
+ CopyObject(const T& obj) : obj(obj) { }
+ const T& obj;
+};
+
+// Return copy wrapper for assignment to another object.
+template<typename T>
+typename pangolin::CopyObject<T> Copy(const T& obj) {
+ return typename pangolin::CopyObject<T>(obj);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/image.h b/externals/Pangolin/include/pangolin/image/image.h
new file mode 100644
index 0000000000000000000000000000000000000000..098638ae877e182f7eb2d86f3d06cb15ae1cf887
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/image.h
@@ -0,0 +1,428 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/image/memcpy.h>
+
+#include <cstddef>
+#include <functional>
+#include <limits>
+#include <cstring>
+
+#ifdef PANGO_ENABLE_BOUNDS_CHECKS
+# define PANGO_BOUNDS_ASSERT(...) PANGO_ENSURE(##__VA_ARGS__)
+#else
+# define PANGO_BOUNDS_ASSERT(...) ((void)0)
+#endif
+
+// Allow user defined macro to be inserted into Image class.
+#ifndef PANGO_EXTENSION_IMAGE
+# define PANGO_EXTENSION_IMAGE
+#endif
+
+namespace pangolin
+{
+
+// Simple image wrapper
+template<typename T>
+struct Image
+{
+ using PixelType = T;
+
+ inline Image()
+ : pitch(0), ptr(0), w(0), h(0)
+ {
+ }
+
+ inline Image(T* ptr, size_t w, size_t h, size_t pitch)
+ : pitch(pitch), ptr(ptr), w(w), h(h)
+ {
+ }
+
+
+ PANGO_HOST_DEVICE inline
+ size_t SizeBytes() const
+ {
+ return pitch * h;
+ }
+
+ PANGO_HOST_DEVICE inline
+ size_t Area() const
+ {
+ return w * h;
+ }
+
+ PANGO_HOST_DEVICE inline
+ bool IsValid() const
+ {
+ return ptr != 0;
+ }
+
+ PANGO_HOST_DEVICE inline
+ bool IsContiguous() const
+ {
+ return w*sizeof(T) == pitch;
+ }
+
+ //////////////////////////////////////////////////////
+ // Iterators
+ //////////////////////////////////////////////////////
+
+ PANGO_HOST_DEVICE inline
+ T* begin()
+ {
+ return ptr;
+ }
+
+ PANGO_HOST_DEVICE inline
+ T* end()
+ {
+ return RowPtr(h-1) + w;
+ }
+
+ PANGO_HOST_DEVICE inline
+ const T* begin() const
+ {
+ return ptr;
+ }
+
+ PANGO_HOST_DEVICE inline
+ const T* end() const
+ {
+ return RowPtr(h-1) + w;
+ }
+
+ PANGO_HOST_DEVICE inline
+ size_t size() const
+ {
+ return w*h;
+ }
+
+ //////////////////////////////////////////////////////
+ // Image transforms
+ //////////////////////////////////////////////////////
+
+ template<typename UnaryOperation>
+ PANGO_HOST_DEVICE inline
+ void Transform(UnaryOperation unary_op)
+ {
+ PANGO_ASSERT(IsValid());
+
+ for(size_t y=0; y < h; ++y) {
+ T* el = RowPtr(y);
+ const T* el_end = el+w;
+ for( ; el != el_end; ++el) {
+ *el = unary_op(*el);
+ }
+ }
+ }
+
+ PANGO_HOST_DEVICE inline
+ void Fill(const T& val)
+ {
+ Transform( [&](const T&) {return val;} );
+ }
+
+ PANGO_HOST_DEVICE inline
+ void Replace(const T& oldval, const T& newval)
+ {
+ Transform( [&](const T& val) {
+ return (val == oldval) ? newval : val;
+ });
+ }
+
+ inline
+ void Memset(unsigned char v = 0)
+ {
+ PANGO_ASSERT(IsValid());
+ if(IsContiguous()) {
+ ::pangolin::Memset((char*)ptr, v, pitch*h);
+ }else{
+ for(size_t y=0; y < h; ++y) {
+ ::pangolin::Memset((char*)RowPtr(y), v, pitch);
+ }
+ }
+ }
+
+ inline
+ void CopyFrom(const Image<T>& img)
+ {
+ if(IsValid() && img.IsValid()) {
+ PANGO_ASSERT(w >= img.w && h >= img.h);
+ PitchedCopy((char*)ptr,pitch,(char*)img.ptr,img.pitch, std::min(img.w,w)*sizeof(T), std::min(img.h,h) );
+ }else if( img.IsValid() != IsValid() ){
+ PANGO_ASSERT(false && "Cannot copy from / to an unasigned image." );
+ }
+ }
+
+ //////////////////////////////////////////////////////
+ // Reductions
+ //////////////////////////////////////////////////////
+
+ template<typename BinaryOperation>
+ PANGO_HOST_DEVICE inline
+ T Accumulate(const T init, BinaryOperation binary_op)
+ {
+ PANGO_ASSERT(IsValid());
+
+ T val = init;
+ for(size_t y=0; y < h; ++y) {
+ T* el = RowPtr(y);
+ const T* el_end = el+w;
+ for(; el != el_end; ++el) {
+ val = binary_op(val, *el);
+ }
+ }
+ return val;
+ }
+
+ std::pair<T,T> MinMax() const
+ {
+ PANGO_ASSERT(IsValid());
+
+ std::pair<T,T> minmax(std::numeric_limits<T>::max(), std::numeric_limits<T>::lowest());
+ for(size_t r=0; r < h; ++r) {
+ const T* ptr = RowPtr(r);
+ const T* end = ptr + w;
+ while( ptr != end) {
+ minmax.first = std::min(*ptr, minmax.first);
+ minmax.second = std::max(*ptr, minmax.second);
+ ++ptr;
+ }
+ }
+ return minmax;
+ }
+
+ template<typename Tout=T>
+ Tout Sum() const
+ {
+ return Accumulate((T)0, [](const T& lhs, const T& rhs){ return lhs + rhs; });
+ }
+
+ template<typename Tout=T>
+ Tout Mean() const
+ {
+ return Sum<Tout>() / Area();
+ }
+
+
+ //////////////////////////////////////////////////////
+ // Direct Pixel Access
+ //////////////////////////////////////////////////////
+
+ PANGO_HOST_DEVICE inline
+ T* RowPtr(size_t y)
+ {
+ return (T*)((unsigned char*)(ptr) + y*pitch);
+ }
+
+ PANGO_HOST_DEVICE inline
+ const T* RowPtr(size_t y) const
+ {
+ return (T*)((unsigned char*)(ptr) + y*pitch);
+ }
+
+ PANGO_HOST_DEVICE inline
+ T& operator()(size_t x, size_t y)
+ {
+ PANGO_BOUNDS_ASSERT( InBounds(x,y) );
+ return RowPtr(y)[x];
+ }
+
+ PANGO_HOST_DEVICE inline
+ const T& operator()(size_t x, size_t y) const
+ {
+ PANGO_BOUNDS_ASSERT( InBounds(x,y) );
+ return RowPtr(y)[x];
+ }
+
+ template<typename TVec>
+ PANGO_HOST_DEVICE inline
+ T& operator()(const TVec& p)
+ {
+ PANGO_BOUNDS_ASSERT( InBounds(p[0],p[1]) );
+ return RowPtr(p[1])[p[0]];
+ }
+
+ template<typename TVec>
+ PANGO_HOST_DEVICE inline
+ const T& operator()(const TVec& p) const
+ {
+ PANGO_BOUNDS_ASSERT( InBounds(p[0],p[1]) );
+ return RowPtr(p[1])[p[0]];
+ }
+
+ PANGO_HOST_DEVICE inline
+ T& operator[](size_t ix)
+ {
+ PANGO_BOUNDS_ASSERT( InImage(ptr+ix) );
+ return ptr[ix];
+ }
+
+ PANGO_HOST_DEVICE inline
+ const T& operator[](size_t ix) const
+ {
+ PANGO_BOUNDS_ASSERT( InImage(ptr+ix) );
+ return ptr[ix];
+ }
+
+ //////////////////////////////////////////////////////
+ // Bounds Checking
+ //////////////////////////////////////////////////////
+
+ PANGO_HOST_DEVICE
+ bool InImage(const T* ptest) const
+ {
+ return ptr <= ptest && ptest < RowPtr(h);
+ }
+
+ PANGO_HOST_DEVICE inline
+ bool InBounds(int x, int y) const
+ {
+ return 0 <= x && x < (int)w && 0 <= y && y < (int)h;
+ }
+
+ PANGO_HOST_DEVICE inline
+ bool InBounds(float x, float y, float border) const
+ {
+ return border <= x && x < (w-border) && border <= y && y < (h-border);
+ }
+
+ template<typename TVec, typename TBorder>
+ PANGO_HOST_DEVICE inline
+ bool InBounds( const TVec& p, const TBorder border = (TBorder)0 ) const
+ {
+ return border <= p[0] && p[0] < ((int)w - border) && border <= p[1] && p[1] < ((int)h - border);
+ }
+
+ //////////////////////////////////////////////////////
+ // Obtain slices / subimages
+ //////////////////////////////////////////////////////
+
+ PANGO_HOST_DEVICE inline
+ const Image<const T> SubImage(size_t x, size_t y, size_t width, size_t height) const
+ {
+ PANGO_ASSERT( (x+width) <= w && (y+height) <= h);
+ return Image<const T>( RowPtr(y)+x, width, height, pitch);
+ }
+
+ PANGO_HOST_DEVICE inline
+ Image<T> SubImage(size_t x, size_t y, size_t width, size_t height)
+ {
+ PANGO_ASSERT( (x+width) <= w && (y+height) <= h);
+ return Image<T>( RowPtr(y)+x, width, height, pitch);
+ }
+
+ PANGO_HOST_DEVICE inline
+ const Image<T> Row(int y) const
+ {
+ return SubImage(0,y,w,1);
+ }
+
+ PANGO_HOST_DEVICE inline
+ Image<T> Row(int y)
+ {
+ return SubImage(0,y,w,1);
+ }
+
+ PANGO_HOST_DEVICE inline
+ const Image<T> Col(int x) const
+ {
+ return SubImage(x,0,1,h);
+ }
+
+ PANGO_HOST_DEVICE inline
+ Image<T> Col(int x)
+ {
+ return SubImage(x,0,1,h);
+ }
+
+ //////////////////////////////////////////////////////
+ // Data mangling
+ //////////////////////////////////////////////////////
+
+ template<typename TRecast>
+ PANGO_HOST_DEVICE inline
+ Image<TRecast> Reinterpret() const
+ {
+ PANGO_ASSERT(sizeof(TRecast) == sizeof(T), "sizeof(TRecast) must match sizeof(T): % != %", sizeof(TRecast), sizeof(T) );
+ return UnsafeReinterpret<TRecast>();
+ }
+
+ template<typename TRecast>
+ PANGO_HOST_DEVICE inline
+ Image<TRecast> UnsafeReinterpret() const
+ {
+ return Image<TRecast>((TRecast*)ptr,w,h,pitch);
+ }
+
+ //////////////////////////////////////////////////////
+ // Deprecated methods
+ //////////////////////////////////////////////////////
+
+// PANGOLIN_DEPRECATED inline
+ Image(size_t w, size_t h, size_t pitch, T* ptr)
+ : pitch(pitch), ptr(ptr), w(w), h(h)
+ {
+ }
+
+ // Use RAII/move aware pangolin::ManagedImage instead
+// PANGOLIN_DEPRECATED inline
+ void Dealloc()
+ {
+ if(ptr) {
+ ::operator delete(ptr);
+ ptr = nullptr;
+ }
+ }
+
+ // Use RAII/move aware pangolin::ManagedImage instead
+// PANGOLIN_DEPRECATED inline
+ void Alloc(size_t w, size_t h, size_t pitch)
+ {
+ Dealloc();
+ this->w = w;
+ this->h = h;
+ this->pitch = pitch;
+ this->ptr = (T*)::operator new(h*pitch);
+ }
+
+ //////////////////////////////////////////////////////
+ // Data members
+ //////////////////////////////////////////////////////
+
+ size_t pitch;
+ T* ptr;
+ size_t w;
+ size_t h;
+
+ PANGO_EXTENSION_IMAGE
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/image_convert.h b/externals/Pangolin/include/pangolin/image/image_convert.h
new file mode 100644
index 0000000000000000000000000000000000000000..36ae99f51f2d8add7f585cb6d8d7d9a94963f002
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/image_convert.h
@@ -0,0 +1,31 @@
+#pragma once
+
+#include <pangolin/image/managed_image.h>
+#include <pangolin/utils/compontent_cast.h>
+
+namespace pangolin
+{
+
+template <typename To, typename T>
+void ImageConvert(Image<To>& dst, const Image<T>& src, To scale = 1.0)
+{
+ for(unsigned int y = 0; y < dst.h; ++y)
+ {
+ const T* prs = src.RowPtr(y);
+ To* prd = dst.RowPtr(y);
+ for(unsigned int x = 0; x < dst.w; ++x)
+ {
+ *(prd++) = scale * ComponentCast<To, T>::cast(*(prs++));
+ }
+ }
+}
+
+template <typename To, typename T>
+ManagedImage<To> ImageConvert(const Image<T>& src, To scale = 1.0)
+{
+ ManagedImage<To> dst(src.w, src.h);
+ ImageConvert<To,T>(dst,src,scale);
+ return dst;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/image_io.h b/externals/Pangolin/include/pangolin/image/image_io.h
new file mode 100644
index 0000000000000000000000000000000000000000..fcc594bd3b52e56894721c26939c2fdc71efb6bd
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/image_io.h
@@ -0,0 +1,65 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <pangolin/image/typed_image.h>
+#include <pangolin/utils/file_extension.h>
+
+namespace pangolin {
+
+PANGOLIN_EXPORT
+TypedImage LoadImage(std::istream& in, ImageFileType file_type);
+
+PANGOLIN_EXPORT
+TypedImage LoadImage(const std::string& filename, ImageFileType file_type);
+
+PANGOLIN_EXPORT
+TypedImage LoadImage(const std::string& filename);
+
+PANGOLIN_EXPORT
+TypedImage LoadImage(const std::string& filename, const PixelFormat& raw_fmt, size_t raw_width, size_t raw_height, size_t raw_pitch);
+
+/// Quality \in [0..100] for lossy formats
+PANGOLIN_EXPORT
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, ImageFileType file_type, bool top_line_first = true, float quality = 100.0f);
+
+/// Quality \in [0..100] for lossy formats
+PANGOLIN_EXPORT
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, ImageFileType file_type, bool top_line_first = true, float quality = 100.0f);
+
+/// Quality \in [0..100] for lossy formats
+PANGOLIN_EXPORT
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, bool top_line_first = true, float quality = 100.0f);
+
+/// Quality \in [0..100] for lossy formats
+PANGOLIN_EXPORT
+void SaveImage(const TypedImage& image, const std::string& filename, bool top_line_first = true, float quality = 100.0f);
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/image_utils.h b/externals/Pangolin/include/pangolin/image/image_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..92b59848597c028a292c3f46bb989a299ef3e8bf
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/image_utils.h
@@ -0,0 +1,185 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <limits>
+#include <utility>
+
+#include <pangolin/image/image.h>
+#include <pangolin/plot/range.h>
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glpixformat.h>
+
+namespace pangolin
+{
+
+namespace internal
+{
+
+template <typename T>
+std::pair<float, float> GetMinMax(const Image<T>& img, size_t channels)
+{
+ const size_t max_channels = 3;
+ const size_t colour_channels = std::min(channels, max_channels);
+ std::pair<float, float> chan_mm[max_channels];
+ for(size_t c = 0; c < max_channels; ++c)
+ {
+ chan_mm[c].first = +std::numeric_limits<float>::max();
+ chan_mm[c].second = -std::numeric_limits<float>::max();
+ }
+
+ for(size_t y = 0; y < img.h; ++y)
+ {
+ T* pix = (T*)((char*)img.ptr + y * img.pitch);
+ for(size_t x = 0; x < img.w; ++x)
+ {
+ for(size_t c = 0; c < colour_channels; ++c)
+ {
+ if(pix[c] < chan_mm[c].first)
+ chan_mm[c].first = (float)pix[c];
+ if(pix[c] > chan_mm[c].second)
+ chan_mm[c].second = (float)pix[c];
+ }
+
+ pix += channels;
+ }
+ }
+
+ // Find min / max of all channels, ignoring 4th alpha channel
+ std::pair<float, float> mm = chan_mm[0];
+ for(size_t c = 1; c < colour_channels; ++c)
+ {
+ mm.first = std::min(mm.first, chan_mm[c].first);
+ mm.second = std::max(mm.second, chan_mm[c].second);
+ }
+
+ return mm;
+}
+
+template<typename T>
+pangolin::Image<T> GetImageRoi( pangolin::Image<T> img, size_t channels, const pangolin::XYRangei& roi )
+{
+ return pangolin::Image<T>(
+ img.RowPtr(std::min(roi.y.min,roi.y.max)) + channels*std::min(roi.x.min,roi.x.max),
+ roi.x.AbsSize(), roi.y.AbsSize(),
+ img.pitch
+ );
+}
+
+template<typename T>
+std::pair<float,float> GetOffsetScale(const pangolin::Image<T>& img, size_t channels, float type_max, float format_max)
+{
+ // Find min / max of all channels, ignoring 4th alpha channel
+ const std::pair<float,float> mm = internal::GetMinMax<T>(img,channels);
+ const float type_scale = format_max / type_max;
+ const float offset = -type_scale* mm.first;
+ const float scale = type_max / (mm.second - mm.first);
+ return std::pair<float,float>(offset, scale);
+}
+
+template<typename T>
+float GetScaleOnly(const pangolin::Image<T>& img, size_t channels, float type_max, float /*format_max*/)
+{
+ // Find min / max of all channels, ignoring 4th alpha channel
+ const std::pair<float,float> mm = internal::GetMinMax<T>(img,channels);
+ const float scale = type_max / mm.second;
+ return scale;
+}
+
+} // internal
+
+inline std::pair<float, float> GetMinMax(
+ const Image<unsigned char>& img,
+ XYRangei iroi, const GlPixFormat& glfmt
+) {
+ using namespace internal;
+
+ iroi.Clamp(0, (int)img.w - 1, 0, (int)img.h - 1);
+
+ const size_t num_channels = pangolin::GlFormatChannels(glfmt.glformat);
+
+ if(glfmt.gltype == GL_UNSIGNED_BYTE) {
+ return GetMinMax(GetImageRoi(img.template UnsafeReinterpret<unsigned char>(), num_channels, iroi), num_channels);
+ } else if(glfmt.gltype == GL_UNSIGNED_SHORT) {
+ return GetMinMax(GetImageRoi(img.template UnsafeReinterpret<unsigned short>(), num_channels, iroi), num_channels);
+ } else if(glfmt.gltype == GL_FLOAT) {
+ return GetMinMax(GetImageRoi(img.template UnsafeReinterpret<float>(), num_channels, iroi), num_channels);
+ } else if(glfmt.gltype == GL_DOUBLE) {
+ return GetMinMax(GetImageRoi(img.template UnsafeReinterpret<double>(), num_channels, iroi), num_channels);
+ } else {
+ return std::pair<float, float>(std::numeric_limits<float>::max(), std::numeric_limits<float>::lowest());
+ }
+}
+
+inline std::pair<float,float> GetOffsetScale(
+ const pangolin::Image<unsigned char>& img,
+ pangolin::XYRangei iroi, const pangolin::GlPixFormat& glfmt
+) {
+ using namespace internal;
+
+ iroi.Clamp(0, (int)img.w-1, 0, (int)img.h-1 );
+
+ const size_t num_channels = pangolin::GlFormatChannels(glfmt.glformat);
+
+ if(glfmt.gltype == GL_UNSIGNED_BYTE) {
+ return GetOffsetScale(GetImageRoi(img.template UnsafeReinterpret<unsigned char>(), num_channels, iroi), num_channels, 255.0f, 1.0f);
+ }else if(glfmt.gltype == GL_UNSIGNED_SHORT) {
+ return GetOffsetScale(GetImageRoi(img.template UnsafeReinterpret<unsigned short>(), num_channels, iroi), num_channels, 65535.0f, 1.0f);
+ }else if(glfmt.gltype == GL_FLOAT) {
+ return GetOffsetScale(GetImageRoi(img.template UnsafeReinterpret<float>(), num_channels, iroi), num_channels, 1.0f, 1.0f);
+ }else if(glfmt.gltype == GL_DOUBLE) {
+ return GetOffsetScale(GetImageRoi(img.template UnsafeReinterpret<double>(), num_channels, iroi), num_channels, 1.0f, 1.0f);
+ }else{
+ return std::pair<float,float>(0.0f, 1.0f);
+ }
+}
+
+inline float GetScaleOnly(
+ const pangolin::Image<unsigned char>& img,
+ pangolin::XYRangei iroi, const pangolin::GlPixFormat& glfmt
+) {
+ using namespace internal;
+
+ iroi.Clamp(0, (int)img.w-1, 0, (int)img.h-1 );
+
+ const size_t num_channels = pangolin::GlFormatChannels(glfmt.glformat);
+
+ if(glfmt.gltype == GL_UNSIGNED_BYTE) {
+ return GetScaleOnly(GetImageRoi(img.template UnsafeReinterpret<unsigned char>(), num_channels, iroi), num_channels, 255.0f, 1.0f);
+ }else if(glfmt.gltype == GL_UNSIGNED_SHORT) {
+ return GetScaleOnly(GetImageRoi(img.template UnsafeReinterpret<unsigned short>(), num_channels, iroi), num_channels, 65535.0f, 1.0f);
+ }else if(glfmt.gltype == GL_FLOAT) {
+ return GetScaleOnly(GetImageRoi(img.template UnsafeReinterpret<float>(), num_channels, iroi), num_channels, 1.0f, 1.0f);
+ }else if(glfmt.gltype == GL_DOUBLE) {
+ return GetScaleOnly(GetImageRoi(img.template UnsafeReinterpret<double>(), num_channels, iroi), num_channels, 1.0f, 1.0f);
+ }else{
+ return 1.0f;
+ }
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/managed_image.h b/externals/Pangolin/include/pangolin/image/managed_image.h
new file mode 100644
index 0000000000000000000000000000000000000000..dd21c43e0b3e62ed486f195eeb88c8851a7c3193
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/managed_image.h
@@ -0,0 +1,175 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/image/image.h>
+#include <pangolin/image/copy.h>
+
+namespace pangolin {
+
+template<class T> using DefaultImageAllocator = std::allocator<T>;
+
+// Image that manages it's own memory, storing a strong pointer to it's memory
+template<typename T, class Allocator = DefaultImageAllocator<T> >
+class ManagedImage : public Image<T>
+{
+public:
+ // Destructor
+ inline
+ ~ManagedImage()
+ {
+ Deallocate();
+ }
+
+ // Null image
+ inline
+ ManagedImage()
+ {
+ }
+
+ // Row image
+ inline
+ ManagedImage(size_t w)
+ : Image<T>(
+ Allocator().allocate(w),
+ w, 1, w*sizeof(T)
+ )
+ {
+ }
+
+ inline
+ ManagedImage(size_t w, size_t h)
+ : Image<T>(
+ Allocator().allocate(w*h),
+ w, h, w*sizeof(T)
+ )
+ {
+ }
+
+ inline
+ ManagedImage(size_t w, size_t h, size_t pitch_bytes)
+ : Image<T>(
+ Allocator().allocate( (h*pitch_bytes) / sizeof(T) + 1),
+ w, h, pitch_bytes
+ )
+ {
+ }
+
+ // Not copy constructable
+ inline
+ ManagedImage( const ManagedImage<T>& other ) = delete;
+
+ // Move constructor
+ inline
+ ManagedImage(ManagedImage<T,Allocator>&& img)
+ {
+ *this = std::move(img);
+ }
+
+ // Move asignment
+ inline
+ void operator=(ManagedImage<T,Allocator>&& img)
+ {
+ Deallocate();
+ Image<T>::pitch = img.pitch;
+ Image<T>::ptr = img.ptr;
+ Image<T>::w = img.w;
+ Image<T>::h = img.h;
+ img.ptr = nullptr;
+ }
+
+ // Explicit copy constructor
+ template<typename TOther>
+ ManagedImage( const CopyObject<TOther>& other )
+ {
+ CopyFrom(other.obj);
+ }
+
+ // Explicit copy assignment
+ template<typename TOther>
+ void operator=(const CopyObject<TOther>& other)
+ {
+ CopyFrom(other.obj);
+ }
+
+ inline
+ void Swap(ManagedImage<T>& img)
+ {
+ std::swap(img.pitch, Image<T>::pitch);
+ std::swap(img.ptr, Image<T>::ptr);
+ std::swap(img.w, Image<T>::w);
+ std::swap(img.h, Image<T>::h);
+ }
+
+ inline
+ void CopyFrom(const Image<T>& img)
+ {
+ if(!Image<T>::IsValid() || Image<T>::w != img.w || Image<T>::h != img.h) {
+ Reinitialise(img.w,img.h);
+ }
+ Image<T>::CopyFrom(img);
+ }
+
+ inline
+ void Reinitialise(size_t w, size_t h)
+ {
+ if(!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h) {
+ *this = ManagedImage<T,Allocator>(w,h);
+ }
+ }
+
+ inline
+ void Reinitialise(size_t w, size_t h, size_t pitch)
+ {
+ if(!Image<T>::ptr || Image<T>::w != w || Image<T>::h != h || Image<T>::pitch != pitch) {
+ *this = ManagedImage<T,Allocator>(w,h,pitch);
+ }
+ }
+
+ inline void Deallocate()
+ {
+ if (Image<T>::ptr) {
+ Allocator().deallocate(Image<T>::ptr, (Image<T>::h * Image<T>::pitch) / sizeof(T) );
+ Image<T>::ptr = nullptr;
+ }
+ }
+
+ // Move asignment
+ template<typename TOther, typename AllocOther> inline
+ void OwnAndReinterpret(ManagedImage<TOther,AllocOther>&& img)
+ {
+ Deallocate();
+ Image<T>::pitch = img.pitch;
+ Image<T>::ptr = (T*)img.ptr;
+ Image<T>::w = img.w;
+ Image<T>::h = img.h;
+ img.ptr = nullptr;
+ }
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/memcpy.h b/externals/Pangolin/include/pangolin/image/memcpy.h
new file mode 100644
index 0000000000000000000000000000000000000000..88d3f0ce3b07312d4732badeb973e6abae9b303a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/memcpy.h
@@ -0,0 +1,110 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <cstring>
+
+#ifdef HAVE_CUDA
+# include <cuda_runtime.h>
+#endif
+
+namespace pangolin {
+
+template<typename T>
+PANGO_HOST_DEVICE inline
+bool IsDevicePtr(T* ptr)
+{
+#ifdef HAVE_CUDA
+ cudaPointerAttributes attributes;
+ cudaError_t res = cudaPointerGetAttributes(&attributes,ptr);
+
+ //Flushing the error flag for future CUDA error checks
+ if(res != cudaSuccess)
+ {
+ cudaGetLastError();
+ return false;
+ }
+
+ return attributes.memoryType == cudaMemoryTypeDevice;
+#else
+ PANGOLIN_UNUSED(ptr);
+ return false;
+#endif
+}
+
+PANGO_HOST_DEVICE inline
+void MemCopy(void *dst, const void *src, size_t size_bytes)
+{
+#ifdef HAVE_CUDA
+ cudaMemcpy(dst,src, size_bytes, cudaMemcpyDefault);
+#else
+ std::memcpy(dst, src, size_bytes);
+#endif
+}
+
+inline
+void PitchedCopy(char* dst, unsigned int dst_pitch_bytes, const char* src, unsigned int src_pitch_bytes, unsigned int width_bytes, unsigned int height)
+{
+#ifdef HAVE_CUDA
+ cudaMemcpy2D(dst, dst_pitch_bytes, src, src_pitch_bytes, width_bytes, height, cudaMemcpyDefault);
+#else
+ if(dst_pitch_bytes == width_bytes && src_pitch_bytes == width_bytes ) {
+ std::memcpy(dst, src, height * width_bytes);
+ }else{
+ for(unsigned int row=0; row < height; ++row) {
+ std::memcpy(dst, src, width_bytes);
+ dst += dst_pitch_bytes;
+ src += src_pitch_bytes;
+ }
+ }
+#endif
+}
+
+PANGO_HOST_DEVICE inline
+void Memset(char* ptr, unsigned char v, size_t size_bytes)
+{
+#ifdef __CUDA_ARCH__
+ // Called in kernel
+ char* end = ptr + size_bytes;
+ for(char* p=ptr; p != end; ++p) *p = v;
+#else
+# ifdef HAVE_CUDA
+ if(IsDevicePtr(ptr))
+ {
+ cudaMemset(ptr, v, size_bytes);
+ }else
+# endif // HAVE_CUDA
+ {
+ std::memset(ptr, v, size_bytes);
+ }
+#endif // __CUDA_ARCH__
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/pixel_format.h b/externals/Pangolin/include/pangolin/image/pixel_format.h
new file mode 100644
index 0000000000000000000000000000000000000000..2494a3a2db92c429e349b0f3e1b1243afac74d59
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/pixel_format.h
@@ -0,0 +1,66 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <string>
+
+namespace pangolin
+{
+
+struct PANGOLIN_EXPORT PixelFormat
+{
+ // Previously, VideoInterface::PixFormat returned a string.
+ // For compatibility, make this string convertable
+ inline operator std::string() const { return format; }
+
+ std::string format;
+ unsigned int channels;
+ unsigned int channel_bits[4]; //Of the data type
+ unsigned int bpp; //Of the data type
+ unsigned int channel_bit_depth; //Of the data
+ bool planar;
+};
+
+
+//! Return Pixel Format properties given string specification in
+//! FFMPEG notation.
+PANGOLIN_EXPORT
+PixelFormat PixelFormatFromString(const std::string& format);
+
+////////////////////////////////////////////////////////////////////
+/// Deprecated aliases for above
+
+PANGOLIN_DEPRECATED
+typedef PixelFormat VideoPixelFormat;
+PANGOLIN_DEPRECATED
+inline PixelFormat VideoFormatFromString(const std::string& format) {
+ return PixelFormatFromString(format);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/image/typed_image.h b/externals/Pangolin/include/pangolin/image/typed_image.h
new file mode 100644
index 0000000000000000000000000000000000000000..39e5062e6770fc7c60786d541e71ec895f17a3cc
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/image/typed_image.h
@@ -0,0 +1,91 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/image/managed_image.h>
+#include <pangolin/image/pixel_format.h>
+
+namespace pangolin {
+
+struct TypedImage : public ManagedImage<unsigned char>
+{
+ typedef ManagedImage<unsigned char> Base;
+
+ inline TypedImage()
+ : Base()
+ {
+ }
+
+ inline TypedImage(size_t w, size_t h, const PixelFormat& fmt)
+ : Base(w,h,w*fmt.bpp/8), fmt(fmt)
+ {
+ }
+
+ inline TypedImage(size_t w, size_t h, const PixelFormat& fmt, size_t pitch )
+ : Base(w,h, pitch), fmt(fmt)
+ {
+ }
+
+ inline
+ void Reinitialise(size_t w, size_t h, const PixelFormat& fmt)
+ {
+ Base::Reinitialise(w, h, w*fmt.bpp/8);
+ this->fmt = fmt;
+ }
+
+ inline
+ void Reinitialise(size_t w, size_t h, const PixelFormat& fmt, size_t pitch)
+ {
+ Base::Reinitialise(w, h, pitch);
+ this->fmt = fmt;
+ }
+
+ // Not copy constructable
+ inline
+ TypedImage( const TypedImage& other ) = delete;
+
+ // Move constructor
+ inline
+ TypedImage(TypedImage&& img)
+ {
+ *this = std::move(img);
+ }
+
+ // Move asignment
+ inline
+ void operator=(TypedImage&& img)
+ {
+ fmt = img.fmt;
+ Base::operator =( std::move(img));
+ }
+
+
+ PixelFormat fmt;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/ios/PangolinAppDelegate.h b/externals/Pangolin/include/pangolin/ios/PangolinAppDelegate.h
new file mode 100644
index 0000000000000000000000000000000000000000..6bae2087b9e6d57b0b9c8ddbc553d474e08bd0bd
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/ios/PangolinAppDelegate.h
@@ -0,0 +1,36 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#import <UIKit/UIKit.h>
+
+#import "PangolinUIView.h"
+
+@interface PangolinAppDelegate : UIResponder <UIApplicationDelegate>
+
+@property (strong, nonatomic) UIWindow *window;
+
+@end
diff --git a/externals/Pangolin/include/pangolin/ios/PangolinUIView.h b/externals/Pangolin/include/pangolin/ios/PangolinUIView.h
new file mode 100644
index 0000000000000000000000000000000000000000..f7aea7ff465cef0d60f09c34892ae3a6ea9ea5e5
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/ios/PangolinUIView.h
@@ -0,0 +1,22 @@
+//
+// GlTestViewController.h
+// gltest
+//
+// Created by Steven Lovegrove on 30/01/2014.
+// Copyright (c) 2014 Steven Lovegrove. All rights reserved.
+//
+
+#import <UIKit/UIKit.h>
+
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+
+@interface PangolinUIView : UIView {
+ CAEAGLLayer* _eaglLayer;
+ EAGLContext* _context;
+
+ GLuint _colorRenderBuffer;
+ GLuint _depthRenderBuffer;
+}
+
+@end
\ No newline at end of file
diff --git a/externals/Pangolin/include/pangolin/log/packet.h b/externals/Pangolin/include/pangolin/log/packet.h
new file mode 100644
index 0000000000000000000000000000000000000000..a904373dd0fdc0576f4e73560b54c53dbf906673
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packet.h
@@ -0,0 +1,70 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <mutex>
+
+#include <pangolin/log/packetstream.h>
+#include <pangolin/log/packetstream_source.h>
+
+namespace pangolin {
+
+// Encapsulate serialized reading of Packet from stream.
+struct Packet
+{
+ Packet(PacketStream& s, std::unique_lock<std::recursive_mutex>&& mutex, std::vector<PacketStreamSource>& srcs);
+ Packet(const Packet&) = delete;
+ Packet(Packet&& o);
+ ~Packet();
+
+ size_t BytesRead() const;
+ int BytesRemaining() const;
+
+ PacketStream& Stream()
+ {
+ return _stream;
+ }
+
+ PacketStreamSourceId src;
+ int64_t time;
+ size_t size;
+ size_t sequence_num;
+ picojson::value meta;
+ std::streampos frame_streampos;
+
+private:
+ void ParsePacketHeader(PacketStream& s, std::vector<PacketStreamSource>& srcs);
+ void ReadRemaining();
+
+ PacketStream& _stream;
+
+ std::unique_lock<std::recursive_mutex> lock;
+
+ std::streampos data_streampos;
+ size_t _data_len;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/packetstream.h b/externals/Pangolin/include/pangolin/log/packetstream.h
new file mode 100644
index 0000000000000000000000000000000000000000..1b9c0f3bd53b174107232ce93d6ac41c8b498316
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packetstream.h
@@ -0,0 +1,111 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <fstream>
+
+#include <pangolin/platform.h>
+
+#include <pangolin/log/packetstream_tags.h>
+#include <pangolin/utils/file_utils.h>
+
+namespace pangolin
+{
+
+class PacketStream: public std::ifstream
+{
+public:
+ PacketStream()
+ : _is_pipe(false)
+ {
+ cclear();
+ }
+
+ PacketStream(const std::string& filename)
+ : Base(filename.c_str(), std::ios::in | std::ios::binary),
+ _is_pipe(IsPipe(filename))
+ {
+ cclear();
+ }
+
+ bool seekable() const
+ {
+ return is_open() && !_is_pipe;
+ }
+
+ void open(const std::string& filename)
+ {
+ close();
+ _is_pipe = IsPipe(filename);
+ Base::open(filename.c_str(), std::ios::in | std::ios::binary);
+ }
+
+ void close()
+ {
+ cclear();
+ if (Base::is_open()) Base::close();
+ }
+
+ void seekg(std::streampos target);
+
+ void seekg(std::streamoff off, std::ios_base::seekdir way);
+
+ std::streampos tellg();
+
+ size_t read(char* target, size_t len);
+
+ char get();
+
+ size_t skip(size_t len);
+
+ size_t readUINT();
+
+ int64_t readTimestamp();
+
+ pangoTagType peekTag();
+
+ pangoTagType readTag();
+
+ pangoTagType readTag(pangoTagType);
+
+ pangoTagType syncToTag();
+
+private:
+ using Base = std::ifstream;
+
+ bool _is_pipe;
+ pangoTagType _tag;
+
+ // Amount of frame data left to read. Tracks our position within a data block.
+
+
+ void cclear() {
+ _tag = 0;
+ }
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/packetstream_reader.h b/externals/Pangolin/include/pangolin/log/packetstream_reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..d929eb4be89f5e324976ee8170a455e7904b914b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packetstream_reader.h
@@ -0,0 +1,120 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <fstream>
+#include <mutex>
+#include <thread>
+
+#include <pangolin/log/packet.h>
+
+#include <pangolin/log/sync_time.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/timer.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT PacketStreamReader
+{
+public:
+ PacketStreamReader();
+
+ PacketStreamReader(const std::string& filename);
+
+ ~PacketStreamReader();
+
+ void Open(const std::string& filename);
+
+ void Close();
+
+ const std::vector<PacketStreamSource>&
+ Sources() const
+ {
+ return _sources;
+ }
+
+ // Grab Next available frame packetstream
+ Packet NextFrame();
+
+ // Grab Next available frame in packetstream from src, discarding other frames.
+ Packet NextFrame(PacketStreamSourceId src);
+
+ bool Good() const
+ {
+ return _stream.good();
+ }
+
+ // Jumps to a particular packet.
+ size_t Seek(PacketStreamSourceId src, size_t framenum);
+
+ // Jumps to the first packet with time >= time
+ size_t Seek(PacketStreamSourceId src, SyncTime::TimePoint time);
+
+ void FixFileIndex();
+
+private:
+ bool GoodToRead();
+
+ bool SetupIndex();
+
+ void ParseHeader();
+
+ void ParseNewSource();
+
+ bool ParseIndex();
+
+ void RebuildIndex();
+
+ void AppendIndex();
+
+ std::streampos ParseFooter();
+
+ void SkipSync();
+
+ void ReSync() {
+ _stream.syncToTag();
+ }
+
+ std::string _filename;
+ std::vector<PacketStreamSource> _sources;
+ SyncTime::TimePoint packet_stream_start;
+
+ PacketStream _stream;
+ std::recursive_mutex _mutex;
+
+ bool _is_pipe;
+ int _pipe_fd;
+};
+
+
+
+
+
+
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/packetstream_source.h b/externals/Pangolin/include/pangolin/log/packetstream_source.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a79e490ca25358e9383e36de04ce9ce73b84298
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packetstream_source.h
@@ -0,0 +1,63 @@
+#pragma once
+
+#include <iostream>
+#include <pangolin/platform.h>
+#include <pangolin/utils/picojson.h>
+
+namespace pangolin {
+
+using PacketStreamSourceId = size_t;
+
+struct PANGOLIN_EXPORT PacketStreamSource
+{
+ struct PacketInfo
+ {
+ std::streampos pos;
+ int64_t capture_time;
+ };
+
+ PacketStreamSource()
+ : id(static_cast<PacketStreamSourceId>(-1)),
+ version(0),
+ data_alignment_bytes(1),
+ data_size_bytes(0),
+ next_packet_id(0)
+ {
+ }
+
+ std::streampos FindSeekLocation(size_t packet_id)
+ {
+ if(packet_id < index.size()) {
+ return index[packet_id].pos;
+ }else{
+ return std::streampos(-1);
+ }
+
+ }
+
+ int64_t NextPacketTime() const
+ {
+ if(next_packet_id < index.size()) {
+ return index[next_packet_id].capture_time;
+ }else{
+ return 0;
+ }
+ }
+
+ std::string driver;
+ size_t id;
+ std::string uri;
+ picojson::value info;
+ int64_t version;
+ int64_t data_alignment_bytes;
+ std::string data_definitions;
+ int64_t data_size_bytes;
+
+ // Index keyed by packet_id
+ std::vector<PacketInfo> index;
+
+ // Based on current position in stream
+ size_t next_packet_id;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/packetstream_tags.h b/externals/Pangolin/include/pangolin/log/packetstream_tags.h
new file mode 100644
index 0000000000000000000000000000000000000000..13216f3d8d56f5f9dc34b94a69bb98da3cf0d1a3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packetstream_tags.h
@@ -0,0 +1,46 @@
+#pragma once
+
+#include <string>
+
+namespace pangolin {
+
+using pangoTagType = uint32_t;
+
+const static std::string PANGO_MAGIC = "PANGO";
+
+const static std::string pss_src_driver = "driver";
+const static std::string pss_src_id = "id";
+const static std::string pss_src_info = "info";
+const static std::string pss_src_uri = "uri";
+const static std::string pss_src_packet = "packet";
+const static std::string pss_src_version = "version";
+const static std::string pss_pkt_alignment_bytes = "alignment_bytes";
+const static std::string pss_pkt_definitions = "definitions";
+const static std::string pss_pkt_size_bytes = "size_bytes";
+const static std::string pss_pkt_format_written = "format_written";
+
+const unsigned int TAG_LENGTH = 3;
+
+#define PANGO_TAG(a,b,c) ( (c<<16) | (b<<8) | a)
+const uint32_t TAG_PANGO_HDR = PANGO_TAG('L', 'I', 'N');
+const uint32_t TAG_PANGO_MAGIC = PANGO_TAG('P', 'A', 'N');
+const uint32_t TAG_PANGO_SYNC = PANGO_TAG('S', 'Y', 'N');
+const uint32_t TAG_PANGO_STATS = PANGO_TAG('S', 'T', 'A');
+const uint32_t TAG_PANGO_FOOTER = PANGO_TAG('F', 'T', 'R');
+const uint32_t TAG_ADD_SOURCE = PANGO_TAG('S', 'R', 'C');
+const uint32_t TAG_SRC_JSON = PANGO_TAG('J', 'S', 'N');
+const uint32_t TAG_SRC_PACKET = PANGO_TAG('P', 'K', 'T');
+const uint32_t TAG_END = PANGO_TAG('E', 'N', 'D');
+#undef PANGO_TAG
+
+inline std::string tagName(int v)
+{
+ char b[4];
+ b[0] = v&0xff;
+ b[1] = (v>>8)&0xff;
+ b[2] = (v>>16)&0xff;
+ b[3] = 0x00;
+ return std::string(b);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/packetstream_writer.h b/externals/Pangolin/include/pangolin/log/packetstream_writer.h
new file mode 100644
index 0000000000000000000000000000000000000000..195ff92fefb0a5a3091b34cb03cf4469cdd59e3f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/packetstream_writer.h
@@ -0,0 +1,173 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <ostream>
+
+#include <pangolin/log/packetstream.h>
+#include <pangolin/log/packetstream_source.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/threadedfilebuf.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT PacketStreamWriter
+{
+public:
+ PacketStreamWriter()
+ : _stream(&_buffer), _indexable(false), _open(false), _bytes_written(0)
+ {
+ _stream.exceptions(std::ostream::badbit);
+ }
+
+ PacketStreamWriter(const std::string& filename, size_t buffer_size = 100*1024*1024)
+ : _buffer(pangolin::PathExpand(filename), buffer_size), _stream(&_buffer),
+ _indexable(!IsPipe(filename)), _open(_stream.good()), _bytes_written(0)
+ {
+ _stream.exceptions(std::ostream::badbit);
+ WriteHeader();
+ }
+
+ ~PacketStreamWriter() {
+ Close();
+ }
+
+ void Open(const std::string& filename, size_t buffer_size = 100 * 1024 * 1024)
+ {
+ Close();
+ _buffer.open(filename, buffer_size);
+ _open = _stream.good();
+ _bytes_written = 0;
+ _indexable = !IsPipe(filename);
+ WriteHeader();
+ }
+
+ void Close()
+ {
+ if (_open)
+ {
+ if (_indexable) {
+ WriteEnd();
+ }
+ _buffer.close();
+ _open = false;
+ }
+ }
+
+ // Does not write footer or index.
+ void ForceClose()
+ {
+ if (_open)
+ {
+ _buffer.force_close();
+ Close();
+ }
+ }
+
+
+ // Writes to the stream immediately upon add. Return source id # and writes
+ // source id # to argument struct
+ PacketStreamSourceId AddSource(PacketStreamSource& source);
+
+ // If constructor is called inline
+ PacketStreamSourceId AddSource(const PacketStreamSource& source);
+
+ void WriteSourcePacket(
+ PacketStreamSourceId src, const char* source,const int64_t receive_time_us,
+ size_t sourcelen, const picojson::value& meta = picojson::value()
+ );
+
+ // For stream read/write synchronization. Note that this is NOT the same as
+ // time synchronization on playback of iPacketStreams.
+ void WriteSync();
+
+ // Writes the end of the stream data, including the index. Does NOT close
+ // the underlying ostream.
+ void WriteEnd();
+
+ const std::vector<PacketStreamSource>& Sources() const {
+ return _sources;
+ }
+
+ bool IsOpen() const {
+ return _open;
+ }
+
+private:
+ void WriteHeader();
+ void Write(const PacketStreamSource&);
+ void WriteMeta(PacketStreamSourceId src, const picojson::value& data);
+
+ threadedfilebuf _buffer;
+ std::ostream _stream;
+ bool _indexable, _open;
+
+ std::vector<PacketStreamSource> _sources;
+ size_t _bytes_written;
+ std::recursive_mutex _lock;
+};
+
+inline void writeCompressedUnsignedInt(std::ostream& writer, size_t n)
+{
+ while (n >= 0x80)
+ {
+ writer.put(0x80 | (n & 0x7F));
+ n >>= 7;
+ }
+ writer.put(static_cast<unsigned char>(n));
+}
+
+inline void writeTimestamp(std::ostream& writer, int64_t time_us)
+{
+ writer.write(reinterpret_cast<const char*>(&time_us), sizeof(decltype(time_us)));
+}
+
+inline void writeTag(std::ostream& writer, const pangoTagType tag)
+{
+ writer.write(reinterpret_cast<const char*>(&tag), TAG_LENGTH);
+}
+
+inline picojson::value SourceStats(const std::vector<PacketStreamSource>& srcs)
+{
+ picojson::value stat;
+ stat["num_sources"] = srcs.size();
+ stat["src_packet_index"] = picojson::array();
+ stat["src_packet_times"] = picojson::array();
+
+ for(auto& src : srcs) {
+ picojson::array pkt_index, pkt_times;
+ for (const PacketStreamSource::PacketInfo& frame : src.index) {
+ pkt_index.emplace_back(frame.pos);
+ pkt_times.emplace_back(frame.capture_time);
+ }
+ stat["src_packet_index"].push_back(std::move(pkt_index));
+ stat["src_packet_times"].push_back(std::move(pkt_times));
+ }
+ return stat;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/playback_session.h b/externals/Pangolin/include/pangolin/log/playback_session.h
new file mode 100644
index 0000000000000000000000000000000000000000..fb28d13fc4e355a628798af373dbb39d7610a078
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/playback_session.h
@@ -0,0 +1,48 @@
+#pragma once
+
+#include <memory>
+#include <vector>
+
+#include <pangolin/log/packetstream_reader.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/registration.h>
+
+namespace pangolin {
+
+class Params;
+
+class PlaybackSession
+{
+public:
+ // Singleton Instance
+ static std::shared_ptr<PlaybackSession> Default();
+
+ // Return thread-safe, shared instance of PacketStreamReader, providing
+ // serialised read for PacketStreamReader
+ std::shared_ptr<PacketStreamReader> Open(const std::string& filename)
+ {
+ const std::string path = SanitizePath(PathExpand(filename));
+
+ auto i = readers.find(path);
+ if(i == readers.end()) {
+ auto psr = std::make_shared<PacketStreamReader>(path);
+ readers[path] = psr;
+ return psr;
+ }else{
+ return i->second;
+ }
+ }
+
+ SyncTime& Time()
+ {
+ return time;
+ }
+
+ static std::shared_ptr<PlaybackSession> ChooseFromParams(const Params& params);
+
+private:
+ std::map<std::string,std::shared_ptr<PacketStreamReader>> readers;
+ SyncTime time;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/log/sync_time.h b/externals/Pangolin/include/pangolin/log/sync_time.h
new file mode 100644
index 0000000000000000000000000000000000000000..10dfe71588bd7cf1ef35c4da8b4da754c9215091
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/log/sync_time.h
@@ -0,0 +1,230 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/utils/signal_slot.h>
+#include <pangolin/utils/timer.h>
+
+#include <atomic>
+#include <condition_variable>
+#include <mutex>
+#include <queue>
+
+namespace pangolin
+{
+
+// Lightweight timestamp class to allow synchronized playback from the same (or a different) stream.
+// All playback functions called with the same SyncTime will be time-synchronized, and will remain synchronized on seek() if the SyncTime is passed in when seeking.
+// Playback with multiple SyncTimes (on the same or different streams) should also be synced, even in different processes or systems (underlying clock sync is not required).
+// However, playback with multiple SyncTimes will break on seek().
+class PANGOLIN_EXPORT SyncTime
+{
+public:
+ using Clock = baseclock;
+ using Duration = Clock::duration;
+ using TimePoint = Clock::time_point;
+
+ struct SeekInterruption: std::runtime_error
+ {
+ SeekInterruption() : std::runtime_error("Time queue invalidated by seek"){}
+ };
+
+ SyncTime(Duration virtual_clock_offset = std::chrono::milliseconds(0))
+ : seeking(false)
+ {
+ SetOffset(virtual_clock_offset);
+ }
+
+ // No copy constructor
+ SyncTime(const SyncTime&) = delete;
+
+ void SetOffset(Duration virtual_clock_offset)
+ {
+ virtual_offset = virtual_clock_offset;
+ }
+
+ void SetClock(TimePoint virtual_now)
+ {
+ virtual_offset = virtual_now - Clock::now();
+ }
+
+ TimePoint TimeNow() const
+ {
+ return Clock::now() + virtual_offset;
+ }
+
+ TimePoint ToVirtual(TimePoint real) const
+ {
+ return real + virtual_offset;
+ }
+
+ TimePoint ToReal(TimePoint virt) const
+ {
+ return virt - virtual_offset;
+ }
+
+ void WaitUntil(TimePoint virtual_time) const
+ {
+ std::this_thread::sleep_until( ToReal(virtual_time) );
+ }
+
+ int64_t QueueEvent(int64_t new_event_time_us)
+ {
+ return WaitDequeueAndQueueEvent(0, new_event_time_us);
+ }
+
+ void DequeueEvent(int64_t event_time_us)
+ {
+ std::unique_lock<std::mutex> l(time_mutex);
+ auto i = std::find(time_queue_us.begin(), time_queue_us.end(), event_time_us);
+ PANGO_ENSURE(i != time_queue_us.end());
+ time_queue_us.erase(i);
+ queue_changed.notify_all();
+ }
+
+ int64_t WaitDequeueAndQueueEvent(int64_t event_time_us, int64_t new_event_time_us =0 )
+ {
+ std::unique_lock<std::mutex> l(time_mutex);
+
+ if(event_time_us) {
+ PANGO_ENSURE(time_queue_us.size());
+
+ // Wait until we're top the priority-queue
+ queue_changed.wait(l, [&](){
+ if(seeking) {
+ // Time queue will be invalidated on seek.
+ // Unblock without action
+ throw SeekInterruption();
+ }
+ return time_queue_us.back() == event_time_us;
+ });
+
+ // Dequeue
+ time_queue_us.pop_back();
+ }
+
+ if(new_event_time_us) {
+ // Add the new event whilst we still hold the lock, so that our
+ // event can't be missed
+ insert_sorted(time_queue_us, new_event_time_us, std::greater<int64_t>());
+
+ if(time_queue_us.back() == new_event_time_us) {
+ // Return to avoid yielding when we're next.
+ return new_event_time_us;
+ }
+ }
+
+ // Only yield if another device is next
+ queue_changed.notify_all();
+ return new_event_time_us;
+ }
+
+ void NotifyAll()
+ {
+ queue_changed.notify_all();
+ }
+
+ std::mutex& TimeMutex()
+ {
+ return time_mutex;
+ }
+
+ void Stop()
+ {
+ seeking = true;
+ OnTimeStop();
+ queue_changed.notify_all();
+ }
+
+ void Start()
+ {
+ OnTimeStart();
+ seeking=false;
+ }
+
+ void Seek(TimePoint t)
+ {
+ Stop();
+ OnSeek(t);
+ Start();
+ }
+
+ Signal<> OnTimeStart;
+
+ Signal<> OnTimeStop;
+
+ Signal<TimePoint> OnSeek;
+
+private:
+ template< typename T, typename Pred >
+ static typename std::vector<T>::iterator
+ insert_sorted( std::vector<T> & vec, T const& item, Pred pred )
+ {
+ return vec.insert (
+ std::upper_bound( vec.begin(), vec.end(), item, pred ), item
+ );
+ }
+
+ std::vector<int64_t> time_queue_us;
+ Duration virtual_offset;
+ std::mutex time_mutex;
+ std::condition_variable queue_changed;
+ bool seeking;
+};
+
+struct SyncTimeEventPromise
+{
+ SyncTimeEventPromise(SyncTime& sync, int64_t time_us = 0)
+ : sync(sync), time_us(time_us)
+ {
+ sync.QueueEvent(time_us);
+ }
+
+ ~SyncTimeEventPromise()
+ {
+ Cancel();
+ }
+
+ void Cancel()
+ {
+ if(time_us) {
+ sync.DequeueEvent(time_us);
+ time_us = 0;
+ }
+ }
+
+ void WaitAndRenew(int64_t new_time_us)
+ {
+ time_us = sync.WaitDequeueAndQueueEvent(time_us, new_time_us);
+ }
+
+private:
+ SyncTime& sync;
+ int64_t time_us;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/pangolin.h b/externals/Pangolin/include/pangolin/pangolin.h
new file mode 100644
index 0000000000000000000000000000000000000000..370752acd460c297f94a6401c18904305424657d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/pangolin.h
@@ -0,0 +1,64 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#ifdef BUILD_PANGOLIN_GUI
+ #include <pangolin/gl/gl.h>
+ #include <pangolin/gl/gldraw.h>
+ #include <pangolin/gl/glvbo.h>
+ #include <pangolin/gl/glstate.h>
+ #include <pangolin/gl/colour.h>
+ #include <pangolin/display/display.h>
+ #include <pangolin/display/view.h>
+ #ifdef _ANDROID_
+ #include <pangolin/display/device/display_android.h>
+ #endif
+ #if !defined(HAVE_GLES) || defined(HAVE_GLES_2)
+ #include <pangolin/plot/plotter.h>
+ #endif
+#endif // BUILD_PANGOLIN_GUI
+
+#ifdef BUILD_PANGOLIN_VARS
+ #include <pangolin/var/varextra.h>
+ #ifdef BUILD_PANGOLIN_GUI
+ #include <pangolin/display/widgets/widgets.h>
+ #endif // BUILD_PANGOLIN_GUI
+#endif // BUILD_PANGOLIN_VARS
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ #include <pangolin/video/video.h>
+ #include <pangolin/video/video_input.h>
+ #include <pangolin/video/video_output.h>
+#endif // BUILD_PANGOLIN_VIDEO
+
+#include <pangolin/image/image_io.h>
+
+// Let other libraries headers know about Pangolin
+#define HAVE_PANGOLIN
diff --git a/externals/Pangolin/include/pangolin/platform.h b/externals/Pangolin/include/pangolin/platform.h
new file mode 100644
index 0000000000000000000000000000000000000000..b5a223b76f901e6603f23048186c128520b9755f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/platform.h
@@ -0,0 +1,81 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/config.h>
+
+// Include portable printf-style format macros
+#define __STDC_FORMAT_MACROS
+
+#ifdef _GCC_
+# define PANGOLIN_DEPRECATED __attribute__((deprecated))
+#elif defined _MSVC_
+# define PANGOLIN_DEPRECATED __declspec(deprecated)
+#else
+# define PANGOLIN_DEPRECATED
+#endif
+
+#ifdef _MSVC_
+# define __thread __declspec(thread)
+# include <pangolin/pangolin_export.h>
+#else
+# define PANGOLIN_EXPORT
+#endif //_MSVC_
+
+#define PANGOLIN_UNUSED(x) (void)(x)
+
+#ifdef _APPLE_IOS_
+// Not supported on this platform.
+#define __thread
+#endif // _APPLE_IOS_
+
+// HOST / DEVICE Annotations
+#ifdef __CUDACC__
+# include <cuda_runtime.h>
+# define PANGO_HOST_DEVICE __host__ __device__
+#else
+# define PANGO_HOST_DEVICE
+#endif
+
+// Non-standard check that header exists (Clang, GCC 5.X)
+// Useful for
+#if defined(__has_include)
+# define PANGO_HEADER_EXISTS(x) __has_include(x)
+#else
+# define PANGO_HEADER_EXISTS(x) 0
+#endif
+
+// Workaround for Apple-Clangs lack of thread_local support
+#if defined(_CLANG_) && defined(_OSX_)
+# if !__has_feature(cxx_thread_local)
+# define PANGO_NO_THREADLOCAL
+# endif
+#endif
+
+#include <pangolin/utils/assert.h>
+#include <pangolin/utils/log.h>
diff --git a/externals/Pangolin/include/pangolin/plot/datalog.h b/externals/Pangolin/include/pangolin/plot/datalog.h
new file mode 100644
index 0000000000000000000000000000000000000000..d0120b0d34bf0df2dcd40cd62b2c1de290ea9590
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/plot/datalog.h
@@ -0,0 +1,243 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <algorithm> // std::min, std::max
+#include <limits>
+#include <memory>
+#include <mutex>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__) //prevent including Eigen in cuda files
+#define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+#include <Eigen/Core>
+#endif
+
+namespace pangolin
+{
+
+/// Simple statistics recorded for a logged input dimension.
+struct DimensionStats
+{
+ DimensionStats()
+ {
+ Reset();
+ }
+
+ void Reset()
+ {
+ isMonotonic = true;
+ sum = 0.0f;
+ sum_sq = 0.0f;
+ min = std::numeric_limits<float>::max();
+ max = std::numeric_limits<float>::lowest();
+ }
+
+ void Add(const float v)
+ {
+ isMonotonic = isMonotonic && (v >= max);
+ sum += v;
+ sum_sq += v*v;
+ min = std::min(min, v);
+ max = std::max(max, v);
+ }
+
+ bool isMonotonic;
+ float sum;
+ float sum_sq;
+ float min;
+ float max;
+};
+
+class DataLogBlock
+{
+public:
+ /// @param dim: dimension of sample
+ /// @param max_samples: maximum number of samples this block can hold
+ /// @param start_id: index of first sample (from entire dataset) in this buffer
+ DataLogBlock(size_t dim, size_t max_samples, size_t start_id)
+ : dim(dim), max_samples(max_samples), samples(0),
+ start_id(start_id)
+ {
+ sample_buffer = std::unique_ptr<float[]>(new float[dim*max_samples]);
+// stats = std::unique_ptr<DimensionStats[]>(new DimensionStats[dim]);
+ }
+
+ ~DataLogBlock()
+ {
+ }
+
+ size_t Samples() const
+ {
+ return samples;
+ }
+
+ size_t MaxSamples() const
+ {
+ return max_samples;
+ }
+
+ /// Return how many more samples can fit in this block
+ size_t SampleSpaceLeft() const
+ {
+ return MaxSamples()- Samples();
+ }
+
+ bool IsFull() const
+ {
+ return Samples() >= MaxSamples();
+ }
+
+ /// Add data to block
+ void AddSamples(size_t num_samples, size_t dimensions, const float* data_dim_major );
+
+ /// Delete all samples
+ void ClearLinked()
+ {
+ samples = 0;
+ nextBlock.reset();
+ }
+
+ DataLogBlock* NextBlock() const
+ {
+ return nextBlock.get();
+ }
+
+ size_t StartId() const
+ {
+ return start_id;
+ }
+
+ float* DimData(size_t d) const
+ {
+ return sample_buffer.get() + d;
+ }
+
+ size_t Dimensions() const
+ {
+ return dim;
+ }
+
+ const float* Sample(size_t n) const
+ {
+ const int id = (int)n - (int)start_id;
+
+ if( 0 <= id && id < (int)samples ) {
+ return sample_buffer.get() + dim*id;
+ }else{
+ if(nextBlock) {
+ return nextBlock->Sample(n);
+ }else{
+ throw std::out_of_range("Index out of range.");
+ }
+ }
+ }
+
+protected:
+ size_t dim;
+ size_t max_samples;
+ size_t samples;
+ size_t start_id;
+ std::unique_ptr<float[]> sample_buffer;
+// std::unique_ptr<DimensionStats[]> stats;
+ std::unique_ptr<DataLogBlock> nextBlock;
+};
+
+/// A DataLog can efficiently record floating point sample data of any size.
+/// Memory is allocated in blocks is transparent to the user.
+class PANGOLIN_EXPORT DataLog
+{
+public:
+ /// @param block_samples_alloc number of samples each memory block can hold.
+ DataLog(unsigned int block_samples_alloc = 10000 );
+
+ ~DataLog();
+
+ /// Provide textual labels corresponding to each dimension logged.
+ /// This information may be used by graphical interfaces to DataLog.
+ void SetLabels(const std::vector<std::string> & labels);
+ const std::vector<std::string>& Labels() const;
+
+ void Log(size_t dimension, const float * vals, unsigned int samples = 1);
+ void Log(float v);
+ void Log(float v1, float v2);
+ void Log(float v1, float v2, float v3);
+ void Log(float v1, float v2, float v3, float v4);
+ void Log(float v1, float v2, float v3, float v4, float v5);
+ void Log(float v1, float v2, float v3, float v4, float v5, float v6);
+ void Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7);
+ void Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8);
+ void Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8, float v9);
+ void Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8, float v9, float v10);
+ void Log(const std::vector<float> & vals);
+
+#ifdef USE_EIGEN
+ template<typename Derived>
+ void Log(const Eigen::MatrixBase<Derived>& M)
+ {
+ Log( M.rows() * M.cols(), M.template cast<float>().eval().data() );
+ }
+#endif
+
+ void Clear();
+ void Save(std::string filename);
+
+ // Return first block of stored data
+ const DataLogBlock* FirstBlock() const;
+
+ // Return last block of stored data
+ const DataLogBlock* LastBlock() const;
+
+ // Return number of samples stored in this DataLog
+ size_t Samples() const;
+
+ // Return pointer to stored sample n
+ const float* Sample(int n) const;
+
+ // Return stats computed for each dimension if enabled.
+ const DimensionStats& Stats(size_t dim) const;
+
+ std::mutex access_mutex;
+
+protected:
+ unsigned int block_samples_alloc;
+ std::vector<std::string> labels;
+ std::unique_ptr<DataLogBlock> block0;
+ DataLogBlock* blockn;
+ std::vector<DimensionStats> stats;
+ bool record_stats;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/plot/plotter.h b/externals/Pangolin/include/pangolin/plot/plotter.h
new file mode 100644
index 0000000000000000000000000000000000000000..b33219e2b15fe4bcc29bc681cffb2d74eb668f90
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/plot/plotter.h
@@ -0,0 +1,279 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#ifndef PLOTTER_H
+#define PLOTTER_H
+
+#include <limits>
+
+#include <pangolin/display/view.h>
+#include <pangolin/gl/colour.h>
+#include <pangolin/gl/gl.h>
+#include <pangolin/gl/glfont.h>
+#include <pangolin/gl/glsl.h>
+#include <pangolin/handler/handler.h>
+#include <pangolin/plot/datalog.h>
+#include <pangolin/plot/range.h>
+
+#include <set>
+
+namespace pangolin
+{
+
+enum DrawingMode
+{
+ DrawingModePoints = GL_POINTS,
+ DrawingModeDashed = GL_LINES,
+ DrawingModeLine = GL_LINE_STRIP,
+ DrawingModeNone,
+};
+
+struct Marker
+{
+ enum Direction
+ {
+ Horizontal,
+ Vertical
+ };
+
+ enum Equality
+ {
+ LessThan = -1,
+ Equal = 0,
+ GreaterThan = 1
+ };
+
+ Marker(Direction d, float value, Equality leg = Equal, Colour c = Colour() )
+ : colour(c)
+ {
+ if(d == Horizontal) {
+ range.x = Rangef::Open();
+ range.y = Rangef::Containing(value);
+ if(leg == LessThan) {
+ range.y.Insert(std::numeric_limits<float>::lowest() );
+ }else if(leg == GreaterThan) {
+ range.y.Insert(std::numeric_limits<float>::max() );
+ }
+ }else if(d == Vertical) {
+ range.x = Rangef::Containing(value);
+ range.y = Rangef::Open();
+ if(leg == LessThan) {
+ range.x.Insert(std::numeric_limits<float>::lowest() );
+ }else if(leg == GreaterThan) {
+ range.x.Insert(std::numeric_limits<float>::max() );
+ }
+ }
+ }
+
+ Marker(const XYRangef& range, const Colour& c = Colour() )
+ : range(range), colour(c)
+ {
+ }
+
+ XYRangef range;
+ Colour colour;
+};
+
+class PANGOLIN_EXPORT Plotter : public View, Handler
+{
+public:
+ Plotter(
+ DataLog* default_log,
+ float left=0, float right=600, float bottom=-1, float top=1,
+ float tickx=30, float ticky=0.5,
+ Plotter* linked_plotter_x = 0,
+ Plotter* linked_plotter_y = 0
+ );
+
+ virtual ~Plotter();
+
+ void Render();
+
+ XYRangef& GetSelection();
+
+ XYRangef& GetDefaultView();
+ void SetDefaultView(const XYRangef& range);
+
+ XYRangef& GetView();
+ void SetView(const XYRangef& range);
+ void SetViewSmooth(const XYRangef& range);
+
+ void ScrollView(float x, float y);
+ void ScrollViewSmooth(float x, float y);
+
+ void ScaleView(float x, float y, float cx, float cy);
+ void ScaleViewSmooth(float x, float y, float cx, float cy);
+
+ void ResetView();
+
+ void SetTicks(float tickx, float ticky);
+
+ void Track(const std::string& x="$i", const std::string& y = "");
+ void ToggleTracking();
+
+ void Trigger(const std::string& x="$0", int edge = -1, float value = 0.0f);
+ void ToggleTrigger();
+
+ void SetBackgroundColour(const Colour& col);
+ void SetAxisColour(const Colour& col);
+ void SetTickColour(const Colour& col);
+
+ void ScreenToPlot(int xpix, int ypix, float &xplot, float &yplot);
+ void Keyboard(View&, unsigned char key, int x, int y, bool pressed);
+ void Mouse(View&, MouseButton button, int x, int y, bool pressed, int mouse_state);
+ void MouseMotion(View&, int x, int y, int mouse_state);
+ void PassiveMouseMotion(View&, int x, int y, int button_state);
+ void Special(View&, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state);
+
+ /// Remove all current series plots
+ void ClearSeries();
+
+ /// Add series X,Y plot from glsl compatible expressions x_expr, y_expr
+ /// $i refers to integral index of datum in log.
+ /// $0, $1, $2, ... refers to nth series in log.
+ /// e.g. x_expr = "$i", y_expr = "$0" // index - data[0] plot
+ /// e.g. x_expr = "$0", y_expr = "$1" // data[0], data[1] X-Y plot
+ /// e.g. x_exptr ="$i", y_expr = "sqrt($1)} // index - sqrt(data[0]) plot
+ void AddSeries(const std::string& x_expr, const std::string& y_expr,
+ DrawingMode drawing_mode = DrawingModeLine, Colour colour = Colour::Unspecified(),
+ const std::string &title = "$y", DataLog* log = nullptr
+ );
+
+ std::string PlotTitleFromExpr(const std::string& expr) const;
+
+ /// Remove all current markers
+ void ClearMarkers();
+
+ /// Add horizontal or vertical inequality marker; equal-to, less-than, or greater than.
+ /// This is useful for annotating a critical point or valid region.
+ Marker& AddMarker(
+ Marker::Direction d, float value,
+ Marker::Equality leg = Marker::Equal, Colour c = Colour()
+ );
+
+ Marker& AddMarker( const Marker& marker );
+
+ void ClearImplicitPlots();
+ void AddImplicitPlot();
+
+protected:
+ struct PANGOLIN_EXPORT Tick
+ {
+ float val;
+ float factor;
+ std::string symbol;
+ };
+
+ struct PANGOLIN_EXPORT PlotAttrib
+ {
+ PlotAttrib(std::string name, int plot_id, int location = -1)
+ : name(name), plot_id(plot_id), location(location) { }
+
+ std::string name;
+ int plot_id;
+ int location;
+ };
+
+ struct PANGOLIN_EXPORT PlotSeries
+ {
+ PlotSeries();
+ void CreatePlot(const std::string& x, const std::string& y, Colour c, std::string title);
+
+ GlSlProgram prog;
+ GlText title;
+ bool contains_id;
+ std::vector<PlotAttrib> attribs;
+ DataLog* log;
+ GLenum drawing_mode;
+ Colour colour;
+ bool used;
+ };
+
+ struct PANGOLIN_EXPORT PlotImplicit
+ {
+ // Assign to gl_FragColor
+ void CreatePlot(const std::string& code);
+
+ // Expression uses x,y and assignes colours [0,1] to r,g,b,a
+ void CreateColouredPlot(const std::string& code);
+
+ // Expression uses x,y and evaluates to true/false;
+ void CreateInequality(const std::string& ie, Colour c);
+
+ // Expression uses x,y and evaluates to a number
+ void CreateDistancePlot(const std::string& dist);
+
+ GlSlProgram prog;
+ };
+
+ void FixSelection();
+ void UpdateView();
+ Tick FindTickFactor(float tick);
+
+ DataLog* default_log;
+
+ ColourWheel colour_wheel;
+ Colour colour_bg;
+ Colour colour_tk;
+ Colour colour_ax;
+
+ GlSlProgram prog_lines;
+ GlSlProgram prog_text;
+
+ std::vector<PlotSeries> plotseries;
+ std::vector<Marker> plotmarkers;
+ std::vector<PlotImplicit> plotimplicits;
+
+ Tick tick[2];
+ XYRangef rview_default;
+ XYRangef rview;
+ XYRangef target;
+ XYRangef selection;
+
+ void ComputeTrackValue( float track_val[2] );
+ XYRangef ComputeAutoSelection();
+
+ bool track;
+ std::string track_x;
+ std::string track_y;
+ float last_track_val[2];
+
+ // -1: falling, -0:disable, 1: rising
+ int trigger_edge;
+ float trigger_value;
+ std::string trigger;
+
+ float hover[2];
+ int last_mouse_pos[2];
+
+ Plotter* linked_plotter_x;
+ Plotter* linked_plotter_y;
+};
+
+} // namespace pangolin
+
+#endif // PLOTTER_H
diff --git a/externals/Pangolin/include/pangolin/plot/range.h b/externals/Pangolin/include/pangolin/plot/range.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec2906e73faf5ba67d948a70b40b45de2125f35e
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/plot/range.h
@@ -0,0 +1,372 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <limits>
+#include <algorithm>
+#include <cmath>
+
+//prevent including Eigen in cuda files
+#if defined(HAVE_EIGEN) && !defined(__CUDACC__)
+# define USE_EIGEN
+#endif
+
+#ifdef USE_EIGEN
+# include <Eigen/Core>
+# include <Eigen/src/Geometry/AlignedBox.h>
+#endif // USE_EIGEN
+
+namespace pangolin
+{
+
+
+template<typename T>
+struct Range
+{
+ static Range<T> Open()
+ {
+ return Range<T>(std::numeric_limits<T>::lowest(), std::numeric_limits<T>::max());
+ }
+
+ static Range<T> Empty()
+ {
+ return Range<T>(std::numeric_limits<T>::max(), std::numeric_limits<T>::lowest());
+ }
+
+ static Range<T> Containing(T val)
+ {
+ return Range<T>(val, val);
+ }
+
+ Range()
+ : min(+std::numeric_limits<T>::max()),
+ max(-std::numeric_limits<T>::max())
+ {
+ }
+
+ Range(T rmin, T rmax)
+ : min(rmin), max(rmax)
+ {
+ }
+
+ Range operator+(T v)
+ {
+ return Range(min+v, max+v);
+ }
+
+ Range operator-(T v)
+ {
+ return Range(min-v, max-v);
+ }
+
+ Range& operator+=(T v)
+ {
+ min += v;
+ max += v;
+ return *this;
+ }
+
+ Range& operator-=(T v)
+ {
+ min -= v;
+ max -= v;
+ return *this;
+ }
+
+ Range& operator*=(T v)
+ {
+ min *= v;
+ max *= v;
+ return *this;
+ }
+
+ Range& operator/=(T v)
+ {
+ min /= v;
+ max /= v;
+ return *this;
+ }
+
+ Range& operator+=(const Range& o)
+ {
+ min += o.min;
+ max += o.max;
+ return *this;
+ }
+
+ Range& operator-=(const Range& o)
+ {
+ min -= o.min;
+ max -= o.max;
+ return *this;
+ }
+
+ Range operator+(const Range& o) const
+ {
+ return Range(min + o.min, max + o.max);
+ }
+
+ Range operator-(const Range& o) const
+ {
+ return Range(min - o.min, max - o.max);
+ }
+
+ Range operator*(float s) const
+ {
+ return Range(T(s*min), T(s*max));
+ }
+
+ T Size() const
+ {
+ return max - min;
+ }
+
+ T AbsSize() const
+ {
+ return std::abs(Size());
+ }
+
+ T Mid() const
+ {
+ return (min + max) / (T)2.0f;
+ }
+
+ void Scale(float s, float center = 0.0f)
+ {
+ min = T(s*(min-center) + center);
+ max = T(s*(max-center) + center);
+ }
+
+ void Insert(T v)
+ {
+ min = std::min(min,v);
+ max = std::max(max,v);
+ }
+
+ void Insert(const Range<T>& r)
+ {
+ Insert(r.min);
+ Insert(r.max);
+ }
+
+ void Clamp(T vmin, T vmax)
+ {
+ min = std::min(std::max(vmin, min), vmax);
+ max = std::min(std::max(vmin, max), vmax);
+ }
+
+ void Clamp(const Range& o)
+ {
+ Clamp(o.min, o.max);
+ }
+
+ void Clear()
+ {
+ min = std::numeric_limits<T>::max();
+ max = std::numeric_limits<T>::lowest();
+ }
+
+ bool Contains(T v) const
+ {
+ return min <= v && v <= max;
+ }
+
+ bool ContainsWeak(T v) const
+ {
+ return (min <= v && v <= max)
+ || (max <= v && v <= min);
+ }
+
+ template<typename To>
+ Range<To> Cast() const
+ {
+ return Range<To>(To(min), To(max));
+ }
+
+ T min;
+ T max;
+};
+
+template<typename T>
+struct XYRange
+{
+ static XYRange<T> Open()
+ {
+ return XYRange<T>(
+ Range<T>(std::numeric_limits<T>::lowest(), std::numeric_limits<T>::max()),
+ Range<T>(std::numeric_limits<T>::lowest(), std::numeric_limits<T>::max())
+ );
+ }
+
+ static XYRange<T> Empty()
+ {
+ return XYRange<T>(
+ Range<T>(std::numeric_limits<T>::max(), std::numeric_limits<T>::lowest()),
+ Range<T>(std::numeric_limits<T>::max(), std::numeric_limits<T>::lowest())
+ );
+ }
+
+ static XYRange<T> Containing(T x, T y)
+ {
+ return XYRange<T>(
+ Range<T>(x, x),
+ Range<T>(y, y)
+ );
+ }
+
+ XYRange()
+ {
+ }
+
+ XYRange(const Range<T>& xrange, const Range<T>& yrange)
+ : x(xrange), y(yrange)
+ {
+ }
+
+ XYRange(T xmin, T xmax, T ymin, T ymax)
+ : x(xmin,xmax), y(ymin,ymax)
+ {
+ }
+
+ XYRange operator-(const XYRange& o) const
+ {
+ return XYRange(x - o.x, y - o.y);
+ }
+
+ XYRange operator*(float s) const
+ {
+ return XYRange(x*s, y*s);
+ }
+
+ XYRange& operator+=(const XYRange& o)
+ {
+ x += o.x;
+ y += o.y;
+ return *this;
+ }
+
+ void Scale(float sx, float sy, float centerx, float centery)
+ {
+ x.Scale(sx, centerx);
+ y.Scale(sy, centery);
+ }
+
+ void Clear()
+ {
+ x.Clear();
+ y.Clear();
+ }
+
+ void Clamp(T xmin, T xmax, T ymin, T ymax)
+ {
+ x.Clamp(xmin,xmax);
+ y.Clamp(ymin,ymax);
+ }
+
+ void Clamp(const XYRange& o)
+ {
+ x.Clamp(o.x);
+ y.Clamp(o.y);
+ }
+
+ void Insert(T xval, T yval)
+ {
+ x.Insert(xval);
+ y.Insert(yval);
+ }
+
+ void Insert(XYRange<T> r)
+ {
+ x.Insert(r.x);
+ y.Insert(r.y);
+ }
+
+ float Area() const
+ {
+ return x.Size() * y.Size();
+ }
+
+ bool Contains(float px, float py) const
+ {
+ return x.Contains(px) && y.Contains(py);
+ }
+
+ bool ContainsWeak(float px, float py) const
+ {
+ return x.ContainsWeak(px) && y.ContainsWeak(py);
+ }
+
+ template<typename To>
+ XYRange<To> Cast() const
+ {
+ return XYRange<To>(
+ x.template Cast<To>(),
+ y.template Cast<To>()
+ );
+ }
+
+#ifdef USE_EIGEN
+ operator Eigen::AlignedBox<T,2>() const {
+ return Eigen::AlignedBox<T,2>(
+ Eigen::Matrix<T,2,1>(x.min, y.min),
+ Eigen::Matrix<T,2,1>(x.max, y.max)
+ );
+ }
+
+ Eigen::Matrix<T,2,1> Center() const {
+ return Eigen::Matrix<T,2,1>(x.Mid(), y.Mid());
+ }
+#endif
+
+ Range<T> x;
+ Range<T> y;
+};
+
+typedef Range<int> Rangei;
+typedef Range<float> Rangef;
+typedef Range<double> Ranged;
+
+typedef XYRange<int> XYRangei;
+typedef XYRange<float> XYRangef;
+typedef XYRange<double> XYRanged;
+
+template<typename T> inline
+Rangei Round(const Range<T>& r)
+{
+ return Rangei( int(r.min+0.5), int(r.max+0.5) );
+}
+
+template<typename T> inline
+XYRangei Round(const XYRange<T>& r)
+{
+ return XYRangei( Round(r.x), Round(r.y) );
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/axis.h b/externals/Pangolin/include/pangolin/scene/axis.h
new file mode 100644
index 0000000000000000000000000000000000000000..a0a33883485802d448452317e4f22d79873f1e8d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/axis.h
@@ -0,0 +1,117 @@
+#pragma once
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/display/viewport.h>
+#include <pangolin/gl/gldraw.h>
+
+#include <pangolin/scene/renderable.h>
+#include <pangolin/scene/interactive_index.h>
+
+#ifdef HAVE_EIGEN
+# include <Eigen/Geometry>
+#endif
+
+namespace pangolin {
+
+struct Axis : public Renderable, public Interactive
+{
+ Axis()
+ : axis_length(1.0),
+ label_x(InteractiveIndex::I().Store(this)),
+ label_y(InteractiveIndex::I().Store(this)),
+ label_z(InteractiveIndex::I().Store(this))
+ {
+ }
+
+ void Render(const RenderParams&) override {
+ glColor4f(1,0,0,1);
+ glPushName(label_x.Id());
+ glDrawLine(0,0,0, axis_length,0,0);
+ glPopName();
+
+ glColor4f(0,1,0,1);
+ glPushName(label_y.Id());
+ glDrawLine(0,0,0, 0,axis_length,0);
+ glPopName();
+
+ glColor4f(0,0,1,1);
+ glPushName(label_z.Id());
+ glDrawLine(0,0,0, 0,0,axis_length);
+ glPopName();
+ }
+
+ bool Mouse(
+ int button,
+ const GLprecision /*win*/[3], const GLprecision /*obj*/[3], const GLprecision /*normal*/[3],
+ bool /*pressed*/, int button_state, int pickId
+ ) override
+ {
+ PANGOLIN_UNUSED(button);
+ PANGOLIN_UNUSED(button_state);
+ PANGOLIN_UNUSED(pickId);
+
+#ifdef HAVE_EIGEN
+ if((button == MouseWheelUp || button == MouseWheelDown) ) {
+ float scale = (button == MouseWheelUp) ? 0.01f : -0.01f;
+ if(button_state & KeyModifierShift) scale /= 10;
+
+ Eigen::Vector3d rot = Eigen::Vector3d::Zero();
+ Eigen::Vector3d xyz = Eigen::Vector3d::Zero();
+
+
+ if(button_state & KeyModifierCtrl) {
+ // rotate
+ if(pickId == label_x.Id()) {
+ rot << 1,0,0;
+ }else if(pickId == label_y.Id()) {
+ rot << 0,1,0;
+ }else if(pickId == label_z.Id()) {
+ rot << 0,0,1;
+ }else{
+ return false;
+ }
+ }else if(button_state & KeyModifierShift){
+ // translate
+ if(pickId == label_x.Id()) {
+ xyz << 1,0,0;
+ }else if(pickId == label_y.Id()) {
+ xyz << 0,1,0;
+ }else if(pickId == label_z.Id()) {
+ xyz << 0,0,1;
+ }else{
+ return false;
+ }
+ }else{
+ return false;
+ }
+
+ // old from new
+ Eigen::Matrix<double,4,4> T_on = Eigen::Matrix<double,4,4>::Identity();
+ T_on.block<3,3>(0,0) = Eigen::AngleAxis<double>(scale,rot).toRotationMatrix();
+ T_on.block<3,1>(0,3) = scale*xyz;
+
+ // Update
+ T_pc = (ToEigen<double>(T_pc) * T_on.inverse()).eval();
+
+ return true;
+ }
+#endif // HAVE_EIGEN
+
+ return false;
+ }
+
+ virtual bool MouseMotion(
+ const GLprecision /*win*/[3], const GLprecision /*obj*/[3], const GLprecision /*normal*/[3],
+ int /*button_state*/, int /*pickId*/
+ ) override
+ {
+ return false;
+ }
+
+ float axis_length;
+ const InteractiveIndex::Token label_x;
+ const InteractiveIndex::Token label_y;
+ const InteractiveIndex::Token label_z;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/interactive.h b/externals/Pangolin/include/pangolin/scene/interactive.h
new file mode 100644
index 0000000000000000000000000000000000000000..272282df907bcb8b91610dac78a7a5734fb30957
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/interactive.h
@@ -0,0 +1,67 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/display/opengl_render_state.h>
+
+namespace pangolin {
+
+
+struct Interactive
+{
+ static __thread GLuint current_id;
+
+ virtual bool Mouse(
+ int button,
+ const GLprecision win[3], const GLprecision obj[3], const GLprecision normal[3],
+ bool pressed, int button_state, int pickId
+ ) = 0;
+
+ virtual bool MouseMotion(
+ const GLprecision win[3], const GLprecision obj[3], const GLprecision normal[3],
+ int button_state, int pickId
+ ) = 0;
+};
+
+struct RenderParams
+{
+ RenderParams()
+ : render_mode(GL_RENDER)
+ {
+ }
+
+ GLint render_mode;
+};
+
+struct Manipulator : public Interactive
+{
+ virtual void Render(const RenderParams& params) = 0;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/interactive_index.h b/externals/Pangolin/include/pangolin/scene/interactive_index.h
new file mode 100644
index 0000000000000000000000000000000000000000..af9862f69033121ace1c3a94a657e660e580a9ff
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/interactive_index.h
@@ -0,0 +1,115 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <map>
+
+#include <pangolin/scene/interactive.h>
+
+namespace pangolin {
+
+class InteractiveIndex
+{
+public:
+ class Token
+ {
+ public:
+ friend class InteractiveIndex;
+
+ Token()
+ : id(0)
+ {
+ }
+
+ Token(Token&& o)
+ : id(o.id)
+ {
+ o.id = 0;
+ }
+
+ GLint Id() const
+ {
+ return id;
+ }
+
+ ~Token()
+ {
+ if(id) {
+ InteractiveIndex::I().Unstore(*this);
+ }
+ }
+
+ private:
+ Token(GLint id)
+ : id(id)
+ {
+ }
+
+
+ GLint id;
+ };
+
+ static InteractiveIndex& I()
+ {
+ static InteractiveIndex instance;
+ return instance;
+ }
+
+ Interactive* Find(GLuint id)
+ {
+ auto kv = index.find(id);
+ if(kv != index.end()) {
+ return kv->second;
+ }
+ return nullptr;
+ }
+
+ Token Store(Interactive* r)
+ {
+ index[next_id] = r;
+ return Token(next_id++);
+ }
+
+ void Unstore(Token& t)
+ {
+ index.erase(t.id);
+ t.id = 0;
+ }
+
+private:
+ // Private constructor.
+ InteractiveIndex()
+ : next_id(1)
+ {
+ }
+
+ GLint next_id;
+ std::map<GLint, Interactive*> index;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/renderable.h b/externals/Pangolin/include/pangolin/scene/renderable.h
new file mode 100644
index 0000000000000000000000000000000000000000..3e21c727e2ef3c282539d79a836d8e0062fd456a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/renderable.h
@@ -0,0 +1,117 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <memory>
+#include <map>
+#include <random>
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/scene/interactive.h>
+
+namespace pangolin {
+
+class Renderable
+{
+public:
+ using guid_t = GLuint;
+
+ static guid_t UniqueGuid()
+ {
+ static std::random_device rd;
+ static std::mt19937 gen(rd());
+ return (guid_t)gen();
+ }
+
+ Renderable(const std::weak_ptr<Renderable>& parent = std::weak_ptr<Renderable>())
+ : guid(UniqueGuid()), parent(parent), T_pc(IdentityMatrix()), should_show(true)
+ {
+ }
+
+ virtual ~Renderable()
+ {
+ }
+
+ // Default implementation simply renders children.
+ virtual void Render(const RenderParams& params = RenderParams()) {
+ RenderChildren(params);
+ }
+
+ void RenderChildren(const RenderParams& params)
+ {
+ for(auto& p : children) {
+ Renderable& r = *p.second;
+ if(r.should_show) {
+ glPushMatrix();
+ r.T_pc.Multiply();
+ r.Render(params);
+ if(r.manipulator) {
+ r.manipulator->Render(params);
+ }
+ glPopMatrix();
+ }
+ }
+ }
+
+ std::shared_ptr<Renderable> FindChild(guid_t guid)
+ {
+ auto o = children.find(guid);
+ if(o != children.end()) {
+ return o->second;
+ }
+
+ for(auto& kv : children ) {
+ std::shared_ptr<Renderable> c = kv.second->FindChild(guid);
+ if(c) return c;
+ }
+
+ return std::shared_ptr<Renderable>();
+ }
+
+ Renderable& Add(const std::shared_ptr<Renderable>& child)
+ {
+ if(child) {
+ children[child->guid] = child;
+ };
+ return *this;
+ }
+
+ // Renderable properties
+ const guid_t guid;
+ std::weak_ptr<Renderable> parent;
+ pangolin::OpenGlMatrix T_pc;
+ bool should_show;
+
+ // Children
+ std::map<guid_t, std::shared_ptr<Renderable>> children;
+
+ // Manipulator (handler, thing)
+ std::shared_ptr<Manipulator> manipulator;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/scenehandler.h b/externals/Pangolin/include/pangolin/scene/scenehandler.h
new file mode 100644
index 0000000000000000000000000000000000000000..c23f1309e5a2130a3b55e5cd33ee7b31a8da9388
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/scenehandler.h
@@ -0,0 +1,182 @@
+#pragma once
+
+#include <pangolin/handler/handler.h>
+#include <pangolin/scene/renderable.h>
+#include <pangolin/scene/interactive_index.h>
+
+namespace pangolin {
+
+inline void gluPickMatrix(
+ GLdouble x, GLdouble y,
+ GLdouble width, GLdouble height,
+ GLint viewport[4]
+) {
+ GLfloat m[16];
+ GLfloat sx, sy;
+ GLfloat tx, ty;
+ sx = viewport[2] / (GLfloat)width;
+ sy = viewport[3] / (GLfloat)height;
+ tx = (viewport[2] + 2.0f * (viewport[0] - (GLfloat)x)) / (GLfloat)width;
+ ty = (viewport[3] + 2.0f * (viewport[1] - (GLfloat)y)) / (GLfloat)height;
+#define M(row, col) m[col*4+row]
+ M(0, 0) = sx;
+ M(0, 1) = 0.0f;
+ M(0, 2) = 0.0f;
+ M(0, 3) = tx;
+ M(1, 0) = 0.0f;
+ M(1, 1) = sy;
+ M(1, 2) = 0.0f;
+ M(1, 3) = ty;
+ M(2, 0) = 0.0f;
+ M(2, 1) = 0.0f;
+ M(2, 2) = 1.0f;
+ M(2, 3) = 0.0f;
+ M(3, 0) = 0.0f;
+ M(3, 1) = 0.0f;
+ M(3, 2) = 0.0f;
+ M(3, 3) = 1.0f;
+#undef M
+ glMultMatrixf(m);
+}
+
+
+
+struct SceneHandler : public Handler3D
+{
+ SceneHandler(
+ Renderable& scene,
+ OpenGlRenderState& cam_state
+ ) : Handler3D(cam_state), scene(scene)
+ {
+
+ }
+
+ void ProcessHitBuffer(GLint hits, GLuint* buf, std::map<GLuint, Interactive*>& hit_map )
+ {
+ GLuint* closestNames = 0;
+ GLuint closestNumNames = 0;
+ GLuint closestZ = std::numeric_limits<GLuint>::max();
+ for (int i = 0; i < hits; i++) {
+ if (buf[1] < closestZ) {
+ closestNames = buf + 3;
+ closestNumNames = buf[0];
+ closestZ = buf[1];
+ }
+ buf += buf[0] + 3;
+ }
+ for (unsigned int i = 0; i < closestNumNames; i++) {
+ const int pickId = closestNames[i];
+ hit_map[pickId] = InteractiveIndex::I().Find(pickId);
+ }
+ }
+
+ void ComputeHits(pangolin::View& view,
+ const pangolin::OpenGlRenderState& cam_state,
+ int x, int y, int grab_width,
+ std::map<GLuint, Interactive*>& hit_objects )
+ {
+ // Get views viewport / modelview /projection
+ GLint viewport[4] = {view.v.l, view.v.b, view.v.w, view.v.h};
+ pangolin::OpenGlMatrix mv = cam_state.GetModelViewMatrix();
+ pangolin::OpenGlMatrix proj = cam_state.GetProjectionMatrix();
+
+ // Prepare hit buffer object
+ const unsigned int MAX_SEL_SIZE = 64;
+ GLuint vSelectBuf[MAX_SEL_SIZE];
+ glSelectBuffer(MAX_SEL_SIZE, vSelectBuf);
+
+ // Load and adjust modelview projection matrices
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluPickMatrix(x, y, grab_width, grab_width, viewport);
+ proj.Multiply();
+ glMatrixMode(GL_MODELVIEW);
+ mv.Load();
+
+ // Render scenegraph in 'select' mode
+ glRenderMode(GL_SELECT);
+ glInitNames();
+ RenderParams select;
+ select.render_mode = GL_SELECT;
+ scene.Render(select);
+ glFlush();
+
+ GLint nHits = glRenderMode(GL_RENDER);
+ // std::cout << " -- Number of Hits are: " << nHits << std::endl;
+ // std::cout << " -- size of hitobjects: " << hit_objects.size() << std::endl;
+ if (nHits > 0) {
+ ProcessHitBuffer(nHits, vSelectBuf, hit_objects);
+ }
+ }
+
+ void Mouse(pangolin::View& view, pangolin::MouseButton button,
+ int x, int y, bool pressed, int button_state)
+ {
+ GetPosNormal(view, x, y, p, Pw, Pc, n);
+ bool handled = false;
+
+ if (pressed) {
+ m_selected_objects.clear();
+ ComputeHits(view, *cam_state, x, y, 2*hwin+1, m_selected_objects);
+ }
+
+ for (auto kv : m_selected_objects)
+ {
+ Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+ handled |= ir && ir->Mouse( button, p, Pw, n, pressed, button_state, kv.first);
+ }
+
+ if (!handled) {
+ Handler3D::Mouse(view, button, x, y, pressed, button_state);
+ }
+ }
+
+ void MouseMotion(pangolin::View& view, int x, int y, int button_state)
+ {
+ GetPosNormal(view, x, y, p, Pw, Pc, n);
+ bool handled = false;
+ for (auto kv : m_selected_objects)
+ {
+ Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+
+ handled |= ir && ir->MouseMotion( p, Pw, n, button_state, kv.first);
+ }
+ if (!handled) {
+ pangolin::Handler3D::MouseMotion(view, x, y, button_state);
+ }
+ }
+
+ void Special(pangolin::View& view, pangolin::InputSpecial inType,
+ float x, float y, float p1, float p2, float p3, float p4,
+ int button_state)
+ {
+ GetPosNormal(view, (int)x, (int)y, p, Pw, Pc, n);
+
+ bool handled = false;
+
+ if (inType == pangolin::InputSpecialScroll)
+ {
+ m_selected_objects.clear();
+ ComputeHits(view, *cam_state, (int)x, (int)y, 2*hwin+1, m_selected_objects);
+
+ const MouseButton button = p2 > 0 ? MouseWheelUp : MouseWheelDown;
+
+ for (auto kv : m_selected_objects)
+ {
+ Interactive* ir = dynamic_cast<Interactive*>(kv.second);
+ handled |= ir && ir->Mouse( button, p, Pw, n, true, button_state, kv.first);
+ }
+ }
+
+ if (!handled) {
+ pangolin::Handler3D::Special(view, inType, x, y,
+ p1, p2, p3, p4, button_state);
+ }
+ }
+
+ std::map<GLuint, Interactive*> m_selected_objects;
+ Renderable& scene;
+ unsigned int grab_width;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/scene/tree.h b/externals/Pangolin/include/pangolin/scene/tree.h
new file mode 100644
index 0000000000000000000000000000000000000000..9797b8603b5399b5d5fb8f58c9e0ed2d368a67f7
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/scene/tree.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include <map>
+#include <pangolin/display/opengl_render_state.h>
+
+namespace pangolin {
+
+template<typename T, typename TEdge>
+struct TreeNode
+{
+ struct Edge
+ {
+ TEdge parent_child;
+ TreeNode node;
+ };
+
+ T item;
+ std::vector<Edge> edges;
+};
+
+template<typename T, typename TEdge>
+using NodeFunction = std::function<void(TreeNode<T,TEdge>&,const TEdge&)>;
+
+//template<typename T, typename TEdge>
+//void VisitDepthFirst(TreeNode<T,TEdge>& node, const NodeFunction<T,TEdge>& func, const TEdge& T_root_node = TEdge())
+//{
+// func(node, T_root_node);
+// for(auto& e : node.edges) {
+// const TEdge T_root_child = T_root_node * e.parent_child;
+// VisitDepthFirst(e.node, func, T_root_child);
+// }
+//}
+
+//void Eg()
+//{
+// using RenderNode = TreeNode<std::shared_ptr<Renderable>,OpenGlMatrix>;
+
+// RenderNode root;
+// VisitDepthFirst<std::shared_ptr<Renderable>,OpenGlMatrix>(
+// root, [](RenderNode& node, const OpenGlMatrix& T_root_node) {
+// if(node.item) {
+// node.item->DoRender();
+// }
+// }, IdentityMatrix());
+
+//}
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/tools/video_viewer.h b/externals/Pangolin/include/pangolin/tools/video_viewer.h
new file mode 100644
index 0000000000000000000000000000000000000000..c72d17122b8e5738691e9d1fafee2fdab0ad9620
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/tools/video_viewer.h
@@ -0,0 +1,106 @@
+#pragma once
+
+#include <pangolin/display/window.h>
+#include <pangolin/platform.h>
+#include <pangolin/video/video_input.h>
+
+#include <functional>
+#include <mutex>
+#include <string>
+#include <thread>
+
+namespace pangolin
+{
+
+PANGOLIN_EXPORT
+class VideoViewer
+{
+public:
+ typedef std::function<void(const unsigned char* data,
+ const std::vector<Image<unsigned char> >& images,
+ const picojson::value& properties)> FrameChangedCallbackFn;
+
+ static constexpr int FRAME_SKIP = 30;
+
+ VideoViewer(const std::string& window_name, const std::string& input_uri, const std::string& output_uri = "video.pango" );
+ VideoViewer(const VideoViewer&) = delete;
+
+ virtual ~VideoViewer();
+
+ void Run();
+ void RunAsync();
+
+ void Quit();
+ void QuitAndWait();
+
+ inline int TotalFrames() const
+ {
+ return video_playback ? video_playback->GetTotalFrames() : std::numeric_limits<int>::max();
+ }
+
+ // Control playback
+ void OpenInput(const std::string& input_uri);
+ void CloseInput();
+
+ // Control recording
+ void Record();
+ void RecordOneFrame();
+ void StopRecording();
+
+ // Useful for user-control
+ void TogglePlay();
+ void ToggleRecord();
+ void ToggleDiscardBufferedFrames();
+ void ToggleWaitForFrames();
+ void SetDiscardBufferedFrames(bool new_state);
+ void SetWaitForFrames(bool new_state);
+ void Skip(int frames);
+ bool ChangeExposure(int delta_us);
+ bool ChangeGain(float delta);
+ void SetActiveCamera(int delta);
+ void DrawEveryNFrames(int n);
+
+
+ // Register to be notified of new image data
+ void SetFrameChangedCallback(FrameChangedCallbackFn cb);
+
+ void WaitUntilExit();
+
+
+ VideoInput& Video() {return video;}
+ const VideoInput& Video() const {return video;}
+
+ void SetRecordNthFrame(int record_nth_frame_) {
+ record_nth_frame = record_nth_frame_;
+ }
+
+
+protected:
+ void RegisterDefaultKeyShortcutsAndPangoVariables();
+
+ std::mutex control_mutex;
+ std::string window_name;
+ std::thread vv_thread;
+
+ VideoInput video;
+ VideoPlaybackInterface* video_playback;
+ VideoInterface* video_interface;
+
+ std::string output_uri;
+
+ int current_frame;
+ int grab_until;
+ int record_nth_frame;
+ int draw_nth_frame;
+ bool video_grab_wait;
+ bool video_grab_newest;
+ bool should_run;
+ uint16_t active_cam;
+
+ FrameChangedCallbackFn frame_changed_callback;
+};
+
+
+void PANGOLIN_EXPORT RunVideoViewerUI(const std::string& input_uri, const std::string& output_uri);
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/argagg.hpp b/externals/Pangolin/include/pangolin/utils/argagg.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..70e28526a06a9a02a3b14b0fc9bc44a4c6e3c3eb
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/argagg.hpp
@@ -0,0 +1,1548 @@
+/*
+ * @file
+ * @brief
+ * Defines a very simple command line argument parser.
+ *
+ * @copyright
+ * Copyright (c) 2017 Viet The Nguyen
+ *
+ * @copyright
+ * 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:
+ *
+ * @copyright
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * @copyright
+ * 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.
+ */
+#pragma once
+#ifndef ARGAGG_ARGAGG_ARGAGG_HPP
+#define ARGAGG_ARGAGG_ARGAGG_HPP
+
+#ifdef __unix__
+#include <stdio.h>
+#include <unistd.h>
+#endif // #ifdef __unix__
+
+#include <algorithm>
+#include <array>
+#include <cstdlib>
+#include <cstring>
+#include <cctype>
+#include <iterator>
+#include <ostream>
+#include <sstream>
+#include <stdexcept>
+#include <string>
+#include <unordered_map>
+#include <utility>
+#include <vector>
+
+
+/**
+ * @brief
+ * There are only two hard things in Computer Science: cache invalidation and
+ * naming things (Phil Karlton).
+ *
+ * The names of types have to be succint and clear. This has turned out to be a
+ * more difficult thing than I expected. Here you'll find a quick overview of
+ * the type names you'll find in this namespace (and thus "library").
+ *
+ * When a program is invoked it is passed a number of "command line arguments".
+ * Each of these "arguments" is a string (C-string to be more precise). An
+ * "option" is a command line argument that has special meaning. This library
+ * recognizes a command line argument as a potential option if it starts with a
+ * dash ('-') or double-dash ('--').
+ *
+ * A "parser" is a set of "definitions" (not a literal std::set but rather a
+ * std::vector). A parser is represented by the argagg::parser struct.
+ *
+ * A "definition" is a structure with four components that define what
+ * "options" are recognized. The four components are the name of the option,
+ * the strings that represent the option, the option's help text, and how many
+ * arguments the option should expect. "Flags" are the individual strings that
+ * represent the option ("-v" and "--verbose" are flags for the "verbose"
+ * option). A definition is represented by the argagg::definition struct.
+ *
+ * Note at this point that the word "option" can be used interchangeably to
+ * mean the notion of an option and the actual instance of an option given a
+ * set of command line arguments. To be unambiguous we use a "definition" to
+ * represent the notion of an option and an "option result" to represent an
+ * actual option parsed from a set of command line arguments. An "option
+ * result" is represented by the argagg::option_result struct.
+ *
+ * There's one more wrinkle to this: an option can show up multiple times in a
+ * given set of command line arguments. For example, "-n 1 -n 2 -n 3". This
+ * will parse into three distinct argagg::option_result instances, but all of
+ * them correspond to the same argagg::definition. We aggregate these into the
+ * argagg::option_results struct which represents "all parser results for a
+ * given option definition". This argagg::option_results is basically a
+ * std::vector of argagg::option_result.
+ *
+ * Options aren't the only thing parsed though. Positional arguments are also
+ * parsed. Thus a parser produces a result that contains both option results
+ * and positional arguments. The parser results are represented by the
+ * argagg::parser_results struct. All option results are stored in a mapping
+ * from option name to the argagg::option_results. All positional arguments are
+ * simply stored in a vector of C-strings.
+ */
+namespace argagg {
+
+
+/**
+ * @brief
+ * This exception is thrown when a long option is parsed and is given an
+ * argument using the "=" syntax but the option doesn't expect an argument.
+ */
+struct unexpected_argument_error
+: public std::runtime_error {
+ using std::runtime_error::runtime_error;
+};
+
+
+/**
+ * @brief
+ * This exception is thrown when an option is parsed unexpectedly such as when
+ * an argument was expected for a previous option or if an option was found
+ * that has not been defined.
+ */
+struct unexpected_option_error
+: public std::runtime_error {
+ using std::runtime_error::runtime_error;
+};
+
+
+/**
+ * @brief
+ * This exception is thrown when an option requires an argument but is not
+ * provided one. This can happen if another flag was found after the option or
+ * if we simply reach the end of the command line arguments.
+ */
+struct option_lacks_argument_error
+: public std::runtime_error {
+ using std::runtime_error::runtime_error;
+};
+
+
+/**
+ * @brief
+ * This exception is thrown when an option's flag is invalid. This can be the
+ * case if the flag is not prefixed by one or two hyphens or contains non
+ * alpha-numeric characters after the hypens. See is_valid_flag_definition()
+ * for more details.
+ */
+struct invalid_flag
+: public std::runtime_error {
+ using std::runtime_error::runtime_error;
+};
+
+
+/**
+ * @brief
+ * The set of template instantiations that convert C-strings to other types for
+ * the option_result::as(), option_results::as(), parser_results::as(), and
+ * parser_results::all_as() methods are placed in this namespace.
+ */
+namespace convert {
+
+ /**
+ * @brief
+ * Explicit instantiations of this function are used to convert arguments to
+ * types.
+ */
+ template <typename T>
+ T arg(const char* arg);
+
+}
+
+
+/**
+ * @brief
+ * Represents a single option parse result.
+ *
+ * You can check if this has an argument by using the implicit boolean
+ * conversion.
+ */
+struct option_result {
+
+ /**
+ * @brief
+ * Argument parsed for this single option. If no argument was parsed this
+ * will be set to nullptr.
+ */
+ const char* arg;
+
+ /**
+ * @brief
+ * Converts the argument parsed for this single option instance into the
+ * given type using the type matched conversion function
+ * argagg::convert::arg(). If there was not an argument parsed for this
+ * single option instance then a argagg::option_lacks_argument_error
+ * exception is thrown. The specific conversion function may throw other
+ * exceptions.
+ */
+ template <typename T>
+ T as() const;
+
+ /**
+ * @brief
+ * Converts the argument parsed for this single option instance into the
+ * given type using the type matched conversion function
+ * argagg::convert::arg(). If there was not an argument parsed for this
+ * single option instance then the provided default value is returned
+ * instead. If the conversion function throws an exception then it is ignored
+ * and the default value is returned.
+ */
+ template <typename T>
+ T as(const T& t) const;
+
+ /**
+ * @brief
+ * Since we have the argagg::option_result::as() API we might as well alias
+ * it as an implicit conversion operator. This performs implicit conversion
+ * using the argagg::option_result::as() method.
+ *
+ * @note
+ * An implicit boolean conversion specialization exists which returns false
+ * if there is no argument for this single option instance and true
+ * otherwise. This specialization DOES NOT convert the argument to a bool. If
+ * you need to convert the argument to a bool then use the as() API.
+ */
+ template <typename T>
+ operator T () const;
+
+};
+
+
+/**
+ * @brief
+ * Represents multiple option parse results for a single option. If treated as
+ * a single parse result it defaults to the last parse result. Note that an
+ * instance of this struct is always created even if no option results are
+ * parsed for a given definition. In that case it will simply be empty.
+ *
+ * To check if the associated option showed up at all simply use the implicit
+ * boolean conversion or check if count() is greater than zero.
+ */
+struct option_results {
+
+ /**
+ * @brief
+ * All option parse results for this option.
+ */
+ std::vector<option_result> all;
+
+ /**
+ * @brief
+ * Gets the number of times the option shows up.
+ */
+ std::size_t count() const;
+
+ /**
+ * @brief
+ * Gets a single option parse result by index.
+ */
+ option_result& operator [] (std::size_t index);
+
+ /**
+ * @brief
+ * Gets a single option result by index.
+ */
+ const option_result& operator [] (std::size_t index) const;
+
+ /**
+ * @brief
+ * Converts the argument parsed for the LAST option parse result for the
+ * parent definition to the provided type. For example, if this was for "-f 1
+ * -f 2 -f 3" then calling this method for an integer type will return 3. If
+ * there are no option parse results then a std::out_of_range exception is
+ * thrown. Any exceptions thrown by option_result::as() are not
+ * handled.
+ */
+ template <typename T>
+ T as() const;
+
+ /**
+ * @brief
+ * Converts the argument parsed for the LAST option parse result for the
+ * parent definition to the provided type. For example, if this was for "-f 1
+ * -f 2 -f 3" then calling this method for an integer type will return 3. If
+ * there are no option parse results then the provided default value is
+ * returned instead.
+ */
+ template <typename T>
+ T as(const T& t) const;
+
+ /**
+ * @brief
+ * Since we have the option_results::as() API we might as well alias
+ * it as an implicit conversion operator. This performs implicit conversion
+ * using the option_results::as() method.
+ *
+ * @note
+ * An implicit boolean conversion specialization exists which returns false
+ * if there is no argument for this single option instance and true
+ * otherwise. This specialization DOES NOT convert the argument to a bool. If
+ * you need to convert the argument to a bool then use the as() API.
+ */
+ template <typename T>
+ operator T () const;
+
+};
+
+
+/**
+ * @brief
+ * Represents all results of the parser including options and positional
+ * arguments.
+ */
+struct parser_results {
+
+ /**
+ * @brief
+ * Returns the name of the program from the original arguments list. This is
+ * always the first argument.
+ */
+ const char* program;
+
+ /**
+ * @brief
+ * Maps from definition name to the structure which contains the parser
+ * results for that definition.
+ */
+ std::unordered_map<std::string, option_results> options;
+
+ /**
+ * @brief
+ * Vector of positional arguments.
+ */
+ std::vector<const char*> pos;
+
+ /**
+ * @brief
+ * Used to check if an option was specified at all.
+ */
+ bool has_option(const std::string& name) const;
+
+ /**
+ * @brief
+ * Get the parser results for the given definition. If the definition never
+ * showed up then the exception from the unordered_map access will bubble
+ * through so check if the flag exists in the first place with has_option().
+ */
+ option_results& operator [] (const std::string& name);
+
+ /**
+ * @brief
+ * Get the parser results for the given definition. If the definition never
+ * showed up then the exception from the unordered_map access will bubble
+ * through so check if the flag exists in the first place with has_option().
+ */
+ const option_results& operator [] (const std::string& name) const;
+
+ /**
+ * @brief
+ * Gets the number of positional arguments.
+ */
+ std::size_t count() const;
+
+ /**
+ * @brief
+ * Gets a positional argument by index.
+ */
+ const char* operator [] (std::size_t index) const;
+
+ /**
+ * @brief
+ * Gets a positional argument converted to the given type.
+ */
+ template <typename T>
+ T as(std::size_t i = 0) const;
+
+ /**
+ * @brief
+ * Gets all positional arguments converted to the given type.
+ */
+ template <typename T>
+ std::vector<T> all_as() const;
+
+};
+
+
+/**
+ * @brief
+ * An option definition which essentially represents what an option is.
+ */
+struct definition {
+
+ /**
+ * @brief
+ * Name of the option. Option parser results are keyed by this name.
+ */
+ const std::string name;
+
+ /**
+ * @brief
+ * List of strings to match that correspond to this option. Should be fully
+ * specified with hyphens (e.g. "-v" or "--verbose").
+ */
+ std::vector<std::string> flags;
+
+ /**
+ * @brief
+ * Help string for this option.
+ */
+ std::string help;
+
+ /**
+ * @brief
+ * Number of arguments this option requires. Must be 0 or 1. All other values
+ * have undefined behavior. Okay, the code actually works with positive
+ * values in general, but it's unorthodox command line behavior.
+ */
+ unsigned int num_args;
+
+ /**
+ * @brief
+ * Returns true if this option does not want any arguments.
+ */
+ bool wants_no_arguments() const;
+
+ /**
+ * @brief
+ * Returns true if this option requires arguments.
+ */
+ bool requires_arguments() const;
+
+};
+
+
+/**
+ * @brief
+ * Checks whether or not a command line argument should be processed as an
+ * option flag. This is very similar to is_valid_flag_definition() but must
+ * allow for short flag groups (e.g. "-abc") and equal-assigned long flag
+ * arguments (e.g. "--output=foo.txt").
+ */
+bool cmd_line_arg_is_option_flag(
+ const char* s);
+
+
+/**
+ * @brief
+ * Checks whether a flag in an option definition is valid. I suggest reading
+ * through the function source to understand what dictates a valid.
+ */
+bool is_valid_flag_definition(
+ const char* s);
+
+
+/**
+ * @brief
+ * Tests whether or not a valid flag is short. Assumes the provided cstring is
+ * already a valid flag.
+ */
+bool flag_is_short(
+ const char* s);
+
+
+/**
+ * @brief
+ * Contains two maps which aid in option parsing. The first map, @ref
+ * short_map, maps from a short flag (just a character) to a pointer to the
+ * original @ref definition that the flag represents. The second map, @ref
+ * long_map, maps from a long flag (an std::string) to a pointer to the
+ * original @ref definition that the flag represents.
+ *
+ * This object is usually a temporary that only exists during the parsing
+ * operation. It is typically constructed using @ref validate_definitions().
+ */
+struct parser_map {
+
+ /**
+ * @brief
+ * Maps from a short flag (just a character) to a pointer to the original
+ * @ref definition that the flag represents.
+ */
+ std::array<const definition*, 256> short_map;
+
+ /**
+ * @brief
+ * Maps from a long flag (an std::string) to a pointer to the original @ref
+ * definition that the flag represents.
+ */
+ std::unordered_map<std::string, const definition*> long_map;
+
+ /**
+ * @brief
+ * Returns true if the provided short flag exists in the map object.
+ */
+ bool known_short_flag(
+ const char flag) const;
+
+ /**
+ * @brief
+ * If the short flag exists in the map object then it is returned by this
+ * method. If it doesn't then nullptr will be returned.
+ */
+ const definition* get_definition_for_short_flag(
+ const char flag) const;
+
+ /**
+ * @brief
+ * Returns true if the provided long flag exists in the map object.
+ */
+ bool known_long_flag(
+ const std::string& flag) const;
+
+ /**
+ * @brief
+ * If the long flag exists in the map object then it is returned by this
+ * method. If it doesn't then nullptr will be returned.
+ */
+ const definition* get_definition_for_long_flag(
+ const std::string& flag) const;
+
+};
+
+
+/**
+ * @brief
+ * Validates a collection (specifically an std::vector) of @ref definition
+ * objects by checking if the contained flags are valid. If the set of @ref
+ * definition objects is not valid then an exception is thrown. Upon successful
+ * validation a @ref parser_map object is returned.
+ */
+parser_map validate_definitions(
+ const std::vector<definition>& definitions);
+
+
+/**
+ * @brief
+ * A list of option definitions used to inform how to parse arguments.
+ */
+struct parser {
+
+ /**
+ * @brief
+ * Vector of the option definitions which inform this parser how to parse
+ * the command line arguments.
+ */
+ std::vector<definition> definitions;
+
+ /**
+ * @brief
+ * Parses the provided command line arguments and returns the results as
+ * @ref parser_results.
+ *
+ * @note
+ * This method is not thread-safe and assumes that no modifications are made
+ * to the definitions member field during the extent of this method call.
+ */
+ parser_results parse(int argc, const char** argv) const;
+
+ /**
+ * @brief
+ * Through strict interpretation of pointer casting rules, despite this being
+ * a safe operation, C++ doesn't allow implicit casts from <tt>char**</tt> to
+ * <tt>const char**</tt> so here's an overload that performs a const_cast,
+ * which is typically frowned upon but is safe here.
+ */
+ parser_results parse(int argc, char** argv) const;
+
+};
+
+
+/**
+ * @brief
+ * A convenience output stream that will accumulate what is streamed to it and
+ * then, on destruction, format the accumulated string using the fmt program
+ * (via the argagg::fmt_string() function) to the provided std::ostream.
+ *
+ * Example use:
+ *
+ * @code
+ * {
+ * argagg::fmt_ostream f(std::cerr);
+ * f << "Usage: " << really_long_string << std::endl;
+ * } // on destruction here the formatted string will be streamed to std::cerr
+ * @endcode
+ *
+ * @note
+ * This only has formatting behavior if the <tt>__unix__</tt> preprocessor
+ * definition is defined since formatting relies on the POSIX API for forking,
+ * executing a process, and reading/writing to/from file descriptors. If that
+ * preprocessor definition is not defined then this class has the same overall
+ * behavior except the output string is not formatted (basically streams
+ * whatever the accumulated string is). See arggg::fmt_string().
+ */
+struct fmt_ostream : public std::ostringstream {
+
+ /**
+ * @brief
+ * Reference to the final output stream that the formatted string will be
+ * streamed to.
+ */
+ std::ostream& output;
+
+ /**
+ * @brief
+ * Construct to output to the provided output stream when this object is
+ * destroyed.
+ */
+ fmt_ostream(std::ostream& output);
+
+ /**
+ * @brief
+ * Special destructor that will format the accumulated string using fmt (via
+ * the argagg::fmt_string() function) and stream it to the std::ostream
+ * stored.
+ */
+ ~fmt_ostream();
+
+};
+
+
+/**
+ * @brief
+ * Processes the provided string using the fmt util and returns the resulting
+ * output as a string. Not the most efficient (in time or space) but gets the
+ * job done.
+ *
+ * This function is cowardly so if there are any errors encountered such as a
+ * syscall returning -1 then the input string is returned.
+ *
+ * @note
+ * This only has formatting behavior if the <tt>__unix__</tt> preprocessor
+ * definition is defined since it relies on the POSIX API for forking,
+ * executing a process, reading/writing to/from file descriptors, and the
+ * existence of the fmt util.
+ */
+std::string fmt_string(const std::string& s);
+
+
+} // namespace argagg
+
+
+/**
+ * @brief
+ * Writes the option help to the given stream.
+ */
+std::ostream& operator << (std::ostream& os, const argagg::parser& x);
+
+
+// ---- end of declarations, header-only implementations follow ----
+
+
+namespace argagg {
+
+
+template <typename T>
+T option_result::as() const
+{
+ if (this->arg) {
+ return convert::arg<T>(this->arg);
+ } else {
+ throw option_lacks_argument_error("option has no argument");
+ }
+}
+
+
+template <typename T>
+T option_result::as(const T& t) const
+{
+ if (this->arg) {
+ try {
+ return convert::arg<T>(this->arg);
+ } catch (...) {
+ return t;
+ }
+ } else {
+ // I actually think this will never happen. To call this method you have
+ // to access a specific option_result for an option. If there's a
+ // specific option_result then the option was found. If the option
+ // requires an argument then it will definitely have an argument
+ // otherwise the parser would have complained.
+ return t;
+ }
+}
+
+
+template <typename T>
+option_result::operator T () const
+{
+ return this->as<T>();
+}
+
+
+template <> inline
+option_result::operator bool () const
+{
+ return this->arg != nullptr;
+}
+
+
+inline
+std::size_t option_results::count() const
+{
+ return this->all.size();
+}
+
+
+inline
+option_result& option_results::operator [] (std::size_t index)
+{
+ return this->all[index];
+}
+
+
+inline
+const option_result& option_results::operator [] (std::size_t index) const
+{
+ return this->all[index];
+}
+
+
+template <typename T>
+T option_results::as() const
+{
+ if (this->all.size() == 0) {
+ throw std::out_of_range("no option arguments to convert");
+ }
+ return this->all.back().as<T>();
+}
+
+
+template <typename T>
+T option_results::as(const T& t) const
+{
+ if (this->all.size() == 0) {
+ return t;
+ }
+ return this->all.back().as<T>(t);
+}
+
+
+template <typename T>
+option_results::operator T () const
+{
+ return this->as<T>();
+}
+
+
+template <> inline
+option_results::operator bool () const
+{
+ return this->all.size() > 0;
+}
+
+
+inline
+bool parser_results::has_option(const std::string& name) const
+{
+ const auto it = this->options.find(name);
+ return ( it != this->options.end()) && it->second.all.size() > 0;
+}
+
+
+inline
+option_results& parser_results::operator [] (const std::string& name)
+{
+ return this->options.at(name);
+}
+
+
+inline
+const option_results&
+parser_results::operator [] (const std::string& name) const
+{
+ return this->options.at(name);
+}
+
+
+inline
+std::size_t parser_results::count() const
+{
+ return this->pos.size();
+}
+
+
+inline
+const char* parser_results::operator [] (std::size_t index) const
+{
+ return this->pos[index];
+}
+
+
+template <typename T>
+T parser_results::as(std::size_t i) const
+{
+ return convert::arg<T>(this->pos[i]);
+}
+
+
+template <typename T>
+std::vector<T> parser_results::all_as() const
+{
+ std::vector<T> v(this->pos.size());
+ std::transform(
+ this->pos.begin(), this->pos.end(), v.begin(),
+ [](const char* arg) {
+ return convert::arg<T>(arg);
+ });
+ return v;
+}
+
+
+inline
+bool definition::wants_no_arguments() const
+{
+ return this->num_args == 0;
+}
+
+
+inline
+bool definition::requires_arguments() const
+{
+ return this->num_args > 0;
+}
+
+
+inline
+bool cmd_line_arg_is_option_flag(
+ const char* s)
+{
+ auto len = std::strlen(s);
+
+ // The shortest possible flag has two characters: a hyphen and an
+ // alpha-numeric character.
+ if (len < 2) {
+ return false;
+ }
+
+ // All flags must start with a hyphen.
+ if (s[0] != '-') {
+ return false;
+ }
+
+ // Shift the name forward by a character to account for the initial hyphen.
+ // This means if s was originally "-v" then name will be "v".
+ const char* name = s + 1;
+
+ // Check if we're dealing with a long flag.
+ bool is_long = false;
+ if (s[1] == '-') {
+ is_long = true;
+
+ // Just -- is not a valid flag.
+ if (len == 2) {
+ return false;
+ }
+
+ // Shift the name forward to account for the extra hyphen. This means if s
+ // was originally "--output" then name will be "output".
+ name = s + 2;
+ }
+
+ // The first character of the flag name must be alpha-numeric. This is to
+ // prevent things like "---a" from being valid flags.
+ len = std::strlen(name);
+ if (!std::isalnum(name[0])) {
+ return false;
+ }
+
+ // At this point in is_valid_flag_definition() we would check if the short
+ // flag has only one character. At command line specification you can group
+ // short flags together or even add an argument to a short flag without a
+ // space delimiter. Thus we don't check if this has only one character
+ // because it might not.
+
+ // If this is a long flag then we expect all characters *up to* an equal sign
+ // to be alpha-numeric or a hyphen. After the equal sign you are specify the
+ // argument to a long flag which can be basically anything.
+ if (is_long) {
+ bool encountered_equal = false;
+ return std::all_of(name, name + len, [&](const char& c) {
+ if (encountered_equal) {
+ return true;
+ } else {
+ if (c == '=') {
+ encountered_equal = true;
+ return true;
+ }
+ return std::isalnum(c) || c == '-';
+ }
+ });
+ }
+
+ // At this point we are not dealing with a long flag. We already checked that
+ // the first character is alpha-numeric so we've got the case of a single
+ // short flag covered. This might be a short flag group though and we might
+ // be tempted to check that each character of the short flag group is
+ // alpha-numeric. However, you can specify the argument for a short flag
+ // without a space delimiter (e.g. "-I/usr/local/include") so you can't tell
+ // if the rest of a short flag group is part of the argument or not unless
+ // you know what is a defined flag or not. We leave that kind of processing
+ // to the parser.
+ return true;
+}
+
+
+inline
+bool is_valid_flag_definition(
+ const char* s)
+{
+ auto len = std::strlen(s);
+
+ // The shortest possible flag has two characters: a hyphen and an
+ // alpha-numeric character.
+ if (len < 2) {
+ return false;
+ }
+
+ // All flags must start with a hyphen.
+ if (s[0] != '-') {
+ return false;
+ }
+
+ // Shift the name forward by a character to account for the initial hyphen.
+ // This means if s was originally "-v" then name will be "v".
+ const char* name = s + 1;
+
+ // Check if we're dealing with a long flag.
+ bool is_long = false;
+ if (s[1] == '-') {
+ is_long = true;
+
+ // Just -- is not a valid flag.
+ if (len == 2) {
+ return false;
+ }
+
+ // Shift the name forward to account for the extra hyphen. This means if s
+ // was originally "--output" then name will be "output".
+ name = s + 2;
+ }
+
+ // The first character of the flag name must be alpha-numeric. This is to
+ // prevent things like "---a" from being valid flags.
+ len = std::strlen(name);
+ if (!std::isalnum(name[0])) {
+ return false;
+ }
+
+ // If this is a short flag then it must only have one character.
+ if (!is_long && len > 1) {
+ return false;
+ }
+
+ // The rest of the characters must be alpha-numeric, but long flags are
+ // allowed to have hyphens too.
+ return std::all_of(name + 1, name + len, [&](const char& c) {
+ return std::isalnum(c) || (c == '-' && is_long);
+ });
+}
+
+
+inline
+bool flag_is_short(
+ const char* s)
+{
+ return s[0] == '-' && std::isalnum(s[1]);
+}
+
+
+inline
+bool parser_map::known_short_flag(
+ const char flag) const
+{
+ return this->short_map[flag] != nullptr;
+}
+
+
+inline
+const definition* parser_map::get_definition_for_short_flag(
+ const char flag) const
+{
+ return this->short_map[flag];
+}
+
+
+inline
+bool parser_map::known_long_flag(
+ const std::string& flag) const
+{
+ const auto existing_long_flag = this->long_map.find(flag);
+ return existing_long_flag != long_map.end();
+}
+
+
+inline
+const definition* parser_map::get_definition_for_long_flag(
+ const std::string& flag) const
+{
+ const auto existing_long_flag = this->long_map.find(flag);
+ if (existing_long_flag == long_map.end()) {
+ return nullptr;
+ }
+ return existing_long_flag->second;
+}
+
+
+inline
+parser_map validate_definitions(
+ const std::vector<definition>& definitions)
+{
+ std::unordered_map<std::string, const definition*> long_map;
+ parser_map map {{{nullptr}}, std::move(long_map)};
+
+ for (auto& defn : definitions) {
+
+ if (defn.flags.size() == 0) {
+ std::ostringstream msg;
+ msg << "option \"" << defn.name << "\" has no flag definitions";
+ throw invalid_flag(msg.str());
+ }
+
+ for (auto& flag : defn.flags) {
+
+ if (!is_valid_flag_definition(flag.data())) {
+ std::ostringstream msg;
+ msg << "flag \"" << flag << "\" specified for option \"" << defn.name
+ << "\" is invalid";
+ throw invalid_flag(msg.str());
+ }
+
+ if (flag_is_short(flag.data())) {
+ const int short_flag_letter = flag[1];
+ const auto existing_short_flag = map.short_map[short_flag_letter];
+ bool short_flag_already_exists = (existing_short_flag != nullptr);
+ if (short_flag_already_exists) {
+ std::ostringstream msg;
+ msg << "duplicate short flag \"" << flag
+ << "\" found, specified by both option \"" << defn.name
+ << "\" and option \"" << existing_short_flag->name;
+ throw invalid_flag(msg.str());
+ }
+ map.short_map[short_flag_letter] = &defn;
+ continue;
+ }
+
+ // If we're here then this is a valid, long-style flag.
+ if (map.known_long_flag(flag)) {
+ const auto existing_long_flag = map.get_definition_for_long_flag(flag);
+ std::ostringstream msg;
+ msg << "duplicate long flag \"" << flag
+ << "\" found, specified by both option \"" << defn.name
+ << "\" and option \"" << existing_long_flag->name;
+ throw invalid_flag(msg.str());
+ }
+ map.long_map.insert(std::make_pair(flag, &defn));
+ }
+ }
+
+ return map;
+}
+
+
+inline
+parser_results parser::parse(int argc, const char** argv) const
+{
+ // Inspect each definition to see if its valid. You may wonder "why don't
+ // you do this validation on construction?" I had thought about it but
+ // realized that since I've made the parser an aggregate type (granted it
+ // just "aggregates" a single vector) I would need to track any changes to
+ // the definitions vector and re-run the validity check in order to
+ // maintain this expected "validity invariant" on the object. That would
+ // then require hiding the definitions vector as a private entry and then
+ // turning the parser into a thin interface (by re-exposing setters and
+ // getters) to the vector methods just so that I can catch when the
+ // definition has been modified. It seems much simpler to just enforce the
+ // validity when you actually want to parser because it's at the moment of
+ // parsing that you know the definitions are complete.
+ parser_map map = validate_definitions(this->definitions);
+
+ // Initialize the parser results that we'll be returning. Store the program
+ // name (assumed to be the first command line argument) and initialize
+ // everything else as empty.
+ std::unordered_map<std::string, option_results> options {};
+ std::vector<const char*> pos;
+ parser_results results {argv[0], std::move(options), std::move(pos)};
+
+ // Add an empty option result for each definition.
+ for (const auto& defn : this->definitions) {
+ option_results opt_results {{}};
+ results.options.insert(
+ std::make_pair(defn.name, opt_results));
+ }
+
+ // Don't start off ignoring flags. We only ignore flags after a -- shows up
+ // in the command line arguments.
+ bool ignore_flags = false;
+
+ // Keep track of any options that are expecting arguments.
+ const char* last_flag_expecting_args = nullptr;
+ option_result* last_option_expecting_args = nullptr;
+ unsigned int num_option_args_to_consume = 0;
+
+ // Get pointers to pointers so we can treat the raw pointer array as an
+ // iterator for standard library algorithms. This isn't used yet but can be
+ // used to template this function to work on iterators over strings or
+ // C-strings.
+ const char** arg_i = argv + 1;
+ const char** arg_end = argv + argc;
+
+ while (arg_i != arg_end) {
+ auto arg_i_cstr = *arg_i;
+ auto arg_i_len = std::strlen(arg_i_cstr);
+
+ // Some behavior to note: if the previous option is expecting an argument
+ // then the next entry will be treated as a positional argument even if
+ // it looks like a flag.
+ bool treat_as_positional_argument = (
+ ignore_flags
+ || num_option_args_to_consume > 0
+ || !cmd_line_arg_is_option_flag(arg_i_cstr)
+ );
+ if (treat_as_positional_argument) {
+
+ // If last option is expecting some specific positive number of
+ // arguments then give this argument to that option, *regardless of
+ // whether or not the argument looks like a flag or is the special "--"
+ // argument*.
+ if (num_option_args_to_consume > 0) {
+ last_option_expecting_args->arg = arg_i_cstr;
+ --num_option_args_to_consume;
+ ++arg_i;
+ continue;
+ }
+
+ // Now we check if this is just "--" which is a special argument that
+ // causes all following arguments to be treated as non-options and is
+ // itselve discarded.
+ if (std::strncmp(arg_i_cstr, "--", 2) == 0 && arg_i_len == 2) {
+ ignore_flags = true;
+ ++arg_i;
+ continue;
+ }
+
+ // If there are no expectations for option arguments then simply use
+ // this argument as a positional argument.
+ results.pos.push_back(arg_i_cstr);
+ ++arg_i;
+ continue;
+ }
+
+ // Reset the "expecting argument" state.
+ last_flag_expecting_args = nullptr;
+ last_option_expecting_args = nullptr;
+ num_option_args_to_consume = 0;
+
+ // If we're at this point then we're definitely dealing with something
+ // that is flag-like and has hyphen as the first character and has a
+ // length of at least two characters. How we handle this potential flag
+ // depends on whether or not it is a long-option so we check that first.
+ bool is_long_flag = (arg_i_cstr[1] == '-');
+
+ if (is_long_flag) {
+
+ // Long flags have a complication: their arguments can be specified
+ // using an '=' character right inside the argument. That means an
+ // argument like "--output=foobar.txt" is actually an option with flag
+ // "--output" and argument "foobar.txt". So we look for the first
+ // instance of the '=' character and keep it in long_flag_arg. If
+ // long_flag_arg is nullptr then we didn't find '='. We need the
+ // flag_len to construct long_flag_str below.
+ auto long_flag_arg = std::strchr(arg_i_cstr, '=');
+ std::size_t flag_len = arg_i_len;
+ if (long_flag_arg != nullptr) {
+ flag_len = long_flag_arg - arg_i_cstr;
+ }
+ std::string long_flag_str(arg_i_cstr, flag_len);
+
+ if (!map.known_long_flag(long_flag_str)) {
+ std::ostringstream msg;
+ msg << "found unexpected flag: " << long_flag_str;
+ throw unexpected_option_error(msg.str());
+ }
+
+ const auto defn = map.get_definition_for_long_flag(long_flag_str);
+
+ if (long_flag_arg != nullptr && defn->num_args == 0) {
+ std::ostringstream msg;
+ msg << "found argument for option not expecting an argument: "
+ << arg_i_cstr;
+ throw unexpected_argument_error(msg.str());
+ }
+
+ // We've got a legitimate, known long flag option so we add an option
+ // result. This option result initially has an arg of nullptr, but that
+ // might change in the following block.
+ auto& opt_results = results.options[defn->name];
+ option_result opt_result {nullptr};
+ opt_results.all.push_back(std::move(opt_result));
+
+ if (defn->requires_arguments()) {
+ bool there_is_an_equal_delimited_arg = (long_flag_arg != nullptr);
+ if (there_is_an_equal_delimited_arg) {
+ // long_flag_arg would be "=foo" in the "--output=foo" case so we
+ // increment by 1 to get rid of the equal sign.
+ opt_results.all.back().arg = long_flag_arg + 1;
+ } else {
+ last_flag_expecting_args = arg_i_cstr;
+ last_option_expecting_args = &(opt_results.all.back());
+ num_option_args_to_consume = defn->num_args;
+ }
+ }
+
+ ++arg_i;
+ continue;
+ }
+
+ // If we've made it here then we're looking at either a short flag or a
+ // group of short flags. Short flags can be grouped together so long as
+ // they don't require any arguments unless the option that does is the
+ // last in the group ("-o x -v" is okay, "-vo x" is okay, "-ov x" is
+ // not). So starting after the dash we're going to process each character
+ // as if it were a separate flag. Note "sf_idx" stands for "short flag
+ // index".
+ for (std::size_t sf_idx = 1; sf_idx < arg_i_len; ++sf_idx) {
+ const auto short_flag = arg_i_cstr[sf_idx];
+
+ if (!std::isalnum(short_flag)) {
+ std::ostringstream msg;
+ msg << "found non-alphanumeric character '" << arg_i_cstr[sf_idx]
+ << "' in flag group '" << arg_i_cstr << "'";
+ throw std::domain_error(msg.str());
+ }
+
+ if (!map.known_short_flag(short_flag)) {
+ std::ostringstream msg;
+ msg << "found unexpected flag '" << arg_i_cstr[sf_idx]
+ << "' in flag group '" << arg_i_cstr << "'";
+ throw unexpected_option_error(msg.str());
+ }
+
+ auto defn = map.get_definition_for_short_flag(short_flag);
+ auto& opt_results = results.options[defn->name];
+
+ // Create an option result with an empty argument (for now) and add it
+ // to this option's results.
+ option_result opt_result {nullptr};
+ opt_results.all.push_back(std::move(opt_result));
+
+ if (defn->requires_arguments()) {
+
+ // If this short flag's option requires an argument and we're the
+ // last flag in the short flag group then just put the parser into
+ // "expecting argument for last option" state and move onto the next
+ // command line argument.
+ bool is_last_short_flag_in_group = (sf_idx == arg_i_len - 1);
+ if (is_last_short_flag_in_group) {
+ last_flag_expecting_args = arg_i_cstr;
+ last_option_expecting_args = &(opt_results.all.back());
+ num_option_args_to_consume = defn->num_args;
+ break;
+ }
+
+ // If this short flag's option requires an argument and we're NOT the
+ // last flag in the short flag group then we automatically consume
+ // the rest of the short flag group as the argument for this flag.
+ // This is how we get the POSIX behavior of being able to specify a
+ // flag's arguments without a white space delimiter (e.g.
+ // "-I/usr/local/include").
+ opt_results.all.back().arg = arg_i_cstr + sf_idx + 1;
+ break;
+ }
+ }
+
+ ++arg_i;
+ continue;
+ }
+
+ // If we're done with all of the arguments but are still expecting
+ // arguments for a previous option then we haven't satisfied that option.
+ // This is an error.
+ if (num_option_args_to_consume > 0) {
+ std::ostringstream msg;
+ msg << "last option \"" << last_flag_expecting_args
+ << "\" expects an argument but the parser ran out of command line "
+ << "arguments to parse";
+ throw option_lacks_argument_error(msg.str());
+ }
+
+ return results;
+}
+
+
+inline
+parser_results parser::parse(int argc, char** argv) const
+{
+ return parse(argc, const_cast<const char**>(argv));
+}
+
+
+namespace convert {
+
+
+ /**
+ * @brief
+ * Templated function for conversion to T using the @ref std::strtol()
+ * function. This is used for anything long length or shorter (long, int,
+ * short, char).
+ */
+ template <typename T> inline
+ T long_(const char* arg)
+ {
+ char* endptr = nullptr;
+ errno = 0;
+ T ret = static_cast<T>(std::strtol(arg, &endptr, 0));
+ if (endptr == arg) {
+ std::ostringstream msg;
+ msg << "unable to convert argument to integer: \"" << arg << "\"";
+ throw std::invalid_argument(msg.str());
+ }
+ if (errno == ERANGE) {
+ throw std::out_of_range("argument numeric value out of range");
+ }
+ return ret;
+ }
+
+
+ /**
+ * @brief
+ * Templated function for conversion to T using the @ref std::strtoll()
+ * function. This is used for anything long long length or shorter (long
+ * long).
+ */
+ template <typename T> inline
+ T long_long_(const char* arg)
+ {
+ char* endptr = nullptr;
+ errno = 0;
+ T ret = static_cast<T>(std::strtoll(arg, &endptr, 0));
+ if (endptr == arg) {
+ std::ostringstream msg;
+ msg << "unable to convert argument to integer: \"" << arg << "\"";
+ throw std::invalid_argument(msg.str());
+ }
+ if (errno == ERANGE) {
+ throw std::out_of_range("argument numeric value out of range");
+ }
+ return ret;
+ }
+
+
+#define DEFINE_CONVERSION_FROM_LONG_(TYPE) \
+ template <> inline \
+ TYPE arg(const char* arg) \
+ { \
+ return long_<TYPE>(arg); \
+ }
+
+ DEFINE_CONVERSION_FROM_LONG_(char)
+ DEFINE_CONVERSION_FROM_LONG_(unsigned char)
+ DEFINE_CONVERSION_FROM_LONG_(signed char)
+ DEFINE_CONVERSION_FROM_LONG_(short)
+ DEFINE_CONVERSION_FROM_LONG_(unsigned short)
+ DEFINE_CONVERSION_FROM_LONG_(int)
+ DEFINE_CONVERSION_FROM_LONG_(unsigned int)
+ DEFINE_CONVERSION_FROM_LONG_(long)
+ DEFINE_CONVERSION_FROM_LONG_(unsigned long)
+
+#undef DEFINE_CONVERSION_FROM_LONG_
+
+
+#define DEFINE_CONVERSION_FROM_LONG_LONG_(TYPE) \
+ template <> inline \
+ TYPE arg(const char* arg) \
+ { \
+ return long_long_<TYPE>(arg); \
+ }
+
+ DEFINE_CONVERSION_FROM_LONG_LONG_(long long)
+ DEFINE_CONVERSION_FROM_LONG_LONG_(unsigned long long)
+
+#undef DEFINE_CONVERSION_FROM_LONG_LONG_
+
+
+ template <> inline
+ bool arg(const char* arg)
+ {
+ return argagg::convert::arg<int>(arg) != 0;
+ }
+
+
+ template <> inline
+ float arg(const char* arg)
+ {
+ char* endptr = nullptr;
+ errno = 0;
+ float ret = std::strtof(arg, &endptr);
+ if (endptr == arg) {
+ std::ostringstream msg;
+ msg << "unable to convert argument to integer: \"" << arg << "\"";
+ throw std::invalid_argument(msg.str());
+ }
+ if (errno == ERANGE) {
+ throw std::out_of_range("argument numeric value out of range");
+ }
+ return ret;
+ }
+
+
+ template <> inline
+ double arg(const char* arg)
+ {
+ char* endptr = nullptr;
+ errno = 0;
+ double ret = std::strtod(arg, &endptr);
+ if (endptr == arg) {
+ std::ostringstream msg;
+ msg << "unable to convert argument to integer: \"" << arg << "\"";
+ throw std::invalid_argument(msg.str());
+ }
+ if (errno == ERANGE) {
+ throw std::out_of_range("argument numeric value out of range");
+ }
+ return ret;
+ }
+
+
+ template <> inline
+ const char* arg(const char* arg)
+ {
+ return arg;
+ }
+
+
+ template <> inline
+ std::string arg(const char* arg)
+ {
+ return std::string(arg);
+ }
+
+}
+
+
+inline
+fmt_ostream::fmt_ostream(std::ostream& output)
+: std::ostringstream(), output(output)
+{
+}
+
+
+inline
+fmt_ostream::~fmt_ostream()
+{
+ output << fmt_string(this->str());
+}
+
+
+#ifdef __unix__
+
+
+inline
+std::string fmt_string(const std::string& s)
+{
+ constexpr int read_end = 0;
+ constexpr int write_end = 1;
+
+ // TODO (vnguyen): This function overall needs to handle possible error
+ // returns from the various syscalls.
+
+ int read_pipe[2];
+ int write_pipe[2];
+ if (pipe(read_pipe) == -1) {
+ return s;
+ }
+ if (pipe(write_pipe) == -1) {
+ return s;
+ }
+
+ auto parent_pid = fork();
+ bool is_fmt_proc = (parent_pid == 0);
+ if (is_fmt_proc) {
+ dup2(write_pipe[read_end], STDIN_FILENO);
+ dup2(read_pipe[write_end], STDOUT_FILENO);
+ close(write_pipe[read_end]);
+ close(write_pipe[write_end]);
+ close(read_pipe[read_end]);
+ close(read_pipe[write_end]);
+ const char* argv[] = {"fmt", NULL};
+ execvp(const_cast<char*>(argv[0]), const_cast<char**>(argv));
+ }
+
+ close(write_pipe[read_end]);
+ close(read_pipe[write_end]);
+ auto fmt_write_fd = write_pipe[write_end];
+ auto write_result = write(fmt_write_fd, s.c_str(), s.length());
+ if (write_result != static_cast<ssize_t>(s.length())) {
+ return s;
+ }
+ close(fmt_write_fd);
+
+ auto fmt_read_fd = read_pipe[read_end];
+ std::ostringstream os;
+ char buf[64];
+ while (true) {
+ auto read_count = read(
+ fmt_read_fd, reinterpret_cast<void*>(buf), sizeof(buf));
+ if (read_count <= 0) {
+ break;
+ }
+ os.write(buf, static_cast<std::streamsize>(read_count));
+ }
+ close(fmt_read_fd);
+
+ return os.str();
+}
+
+
+#else // #ifdef __unix__
+
+
+inline
+std::string fmt_string(const std::string& s)
+{
+ return s;
+}
+
+
+#endif // #ifdef __unix__
+
+
+} // namespace argagg
+
+
+inline
+std::ostream& operator << (std::ostream& os, const argagg::parser& x)
+{
+ for (auto& definition : x.definitions) {
+ os << " ";
+ for (auto& flag : definition.flags) {
+ os << flag;
+ if (flag != definition.flags.back()) {
+ os << ", ";
+ }
+ }
+ os << std::endl;
+ os << " " << definition.help << std::endl;
+ }
+ return os;
+}
+
+
+#endif // ARGAGG_ARGAGG_ARGAGG_HPP
diff --git a/externals/Pangolin/include/pangolin/utils/assert.h b/externals/Pangolin/include/pangolin/utils/assert.h
new file mode 100644
index 0000000000000000000000000000000000000000..f0e70978850f4d9efa729aaa710c5681def34a9d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/assert.h
@@ -0,0 +1,60 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Hauke Strasdat, Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/utils/format_string.h>
+#include <iostream>
+
+#ifdef __GNUC__
+# define PANGO_FUNCTION __PRETTY_FUNCTION__
+#elif (_MSC_VER >= 1310)
+# define PANGO_FUNCTION __FUNCTION__
+#else
+# define PANGO_FUNCTION "unknown"
+#endif
+
+namespace pangolin {
+
+template <typename... Args> PANGO_HOST_DEVICE
+void abort(const char* function, const char* file, int line, Args&&... args)
+{
+ std::printf("pangolin::abort() in function '%s', file '%s', line %d.\n", function, file, line);
+#ifndef __CUDACC__
+ std::cout << FormatString(std::forward<Args>(args)...) << std::endl;
+ std::abort();
+#endif
+}
+
+}
+
+// Always check, even in debug
+#define PANGO_ENSURE(expr, ...) ((expr) ? ((void)0) : pangolin::abort(PANGO_FUNCTION, __FILE__, __LINE__, ##__VA_ARGS__))
+
+// May be disabled for optimisation
+#define PANGO_ASSERT(expr, ...) ((expr) ? ((void)0) : pangolin::abort(PANGO_FUNCTION, __FILE__, __LINE__, ##__VA_ARGS__))
diff --git a/externals/Pangolin/include/pangolin/utils/compontent_cast.h b/externals/Pangolin/include/pangolin/utils/compontent_cast.h
new file mode 100644
index 0000000000000000000000000000000000000000..9482f235ee34e9e706aa873d744eb116804b9569
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/compontent_cast.h
@@ -0,0 +1,42 @@
+#pragma once
+
+#include <pangolin/platform.h>
+
+#ifdef HAVE_EIGEN
+# include <Eigen/Core>
+#endif
+
+namespace pangolin
+{
+
+// Scalar / Vector agnostic static_cast-like thing
+//
+// e.g. Promote float to double:
+// ComponentCast<double,float>::cast(0.14f);
+//
+// e.g. Promote Eigen::Vector2f to Eigen::Vector2d:
+// ComponentCast<Eigen::Vector2d,Eigen::Vector2f>::cast(Eigen::Vector2f(0.1,0.2);
+
+template <typename To, typename From>
+struct ComponentCast
+{
+ PANGO_HOST_DEVICE
+ static To cast(const From& val)
+ {
+ return static_cast<To>(val);
+ }
+};
+
+#ifdef HAVE_EIGEN
+template <typename To, typename FromDerived>
+struct ComponentCast<To, Eigen::MatrixBase<FromDerived> >
+{
+ PANGO_HOST_DEVICE
+ static To cast(const Eigen::MatrixBase<FromDerived>& val)
+ {
+ return val.template cast<typename To::Scalar>();
+ }
+};
+#endif
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/file_extension.h b/externals/Pangolin/include/pangolin/utils/file_extension.h
new file mode 100644
index 0000000000000000000000000000000000000000..2912fac6a4a424d29ee8d6ed2143470ed0cc24f8
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/file_extension.h
@@ -0,0 +1,74 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <string>
+
+namespace pangolin
+{
+
+enum ImageFileType
+{
+ ImageFileTypePpm,
+ ImageFileTypeTga,
+ ImageFileTypePng,
+ ImageFileTypeJpg,
+ ImageFileTypeTiff,
+ ImageFileTypeGif,
+ ImageFileTypeExr,
+ ImageFileTypePango,
+ ImageFileTypePvn,
+ ImageFileTypeZstd,
+ ImageFileTypeLz4,
+ ImageFileTypeP12b,
+ ImageFileTypePly,
+ ImageFileTypeObj,
+ ImageFileTypeUnknown
+};
+
+
+PANGOLIN_EXPORT
+std::string ImageFileTypeToName(ImageFileType);
+
+PANGOLIN_EXPORT
+ImageFileType NameToImageFileType(const std::string&);
+
+PANGOLIN_EXPORT
+std::string FileLowercaseExtention(const std::string& filename);
+
+PANGOLIN_EXPORT
+ImageFileType FileTypeMagic(const unsigned char data[], size_t bytes);
+
+PANGOLIN_EXPORT
+ImageFileType FileTypeExtension(const std::string& ext);
+
+PANGOLIN_EXPORT
+ImageFileType FileType(const std::string& filename);
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/file_utils.h b/externals/Pangolin/include/pangolin/utils/file_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..16905abf918fb048c464b3ceab1e5d8159cd262c
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/file_utils.h
@@ -0,0 +1,151 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+
+#include <string>
+#include <vector>
+#include <algorithm>
+
+namespace pangolin
+{
+
+PANGOLIN_EXPORT
+std::vector<std::string>& Split(const std::string& s, char delim, std::vector<std::string>& elements);
+
+PANGOLIN_EXPORT
+std::vector<std::string> Split(const std::string &s, char delim);
+
+PANGOLIN_EXPORT
+std::vector<std::string> Expand(const std::string &s, char open='[', char close=']', char delim=',');
+
+PANGOLIN_EXPORT
+std::string SanitizePath(const std::string& path);
+
+PANGOLIN_EXPORT
+std::string PathParent(const std::string& path, int levels = 1);
+
+PANGOLIN_EXPORT
+bool FileExists(const std::string& filename);
+
+PANGOLIN_EXPORT
+std::string FindPath(const std::string& child_path, const std::string& signature_path);
+
+PANGOLIN_EXPORT
+std::string PathExpand(const std::string& sPath);
+
+PANGOLIN_EXPORT
+bool MatchesWildcard(const std::string& str, const std::string& wildcard);
+
+// Fill 'file_vec' with the files that match the glob-like 'wildcard_file_path'
+// ? can be used to match any single charector
+// * can be used to match any sequence of charectors in a directory
+// ** can be used to match any directories across any number of levels
+// e.g. FilesMatchingWildcard("~/*/code/*.h", vec);
+// e.g. FilesMatchingWildcard("~/**/*.png", vec);
+PANGOLIN_EXPORT
+bool FilesMatchingWildcard(const std::string& wildcard_file_path, std::vector<std::string>& file_vec);
+
+PANGOLIN_EXPORT
+std::string MakeUniqueFilename(const std::string& filename);
+
+PANGOLIN_EXPORT
+bool IsPipe(const std::string& file);
+
+PANGOLIN_EXPORT
+bool IsPipe(int fd);
+
+PANGOLIN_EXPORT
+int WritablePipeFileDescriptor(const std::string& file);
+
+/**
+ * Open the file for reading. Note that it is opened with O_NONBLOCK. The pipe
+ * open is done in two stages so that the producer knows a reader is waiting
+ * (but not blocked). The reader then checks PipeHasDataToRead() until it
+ * returns true. The file can then be opened. Note that the file descriptor
+ * should be closed after the read stream has been created so that the write
+ * side of the pipe does not get signaled.
+ */
+PANGOLIN_EXPORT
+int ReadablePipeFileDescriptor(const std::string& file);
+
+PANGOLIN_EXPORT
+bool PipeHasDataToRead(int fd);
+
+PANGOLIN_EXPORT
+void FlushPipe(const std::string& file);
+
+// TODO: Tidy these inlines up / move them
+
+inline bool StartsWith(const std::string& str, const std::string& prefix)
+{
+ return !str.compare(0, prefix.size(), prefix);
+}
+
+inline bool EndsWith(const std::string& str, const std::string& prefix)
+{
+ return !str.compare(str.size() - prefix.size(), prefix.size(), prefix);
+}
+
+inline std::string Trim(const std::string& str, const std::string& delimiters = " \f\n\r\t\v" )
+{
+ const size_t f = str.find_first_not_of( delimiters );
+ return f == std::string::npos ?
+ "" :
+ str.substr( f, str.find_last_not_of( delimiters ) + 1 );
+}
+
+inline void ToUpper( std::string& str )
+{
+ std::transform(str.begin(), str.end(), str.begin(), ::toupper);
+}
+
+inline void ToLower( std::string& str )
+{
+ std::transform(str.begin(), str.end(), str.begin(), ::tolower);
+}
+
+inline std::string ToUpperCopy( const std::string& str )
+{
+ std::string out;
+ out.resize(str.size());
+ std::transform(str.begin(), str.end(), out.begin(), ::toupper);
+ return out;
+}
+
+inline std::string ToLowerCopy( const std::string& str )
+{
+ std::string out;
+ out.resize(str.size());
+ std::transform(str.begin(), str.end(), out.begin(), ::tolower);
+ return out;
+}
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/fix_size_buffer_queue.h b/externals/Pangolin/include/pangolin/utils/fix_size_buffer_queue.h
new file mode 100644
index 0000000000000000000000000000000000000000..ce4d93203897a91c41b4d5d69e382df867b150f1
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/fix_size_buffer_queue.h
@@ -0,0 +1,153 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <condition_variable>
+#include <list>
+#include <mutex>
+#include <mutex>
+#include <thread>
+
+namespace pangolin
+{
+
+template<typename BufPType>
+class FixSizeBuffersQueue
+{
+
+public:
+ FixSizeBuffersQueue() {}
+
+ ~FixSizeBuffersQueue() {
+ }
+
+ BufPType getNewest() {
+ std::lock_guard<std::mutex> vlock(vMtx);
+ std::lock_guard<std::mutex> elock(eMtx);
+ if(validBuffers.size() == 0) {
+ // Empty queue.
+ return 0;
+ } else {
+ // Requeue all but newest buffers.
+ while(validBuffers.size() > 1) {
+ emptyBuffers.push_back(std::move(validBuffers.front()));
+ validBuffers.pop_front();
+ }
+ // Return newest buffer.
+ BufPType bp = std::move(validBuffers.front());
+ validBuffers.pop_front();
+ return bp;
+ }
+ }
+
+ BufPType getNext() {
+ std::lock_guard<std::mutex> vlock(vMtx);
+ if(validBuffers.size() == 0) {
+ // Empty queue.
+ return 0;
+ } else {
+ // Return oldest buffer.
+ BufPType bp = std::move(validBuffers.front());
+ validBuffers.pop_front();
+ return bp;
+ }
+ }
+
+ BufPType getFreeBuffer() {
+ std::lock_guard<std::mutex> vlock(vMtx);
+ std::lock_guard<std::mutex> elock(eMtx);
+ if(emptyBuffers.size() > 0) {
+ // Simply get a free buffer from the free buffers list.
+ BufPType bp = std::move(emptyBuffers.front());
+ emptyBuffers.pop_front();
+ return bp;
+ } else {
+ if(validBuffers.size() == 0) {
+ // Queue not yet initialized.
+ throw std::runtime_error("Queue not yet initialised.");
+ } else {
+ std::cerr << "Out of free buffers." << std::endl;
+ // No free buffers return oldest among the valid buffers.
+ BufPType bp = std::move(validBuffers.front());
+ validBuffers.pop_front();
+ return bp;
+ }
+ }
+ }
+
+ void addValidBuffer(BufPType bp) {
+ // Add buffer to valid buffers queue.
+ std::lock_guard<std::mutex> vlock(vMtx);
+ validBuffers.push_back(std::move(bp));
+ }
+
+ void returnOrAddUsedBuffer(BufPType bp) {
+ // Add buffer back to empty buffers queue.
+ std::lock_guard<std::mutex> elock(eMtx);
+ emptyBuffers.push_back(std::move(bp));
+ }
+
+ size_t AvailableFrames() const {
+ std::lock_guard<std::mutex> vlock(vMtx);
+ return validBuffers.size();
+ }
+
+ size_t EmptyBuffers() const {
+ std::lock_guard<std::mutex> elock(eMtx);
+ return emptyBuffers.size();
+ }
+
+ bool DropNFrames(size_t n) {
+ std::lock_guard<std::mutex> vlock(vMtx);
+ if(validBuffers.size() < n) {
+ return false;
+ } else {
+ std::lock_guard<std::mutex> elock(eMtx);
+ // Requeue all but newest buffers.
+ for(unsigned int i=0; i<n; ++i) {
+ emptyBuffers.push_back(std::move(validBuffers.front()));
+ validBuffers.pop_front();
+ }
+ return true;
+ }
+ }
+
+// unsigned int BufferSizeBytes(){
+// return bufferSizeBytes;
+// }
+
+private:
+ std::list<BufPType> validBuffers;
+ std::list<BufPType> emptyBuffers;
+ mutable std::mutex vMtx;
+ mutable std::mutex eMtx;
+// unsigned int maxNumBuffers;
+// unsigned int bufferSizeBytes;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/format_string.h b/externals/Pangolin/include/pangolin/utils/format_string.h
new file mode 100644
index 0000000000000000000000000000000000000000..7b39f9a602e9ab67eb6fd7570df109908d18c355
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/format_string.h
@@ -0,0 +1,87 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Hauke Strasdat, Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <sstream>
+
+namespace pangolin {
+namespace details {
+
+// Following: http://stackoverflow.com/a/22759544
+template <typename T>
+class IsStreamable {
+private:
+ template <typename TT>
+ static auto test(int) -> decltype( (std::declval<std::stringstream&>() << std::declval<TT>(), std::true_type()) );
+
+ template <typename>
+ static auto test(...) -> std::false_type;
+
+public:
+ static const bool value = decltype(test<T>(0))::value;
+};
+
+inline void FormatStream(std::stringstream& stream, const char* text)
+{
+ stream << text;
+}
+
+// Following: http://en.cppreference.com/w/cpp/language/parameter_pack
+template <typename T, typename... Args>
+void FormatStream(std::stringstream& stream, const char* text, T arg, Args... args)
+{
+ static_assert(IsStreamable<T>::value,
+ "One of the args has not an ostream overload!");
+ for (; *text != '\0'; ++text) {
+ if (*text == '%') {
+ stream << arg;
+ FormatStream(stream, text + 1, args...);
+ return;
+ }
+ stream << *text;
+ }
+ stream << "\nFormat-Warning: There are " << sizeof...(Args) + 1
+ << " args unused.";
+}
+
+} // namespace details
+
+template <typename... Args>
+std::string FormatString(const char* text, Args... args)
+{
+ std::stringstream stream;
+ details::FormatStream(stream, text, args...);
+ return stream.str();
+}
+
+inline std::string FormatString()
+{
+ return std::string();
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/log.h b/externals/Pangolin/include/pangolin/utils/log.h
new file mode 100644
index 0000000000000000000000000000000000000000..e9e3536cbe41edf75325c605e02995d27ca6c221
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/log.h
@@ -0,0 +1,44 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+// TODO: Something a bit more useful here, probably using format_string.h
+
+#pragma once
+
+#ifndef _ANDROID_
+# include <cstdio>
+# define pango_print_debug(...) printf(__VA_ARGS__)
+# define pango_print_info(...) printf(__VA_ARGS__)
+# define pango_print_error(...) fprintf(stderr, __VA_ARGS__)
+# define pango_print_warn(...) fprintf(stderr, __VA_ARGS__)
+#else
+# include <android/log.h>
+# define pango_print_debug(...) __android_log_print(ANDROID_LOG_DEBUG, "pango", __VA_ARGS__ );
+# define pango_print_info(...) __android_log_print(ANDROID_LOG_INFO, "pango", __VA_ARGS__ );
+# define pango_print_error(...) __android_log_print(ANDROID_LOG_ERROR, "pango", __VA_ARGS__ );
+# define pango_print_warn(...) __android_log_print(ANDROID_LOG_ERROR, "pango", __VA_ARGS__ );
+#endif
diff --git a/externals/Pangolin/include/pangolin/utils/memstreambuf.h b/externals/Pangolin/include/pangolin/utils/memstreambuf.h
new file mode 100644
index 0000000000000000000000000000000000000000..7cdafb4c53b60815367251bbaee9dcbe499aa334
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/memstreambuf.h
@@ -0,0 +1,55 @@
+#pragma once
+
+#include <streambuf>
+#include <vector>
+
+namespace pangolin {
+
+// A simple streambuf wrapper around std::vector for memory buffer use
+struct memstreambuf : public std::streambuf
+{
+public:
+ memstreambuf(size_t initial_buffer_size)
+ {
+ buffer.reserve(initial_buffer_size);
+ }
+
+ // Avoiding use of std::streambuf's move constructor, since it is missing for old GCC
+ memstreambuf(memstreambuf&& o)
+ : buffer(std::move(o.buffer))
+ {
+ pubseekpos(o.pubseekoff(0, std::ios_base::cur));
+ }
+
+ size_t size() const
+ {
+ return buffer.size();
+ }
+
+ const unsigned char* data() const
+ {
+ return buffer.data();
+ }
+
+ void clear()
+ {
+ buffer.clear();
+ }
+
+ std::vector<unsigned char> buffer;
+
+protected:
+ std::streamsize xsputn(const char_type* __s, std::streamsize __n) override
+ {
+ buffer.insert(buffer.end(), __s, __s + __n);
+ return __n;
+ }
+
+ int_type overflow(int_type __c) override
+ {
+ buffer.push_back( static_cast<unsigned char>(__c) );
+ return __c;
+ }
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/params.h b/externals/Pangolin/include/pangolin/utils/params.h
new file mode 100644
index 0000000000000000000000000000000000000000..beebeae85c666b678c8676edd2fc7c9fa5c0e0d1
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/params.h
@@ -0,0 +1,80 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/utils/type_convert.h>
+
+#include <string>
+#include <vector>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT Params
+{
+public:
+ typedef std::vector<std::pair<std::string,std::string>> ParamMap;
+
+ Params()
+ {
+ }
+
+ Params(std::initializer_list<std::pair<std::string,std::string>> l)
+ : params(l)
+ {
+ }
+
+ bool Contains(const std::string& key) const
+ {
+ for(ParamMap::const_iterator it = params.begin(); it!=params.end(); ++it) {
+ if(it->first == key) return true;
+ }
+ return false;
+ }
+
+ template<typename T>
+ T Get(const std::string& key, T default_val) const
+ {
+ // Return last value passed to the key.
+ for(ParamMap::const_reverse_iterator it = params.rbegin(); it!=params.rend(); ++it) {
+ if(it->first == key) return Convert<T, std::string>::Do(it->second);
+ }
+ return default_val;
+ }
+
+ template<typename T>
+ void Set(const std::string& key, const T& val)
+ {
+ params.push_back(std::pair<std::string,std::string>(key,Convert<std::string,T>::Do(val)));
+ }
+
+ ParamMap params;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/parse.h b/externals/Pangolin/include/pangolin/utils/parse.h
new file mode 100644
index 0000000000000000000000000000000000000000..eedbd3510478baeda8a73c624a2dbdf3ccc5cefe
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/parse.h
@@ -0,0 +1,108 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#ifndef PANGOLIN_PARSE_H
+#define PANGOLIN_PARSE_H
+
+#include <string>
+#include <cctype>
+
+namespace pangolin
+{
+
+const unsigned int parse_max_token_size = 1024;
+
+inline void ConsumeWhitespace(std::istream& is)
+{
+ while(is.good() && std::isspace(is.peek()) ) {
+ is.get();
+ }
+}
+
+inline bool ConsumeToNewline(std::istream& is)
+{
+ while(is.good()) {
+ if(is.get() == '\n') {
+ return true;
+ }
+ }
+ return false;
+}
+
+template<size_t buffer_size> inline
+size_t ReadToken(std::istream& is, char buffer[buffer_size])
+{
+ size_t r = 0;
+ while(is.good() && r < buffer_size-1) {
+ int c = is.peek();
+ if( std::isgraph(c) ) {
+ buffer[r++] = (char)is.get();
+ }else{
+ break;
+ }
+ }
+ buffer[r] = '\0';
+ return r;
+}
+
+inline std::string ReadToken(std::istream &is)
+{
+ char str_token[parse_max_token_size];
+ ReadToken<parse_max_token_size>(is, str_token);
+ return std::string(str_token);
+}
+
+template<size_t buffer_size> inline
+size_t ConsumeWhitespaceReadToken(std::istream& is, char buffer[buffer_size])
+{
+ ConsumeWhitespace(is);
+ return ReadToken<buffer_size>(is, buffer);
+}
+
+inline int ParseToken(const char* token, const char* token_list[], size_t token_list_size)
+{
+ for(size_t i=0; i < token_list_size; ++i) {
+ if( strcmp(token, token_list[i]) == 0 ) {
+ return i;
+ }
+ }
+ return -1;
+}
+
+#define PANGOLIN_DEFINE_PARSE_TOKEN(x) \
+ inline x ParseToken##x(const char* token) { \
+ return (x)ParseToken(token, x##String, x##Size); \
+ } \
+ inline x ParseToken##x(std::istream& is) { \
+ char str_token[parse_max_token_size]; \
+ ReadToken<parse_max_token_size>(is, str_token); \
+ return ParseToken##x( str_token ); \
+ }
+
+}
+
+#endif // PANGOLIN_PARSE_H
diff --git a/externals/Pangolin/include/pangolin/utils/picojson.h b/externals/Pangolin/include/pangolin/utils/picojson.h
new file mode 100644
index 0000000000000000000000000000000000000000..3791ee6b777fe1a932fcca42569720ef4f3ccbaf
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/picojson.h
@@ -0,0 +1,1414 @@
+/* This is a (butchered) derivative of picojson, incorporated into the
+ * Pangolin Project (http://github.com/stevenlovegrove/Pangolin)
+ *
+ * Modifications Copyright (c) 2014 Steven Lovegrove,
+ * Original licence applies (below), https://github.com/kazuho/picojson
+ */
+
+/*
+ * Copyright 2009-2010 Cybozu Labs, Inc.
+ * Copyright 2011-2014 Kazuho Oku
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef picojson_h
+#define picojson_h
+
+#include <algorithm>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <iostream>
+#include <iterator>
+#include <limits>
+#include <map>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#include <pangolin/compat/type_traits.h>
+
+// Enable INT64 support
+#define PICOJSON_USE_INT64
+
+// for isnan/isinf
+#if __cplusplus>=201103L
+# include <cmath>
+#else
+extern "C" {
+# ifdef _MSC_VER
+# include <float.h>
+# elif defined(__INTEL_COMPILER)
+# include <mathimf.h>
+# else
+# include <math.h>
+# endif
+}
+#endif
+
+// experimental support for int64_t (see README.mkdn for detail)
+#ifdef PICOJSON_USE_INT64
+# define __STDC_FORMAT_MACROS
+# include <errno.h>
+# include <inttypes.h>
+#endif // PICOJSON_USE_INT64
+
+// to disable the use of localeconv(3), set PICOJSON_USE_LOCALE to 0
+#ifndef PICOJSON_USE_LOCALE
+# define PICOJSON_USE_LOCALE 1
+#endif // PICOJSON_USE_LOCALE
+
+#if PICOJSON_USE_LOCALE
+extern "C" {
+# include <locale.h>
+}
+#endif // PICOJSON_USE_LOCALE
+
+#ifndef PICOJSON_ASSERT
+# define PICOJSON_ASSERT(e) do { if (! (e)) throw std::runtime_error(#e); } while (0)
+#endif
+
+#ifdef _MSC_VER
+#define SNPRINTF _snprintf_s
+#pragma warning(push)
+#pragma warning(disable : 4244) // conversion from int to char
+#else
+#define SNPRINTF snprintf
+#endif
+
+#ifdef _MSC_VER
+#define PICOJSON_FALSE_TEMPLATE_TYPE nullptr
+#else
+#define PICOJSON_FALSE_TEMPLATE_TYPE ((void*)0)
+#endif
+
+namespace picojson {
+
+enum {
+ null_type,
+ boolean_type,
+ number_type,
+ string_type,
+ array_type,
+ object_type
+#ifdef PICOJSON_USE_INT64
+ , int64_type
+#endif
+};
+
+enum {
+ INDENT_WIDTH = 2
+};
+
+struct null {};
+
+class value {
+public:
+ typedef std::vector<value> array;
+ typedef std::map<std::string, value> object;
+
+ union _storage {
+ bool boolean_;
+ double number_;
+#ifdef PICOJSON_USE_INT64
+ int64_t int64_;
+#endif
+ std::string* string_;
+ array* array_;
+ object* object_;
+ };
+protected:
+ int type_;
+ _storage u_;
+public:
+
+
+ /////////////////////////////
+ // Constructors / Destructor
+ /////////////////////////////
+
+ ~value();
+ value();
+ value(int type, bool);
+
+ /////////////////////////////
+ // Implicit type constructors
+ /////////////////////////////
+
+ value(bool b) : type_(boolean_type) {
+ u_.boolean_ = b;
+ }
+
+#ifdef PICOJSON_USE_INT64
+ value(short v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(unsigned short v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(int v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(unsigned int v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(long v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(unsigned long v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(long long v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+ value(unsigned long long v) : type_(int64_type) {
+ u_.int64_ = static_cast<int64_t>(v);
+ }
+#endif
+
+ value(float n) : type_(number_type) {
+ if (
+ #ifdef _MSC_VER
+ ! _finite(n)
+ #elif __cplusplus>=201103L || !(defined(isnan) && defined(isinf))
+ std::isnan(n) || std::isinf(n)
+ #else
+ isnan(n) || isinf(n)
+ #endif
+ ) {
+ throw std::overflow_error("");
+ }
+ u_.number_ = static_cast<double>(n);
+ }
+
+ value(double n) : type_(number_type) {
+ if (
+ #ifdef _MSC_VER
+ ! _finite(n)
+ #elif __cplusplus>=201103L || !(defined(isnan) && defined(isinf))
+ std::isnan(n) || std::isinf(n)
+ #else
+ isnan(n) || isinf(n)
+ #endif
+ ) {
+ throw std::overflow_error("");
+ }
+ u_.number_ = n;
+ }
+
+ value(const array& a);
+ value(const object& o);
+
+ value(const std::string& s);
+ value(const char* s);
+ value(const char* s, size_t len);
+
+ value(const value& x);
+ value& operator=(const value& x);
+
+ /////////////////////////////
+ // Query / get type
+ /////////////////////////////
+
+ void swap(value& x);
+ template <typename T> bool is() const;
+ template <typename T> const T& get() const;
+ template <typename T> T& get();
+ bool evaluate_as_boolean() const;
+ size_t size() const;
+
+ /////////////////////////////
+ // Usage as array
+ /////////////////////////////
+
+ value& operator[](size_t idx);
+ const value& operator[](size_t idx) const;
+ bool contains(size_t idx) const;
+ value& push_back(const value& = value() );
+
+ /////////////////////////////
+ // Usage as object
+ /////////////////////////////
+
+ value& operator[](const std::string& key);
+ const value& operator[](const std::string& key) const;
+ bool contains(const std::string& key) const;
+
+ template <typename T>
+ T get_value(const std::string& key, T default_value) const;
+
+ /////////////////////////////
+ // Serialization
+ /////////////////////////////
+
+ std::string to_str() const;
+ template <typename Iter> void serialize(Iter os, bool prettify = false) const;
+ std::string serialize(bool prettify = false) const;
+private:
+ template <typename T> value(const T*); // intentionally defined to block implicit conversion of pointer to bool
+ template <typename Iter> static void _indent(Iter os, int indent);
+ template <typename Iter> void _serialize(Iter os, int indent) const;
+ std::string _serialize(int indent) const;
+};
+
+typedef value::array array;
+typedef value::object object;
+
+inline value::value() : type_(null_type), u_({false}) {}
+
+inline value::value(int type, bool) : type_(type) {
+ switch (type) {
+#define INIT(p, v) case p##type: u_.p = v; break
+ INIT(boolean_, false);
+ INIT(number_, 0.0);
+#ifdef PICOJSON_USE_INT64
+ INIT(int64_, 0);
+#endif
+ INIT(string_, new std::string());
+ INIT(array_, new array());
+ INIT(object_, new object());
+#undef INIT
+ default: break;
+ }
+}
+
+inline value::value(const std::string& s) : type_(string_type) {
+ u_.string_ = new std::string(s);
+}
+
+inline value::value(const array& a) : type_(array_type) {
+ u_.array_ = new array(a);
+}
+
+inline value::value(const object& o) : type_(object_type) {
+ u_.object_ = new object(o);
+}
+
+inline value::value(const char* s) : type_(string_type) {
+ u_.string_ = new std::string(s);
+}
+
+inline value::value(const char* s, size_t len) : type_(string_type) {
+ u_.string_ = new std::string(s, len);
+}
+
+inline value::~value() {
+ switch (type_) {
+#define DEINIT(p) case p##type: delete u_.p; break
+ DEINIT(string_);
+ DEINIT(array_);
+ DEINIT(object_);
+#undef DEINIT
+ default: break;
+ }
+}
+
+inline value::value(const value& x) : type_(x.type_) {
+ switch (type_) {
+#define INIT(p, v) case p##type: u_.p = v; break
+ INIT(string_, new std::string(*x.u_.string_));
+ INIT(array_, new array(*x.u_.array_));
+ INIT(object_, new object(*x.u_.object_));
+#undef INIT
+ default:
+ u_ = x.u_;
+ break;
+ }
+}
+
+inline value& value::operator=(const value& x) {
+ if (this != &x) {
+ this->~value();
+ new (this) value(x);
+ }
+ return *this;
+}
+
+inline void value::swap(value& x) {
+ std::swap(type_, x.type_);
+ std::swap(u_, x.u_);
+}
+
+#define IS(ctype, jtype) \
+ template <> inline bool value::is<ctype>() const { \
+ return type_ == jtype##_type; \
+}
+IS(null, null)
+IS(bool, boolean)
+#ifdef PICOJSON_USE_INT64
+IS(int64_t, int64)
+#endif
+IS(std::string, string)
+IS(array, array)
+IS(object, object)
+#undef IS
+template <> inline bool value::is<double>() const {
+ return type_ == number_type
+ #ifdef PICOJSON_USE_INT64
+ || type_ == int64_type
+ #endif
+ ;
+}
+
+#define GET(ctype, var) \
+ template <> inline const ctype& value::get<ctype>() const { \
+ PICOJSON_ASSERT("type mismatch! call is<type>() before get<type>()" \
+ && is<ctype>()); \
+ return var; \
+} \
+ template <> inline ctype& value::get<ctype>() { \
+ PICOJSON_ASSERT("type mismatch! call is<type>() before get<type>()" \
+ && is<ctype>()); \
+ return var; \
+}
+GET(bool, u_.boolean_)
+GET(std::string, *u_.string_)
+GET(array, *u_.array_)
+GET(object, *u_.object_)
+#ifdef PICOJSON_USE_INT64
+GET(double, (type_ == int64_type && (const_cast<value*>(this)->type_ = number_type, const_cast<value*>(this)->u_.number_ = u_.int64_), u_.number_))
+GET(int64_t, u_.int64_)
+#else
+GET(double, u_.number_)
+#endif
+#undef GET
+
+inline bool value::evaluate_as_boolean() const {
+ switch (type_) {
+ case null_type:
+ return false;
+ case boolean_type:
+ return u_.boolean_;
+ case number_type:
+ return u_.number_ != 0;
+ case string_type:
+ return ! u_.string_->empty();
+ default:
+ return true;
+ }
+}
+
+inline size_t value::size() const
+{
+ PICOJSON_ASSERT("Type mismatch! Not array." && is<array>());
+ return u_.array_->size();
+}
+
+inline const value& value::operator[](size_t idx) const {
+ PICOJSON_ASSERT("Type mismatch! Not array." && is<array>());
+ PICOJSON_ASSERT("Out of bounds." && idx < u_.array_->size() );
+ return (*u_.array_)[idx];
+}
+
+inline value& value::operator[](size_t idx) {
+ if( type_ == null_type ) {
+ // instantiate as array
+ type_ = array_type;
+ u_.array_ = new array();
+ }
+
+ PICOJSON_ASSERT("Type mismatch! Not array." && is<array>());
+ PICOJSON_ASSERT("Out of bounds." && idx < u_.array_->size() );
+ return (*u_.array_)[idx];
+}
+
+inline bool value::contains(size_t idx) const {
+ if( type_ == array_type) {
+ return idx < u_.array_->size();
+ }else{
+ return false;
+ }
+}
+
+inline value& value::push_back(const value& val)
+{
+ if( type_ == null_type ) {
+ // instantiate as array
+ type_ = array_type;
+ u_.array_ = new array();
+ }
+ PICOJSON_ASSERT("Type mismatch! Not array." && is<array>());
+ u_.array_->push_back( val );
+ return u_.array_->back();
+}
+
+inline const value& value::operator[](const std::string& key) const {
+ PICOJSON_ASSERT("Type mismatch! Not object." && is<object>() );
+ object::const_iterator i = u_.object_->find(key);
+ PICOJSON_ASSERT("Key not found." && i != u_.object_->end() );
+ return i->second;
+}
+
+inline value& value::operator[](const std::string& key) {
+ if( type_ == null_type ) {
+ // instantiate as object
+ type_ = object_type;
+ u_.object_ = new object();
+ }
+ PICOJSON_ASSERT("Type mismatch! Not object." && is<object>());
+ return u_.object_->operator [](key);
+}
+
+inline bool value::contains(const std::string& key) const {
+ if( type_ == object_type) {
+ object::const_iterator i = u_.object_->find(key);
+ return i != u_.object_->end();
+ }else{
+ return false;
+ }
+}
+
+template <typename T>
+inline T value::get_value(const std::string& key, T default_value) const {
+ if(contains(key)) {
+ return (*this)[key].get<T>();
+ }else{
+ return default_value;
+ }
+}
+
+inline std::string value::to_str() const {
+ switch (type_) {
+ case null_type: return "null";
+ case boolean_type: return u_.boolean_ ? "true" : "false";
+#ifdef PICOJSON_USE_INT64
+ case int64_type: {
+ char buf[sizeof("-9223372036854775808")];
+ SNPRINTF(buf, sizeof(buf), "%" PRId64, u_.int64_);
+ return buf;
+ }
+#endif
+ case number_type: {
+ char buf[256];
+ double tmp;
+ SNPRINTF(buf, sizeof(buf), fabs(u_.number_) < (1ULL << 53) && modf(u_.number_, &tmp) == 0 ? "%.f" : "%.17g", u_.number_);
+#if PICOJSON_USE_LOCALE
+ char *decimal_point = localeconv()->decimal_point;
+ if (strcmp(decimal_point, ".") != 0) {
+ size_t decimal_point_len = strlen(decimal_point);
+ for (char *p = buf; *p != '\0'; ++p) {
+ if (strncmp(p, decimal_point, decimal_point_len) == 0) {
+ return std::string(buf, p) + "." + (p + decimal_point_len);
+ }
+ }
+ }
+#endif
+ return buf;
+ }
+ case string_type: return *u_.string_;
+ case array_type: return "array";
+ case object_type: return "object";
+ default: PICOJSON_ASSERT("value::to_str() assert failed." && 0);
+#ifdef _MSC_VER
+ __assume(0);
+#endif
+ }
+}
+
+template <typename Iter> void copy(const std::string& s, Iter oi) {
+ std::copy(s.begin(), s.end(), oi);
+}
+
+template <typename Iter> void serialize_str(const std::string& s, Iter oi) {
+ *oi++ = '"';
+ for (std::string::const_iterator i = s.begin(); i != s.end(); ++i) {
+ switch (*i) {
+#define MAP(val, sym) case val: copy(sym, oi); break
+ MAP('"', "\\\"");
+ MAP('\\', "\\\\");
+ MAP('/', "\\/");
+ MAP('\b', "\\b");
+ MAP('\f', "\\f");
+ MAP('\n', "\\n");
+ MAP('\r', "\\r");
+ MAP('\t', "\\t");
+#undef MAP
+ default:
+ if (static_cast<unsigned char>(*i) < 0x20 || *i == 0x7f) {
+ char buf[7];
+ SNPRINTF(buf, sizeof(buf), "\\u%04x", *i & 0xff);
+ copy(buf, buf + 6, oi);
+ } else {
+ *oi++ = *i;
+ }
+ break;
+ }
+ }
+ *oi++ = '"';
+}
+
+template <typename Iter> void value::serialize(Iter oi, bool prettify) const {
+ return _serialize(oi, prettify ? 0 : -1);
+}
+
+inline std::string value::serialize(bool prettify) const {
+ return _serialize(prettify ? 0 : -1);
+}
+
+template <typename Iter> void value::_indent(Iter oi, int indent) {
+ *oi++ = '\n';
+ for (int i = 0; i < indent * INDENT_WIDTH; ++i) {
+ *oi++ = ' ';
+ }
+}
+
+template <typename Iter> void value::_serialize(Iter oi, int indent) const {
+ switch (type_) {
+ case string_type:
+ serialize_str(*u_.string_, oi);
+ break;
+ case array_type: {
+ *oi++ = '[';
+ if (indent != -1) {
+ ++indent;
+ }
+ for (array::const_iterator i = u_.array_->begin();
+ i != u_.array_->end();
+ ++i) {
+ if (i != u_.array_->begin()) {
+ *oi++ = ',';
+ }
+ if (indent != -1) {
+ _indent(oi, indent);
+ }
+ i->_serialize(oi, indent);
+ }
+ if (indent != -1) {
+ --indent;
+ if (! u_.array_->empty()) {
+ _indent(oi, indent);
+ }
+ }
+ *oi++ = ']';
+ break;
+ }
+ case object_type: {
+ *oi++ = '{';
+ if (indent != -1) {
+ ++indent;
+ }
+ for (object::const_iterator i = u_.object_->begin();
+ i != u_.object_->end();
+ ++i) {
+ if (i != u_.object_->begin()) {
+ *oi++ = ',';
+ }
+ if (indent != -1) {
+ _indent(oi, indent);
+ }
+ serialize_str(i->first, oi);
+ *oi++ = ':';
+ if (indent != -1) {
+ *oi++ = ' ';
+ }
+ i->second._serialize(oi, indent);
+ }
+ if (indent != -1) {
+ --indent;
+ if (! u_.object_->empty()) {
+ _indent(oi, indent);
+ }
+ }
+ *oi++ = '}';
+ break;
+ }
+ default:
+ copy(to_str(), oi);
+ break;
+ }
+ if (indent == 0) {
+ *oi++ = '\n';
+ }
+}
+
+inline std::string value::_serialize(int indent) const {
+ std::string s;
+ _serialize(std::back_inserter(s), indent);
+ return s;
+}
+
+template <typename Iter> class input {
+protected:
+ Iter cur_, end_;
+ int last_ch_;
+ bool ungot_;
+ int line_;
+public:
+ input(const Iter& first, const Iter& last) : cur_(first), end_(last), last_ch_(-1), ungot_(false), line_(1) {}
+ int getc() {
+ if (ungot_) {
+ ungot_ = false;
+ return last_ch_;
+ }
+ if (cur_ == end_) {
+ last_ch_ = -1;
+ return -1;
+ }
+ if (last_ch_ == '\n') {
+ line_++;
+ }
+ last_ch_ = *cur_ & 0xff;
+ ++cur_;
+ return last_ch_;
+ }
+ void ungetc() {
+ if (last_ch_ != -1) {
+ PICOJSON_ASSERT(! ungot_);
+ ungot_ = true;
+ }
+ }
+ Iter cur() const { return cur_; }
+ int line() const { return line_; }
+ void skip_ws() {
+ while (1) {
+ int ch = getc();
+ if (! (ch == ' ' || ch == '\t' || ch == '\n' || ch == '\r')) {
+ ungetc();
+ break;
+ }
+ }
+ }
+ bool expect(int expect) {
+ skip_ws();
+ if (getc() != expect) {
+ ungetc();
+ return false;
+ }
+ return true;
+ }
+ bool match(const std::string& pattern) {
+ for (std::string::const_iterator pi(pattern.begin());
+ pi != pattern.end();
+ ++pi) {
+ if (getc() != *pi) {
+ ungetc();
+ return false;
+ }
+ }
+ return true;
+ }
+};
+
+template<typename Iter> inline int _parse_quadhex(input<Iter> &in) {
+ int uni_ch = 0, hex;
+ for (int i = 0; i < 4; i++) {
+ if ((hex = in.getc()) == -1) {
+ return -1;
+ }
+ if ('0' <= hex && hex <= '9') {
+ hex -= '0';
+ } else if ('A' <= hex && hex <= 'F') {
+ hex -= 'A' - 0xa;
+ } else if ('a' <= hex && hex <= 'f') {
+ hex -= 'a' - 0xa;
+ } else {
+ in.ungetc();
+ return -1;
+ }
+ uni_ch = uni_ch * 16 + hex;
+ }
+ return uni_ch;
+}
+
+template<typename String, typename Iter> inline bool _parse_codepoint(String& out, input<Iter>& in) {
+ int uni_ch;
+ if ((uni_ch = _parse_quadhex(in)) == -1) {
+ return false;
+ }
+ if (0xd800 <= uni_ch && uni_ch <= 0xdfff) {
+ if (0xdc00 <= uni_ch) {
+ // a second 16-bit of a surrogate pair appeared
+ return false;
+ }
+ // first 16-bit of surrogate pair, get the next one
+ if (in.getc() != '\\' || in.getc() != 'u') {
+ in.ungetc();
+ return false;
+ }
+ int second = _parse_quadhex(in);
+ if (! (0xdc00 <= second && second <= 0xdfff)) {
+ return false;
+ }
+ uni_ch = ((uni_ch - 0xd800) << 10) | ((second - 0xdc00) & 0x3ff);
+ uni_ch += 0x10000;
+ }
+ if (uni_ch < 0x80) {
+ out.push_back(uni_ch);
+ } else {
+ if (uni_ch < 0x800) {
+ out.push_back(0xc0 | (uni_ch >> 6));
+ } else {
+ if (uni_ch < 0x10000) {
+ out.push_back(0xe0 | (uni_ch >> 12));
+ } else {
+ out.push_back(0xf0 | (uni_ch >> 18));
+ out.push_back(0x80 | ((uni_ch >> 12) & 0x3f));
+ }
+ out.push_back(0x80 | ((uni_ch >> 6) & 0x3f));
+ }
+ out.push_back(0x80 | (uni_ch & 0x3f));
+ }
+ return true;
+}
+
+template<typename String, typename Iter> inline bool _parse_string(String& out, input<Iter>& in, int delim='"') {
+ while (1) {
+ int ch = in.getc();
+ if (ch < ' ') {
+ in.ungetc();
+ return false;
+ } else if (ch == delim) {
+ return true;
+ } else if (ch == '\\') {
+ if ((ch = in.getc()) == -1) {
+ return false;
+ }
+ switch (ch) {
+#define MAP(sym, val) case sym: out.push_back(val); break
+ MAP('"', '\"');
+ MAP('\\', '\\');
+ MAP('/', '/');
+ MAP('b', '\b');
+ MAP('f', '\f');
+ MAP('n', '\n');
+ MAP('r', '\r');
+ MAP('t', '\t');
+#undef MAP
+ case 'u':
+ if (! _parse_codepoint(out, in)) {
+ return false;
+ }
+ break;
+ default:
+ return false;
+ }
+ } else {
+ out.push_back(ch);
+ }
+ }
+}
+
+template <typename Context, typename Iter> inline bool _parse_array(Context& ctx, input<Iter>& in) {
+ if (! ctx.parse_array_start()) {
+ return false;
+ }
+ size_t idx = 0;
+ if (in.expect(']')) {
+ return ctx.parse_array_stop(idx);
+ }
+ do {
+ if (! ctx.parse_array_item(in, idx)) {
+ return false;
+ }
+ idx++;
+ } while (in.expect(','));
+ return in.expect(']') && ctx.parse_array_stop(idx);
+}
+
+template <typename Context, typename Iter> inline bool _parse_object(Context& ctx, input<Iter>& in) {
+ if (! ctx.parse_object_start()) {
+ return false;
+ }
+ if (in.expect('}')) {
+ return true;
+ }
+ do {
+ std::string key;
+ // Support " and ' delimited strings
+ if( in.expect('"') ) {
+ if( !_parse_string(key, in, '"') )
+ return false;
+ }else if (!in.expect('\'') || !_parse_string(key, in,'\'') ) {
+ return false;
+ }
+ if (!in.expect(':') || !ctx.parse_object_item(in, key)) {
+ return false;
+ }
+ } while (in.expect(','));
+ return in.expect('}');
+}
+
+template <typename Iter> inline std::string _parse_number(input<Iter>& in) {
+ std::string num_str;
+ while (1) {
+ int ch = in.getc();
+ if (('0' <= ch && ch <= '9') || ch == '+' || ch == '-'
+ || ch == 'e' || ch == 'E') {
+ num_str.push_back(ch);
+ } else if (ch == '.') {
+#if PICOJSON_USE_LOCALE
+ num_str += localeconv()->decimal_point;
+#else
+ num_str.push_back('.');
+#endif
+ } else {
+ in.ungetc();
+ break;
+ }
+ }
+ return num_str;
+}
+
+template <typename Context, typename Iter> inline bool _parse(Context& ctx, input<Iter>& in) {
+ in.skip_ws();
+ int ch = in.getc();
+ switch (ch) {
+#define IS(ch, text, op) case ch: \
+ if (in.match(text) && op) { \
+ return true; \
+ } else { \
+ return false; \
+ }
+ IS('n', "ull", ctx.set_null());
+ IS('f', "alse", ctx.set_bool(false));
+ IS('t', "rue", ctx.set_bool(true));
+#undef IS
+ case '"':
+ return ctx.parse_string(in);
+ case '[':
+ return _parse_array(ctx, in);
+ case '{':
+ return _parse_object(ctx, in);
+ default:
+ if (('0' <= ch && ch <= '9') || ch == '-') {
+ double f;
+ char *endp;
+ in.ungetc();
+ std::string num_str = _parse_number(in);
+ if (num_str.empty()) {
+ return false;
+ }
+#ifdef PICOJSON_USE_INT64
+ {
+ errno = 0;
+ intmax_t ival = strtoimax(num_str.c_str(), &endp, 10);
+ if (errno == 0
+ && std::numeric_limits<int64_t>::min() <= ival
+ && ival <= std::numeric_limits<int64_t>::max()
+ && endp == num_str.c_str() + num_str.size()) {
+ ctx.set_int64(ival);
+ return true;
+ }
+ }
+#endif
+ f = strtod(num_str.c_str(), &endp);
+ if (endp == num_str.c_str() + num_str.size()) {
+ ctx.set_number(f);
+ return true;
+ }
+ return false;
+ }
+ break;
+ }
+ in.ungetc();
+ return false;
+}
+
+class deny_parse_context {
+public:
+ bool set_null() { return false; }
+ bool set_bool(bool) { return false; }
+#ifdef PICOJSON_USE_INT64
+ bool set_int64(int64_t) { return false; }
+#endif
+ bool set_number(double) { return false; }
+ template <typename Iter> bool parse_string(input<Iter>&) { return false; }
+ bool parse_array_start() { return false; }
+ template <typename Iter> bool parse_array_item(input<Iter>&, size_t) {
+ return false;
+ }
+ bool parse_array_stop(size_t) { return false; }
+ bool parse_object_start() { return false; }
+ template <typename Iter> bool parse_object_item(input<Iter>&, const std::string&) {
+ return false;
+ }
+};
+
+class default_parse_context {
+protected:
+ value* out_;
+public:
+ default_parse_context(value* out) : out_(out) {}
+ bool set_null() {
+ *out_ = value();
+ return true;
+ }
+ bool set_bool(bool b) {
+ *out_ = value(b);
+ return true;
+ }
+#ifdef PICOJSON_USE_INT64
+ bool set_int64(int64_t i) {
+ *out_ = value(i);
+ return true;
+ }
+#endif
+ bool set_number(double f) {
+ *out_ = value(f);
+ return true;
+ }
+ template<typename Iter> bool parse_string(input<Iter>& in) {
+ *out_ = value(string_type, false);
+ return _parse_string(out_->get<std::string>(), in);
+ }
+ bool parse_array_start() {
+ *out_ = value(array_type, false);
+ return true;
+ }
+ template <typename Iter> bool parse_array_item(input<Iter>& in, size_t) {
+ array& a = out_->get<array>();
+ a.push_back(value());
+ default_parse_context ctx(&a.back());
+ return _parse(ctx, in);
+ }
+ bool parse_array_stop(size_t) { return true; }
+ bool parse_object_start() {
+ *out_ = value(object_type, false);
+ return true;
+ }
+ template <typename Iter> bool parse_object_item(input<Iter>& in, const std::string& key) {
+ object& o = out_->get<object>();
+ default_parse_context ctx(&o[key]);
+ return _parse(ctx, in);
+ }
+private:
+ default_parse_context(const default_parse_context&);
+ default_parse_context& operator=(const default_parse_context&);
+};
+
+class null_parse_context {
+public:
+ struct dummy_str {
+ void push_back(int) {}
+ };
+public:
+ null_parse_context() {}
+ bool set_null() { return true; }
+ bool set_bool(bool) { return true; }
+#ifdef PICOJSON_USE_INT64
+ bool set_int64(int64_t) { return true; }
+#endif
+ bool set_number(double) { return true; }
+ template <typename Iter> bool parse_string(input<Iter>& in) {
+ dummy_str s;
+ return _parse_string(s, in);
+ }
+ bool parse_array_start() { return true; }
+ template <typename Iter> bool parse_array_item(input<Iter>& in, size_t) {
+ return _parse(*this, in);
+ }
+ bool parse_array_stop(size_t) { return true; }
+ bool parse_object_start() { return true; }
+ template <typename Iter> bool parse_object_item(input<Iter>& in, const std::string&) {
+ return _parse(*this, in);
+ }
+private:
+ null_parse_context(const null_parse_context&);
+ null_parse_context& operator=(const null_parse_context&);
+};
+
+// obsolete, use the version below
+template <typename Iter> inline std::string parse(value& out, Iter& pos, const Iter& last) {
+ std::string err;
+ pos = parse(out, pos, last, &err);
+ return err;
+}
+
+template <typename Context, typename Iter> inline Iter _parse(Context& ctx, const Iter& first, const Iter& last, std::string* err) {
+ input<Iter> in(first, last);
+ if (! _parse(ctx, in) && err != NULL) {
+ char buf[64];
+ SNPRINTF(buf, sizeof(buf), "syntax error at line %d near: ", in.line());
+ *err = buf;
+ while (1) {
+ int ch = in.getc();
+ if (ch == -1 || ch == '\n') {
+ break;
+ } else if (ch >= ' ') {
+ err->push_back(ch);
+ }
+ }
+ }
+ return in.cur();
+}
+
+template <typename Iter> inline Iter parse(value& out, const Iter& first, const Iter& last, std::string* err) {
+ default_parse_context ctx(&out);
+ return _parse(ctx, first, last, err);
+}
+
+inline std::string parse(value &out, const std::string &s) {
+ std::string err;
+ parse(out, s.begin(), s.end(), &err);
+ return err;
+}
+
+inline std::string parse(value& out, std::istream& is) {
+ std::string err;
+ parse(out, std::istreambuf_iterator<char>(is.rdbuf()),
+ std::istreambuf_iterator<char>(), &err);
+ return err;
+}
+
+template <typename T> struct last_error_t {
+ static std::string s;
+};
+template <typename T> std::string last_error_t<T>::s;
+
+inline void set_last_error(const std::string& s) {
+ last_error_t<bool>::s = s;
+}
+
+inline const std::string& get_last_error() {
+ return last_error_t<bool>::s;
+}
+
+inline bool operator==(const value& x, const value& y) {
+ if (x.is<null>())
+ return y.is<null>();
+#define PICOJSON_CMP(type) \
+ if (x.is<type>()) \
+ return y.is<type>() && x.get<type>() == y.get<type>()
+ PICOJSON_CMP(bool);
+ PICOJSON_CMP(double);
+ PICOJSON_CMP(std::string);
+ PICOJSON_CMP(array);
+ PICOJSON_CMP(object);
+#undef PICOJSON_CMP
+ PICOJSON_ASSERT(0);
+#ifdef _MSC_VER
+ __assume(0);
+#endif
+}
+
+inline bool operator!=(const value& x, const value& y) {
+ return ! (x == y);
+}
+
+} // namespace json
+
+inline std::istream& operator>>(std::istream& is, picojson::value& x)
+{
+ picojson::set_last_error(std::string());
+ std::string err = picojson::parse(x, is);
+ if (! err.empty()) {
+ picojson::set_last_error(err);
+ is.setstate(std::ios::failbit);
+ }
+ return is;
+}
+
+inline std::ostream& operator<<(std::ostream& os, const picojson::value& x)
+{
+ x.serialize(std::ostream_iterator<char>(os));
+ return os;
+}
+#ifdef _MSC_VER
+#pragma warning(pop)
+#endif // _MSC_VER
+
+#endif // picojson_h
+
+
+
+
+#ifdef TEST_PICOJSON
+
+#ifdef _MSC_VER
+#pragma warning(disable : 4127) // conditional expression is constant
+#endif // _MSC_VER
+
+using namespace std;
+
+static bool success = true;
+static int test_num = 0;
+
+static void ok(bool b, const char* name = "")
+{
+ if (! b)
+ success = false;
+ printf("%s %d - %s\n", b ? "ok" : "ng", ++test_num, name);
+}
+
+static void done_testing()
+{
+ printf("1..%d\n", test_num);
+}
+
+template <typename T> void is(const T& x, const T& y, const char* name = "")
+{
+ if (x == y) {
+ ok(true, name);
+ } else {
+ ok(false, name);
+ }
+}
+
+#include <algorithm>
+#include <sstream>
+#include <float.h>
+#include <limits.h>
+
+int main(void)
+{
+#if PICOJSON_USE_LOCALE
+ setlocale(LC_ALL, "");
+#endif // PICOJSON_USE_LOCALE
+
+ // constructors
+#define TEST(expr, expected) \
+ is(picojson::value expr .serialize(), string(expected), "picojson::value" #expr)
+
+ TEST( (true), "true");
+ TEST( (false), "false");
+ TEST( (42.0), "42");
+ TEST( (string("hello")), "\"hello\"");
+ TEST( ("hello"), "\"hello\"");
+ TEST( ("hello", 4), "\"hell\"");
+
+ {
+ double a = 1;
+ for (int i = 0; i < 1024; i++) {
+ picojson::value vi(a);
+ std::stringstream ss;
+ ss << vi;
+ picojson::value vo;
+ ss >> vo;
+ double b = vo.get<double>();
+ if ((i < 53 && a != b) || fabs(a - b) / b > 1e-8) {
+ printf("ng i=%d a=%.18e b=%.18e\n", i, a, b);
+ }
+ a *= 2;
+ }
+ }
+
+#undef TEST
+
+#define TEST(in, type, cmp, serialize_test) { \
+ picojson::value v; \
+ const char* s = in; \
+ string err = picojson::parse(v, s, s + strlen(s)); \
+ ok(err.empty(), in " no error"); \
+ ok(v.is<type>(), in " check type"); \
+ is<type>(v.get<type>(), cmp, in " correct output"); \
+ is(*s, '\0', in " read to eof"); \
+ if (serialize_test) { \
+ is(v.serialize(), string(in), in " serialize"); \
+} \
+}
+ TEST("false", bool, false, true);
+ TEST("true", bool, true, true);
+ TEST("90.5", double, 90.5, false);
+ TEST("1.7976931348623157e+308", double, DBL_MAX, false);
+ TEST("\"hello\"", string, string("hello"), true);
+ TEST("\"\\\"\\\\\\/\\b\\f\\n\\r\\t\"", string, string("\"\\/\b\f\n\r\t"),
+ true);
+ TEST("\"\\u0061\\u30af\\u30ea\\u30b9\"", string,
+ string("a\xe3\x82\xaf\xe3\x83\xaa\xe3\x82\xb9"), false);
+ TEST("\"\\ud840\\udc0b\"", string, string("\xf0\xa0\x80\x8b"), false);
+#ifdef PICOJSON_USE_INT64
+ TEST("0", int64_t, 0, true);
+ TEST("-9223372036854775808", int64_t, std::numeric_limits<int64_t>::min(), true);
+ TEST("9223372036854775807", int64_t, std::numeric_limits<int64_t>::max(), true);
+#endif // PICOJSON_USE_INT64
+
+#undef TEST
+
+#define TEST(type, expr) { \
+ picojson::value v; \
+ const char *s = expr; \
+ string err = picojson::parse(v, s, s + strlen(s)); \
+ ok(err.empty(), "empty " #type " no error"); \
+ ok(v.is<picojson::type>(), "empty " #type " check type"); \
+ ok(v.get<picojson::type>().empty(), "check " #type " array size"); \
+}
+ TEST(array, "[]");
+ TEST(object, "{}");
+#undef TEST
+
+ {
+ picojson::value v;
+ const char *s = "[1,true,\"hello\"]";
+ string err = picojson::parse(v, s, s + strlen(s));
+ ok(err.empty(), "array no error");
+ ok(v.is<picojson::array>(), "array check type");
+ is(v.get<picojson::array>().size(), size_t(3), "check array size");
+ ok(v.contains(0), "check contains array[0]");
+ ok(v.get(0).is<double>(), "check array[0] type");
+ is(v.get(0).get<double>(), 1.0, "check array[0] value");
+ ok(v.contains(1), "check contains array[1]");
+ ok(v.get(1).is<bool>(), "check array[1] type");
+ ok(v.get(1).get<bool>(), "check array[1] value");
+ ok(v.contains(2), "check contains array[2]");
+ ok(v.get(2).is<string>(), "check array[2] type");
+ is(v.get(2).get<string>(), string("hello"), "check array[2] value");
+ ok(!v.contains(3), "check not contains array[3]");
+ }
+
+ {
+ picojson::value v;
+ const char *s = "{ \"a\": true }";
+ string err = picojson::parse(v, s, s + strlen(s));
+ ok(err.empty(), "object no error");
+ ok(v.is<picojson::object>(), "object check type");
+ is(v.get<picojson::object>().size(), size_t(1), "check object size");
+ ok(v.contains("a"), "check contains property");
+ ok(v.get("a").is<bool>(), "check bool property exists");
+ is(v.get("a").get<bool>(), true, "check bool property value");
+ is(v.serialize(), string("{\"a\":true}"), "serialize object");
+ ok(!v.contains("z"), "check not contains property");
+ }
+
+#define TEST(json, msg) do { \
+ picojson::value v; \
+ const char *s = json; \
+ string err = picojson::parse(v, s, s + strlen(s)); \
+ is(err, string("syntax error at line " msg), msg); \
+} while (0)
+ TEST("falsoa", "1 near: oa");
+ TEST("{]", "1 near: ]");
+ TEST("\n\bbell", "2 near: bell");
+ TEST("\"abc\nd\"", "1 near: ");
+#undef TEST
+
+ {
+ picojson::value v1, v2;
+ const char *s;
+ string err;
+ s = "{ \"b\": true, \"a\": [1,2,\"three\"], \"d\": 2 }";
+ err = picojson::parse(v1, s, s + strlen(s));
+ s = "{ \"d\": 2.0, \"b\": true, \"a\": [1,2,\"three\"] }";
+ err = picojson::parse(v2, s, s + strlen(s));
+ ok((v1 == v2), "check == operator in deep comparison");
+ }
+
+ {
+ picojson::value v1, v2;
+ const char *s;
+ string err;
+ s = "{ \"b\": true, \"a\": [1,2,\"three\"], \"d\": 2 }";
+ err = picojson::parse(v1, s, s + strlen(s));
+ s = "{ \"d\": 2.0, \"a\": [1,\"three\"], \"b\": true }";
+ err = picojson::parse(v2, s, s + strlen(s));
+ ok((v1 != v2), "check != operator for array in deep comparison");
+ }
+
+ {
+ picojson::value v1, v2;
+ const char *s;
+ string err;
+ s = "{ \"b\": true, \"a\": [1,2,\"three\"], \"d\": 2 }";
+ err = picojson::parse(v1, s, s + strlen(s));
+ s = "{ \"d\": 2.0, \"a\": [1,2,\"three\"], \"b\": false }";
+ err = picojson::parse(v2, s, s + strlen(s));
+ ok((v1 != v2), "check != operator for object in deep comparison");
+ }
+
+ {
+ picojson::value v1, v2;
+ const char *s;
+ string err;
+ s = "{ \"b\": true, \"a\": [1,2,\"three\"], \"d\": 2 }";
+ err = picojson::parse(v1, s, s + strlen(s));
+ picojson::object& o = v1.get<picojson::object>();
+ o.erase("b");
+ picojson::array& a = o["a"].get<picojson::array>();
+ picojson::array::iterator i;
+ i = std::remove(a.begin(), a.end(), picojson::value(std::string("three")));
+ a.erase(i, a.end());
+ s = "{ \"a\": [1,2], \"d\": 2 }";
+ err = picojson::parse(v2, s, s + strlen(s));
+ ok((v1 == v2), "check erase()");
+ }
+
+ ok(picojson::value(3.0).serialize() == "3",
+ "integral number should be serialized as a integer");
+
+ {
+ const char* s = "{ \"a\": [1,2], \"d\": 2 }";
+ picojson::null_parse_context ctx;
+ string err;
+ picojson::_parse(ctx, s, s + strlen(s), &err);
+ ok(err.empty(), "null_parse_context");
+ }
+
+ {
+ picojson::value v;
+ const char *s = "{ \"a\": 1, \"b\": [ 2, { \"b1\": \"abc\" } ], \"c\": {}, \"d\": [] }";
+ string err;
+ err = picojson::parse(v, s, s + strlen(s));
+ ok(err.empty(), "parse test data for prettifying output");
+ ok(v.serialize() == "{\"a\":1,\"b\":[2,{\"b1\":\"abc\"}],\"c\":{},\"d\":[]}", "non-prettifying output");
+ ok(v.serialize(true) == "{\n \"a\": 1,\n \"b\": [\n 2,\n {\n \"b1\": \"abc\"\n }\n ],\n \"c\": {},\n \"d\": []\n}\n", "prettifying output");
+ }
+
+ try {
+ picojson::value v(std::numeric_limits<double>::quiet_NaN());
+ ok(false, "should not accept NaN");
+ } catch (std::overflow_error e) {
+ ok(true, "should not accept NaN");
+ }
+
+ try {
+ picojson::value v(std::numeric_limits<double>::infinity());
+ ok(false, "should not accept infinity");
+ } catch (std::overflow_error e) {
+ ok(true, "should not accept infinity");
+ }
+
+ try {
+ picojson::value v(123.);
+ ok(! v.is<bool>(), "is<wrong_type>() should return false");
+ v.get<bool>();
+ ok(false, "get<wrong_type>() should raise an error");
+ } catch (std::runtime_error e) {
+ ok(true, "get<wrong_type>() should raise an error");
+ }
+
+#ifdef PICOJSON_USE_INT64
+ {
+ picojson::value v1((int64_t)123);
+ ok(v1.is<int64_t>(), "is int64_t");
+ ok(v1.is<double>(), "is double as well");
+ ok(v1.serialize() == "123", "serialize the value");
+ ok(v1.get<int64_t>() == 123, "value is correct as int64_t");
+ ok(v1.get<double>(), "value is correct as double");
+
+ ok(! v1.is<int64_t>(), "is no more int64_type once get<double>() is called");
+ ok(v1.is<double>(), "and is still a double");
+
+ const char *s = "-9223372036854775809";
+ ok(picojson::parse(v1, s, s + strlen(s)).empty(), "parse underflowing int64_t");
+ ok(! v1.is<int64_t>(), "underflowing int is not int64_t");
+ ok(v1.is<double>(), "underflowing int is double");
+ ok(v1.get<double>() + 9.22337203685478e+18 < 65536, "double value is somewhat correct");
+ }
+#endif // PICOJSON_USE_INT64
+
+ done_testing();
+
+ return success ? 0 : 1;
+}
+
+#endif // TEST_PICOJSON
diff --git a/externals/Pangolin/include/pangolin/utils/posix/condition_variable.h b/externals/Pangolin/include/pangolin/utils/posix/condition_variable.h
new file mode 100644
index 0000000000000000000000000000000000000000..77e1b9f0871f3344e5ebce8da963014f840083ed
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/posix/condition_variable.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include <pangolin/utils/timer.h>
+
+#include <memory>
+
+namespace pangolin
+{
+
+class ConditionVariableInterface
+{
+public:
+ virtual ~ConditionVariableInterface()
+ {
+ }
+
+ virtual void wait() = 0;
+ virtual bool wait(timespec t) = 0;
+ virtual void signal() = 0;
+ virtual void broadcast() = 0;
+};
+
+std::shared_ptr<ConditionVariableInterface> create_named_condition_variable(const
+ std::string& name);
+std::shared_ptr<ConditionVariableInterface> open_named_condition_variable(const
+ std::string& name);
+}
diff --git a/externals/Pangolin/include/pangolin/utils/posix/semaphore.h b/externals/Pangolin/include/pangolin/utils/posix/semaphore.h
new file mode 100644
index 0000000000000000000000000000000000000000..8563d2b0fc8f7396a0ec94090e95217995c73e90
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/posix/semaphore.h
@@ -0,0 +1,26 @@
+#pragma once
+
+#include <memory>
+
+#include <string>
+
+namespace pangolin
+{
+
+ class SemaphoreInterface
+ {
+ public:
+
+ virtual ~SemaphoreInterface() {
+ }
+
+ virtual bool tryAcquire() = 0;
+ virtual void acquire() = 0;
+ virtual void release() = 0;
+ };
+
+ std::shared_ptr<SemaphoreInterface> create_named_semaphore(const std::string& name,
+ unsigned int value);
+ std::shared_ptr<SemaphoreInterface> open_named_semaphore(const std::string& name);
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/posix/shared_memory_buffer.h b/externals/Pangolin/include/pangolin/utils/posix/shared_memory_buffer.h
new file mode 100644
index 0000000000000000000000000000000000000000..8ece2bb4c2d3ecb50353271a3abc058d53cdbbee
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/posix/shared_memory_buffer.h
@@ -0,0 +1,25 @@
+#pragma once
+
+#include <memory>
+
+#include <string>
+
+namespace pangolin
+{
+ class SharedMemoryBufferInterface
+ {
+ public:
+ virtual ~SharedMemoryBufferInterface() {
+ }
+ virtual bool tryLock() = 0;
+ virtual void lock() = 0;
+ virtual void unlock() = 0;
+ virtual unsigned char *ptr() = 0;
+ virtual std::string name() = 0;
+ };
+
+ std::shared_ptr<SharedMemoryBufferInterface> create_named_shared_memory_buffer(const
+ std::string& name, size_t size);
+ std::shared_ptr<SharedMemoryBufferInterface> open_named_shared_memory_buffer(const
+ std::string& name, bool readwrite);
+}
diff --git a/externals/Pangolin/include/pangolin/utils/registration.h b/externals/Pangolin/include/pangolin/utils/registration.h
new file mode 100644
index 0000000000000000000000000000000000000000..70a3cae9f814b79391b15fbdb4d1b65a937bf319
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/registration.h
@@ -0,0 +1,64 @@
+#pragma once
+
+#include <functional>
+
+namespace pangolin {
+
+template<typename TokenType=void>
+class Registration
+{
+public:
+ using UnregisterFunc = std::function<void(const TokenType&)>;
+
+ Registration()
+ : token()
+ {
+ }
+
+ Registration(TokenType token, UnregisterFunc unregister)
+ : token(token), unregister(unregister)
+ {
+
+ }
+
+ // No copy constructor
+ Registration(const Registration&) = delete;
+
+ // Default move constructor
+ Registration(Registration&& o)
+ {
+ *this = std::move(o);
+ }
+
+ Registration<TokenType>& operator =(Registration<TokenType>&& o)
+ {
+ token = o.token;
+ unregister = std::move(o.unregister);
+ o.unregister = nullptr;
+ return *this;
+ }
+
+ ~Registration()
+ {
+ Release();
+ }
+
+ void Release()
+ {
+ if(unregister) {
+ unregister(token);
+ token = TokenType();
+ }
+ }
+
+ const TokenType& Token()
+ {
+ return token;
+ }
+
+private:
+ TokenType token;
+ UnregisterFunc unregister;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/signal_slot.h b/externals/Pangolin/include/pangolin/utils/signal_slot.h
new file mode 100644
index 0000000000000000000000000000000000000000..2d20c39d9b808c9298ed20a8557beca9d86ea3f0
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/signal_slot.h
@@ -0,0 +1,56 @@
+#pragma once
+
+#include <functional>
+#include <map>
+
+#include <pangolin/utils/registration.h>
+
+namespace pangolin {
+
+// Based on http://simmesimme.github.io/tutorials/2015/09/20/signal-slot
+template <typename... Args>
+class Signal
+{
+public:
+ using Id = size_t;
+ using Reg = Registration<Id>;
+
+ Signal()
+ : current_id_(0)
+ {
+ }
+
+ Signal(const Signal&) = delete;
+
+ Signal(Signal&&) = default;
+
+ // connects a std::function to the signal. The returned
+ // value can be used to disconnect the function again
+ Reg Connect(const std::function<void(Args...)>& slot) const {
+ slots_.insert(std::make_pair(++current_id_, slot));
+ return Reg(current_id_, [this](Id id){ Disconnect(id);});
+ }
+
+ // disconnects a previously connected function
+ void Disconnect(Id id) const {
+ slots_.erase(id);
+ }
+
+ // disconnects all previously connected functions
+ void DisconnectAll() const {
+ slots_.clear();
+ }
+
+ // calls all connected functions
+ void operator()(Args... p) {
+ for(auto it : slots_) {
+ it.second(p...);
+ }
+ }
+
+private:
+ mutable std::map<Id, std::function<void(Args...)>> slots_;
+ mutable Id current_id_;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/sigstate.h b/externals/Pangolin/include/pangolin/utils/sigstate.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ea68fd8831a1996fea9b3d5ab326e7ee3b250e3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/sigstate.h
@@ -0,0 +1,75 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+#include <pangolin/platform.h>
+#include <pangolin/utils/file_utils.h>
+#include <csignal>
+
+#ifndef SIGPIPE
+# define SIGPIPE 13
+#endif // !SIGPIPE
+
+namespace pangolin
+{
+
+typedef void (*SigCallbackFn)(int);
+
+struct PANGOLIN_EXPORT SigCallback
+{
+ SigCallback(const int & sig, SigCallbackFn fn, void* data)
+ : sig(sig), fn(fn), data(data), value(false)
+ {
+ std::signal(sig, fn);
+ }
+
+ int sig;
+ SigCallbackFn fn;
+ void * data;
+ volatile sig_atomic_t value;
+};
+
+class PANGOLIN_EXPORT SigState
+{
+public:
+ static SigState& I();
+
+ SigState();
+ ~SigState();
+
+ void Clear();
+
+ std::map<int, SigCallback> sig_callbacks;
+};
+
+PANGOLIN_EXPORT
+void RegisterNewSigCallback(SigCallbackFn callback, void* data, const int signal);
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/simple_math.h b/externals/Pangolin/include/pangolin/utils/simple_math.h
new file mode 100644
index 0000000000000000000000000000000000000000..eff7a43f32cdb6ab854e29ec594314f69d1ad741
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/simple_math.h
@@ -0,0 +1,446 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <iostream>
+#include <string.h>
+#include <algorithm>
+#include <stdarg.h>
+#include <cmath>
+
+namespace pangolin
+{
+
+static const double Identity3d[] = {1,0,0, 0,1,0, 0,0,1};
+static const double Zero3d[] = {0,0,0, 0,0,0, 0,0,0};
+static const double Identity4d[] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
+static const double Zero4d[] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
+
+static const float Identity3f[] = {1,0,0, 0,1,0, 0,0,1};
+static const float Zero3f[] = {0,0,0, 0,0,0, 0,0,0};
+static const float Identity4f[] = {1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1};
+static const float Zero4f[] = {0,0,0,0, 0,0,0,0, 0,0,0,0, 0,0,0,0};
+
+template<int R, int C, typename P>
+void MatPrint(const P m[R*C])
+{
+ for( int r=0; r < R; ++r)
+ {
+ for( int c=0; c < C; ++c )
+ std::cout << m[R*c+r] << " ";
+ std::cout << std::endl;
+ }
+ std::cout << std::endl;
+}
+
+template<int R, int C, typename P>
+void MatPrint(const P m[R*C], std::string name)
+{
+ std::cout << name << " = [" << std::endl;
+ for( int r=0; r < R; ++r)
+ {
+ for( int c=0; c < C; ++c )
+ std::cout << m[R*c+r] << " ";
+ std::cout << std::endl;
+ }
+ std::cout << std::endl << "]" << std::endl;
+}
+
+// Set array using varadic arguments
+template<int R, int C, typename P>
+void MatSet(P m[R*C], ...)
+{
+ va_list ap;
+ va_start(ap,m);
+ for( int i=0; i< R*C; ++i )
+ {
+ *m = (P)va_arg(ap,double);
+ ++m;
+ }
+}
+
+// m = zeroes(N)
+template<int R, int C, typename P>
+void SetZero(P m[R*C] )
+{
+ std::fill_n(m,R*C,0);
+}
+
+// m = identity(N)
+template<int N, typename P>
+void SetIdentity(P m[N*N] )
+{
+ std::fill_n(m,N*N,0);
+ for( int i=0; i< N; ++i )
+ m[N*i+i] = 1;
+}
+
+// mo = m1 * m2
+template<int R, int M, int C, typename P>
+void MatMul(P mo[R*C], const P m1[R*M], const P m2[M*C] )
+{
+ for( int r=0; r < R; ++r)
+ for( int c=0; c < C; ++c )
+ {
+ mo[R*c+r] = 0;
+ for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[M*c+m];
+ }
+}
+
+// mo = m1 * m2 * s
+template<int R, int M, int C, typename P>
+void MatMul(P mo[R*C], const P m1[R*M], const P m2[M*C], P s )
+{
+ for( int r=0; r < R; ++r)
+ for( int c=0; c < C; ++c )
+ {
+ mo[R*c+r] = 0;
+ for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[M*c+m] * s;
+ }
+}
+
+// mo = m1 * transpose(m2)
+template<int R, int M, int C, typename P>
+void MatMulTranspose(P mo[R*C], const P m1[R*M], const P m2[C*M] )
+{
+ for( int r=0; r < R; ++r)
+ for( int c=0; c < C; ++c )
+ {
+ mo[R*c+r] = 0;
+ for( int m=0; m < M; ++ m) mo[R*c+r] += m1[R*m+r] * m2[C*m+c];
+ }
+}
+
+// m = m1 + m2
+template<int R, int C, typename P>
+void MatAdd(P m[R*C], const P m1[R*C], const P m2[R*C])
+{
+ for( int i=0; i< R*C; ++i )
+ m[i] = m1[i] + m2[i];
+}
+
+// m = m1 - m2
+template<int R, int C, typename P>
+void MatSub(P m[R*C], const P m1[R*C], const P m2[R*C])
+{
+ for( int i=0; i< R*C; ++i )
+ m[i] = m1[i] - m2[i];
+}
+
+// m = m1 * scalar
+template<int R, int C, typename P>
+void MatMul(P m[R*C], const P m1[R*C], P scalar)
+{
+ for( int i=0; i< R*C; ++i )
+ m[i] = m1[i] * scalar;
+}
+
+// m = m1 + m2
+template<int R, int C, typename P>
+void MatMul(P m[R*C], P scalar)
+{
+ for( int i=0; i< R*C; ++i )
+ m[i] *= scalar;
+}
+
+template<int N, typename P>
+void MatTranspose(P out[N*N], const P in[N*N] )
+{
+ for( int c=0; c<N; ++c )
+ for( int r=0; r<N; ++r )
+ out[N*c+r] = in[N*r+c];
+}
+
+template<int N, typename P>
+void MatTranspose(P m[N*N] )
+{
+ for( int c=0; c<N; ++c )
+ for( int r=0; r<c; ++r )
+ std::swap<P>(m[N*c+r],m[N*r+c]);
+}
+
+// m = a x b
+template<typename P>
+void VecCross3(P m[3], const P a[3], const P b[3])
+{
+ m[0] = a[1]*b[2] - a[2]*b[1];
+ m[1] = a[2]*b[0] - a[0]*b[2];
+ m[2] = a[0]*b[1] - a[1]*b[0];
+}
+
+// s = skewSymetrixMatrix(v)
+template<typename P>
+void MatSkew(P s[3*3], const P v[3] )
+{
+ s[0] = 0;
+ s[1] = v[2];
+ s[2] = -v[1];
+ s[3] = -v[2];
+ s[4] = 0;
+ s[5] = v[0];
+ s[6] = v[1];
+ s[7] = -v[0];
+ s[8] = 0;
+}
+
+template<int N, typename P>
+void MatOrtho( P m[N*N] )
+{
+ // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/index.htm
+ P Itimes3[N*N];
+ SetIdentity<N>(Itimes3);
+ MatMul<N,N>(Itimes3,(P)3.0);
+
+ P mmT[N*N];
+ MatMulTranspose<N,N,N>(mmT,m,m);
+
+ P _c[N*N];
+ MatSub<N,N>(_c,Itimes3,mmT);
+ P _m[N*N];
+ MatMul<N,N,N>(_m,m,_c,(P)0.5);
+ std::copy(_m,_m+(N*N),m);
+}
+
+template<typename P>
+void Rotation(P R[3*3], P x, P y, P z)
+{
+ P sx = sin(x);
+ P sy = sin(y);
+ P sz = sin(z);
+ P cx = cos(x);
+ P cy = cos(y);
+ P cz = cos(z);
+ // P cx = sqrt( (P)1.0 - sx * sx);
+ // P cy = sqrt( (P)1.0 - sy * sy);
+ // P cz = sqrt( (P)1.0 - sz * sz);
+ R[0] = cy * cz;
+ R[1] = sx * sy * cz + cx * sz;
+ R[2] = -cx * sy * cz + sx * sz;
+ R[3] = -cy * sz;
+ R[4] = -sx * sy * sz + cx * cz;
+ R[5] = cx * sy * sz + sx * cz;
+ R[6] = sy;
+ R[7] = -sx * cy;
+ R[8] = cx * cy;
+}
+
+template<typename P>
+void LieSetIdentity(P T_ba[3*4] )
+{
+ SetIdentity<3>(T_ba);
+ std::fill_n(T_ba+(3*3),3,0);
+}
+
+template<typename P>
+void LieSetRotation(P T_ba[3*4], const P R_ba[3*3] )
+{
+ std::copy(R_ba,R_ba+(3*3),T_ba);
+}
+
+template<typename P>
+void LieSetTranslation(P T_ba[3*4], const P a_b[3] )
+{
+ std::copy(a_b, a_b+3, T_ba+(3*3));
+}
+
+template<typename P>
+void LieSetSE3(P T_ba[3*4], const P R_ba[3*3], const P a_b[3] )
+{
+ LieSetRotation<P>(T_ba,R_ba);
+ LieSetTranslation<P>(T_ba,a_b);
+}
+
+template<typename P>
+void LieGetRotation(P R_ba[3*3], const P T_ba[3*4] )
+{
+ std::copy(T_ba,T_ba+(3*3),R_ba);
+}
+
+template<typename P>
+void LieApplySO3( P out[3], const P R_ba[3*3], const P in[3] )
+{
+ MatMul<3,3,1,P>(out,R_ba,in);
+}
+
+template<typename P>
+void LieApplySE3vec( P x_b[3], const P T_ba[3*4], const P x_a[3] )
+{
+ P rot[3];
+ MatMul<3,3,1,P>(rot,T_ba,x_a);
+ MatAdd<3,1,P>(x_b,rot,T_ba+(3*3));
+}
+
+template<typename P>
+void LieApplySE34x4vec3( P x_b[3], const P T_ba[4*4], const P x_a[3] )
+{
+ x_b[0] = T_ba[0]*x_a[0] + T_ba[4]*x_a[1] + T_ba[8]*x_a[2] + T_ba[12];
+ x_b[1] = T_ba[1]*x_a[0] + T_ba[5]*x_a[1] + T_ba[9]*x_a[2] + T_ba[13];
+ x_b[2] = T_ba[2]*x_a[0] + T_ba[6]*x_a[1] + T_ba[10]*x_a[2] + T_ba[14];
+}
+
+template<typename P>
+void LieMulSO3( P R_ca[3*3], const P R_cb[3*3], const P R_ba[3*3] )
+{
+ MatMul<3,3,3>(R_ca,R_cb,R_ba);
+}
+
+template<typename P>
+void LieMulSE3( P T_ca[3*4], const P T_cb[3*4], const P T_ba[3*4] )
+{
+ LieMulSO3<>(T_ca,T_cb,T_ba);
+ P R_cb_times_a_b[3];
+ LieApplySO3<>(R_cb_times_a_b,T_cb,T_ba+(3*3));
+ MatAdd<3,1>(T_ca+(3*3),R_cb_times_a_b,T_cb+(3*3));
+}
+
+template<typename P>
+void LiePutSE3in4x4(P out[4*4], const P in[3*4] )
+{
+ SetIdentity<4>(out);
+ std::copy(in,in+3, out);
+ std::copy(in+3,in+6, out+4);
+ std::copy(in+6,in+9, out+8);
+ std::copy(in+9,in+12, out+12);
+}
+
+template<typename P>
+void LieSE3from4x4(P out[3*4], const P in[4*4] )
+{
+ std::copy(in,in+3, out);
+ std::copy(in+4,in+7, out+3);
+ std::copy(in+8,in+11, out+6);
+ std::copy(in+12,in+15, out+9);
+}
+
+template<typename P>
+void LieMul4x4bySE3( P T_ca[4*4], const P T_cb[3*4], const P T_ba[4*4] )
+{
+ // TODO: fast
+ P T_cb4[4*4];
+ LiePutSE3in4x4<>(T_cb4,T_cb);
+ P res[4*4];
+ MatMul<4,4,4>(res,T_cb4,T_ba);
+ std::copy(res,res+(4*4),T_ca);
+}
+
+template<typename P>
+void LieTransposeSO3( P R_ab[3*3], const P R_ba[3*3] )
+{
+ MatTranspose<3,P>(R_ab,R_ba);
+}
+
+template<typename P>
+void LieInverseSE3( P T_ab[3*4], const P T_ba[3*4] )
+{
+ LieTransposeSO3<P>(T_ab,T_ba);
+ P minus_b_a[3];
+ LieApplySO3(minus_b_a, T_ab, T_ba+(3*3));
+ MatMul<3,1,P>(T_ab+(3*3),minus_b_a, -1);
+}
+
+// c = a x b
+template<typename P>
+void CrossProduct( P c[3], const P a[3], const P b[3] )
+{
+ c[0] = a[1] * b[2] - a[2] * b[1];
+ c[1] = a[2] * b[0] - a[0] * b[2];
+ c[2] = a[0] * b[1] - a[1] * b[0];
+}
+
+template<int R, typename P>
+P Length( P v[R] )
+{
+ P sum_sq = 0;
+
+ for(size_t r = 0; r < R; ++r ) {
+ sum_sq += v[r] * v[r];
+ }
+
+ return sqrt(sum_sq);
+}
+
+
+template<int R, typename P>
+void Normalise( P v[R] )
+{
+ const P length = Length<R,P>(v);
+
+ for(size_t r = 0; r < R; ++r ) {
+ v[r] /= length;
+ }
+}
+
+template<typename P>
+void EnforceUpT_wc(P T_wc[3*4], const P up_w[3])
+{
+ // Copy R_wc
+ P R_wc[3*3];
+ std::copy(T_wc,T_wc+3*3,R_wc);
+
+ // New R_wc should go into T_wc
+ P* NR_wc = T_wc;
+
+ // // cx_w,cy_w,cz_w are camera axis in world coordinates
+ // // Calculate new camera coordinates (ncx_w,ncy_w,ncz_w)
+
+ // ncx_w = up_w x cz_w
+ CrossProduct(NR_wc + 0*3, up_w, R_wc + 2*3);
+
+ // ncy_w = cz_w x ncx_w
+ CrossProduct(NR_wc + 1*3, R_wc + 2*3, NR_wc + 0*3);
+
+ // ncz_w = cz_w
+ std::copy(R_wc + 2*3, R_wc + 3*3, NR_wc + 2*3);
+
+ Normalise<3,P>(NR_wc + 0*3);
+ Normalise<3,P>(NR_wc + 1*3);
+ Normalise<3,P>(NR_wc + 2*3);
+}
+
+template<typename P>
+void EnforceUpT_cw(P T_cw_4x4[4*4], const P up_w[3])
+{
+ // 3x4 from 4x4
+ P T_cw[3*4];
+ LieSE3from4x4<P>(T_cw,T_cw_4x4);
+
+ // Invert T_cw
+ P T_wc[3*4];
+ LieInverseSE3<P>(T_wc, T_cw);
+
+ // Enforce up for T_wc
+ EnforceUpT_wc<P>(T_wc, up_w);
+
+ // Invert
+ LieInverseSE3<P>(T_cw, T_wc);
+
+ // 4x4 from 3x4
+ LiePutSE3in4x4<P>(T_cw_4x4,T_cw);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/threadedfilebuf.h b/externals/Pangolin/include/pangolin/utils/threadedfilebuf.h
new file mode 100644
index 0000000000000000000000000000000000000000..a8eb2ac9ad05ad8090d3f667e6324a527d53ae18
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/threadedfilebuf.h
@@ -0,0 +1,97 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <iostream>
+#include <streambuf>
+#include <fstream>
+
+#include <pangolin/platform.h>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+
+#ifdef _LINUX_
+// On linux, using posix file i/o to allow sync writes.
+#define USE_POSIX_FILE_IO
+#endif
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT threadedfilebuf : public std::streambuf
+{
+public:
+ ~threadedfilebuf();
+ threadedfilebuf();
+ threadedfilebuf(const std::string& filename, size_t buffer_size_bytes);
+
+ void open(const std::string& filename, size_t buffer_size_bytes);
+ void close();
+ void force_close();
+
+ void operator()();
+
+protected:
+ void soft_close();
+
+ //! Override streambuf::xsputn for asynchronous write
+ std::streamsize xsputn(const char * s, std::streamsize n) override;
+
+ //! Override streambuf::overflow for asynchronous write
+ int overflow(int c) override;
+
+ std::streampos seekoff(
+ std::streamoff off, std::ios_base::seekdir way,
+ std::ios_base::openmode which = std::ios_base::in | std::ios_base::out
+ ) override;
+
+#ifdef USE_POSIX_FILE_IO
+ int filenum = -1;
+#else
+ std::filebuf file;
+#endif
+
+ char* mem_buffer;
+ std::streamsize mem_size;
+ std::streamsize mem_max_size;
+ std::streamsize mem_start;
+ std::streamsize mem_end;
+
+ std::streampos input_pos;
+
+ std::mutex update_mutex;
+ std::condition_variable cond_queued;
+ std::condition_variable cond_dequeued;
+ std::thread write_thread;
+
+ bool should_run;
+ bool is_pipe;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/timer.h b/externals/Pangolin/include/pangolin/utils/timer.h
new file mode 100644
index 0000000000000000000000000000000000000000..6b0418b4791168f84f377e51dd523b0a1c798e74
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/timer.h
@@ -0,0 +1,116 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <chrono>
+#include <thread>
+
+#include <pangolin/platform.h>
+
+namespace pangolin
+{
+
+// These methods exist for backwards compatibility.
+// They are deprecated in favour of direct use of std::chrono in C++11
+
+using baseclock = std::chrono::steady_clock;
+using basetime = baseclock::time_point;
+static_assert(baseclock::is_steady, "baseclock must be steady to be robust against system time settings");
+
+inline basetime TimeNow()
+{
+ return baseclock::now();
+}
+
+inline double Time_s(basetime t)
+{
+ using namespace std::chrono;
+ return duration_cast<seconds>( t.time_since_epoch() ).count();
+}
+
+inline int64_t Time_us(basetime t)
+{
+ using namespace std::chrono;
+ return duration_cast<microseconds>( t.time_since_epoch() ).count();
+}
+
+inline double TimeDiff_s(basetime start, basetime end)
+{
+ const baseclock::duration d = end - start;
+ return Time_s( basetime() + d);
+}
+
+inline int64_t TimeDiff_us(basetime start, basetime end)
+{
+ const baseclock::duration d = end - start;
+ return Time_us( basetime() + d);
+}
+
+inline basetime TimeAdd(basetime t1, basetime t2)
+{
+
+ return t1 + t2.time_since_epoch();
+}
+
+inline double TimeNow_s()
+{
+ return Time_s(TimeNow());
+}
+
+inline int64_t TimeNow_us()
+{
+ return Time_us(TimeNow());
+}
+
+inline basetime WaitUntil(basetime t)
+{
+ std::this_thread::sleep_until(t);
+ return TimeNow();
+}
+
+struct Timer
+{
+ Timer() {
+ Reset();
+ }
+
+ void Reset()
+ {
+ start = TimeNow();
+ }
+
+ double Elapsed_s()
+ {
+ basetime currtime = TimeNow();
+ return TimeDiff_s(start,currtime);
+ }
+
+ basetime start;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/transform.h b/externals/Pangolin/include/pangolin/utils/transform.h
new file mode 100644
index 0000000000000000000000000000000000000000..b29c1e9d99a678f80ea15b73b1fbf721a7fb5a66
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/transform.h
@@ -0,0 +1,102 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <ostream>
+#include <functional>
+
+namespace pangolin
+{
+
+// Find the open brace preceeded by '$'
+inline const char* FirstOpenBrace(const char* str, char token = '$', char open = '{')
+{
+ bool symbol = false;
+
+ for(; *str != '\0'; ++str ) {
+ if( *str == token) {
+ symbol = true;
+ }else{
+ if( symbol ) {
+ if( *str == open ) {
+ return str;
+ } else {
+ symbol = false;
+ }
+ }
+ }
+ }
+ return 0;
+}
+
+// Find the first matching end brace. str includes open brace
+inline const char* MatchingEndBrace(const char* str, char open = '{', char close = '}')
+{
+ int b = 0;
+ for(; *str != '\0'; ++str ) {
+ if( *str == open ) {
+ ++b;
+ }else if( *str == close ) {
+ --b;
+ if( b == 0 ) {
+ return str;
+ }
+ }
+ }
+ return 0;
+}
+
+inline std::string Transform(const std::string& val, std::function<std::string(const std::string&)> dictionary, char token = '$', char open = '{', char close = '}')
+{
+ std::string expanded = val;
+
+ while(true)
+ {
+ const char* brace = FirstOpenBrace(expanded.c_str(), token, open);
+ if(brace)
+ {
+ const char* endbrace = MatchingEndBrace(brace);
+ if( endbrace )
+ {
+ std::ostringstream oss;
+ oss << std::string(expanded.c_str(), brace-1);
+
+ const std::string inexpand = Transform( std::string(brace+1,endbrace), dictionary, token, open, close );
+ oss << dictionary(inexpand);
+ oss << std::string(endbrace+1, expanded.c_str() + expanded.length() );
+ expanded = oss.str();
+ continue;
+ }
+ }
+ break;
+ }
+
+ return expanded;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/type_convert.h b/externals/Pangolin/include/pangolin/utils/type_convert.h
new file mode 100644
index 0000000000000000000000000000000000000000..93d026f7c87f10e89032f35f4d3a3ae906c1299f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/type_convert.h
@@ -0,0 +1,210 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <iostream>
+#include <sstream>
+
+#include <functional>
+#include <pangolin/compat/type_traits.h>
+
+namespace pangolin
+{
+struct BadInputException : std::exception {
+ char const* what() const throw() { return "Failed to serialise type"; }
+};
+}
+
+namespace std
+{
+// Dummy methods to serialise functions / functors / lambdas etc
+#ifdef CALLEE_HAS_VARIADIC_TEMPLATES
+template<typename Ret, typename... Args>
+inline std::istream& operator>>(std::istream& /*is*/, std::function<Ret(Args...)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+template<typename Ret, typename... Args>
+inline std::ostream& operator<<(std::ostream& /*os*/, const std::function<Ret(Args...)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+#else
+template<typename Ret, typename Arg>
+inline std::istream& operator>>(std::istream& /*is*/, std::function<Ret(Arg)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+template<typename Ret, typename Arg>
+inline std::ostream& operator<<(std::ostream& /*os*/, const std::function<Ret(Arg)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+inline std::istream& operator>>(std::istream& /*is*/, std::function<void(void)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+inline std::ostream& operator<<(std::ostream& /*os*/, const std::function<void(void)>& /*f*/) {
+ throw pangolin::BadInputException();
+}
+#endif
+}
+
+namespace pangolin
+{
+
+template<typename T, typename S, typename Enable=void>
+struct Convert;
+
+// Generic conversion through serialisation from / to string
+template<typename T, typename S, typename Enable>
+struct Convert {
+ static T Do(const S& src)
+ {
+ std::ostringstream oss;
+ oss << src;
+ std::istringstream iss(oss.str());
+ T target;
+ iss >> target;
+
+ if(iss.fail())
+ throw BadInputException();
+
+ return target;
+ }
+};
+
+// Between the same types is just a copy
+template<typename T>
+struct Convert<T, T > {
+ static T Do(const T& src)
+ {
+ return src;
+ }
+};
+
+// Apply bool alpha IO manipulator for bool types
+template<>
+struct Convert<bool,std::string> {
+ static bool Do(const std::string& src)
+ {
+ bool target;
+ std::istringstream iss(src);
+ iss >> target;
+
+ if(iss.fail())
+ {
+ std::istringstream iss2(src);
+ iss2 >> std::boolalpha >> target;
+ if( iss2.fail())
+ throw BadInputException();
+ }
+
+ return target;
+ }
+};
+
+// From strings
+template<typename T>
+struct Convert<T,std::string, typename pangolin::enable_if_c<
+ !std::is_same<T,std::string>::value
+ >::type > {
+ static T Do(const std::string& src)
+ {
+ T target;
+ std::istringstream iss(src);
+ iss >> target;
+
+ if(iss.fail())
+ throw BadInputException();
+
+ return target;
+ }
+};
+
+// To strings
+template<typename S>
+struct Convert<std::string, S, typename pangolin::enable_if_c<
+ !std::is_same<S,std::string>::value
+ >::type > {
+ static std::string Do(const S& src)
+ {
+ std::ostringstream oss;
+ oss << src;
+ return oss.str();
+ }
+};
+
+// Between scalars
+template<typename T, typename S>
+struct Convert<T, S, typename pangolin::enable_if_c<
+ std::is_scalar<T>::value && !std::is_same<T, bool>::value &&
+ std::is_scalar<S>::value && !std::is_same<S, bool>::value &&
+ !std::is_same<S,T>::value
+ >::type > {
+ static T Do(const S& src)
+ {
+ return static_cast<T>(src);
+ }
+};
+
+// From Scalars to bool (different than scalar definition to avoid MSVC Warnings)
+template<typename T, typename S>
+struct Convert<T, S, typename pangolin::enable_if_c<
+ std::is_same<T, bool>::value &&
+ std::is_scalar<S>::value &&
+ !std::is_same<S, T>::value
+>::type > {
+ static T Do(const S& src)
+ {
+ return src != static_cast<S>(0);
+ }
+};
+
+// From bool to Scalars (different than scalar definition to avoid MSVC Warnings)
+template<typename T, typename S>
+struct Convert<T, S, typename pangolin::enable_if_c<
+ std::is_scalar<T>::value &&
+ std::is_same<S, bool>::value &&
+ !std::is_same<S, T>::value
+>::type > {
+ static T Do(const S& src)
+ {
+ return src ? static_cast<T>(0) : static_cast<T>(1);
+ }
+};
+
+template<typename S>
+std::string ToString(const S& src)
+{
+ return Convert<std::string,S>::Do(src);
+}
+
+template<typename T>
+T FromString(const std::string& src)
+{
+ return Convert<T,std::string>::Do(src);
+}
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/uri.h b/externals/Pangolin/include/pangolin/utils/uri.h
new file mode 100644
index 0000000000000000000000000000000000000000..0ea376edc713a2640d5030536e2fbf096121a12a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/uri.h
@@ -0,0 +1,49 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/utils/params.h>
+#include <string>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT Uri : public Params
+{
+public:
+ std::string scheme;
+ std::string url;
+ std::string full_uri;
+};
+
+//! Parse string as Video URI
+PANGOLIN_EXPORT
+Uri ParseUri(const std::string& str_uri);
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/variadic_all.h b/externals/Pangolin/include/pangolin/utils/variadic_all.h
new file mode 100644
index 0000000000000000000000000000000000000000..3cc0681e5be7fa736251bfabc679b21e69ce0d7f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/variadic_all.h
@@ -0,0 +1,44 @@
+#pragma once
+
+#include <tuple>
+
+namespace pangolin {
+
+template<typename TPred, typename T>
+bool all_of(TPred pred, const T& i)
+{
+ return pred(i);
+}
+
+template<typename TPred, typename T, typename... Targs>
+bool all_of(const TPred& pred, const T& i, const Targs& ... ins)
+{
+ return pred(i) && all_of(pred, ins...);
+}
+
+template<typename TPred, typename T>
+bool any_of(TPred pred, const T& i)
+{
+ return pred(i);
+}
+
+template<typename TPred, typename T, typename... Targs>
+bool any_of(const TPred& pred, const T& i, Targs& ... ins)
+{
+ return pred(i) || any_of(pred, ins...);
+}
+
+template<typename TContainer, typename... Targs>
+bool all_found(const TContainer& c, const Targs& ... its)
+{
+ using T1 = typename std::tuple_element<0, std::tuple<Targs...>>::type;
+ return all_of([&c](const T1& it){return it != c.end();}, its...);
+}
+
+template<typename T, typename... Targs>
+bool all_equal(const T& v1, const Targs& ... its)
+{
+ return all_of([v1](const T& o){return v1 == o;}, its...);
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/utils/xml/license.txt b/externals/Pangolin/include/pangolin/utils/xml/license.txt
new file mode 100755
index 0000000000000000000000000000000000000000..0095bc72f576af3b47cd01a3d36348efaf68e9c2
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/xml/license.txt
@@ -0,0 +1,52 @@
+Use of this software is granted under one of the following two licenses,
+to be chosen freely by the user.
+
+1. Boost Software License - Version 1.0 - August 17th, 2003
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+Permission is hereby granted, free of charge, to any person or organization
+obtaining a copy of the software and accompanying documentation covered by
+this license (the "Software") to use, reproduce, display, distribute,
+execute, and transmit the Software, and to prepare derivative works of the
+Software, and to permit third-parties to whom the Software is furnished to
+do so, all subject to the following:
+
+The copyright notices in the Software and this entire statement, including
+the above license grant, this restriction and the following disclaimer,
+must be included in all copies of the Software, in whole or in part, and
+all derivative works of the Software, unless such copies or derivative
+works are solely in the form of machine-executable object code generated by
+a source language processor.
+
+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, TITLE AND NON-INFRINGEMENT. IN NO EVENT
+SHALL THE COPYRIGHT HOLDERS OR ANYONE DISTRIBUTING THE SOFTWARE BE LIABLE
+FOR ANY DAMAGES OR OTHER LIABILITY, WHETHER IN CONTRACT, TORT OR OTHERWISE,
+ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER
+DEALINGS IN THE SOFTWARE.
+
+2. The MIT License
+===============================================================================
+
+Copyright (c) 2006, 2007 Marcin Kalicinski
+
+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.
diff --git a/externals/Pangolin/include/pangolin/utils/xml/rapidxml.hpp b/externals/Pangolin/include/pangolin/utils/xml/rapidxml.hpp
new file mode 100755
index 0000000000000000000000000000000000000000..68fa052eb3be4e6d96a4c7f68abae5864182ebe3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/xml/rapidxml.hpp
@@ -0,0 +1,2635 @@
+#ifndef RAPIDXML_HPP_INCLUDED
+#define RAPIDXML_HPP_INCLUDED
+
+// Copyright (C) 2006, 2009 Marcin Kalicinski
+// Version 1.13
+// Revision $DateTime: 2009/05/13 01:46:17 $
+//! \file rapidxml.hpp This file contains rapidxml parser and DOM implementation
+
+// If standard library is disabled, user must provide implementations of required functions and typedefs
+#if !defined(RAPIDXML_NO_STDLIB)
+ #include <cstdlib> // For std::size_t
+ #include <cassert> // For assert
+ #include <new> // For placement new
+ #include <sstream> // for conversion between types
+#endif
+
+// On MSVC, disable "conditional expression is constant" warning (level 4).
+// This warning is almost impossible to avoid with certain types of templated code
+#ifdef _MSC_VER
+ #pragma warning(push)
+ #pragma warning(disable:4127) // Conditional expression is constant
+#endif
+
+///////////////////////////////////////////////////////////////////////////
+// RAPIDXML_PARSE_ERROR
+
+#if defined(RAPIDXML_NO_EXCEPTIONS)
+
+#define RAPIDXML_PARSE_ERROR(what, where) { parse_error_handler(what, where); assert(0); }
+
+namespace rapidxml
+{
+ //! When exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS,
+ //! this function is called to notify user about the error.
+ //! It must be defined by the user.
+ //! <br><br>
+ //! This function cannot return. If it does, the results are undefined.
+ //! <br><br>
+ //! A very simple definition might look like that:
+ //! <pre>
+ //! void %rapidxml::%parse_error_handler(const char *what, void *where)
+ //! {
+ //! std::cout << "Parse error: " << what << "\n";
+ //! std::abort();
+ //! }
+ //! </pre>
+ //! \param what Human readable description of the error.
+ //! \param where Pointer to character data where error was detected.
+ void parse_error_handler(const char *what, void *where);
+}
+
+#else
+
+#include <exception> // For std::exception
+
+#define RAPIDXML_PARSE_ERROR(what, where) throw parse_error(what, where)
+
+namespace rapidxml
+{
+
+ //! Parse error exception.
+ //! This exception is thrown by the parser when an error occurs.
+ //! Use what() function to get human-readable error message.
+ //! Use where() function to get a pointer to position within source text where error was detected.
+ //! <br><br>
+ //! If throwing exceptions by the parser is undesirable,
+ //! it can be disabled by defining RAPIDXML_NO_EXCEPTIONS macro before rapidxml.hpp is included.
+ //! This will cause the parser to call rapidxml::parse_error_handler() function instead of throwing an exception.
+ //! This function must be defined by the user.
+ //! <br><br>
+ //! This class derives from <code>std::exception</code> class.
+ class parse_error: public std::exception
+ {
+
+ public:
+
+ //! Constructs parse error
+ parse_error(const char *what, void *where)
+ : m_what(what)
+ , m_where(where)
+ {
+ }
+
+ //! Gets human readable description of error.
+ //! \return Pointer to null terminated description of the error.
+ virtual const char *what() const throw()
+ {
+ return m_what;
+ }
+
+ //! Gets pointer to character data where error happened.
+ //! Ch should be the same as char type of xml_document that produced the error.
+ //! \return Pointer to location within the parsed string where error occured.
+ template<class Ch>
+ Ch *where() const
+ {
+ return reinterpret_cast<Ch *>(m_where);
+ }
+
+ private:
+
+ const char *m_what;
+ void *m_where;
+
+ };
+}
+
+#endif
+
+///////////////////////////////////////////////////////////////////////////
+// Pool sizes
+
+#ifndef RAPIDXML_STATIC_POOL_SIZE
+ // Size of static memory block of memory_pool.
+ // Define RAPIDXML_STATIC_POOL_SIZE before including rapidxml.hpp if you want to override the default value.
+ // No dynamic memory allocations are performed by memory_pool until static memory is exhausted.
+ #define RAPIDXML_STATIC_POOL_SIZE (64 * 1024)
+#endif
+
+#ifndef RAPIDXML_DYNAMIC_POOL_SIZE
+ // Size of dynamic memory block of memory_pool.
+ // Define RAPIDXML_DYNAMIC_POOL_SIZE before including rapidxml.hpp if you want to override the default value.
+ // After the static block is exhausted, dynamic blocks with approximately this size are allocated by memory_pool.
+ #define RAPIDXML_DYNAMIC_POOL_SIZE (64 * 1024)
+#endif
+
+#ifndef RAPIDXML_ALIGNMENT
+ // Memory allocation alignment.
+ // Define RAPIDXML_ALIGNMENT before including rapidxml.hpp if you want to override the default value, which is the size of pointer.
+ // All memory allocations for nodes, attributes and strings will be aligned to this value.
+ // This must be a power of 2 and at least 1, otherwise memory_pool will not work.
+ #define RAPIDXML_ALIGNMENT sizeof(void *)
+#endif
+
+namespace rapidxml
+{
+ // Forward declarations
+ template<class Ch> class xml_node;
+ template<class Ch> class xml_attribute;
+ template<class Ch> class xml_document;
+
+ //! Enumeration listing all node types produced by the parser.
+ //! Use xml_node::type() function to query node type.
+ enum node_type
+ {
+ node_document, //!< A document node. Name and value are empty.
+ node_element, //!< An element node. Name contains element name. Value contains text of first data node.
+ node_data, //!< A data node. Name is empty. Value contains data text.
+ node_cdata, //!< A CDATA node. Name is empty. Value contains data text.
+ node_comment, //!< A comment node. Name is empty. Value contains comment text.
+ node_declaration, //!< A declaration node. Name and value are empty. Declaration parameters (version, encoding and standalone) are in node attributes.
+ node_doctype, //!< A DOCTYPE node. Name is empty. Value contains DOCTYPE text.
+ node_pi //!< A PI node. Name contains target. Value contains instructions.
+ };
+
+ ///////////////////////////////////////////////////////////////////////
+ // Parsing flags
+
+ //! Parse flag instructing the parser to not create data nodes.
+ //! Text of first data node will still be placed in value of parent element, unless rapidxml::parse_no_element_values flag is also specified.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_no_data_nodes = 0x1;
+
+ //! Parse flag instructing the parser to not use text of first data node as a value of parent element.
+ //! Can be combined with other flags by use of | operator.
+ //! Note that child data nodes of element node take precendence over its value when printing.
+ //! That is, if element has one or more child data nodes <em>and</em> a value, the value will be ignored.
+ //! Use rapidxml::parse_no_data_nodes flag to prevent creation of data nodes if you want to manipulate data using values of elements.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_no_element_values = 0x2;
+
+ //! Parse flag instructing the parser to not place zero terminators after strings in the source text.
+ //! By default zero terminators are placed, modifying source text.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_no_string_terminators = 0x4;
+
+ //! Parse flag instructing the parser to not translate entities in the source text.
+ //! By default entities are translated, modifying source text.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_no_entity_translation = 0x8;
+
+ //! Parse flag instructing the parser to disable UTF-8 handling and assume plain 8 bit characters.
+ //! By default, UTF-8 handling is enabled.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_no_utf8 = 0x10;
+
+ //! Parse flag instructing the parser to create XML declaration node.
+ //! By default, declaration node is not created.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_declaration_node = 0x20;
+
+ //! Parse flag instructing the parser to create comments nodes.
+ //! By default, comment nodes are not created.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_comment_nodes = 0x40;
+
+ //! Parse flag instructing the parser to create DOCTYPE node.
+ //! By default, doctype node is not created.
+ //! Although W3C specification allows at most one DOCTYPE node, RapidXml will silently accept documents with more than one.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_doctype_node = 0x80;
+
+ //! Parse flag instructing the parser to create PI nodes.
+ //! By default, PI nodes are not created.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_pi_nodes = 0x100;
+
+ //! Parse flag instructing the parser to validate closing tag names.
+ //! If not set, name inside closing tag is irrelevant to the parser.
+ //! By default, closing tags are not validated.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_validate_closing_tags = 0x200;
+
+ //! Parse flag instructing the parser to trim all leading and trailing whitespace of data nodes.
+ //! By default, whitespace is not trimmed.
+ //! This flag does not cause the parser to modify source text.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_trim_whitespace = 0x400;
+
+ //! Parse flag instructing the parser to condense all whitespace runs of data nodes to a single space character.
+ //! Trimming of leading and trailing whitespace of data is controlled by rapidxml::parse_trim_whitespace flag.
+ //! By default, whitespace is not normalized.
+ //! If this flag is specified, source text will be modified.
+ //! Can be combined with other flags by use of | operator.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_normalize_whitespace = 0x800;
+
+ // Compound flags
+
+ //! Parse flags which represent default behaviour of the parser.
+ //! This is always equal to 0, so that all other flags can be simply ored together.
+ //! Normally there is no need to inconveniently disable flags by anding with their negated (~) values.
+ //! This also means that meaning of each flag is a <i>negation</i> of the default setting.
+ //! For example, if flag name is rapidxml::parse_no_utf8, it means that utf-8 is <i>enabled</i> by default,
+ //! and using the flag will disable it.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_default = 0;
+
+ //! A combination of parse flags that forbids any modifications of the source text.
+ //! This also results in faster parsing. However, note that the following will occur:
+ //! <ul>
+ //! <li>names and values of nodes will not be zero terminated, you have to use xml_base::name_size() and xml_base::value_size() functions to determine where name and value ends</li>
+ //! <li>entities will not be translated</li>
+ //! <li>whitespace will not be normalized</li>
+ //! </ul>
+ //! See xml_document::parse() function.
+ const int parse_non_destructive = parse_no_string_terminators | parse_no_entity_translation;
+
+ //! A combination of parse flags resulting in fastest possible parsing, without sacrificing important data.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_fastest = parse_non_destructive | parse_no_data_nodes;
+
+ //! A combination of parse flags resulting in largest amount of data being extracted.
+ //! This usually results in slowest parsing.
+ //! <br><br>
+ //! See xml_document::parse() function.
+ const int parse_full = parse_declaration_node | parse_comment_nodes | parse_doctype_node | parse_pi_nodes | parse_validate_closing_tags;
+
+ ///////////////////////////////////////////////////////////////////////
+ // Command option default defaults
+
+ //! Default case sensitivity when searching for node / attribute names.
+ const bool default_case_sensitivity = false;
+
+
+ ///////////////////////////////////////////////////////////////////////
+ // Internals
+
+ //! \cond internal
+ namespace internal
+ {
+
+ // Struct that contains lookup tables for the parser
+ // It must be a template to allow correct linking (because it has static data members, which are defined in a header file).
+ template<int Dummy>
+ struct lookup_tables
+ {
+ static const unsigned char lookup_whitespace[256]; // Whitespace table
+ static const unsigned char lookup_node_name[256]; // Node name table
+ static const unsigned char lookup_text[256]; // Text table
+ static const unsigned char lookup_text_pure_no_ws[256]; // Text table
+ static const unsigned char lookup_text_pure_with_ws[256]; // Text table
+ static const unsigned char lookup_attribute_name[256]; // Attribute name table
+ static const unsigned char lookup_attribute_data_1[256]; // Attribute data table with single quote
+ static const unsigned char lookup_attribute_data_1_pure[256]; // Attribute data table with single quote
+ static const unsigned char lookup_attribute_data_2[256]; // Attribute data table with double quotes
+ static const unsigned char lookup_attribute_data_2_pure[256]; // Attribute data table with double quotes
+ static const unsigned char lookup_digits[256]; // Digits
+ static const unsigned char lookup_upcase[256]; // To uppercase conversion table for ASCII characters
+ };
+
+ // Find length of the string
+ template<class Ch>
+ inline std::size_t measure(const Ch *p)
+ {
+ const Ch *tmp = p;
+ while (*tmp)
+ ++tmp;
+ return tmp - p;
+ }
+
+ // Compare strings for equality
+ template<class Ch>
+ inline bool compare(const Ch *p1, std::size_t size1, const Ch *p2, std::size_t size2, bool case_sensitive)
+ {
+ if (size1 != size2)
+ return false;
+ if (case_sensitive)
+ {
+ for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2)
+ if (*p1 != *p2)
+ return false;
+ }
+ else
+ {
+ for (const Ch *end = p1 + size1; p1 < end; ++p1, ++p2)
+ if (lookup_tables<0>::lookup_upcase[static_cast<unsigned char>(*p1)] != lookup_tables<0>::lookup_upcase[static_cast<unsigned char>(*p2)])
+ return false;
+ }
+ return true;
+ }
+ }
+ //! \endcond
+
+ ///////////////////////////////////////////////////////////////////////
+ // Memory pool
+
+ //! This class is used by the parser to create new nodes and attributes, without overheads of dynamic memory allocation.
+ //! In most cases, you will not need to use this class directly.
+ //! However, if you need to create nodes manually or modify names/values of nodes,
+ //! you are encouraged to use memory_pool of relevant xml_document to allocate the memory.
+ //! Not only is this faster than allocating them by using <code>new</code> operator,
+ //! but also their lifetime will be tied to the lifetime of document,
+ //! possibly simplyfing memory management.
+ //! <br><br>
+ //! Call allocate_node() or allocate_attribute() functions to obtain new nodes or attributes from the pool.
+ //! You can also call allocate_string() function to allocate strings.
+ //! Such strings can then be used as names or values of nodes without worrying about their lifetime.
+ //! Note that there is no <code>free()</code> function -- all allocations are freed at once when clear() function is called,
+ //! or when the pool is destroyed.
+ //! <br><br>
+ //! It is also possible to create a standalone memory_pool, and use it
+ //! to allocate nodes, whose lifetime will not be tied to any document.
+ //! <br><br>
+ //! Pool maintains <code>RAPIDXML_STATIC_POOL_SIZE</code> bytes of statically allocated memory.
+ //! Until static memory is exhausted, no dynamic memory allocations are done.
+ //! When static memory is exhausted, pool allocates additional blocks of memory of size <code>RAPIDXML_DYNAMIC_POOL_SIZE</code> each,
+ //! by using global <code>new[]</code> and <code>delete[]</code> operators.
+ //! This behaviour can be changed by setting custom allocation routines.
+ //! Use set_allocator() function to set them.
+ //! <br><br>
+ //! Allocations for nodes, attributes and strings are aligned at <code>RAPIDXML_ALIGNMENT</code> bytes.
+ //! This value defaults to the size of pointer on target architecture.
+ //! <br><br>
+ //! To obtain absolutely top performance from the parser,
+ //! it is important that all nodes are allocated from a single, contiguous block of memory.
+ //! Otherwise, cache misses when jumping between two (or more) disjoint blocks of memory can slow down parsing quite considerably.
+ //! If required, you can tweak <code>RAPIDXML_STATIC_POOL_SIZE</code>, <code>RAPIDXML_DYNAMIC_POOL_SIZE</code> and <code>RAPIDXML_ALIGNMENT</code>
+ //! to obtain best wasted memory to performance compromise.
+ //! To do it, define their values before rapidxml.hpp file is included.
+ //! \param Ch Character type of created nodes.
+ template<class Ch = char>
+ class memory_pool
+ {
+
+ public:
+
+ //! \cond internal
+ typedef void *(alloc_func)(std::size_t); // Type of user-defined function used to allocate memory
+ typedef void (free_func)(void *); // Type of user-defined function used to free memory
+ //! \endcond
+
+ //! Constructs empty pool with default allocator functions.
+ memory_pool()
+ : m_alloc_func(0)
+ , m_free_func(0)
+ {
+ init();
+ }
+
+ //! Destroys pool and frees all the memory.
+ //! This causes memory occupied by nodes allocated by the pool to be freed.
+ //! Nodes allocated from the pool are no longer valid.
+ ~memory_pool()
+ {
+ clear();
+ }
+
+ //! Allocates a new node from the pool, and optionally assigns name and value to it.
+ //! If the allocation request cannot be accomodated, this function will throw <code>std::bad_alloc</code>.
+ //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function
+ //! will call rapidxml::parse_error_handler() function.
+ //! \param type Type of node to create.
+ //! \param name Name to assign to the node, or 0 to assign no name.
+ //! \param value Value to assign to the node, or 0 to assign no value.
+ //! \param name_size Size of name to assign, or 0 to automatically calculate size from name string.
+ //! \param value_size Size of value to assign, or 0 to automatically calculate size from value string.
+ //! \return Pointer to allocated node. This pointer will never be NULL.
+ xml_node<Ch> *allocate_node(node_type type,
+ const Ch *name = 0, const Ch *value = 0,
+ std::size_t name_size = 0, std::size_t value_size = 0)
+ {
+ void *memory = allocate_aligned(sizeof(xml_node<Ch>));
+ xml_node<Ch> *node = new(memory) xml_node<Ch>(type);
+ if (name)
+ {
+ if (name_size > 0)
+ node->name(name, name_size);
+ else
+ node->name(name);
+ }
+ if (value)
+ {
+ if (value_size > 0)
+ node->value(value, value_size);
+ else
+ node->value(value);
+ }
+ return node;
+ }
+
+ //! Allocates a new attribute from the pool, and optionally assigns name and value to it.
+ //! If the allocation request cannot be accomodated, this function will throw <code>std::bad_alloc</code>.
+ //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function
+ //! will call rapidxml::parse_error_handler() function.
+ //! \param name Name to assign to the attribute, or 0 to assign no name.
+ //! \param value Value to assign to the attribute, or 0 to assign no value.
+ //! \param name_size Size of name to assign, or 0 to automatically calculate size from name string.
+ //! \param value_size Size of value to assign, or 0 to automatically calculate size from value string.
+ //! \return Pointer to allocated attribute. This pointer will never be NULL.
+ xml_attribute<Ch> *allocate_attribute(const Ch *name = 0, const Ch *value = 0,
+ std::size_t name_size = 0, std::size_t value_size = 0)
+ {
+ void *memory = allocate_aligned(sizeof(xml_attribute<Ch>));
+ xml_attribute<Ch> *attribute = new(memory) xml_attribute<Ch>;
+ if (name)
+ {
+ if (name_size > 0)
+ attribute->name(name, name_size);
+ else
+ attribute->name(name);
+ }
+ if (value)
+ {
+ if (value_size > 0)
+ attribute->value(value, value_size);
+ else
+ attribute->value(value);
+ }
+ return attribute;
+ }
+
+ //! Allocates a char array of given size from the pool, and optionally copies a given string to it.
+ //! If the allocation request cannot be accomodated, this function will throw <code>std::bad_alloc</code>.
+ //! If exceptions are disabled by defining RAPIDXML_NO_EXCEPTIONS, this function
+ //! will call rapidxml::parse_error_handler() function.
+ //! \param source String to initialize the allocated memory with, or 0 to not initialize it.
+ //! \param size Number of characters to allocate, or zero to calculate it automatically from source string length; if size is 0, source string must be specified and null terminated.
+ //! \return Pointer to allocated char array. This pointer will never be NULL.
+ Ch *allocate_string(const Ch *source = 0, std::size_t size = 0)
+ {
+ assert(source || size); // Either source or size (or both) must be specified
+ if (size == 0)
+ size = internal::measure(source) + 1;
+ Ch *result = static_cast<Ch *>(allocate_aligned(size * sizeof(Ch)));
+ if (source)
+ for (std::size_t i = 0; i < size; ++i)
+ result[i] = source[i];
+ return result;
+ }
+
+ //! Clones an xml_node and its hierarchy of child nodes and attributes.
+ //! Nodes and attributes are allocated from this memory pool.
+ //! Names and values are not cloned, they are shared between the clone and the source.
+ //! Result node can be optionally specified as a second parameter,
+ //! in which case its contents will be replaced with cloned source node.
+ //! This is useful when you want to clone entire document.
+ //! \param source Node to clone.
+ //! \param result Node to put results in, or 0 to automatically allocate result node
+ //! \return Pointer to cloned node. This pointer will never be NULL.
+ xml_node<Ch> *clone_node(const xml_node<Ch> *source, xml_node<Ch> *result = 0)
+ {
+ // Prepare result node
+ if (result)
+ {
+ result->remove_all_attributes();
+ result->remove_all_nodes();
+ result->type(source->type());
+ }
+ else
+ result = allocate_node(source->type());
+
+ // Clone name and value
+ result->name(source->name(), source->name_size());
+ result->value(source->value(), source->value_size());
+
+ // Clone child nodes and attributes
+ for (xml_node<Ch> *child = source->first_node(); child; child = child->next_sibling())
+ result->append_node(clone_node(child));
+ for (xml_attribute<Ch> *attr = source->first_attribute(); attr; attr = attr->next_attribute())
+ result->append_attribute(allocate_attribute(attr->name(), attr->value(), attr->name_size(), attr->value_size()));
+
+ return result;
+ }
+
+ //! Clears the pool.
+ //! This causes memory occupied by nodes allocated by the pool to be freed.
+ //! Any nodes or strings allocated from the pool will no longer be valid.
+ void clear()
+ {
+ while (m_begin != m_static_memory)
+ {
+ char *previous_begin = reinterpret_cast<header *>(align(m_begin))->previous_begin;
+ if (m_free_func)
+ m_free_func(m_begin);
+ else
+ delete[] m_begin;
+ m_begin = previous_begin;
+ }
+ init();
+ }
+
+ //! Sets or resets the user-defined memory allocation functions for the pool.
+ //! This can only be called when no memory is allocated from the pool yet, otherwise results are undefined.
+ //! Allocation function must not return invalid pointer on failure. It should either throw,
+ //! stop the program, or use <code>longjmp()</code> function to pass control to other place of program.
+ //! If it returns invalid pointer, results are undefined.
+ //! <br><br>
+ //! User defined allocation functions must have the following forms:
+ //! <br><code>
+ //! <br>void *allocate(std::size_t size);
+ //! <br>void free(void *pointer);
+ //! </code><br>
+ //! \param af Allocation function, or 0 to restore default function
+ //! \param ff Free function, or 0 to restore default function
+ void set_allocator(alloc_func *af, free_func *ff)
+ {
+ assert(m_begin == m_static_memory && m_ptr == align(m_begin)); // Verify that no memory is allocated yet
+ m_alloc_func = af;
+ m_free_func = ff;
+ }
+
+ private:
+
+ struct header
+ {
+ char *previous_begin;
+ };
+
+ void init()
+ {
+ m_begin = m_static_memory;
+ m_ptr = align(m_begin);
+ m_end = m_static_memory + sizeof(m_static_memory);
+ }
+
+ char *align(char *ptr)
+ {
+ std::size_t alignment = ((RAPIDXML_ALIGNMENT - (std::size_t(ptr) & (RAPIDXML_ALIGNMENT - 1))) & (RAPIDXML_ALIGNMENT - 1));
+ return ptr + alignment;
+ }
+
+ char *allocate_raw(std::size_t size)
+ {
+ // Allocate
+ void *memory;
+ if (m_alloc_func) // Allocate memory using either user-specified allocation function or global operator new[]
+ {
+ memory = m_alloc_func(size);
+ assert(memory); // Allocator is not allowed to return 0, on failure it must either throw, stop the program or use longjmp
+ }
+ else
+ {
+ memory = new char[size];
+#ifdef RAPIDXML_NO_EXCEPTIONS
+ if (!memory) // If exceptions are disabled, verify memory allocation, because new will not be able to throw bad_alloc
+ RAPIDXML_PARSE_ERROR("out of memory", 0);
+#endif
+ }
+ return static_cast<char *>(memory);
+ }
+
+ void *allocate_aligned(std::size_t size)
+ {
+ // Calculate aligned pointer
+ char *result = align(m_ptr);
+
+ // If not enough memory left in current pool, allocate a new pool
+ if (result + size > m_end)
+ {
+ // Calculate required pool size (may be bigger than RAPIDXML_DYNAMIC_POOL_SIZE)
+ std::size_t pool_size = RAPIDXML_DYNAMIC_POOL_SIZE;
+ if (pool_size < size)
+ pool_size = size;
+
+ // Allocate
+ std::size_t alloc_size = sizeof(header) + (2 * RAPIDXML_ALIGNMENT - 2) + pool_size; // 2 alignments required in worst case: one for header, one for actual allocation
+ char *raw_memory = allocate_raw(alloc_size);
+
+ // Setup new pool in allocated memory
+ char *pool = align(raw_memory);
+ header *new_header = reinterpret_cast<header *>(pool);
+ new_header->previous_begin = m_begin;
+ m_begin = raw_memory;
+ m_ptr = pool + sizeof(header);
+ m_end = raw_memory + alloc_size;
+
+ // Calculate aligned pointer again using new pool
+ result = align(m_ptr);
+ }
+
+ // Update pool and return aligned pointer
+ m_ptr = result + size;
+ return result;
+ }
+
+ char *m_begin; // Start of raw memory making up current pool
+ char *m_ptr; // First free byte in current pool
+ char *m_end; // One past last available byte in current pool
+ char m_static_memory[RAPIDXML_STATIC_POOL_SIZE]; // Static raw memory
+ alloc_func *m_alloc_func; // Allocator function, or 0 if default is to be used
+ free_func *m_free_func; // Free function, or 0 if default is to be used
+ };
+
+ ///////////////////////////////////////////////////////////////////////////
+ // XML base
+
+ //! Base class for xml_node and xml_attribute implementing common functions:
+ //! name(), name_size(), value(), value_size() and parent().
+ //! \param Ch Character type to use
+ template<class Ch = char>
+ class xml_base
+ {
+
+ public:
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Construction & destruction
+
+ // Construct a base with empty name, value and parent
+ xml_base()
+ : m_name(0)
+ , m_value(0)
+ , m_parent(0)
+ {
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Node data access
+
+ //! Gets name of the node.
+ //! Interpretation of name depends on type of node.
+ //! Note that name will not be zero-terminated if rapidxml::parse_no_string_terminators option was selected during parse.
+ //! <br><br>
+ //! Use name_size() function to determine length of the name.
+ //! \return Name of node, or empty string if node has no name.
+ Ch *name() const
+ {
+ return m_name ? m_name : nullstr();
+ }
+
+ //! Gets size of node name, not including terminator character.
+ //! This function works correctly irrespective of whether name is or is not zero terminated.
+ //! \return Size of node name, in characters.
+ std::size_t name_size() const
+ {
+ return m_name ? m_name_size : 0;
+ }
+
+ //! Gets value of node.
+ //! Interpretation of value depends on type of node.
+ //! Note that value will not be zero-terminated if rapidxml::parse_no_string_terminators option was selected during parse.
+ //! <br><br>
+ //! Use value_size() function to determine length of the value.
+ //! \return Value of node, or empty string if node has no value.
+ Ch *value() const
+ {
+ return m_value ? m_value : nullstr();
+ }
+
+ //! Gets size of node value, not including terminator character.
+ //! This function works correctly irrespective of whether value is or is not zero terminated.
+ //! \return Size of node value, in characters.
+ std::size_t value_size() const
+ {
+ return m_value ? m_value_size : 0;
+ }
+
+ //! Gets value of node converted to type T.
+ //! \return Value of node converted to type T or default_val if no value exists.
+ //! \throws parse_error iff conversion to type T fails.
+ template<typename T>
+ T valueT(T default_val = T() )
+ {
+ if(m_value) {
+ std::istringstream ss( std::string(m_value, m_value_size) );
+ T ret_val;
+ ss >> ret_val;
+ if(ss.fail()) {
+ RAPIDXML_PARSE_ERROR("error converting value in valueT()", m_value);
+ }
+ return ret_val;
+ }
+ return default_val;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Node modification
+
+ //! Sets name of node to a non zero-terminated string.
+ //! See \ref ownership_of_strings.
+ //! <br><br>
+ //! Note that node does not own its name or value, it only stores a pointer to it.
+ //! It will not delete or otherwise free the pointer on destruction.
+ //! It is reponsibility of the user to properly manage lifetime of the string.
+ //! The easiest way to achieve it is to use memory_pool of the document to allocate the string -
+ //! on destruction of the document the string will be automatically freed.
+ //! <br><br>
+ //! Size of name must be specified separately, because name does not have to be zero terminated.
+ //! Use name(const Ch *) function to have the length automatically calculated (string must be zero terminated).
+ //! \param name Name of node to set. Does not have to be zero terminated.
+ //! \param size Size of name, in characters. This does not include zero terminator, if one is present.
+ void name(const Ch *name, std::size_t size)
+ {
+ m_name = const_cast<Ch *>(name);
+ m_name_size = size;
+ }
+
+ //! Sets name of node to a zero-terminated string.
+ //! See also \ref ownership_of_strings and xml_node::name(const Ch *, std::size_t).
+ //! \param name Name of node to set. Must be zero terminated.
+ void name(const Ch *name)
+ {
+ this->name(name, internal::measure(name));
+ }
+
+ //! Sets value of node to a non zero-terminated string.
+ //! See \ref ownership_of_strings.
+ //! <br><br>
+ //! Note that node does not own its name or value, it only stores a pointer to it.
+ //! It will not delete or otherwise free the pointer on destruction.
+ //! It is reponsibility of the user to properly manage lifetime of the string.
+ //! The easiest way to achieve it is to use memory_pool of the document to allocate the string -
+ //! on destruction of the document the string will be automatically freed.
+ //! <br><br>
+ //! Size of value must be specified separately, because it does not have to be zero terminated.
+ //! Use value(const Ch *) function to have the length automatically calculated (string must be zero terminated).
+ //! <br><br>
+ //! If an element has a child node of type node_data, it will take precedence over element value when printing.
+ //! If you want to manipulate data of elements using values, use parser flag rapidxml::parse_no_data_nodes to prevent creation of data nodes by the parser.
+ //! \param value value of node to set. Does not have to be zero terminated.
+ //! \param size Size of value, in characters. This does not include zero terminator, if one is present.
+ void value(const Ch *value, std::size_t size)
+ {
+ m_value = const_cast<Ch *>(value);
+ m_value_size = size;
+ }
+
+ //! Sets value of node to a zero-terminated string.
+ //! See also \ref ownership_of_strings and xml_node::value(const Ch *, std::size_t).
+ //! \param value Vame of node to set. Must be zero terminated.
+ void value(const Ch *value)
+ {
+ this->value(value, internal::measure(value));
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Related nodes access
+
+ //! Gets node parent.
+ //! \return Pointer to parent node, or 0 if there is no parent.
+ xml_node<Ch> *parent() const
+ {
+ return m_parent;
+ }
+
+ protected:
+
+ // Return empty string
+ static Ch *nullstr()
+ {
+ static Ch zero = Ch('\0');
+ return &zero;
+ }
+
+ Ch *m_name; // Name of node, or 0 if no name
+ Ch *m_value; // Value of node, or 0 if no value
+ std::size_t m_name_size; // Length of node name, or undefined of no name
+ std::size_t m_value_size; // Length of node value, or undefined if no value
+ xml_node<Ch> *m_parent; // Pointer to parent node, or 0 if none
+
+ };
+
+ //! Class representing attribute node of XML document.
+ //! Each attribute has name and value strings, which are available through name() and value() functions (inherited from xml_base).
+ //! Note that after parse, both name and value of attribute will point to interior of source text used for parsing.
+ //! Thus, this text must persist in memory for the lifetime of attribute.
+ //! \param Ch Character type to use.
+ template<class Ch = char>
+ class xml_attribute: public xml_base<Ch>
+ {
+
+ friend class xml_node<Ch>;
+
+ public:
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Construction & destruction
+
+ //! Constructs an empty attribute with the specified type.
+ //! Consider using memory_pool of appropriate xml_document if allocating attributes manually.
+ xml_attribute()
+ {
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Related nodes access
+
+ //! Gets document of which attribute is a child.
+ //! \return Pointer to document that contains this attribute, or 0 if there is no parent document.
+ xml_document<Ch> *document() const
+ {
+ if (xml_node<Ch> *node = this->parent())
+ {
+ while (node->parent())
+ node = node->parent();
+ return node->type() == node_document ? static_cast<xml_document<Ch> *>(node) : 0;
+ }
+ else
+ return 0;
+ }
+
+ //! Gets previous attribute, optionally matching attribute name.
+ //! \param name Name of attribute to find, or 0 to return previous attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found attribute, or 0 if not found.
+ xml_attribute<Ch> *previous_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_attribute<Ch> *attribute = m_prev_attribute; attribute; attribute = attribute->m_prev_attribute)
+ if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive))
+ return attribute;
+ return 0;
+ }
+ else
+ return this->m_parent ? m_prev_attribute : 0;
+ }
+
+ //! Gets next attribute, optionally matching attribute name.
+ //! \param name Name of attribute to find, or 0 to return next attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found attribute, or 0 if not found.
+ xml_attribute<Ch> *next_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_attribute<Ch> *attribute = m_next_attribute; attribute; attribute = attribute->m_next_attribute)
+ if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive))
+ return attribute;
+ return 0;
+ }
+ else
+ return this->m_parent ? m_next_attribute : 0;
+ }
+
+ private:
+
+ xml_attribute<Ch> *m_prev_attribute; // Pointer to previous sibling of attribute, or 0 if none; only valid if parent is non-zero
+ xml_attribute<Ch> *m_next_attribute; // Pointer to next sibling of attribute, or 0 if none; only valid if parent is non-zero
+
+ };
+
+ ///////////////////////////////////////////////////////////////////////////
+ // XML node
+
+ //! Class representing a node of XML document.
+ //! Each node may have associated name and value strings, which are available through name() and value() functions.
+ //! Interpretation of name and value depends on type of the node.
+ //! Type of node can be determined by using type() function.
+ //! <br><br>
+ //! Note that after parse, both name and value of node, if any, will point interior of source text used for parsing.
+ //! Thus, this text must persist in the memory for the lifetime of node.
+ //! \param Ch Character type to use.
+ template<class Ch = char>
+ class xml_node: public xml_base<Ch>
+ {
+
+ public:
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Construction & destruction
+
+ //! Constructs an empty node with the specified type.
+ //! Consider using memory_pool of appropriate document to allocate nodes manually.
+ //! \param type Type of node to construct.
+ xml_node(node_type type)
+ : m_type(type)
+ , m_first_node(0)
+ , m_first_attribute(0)
+ {
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Node data access
+
+ //! Gets type of node.
+ //! \return Type of node.
+ node_type type() const
+ {
+ return m_type;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Related nodes access
+
+ //! Gets document of which node is a child.
+ //! \return Pointer to document that contains this node, or 0 if there is no parent document.
+ xml_document<Ch> *document() const
+ {
+ xml_node<Ch> *node = const_cast<xml_node<Ch> *>(this);
+ while (node->parent())
+ node = node->parent();
+ return node->type() == node_document ? static_cast<xml_document<Ch> *>(node) : 0;
+ }
+
+ //! Gets first child node, optionally matching node name.
+ //! \param name Name of child to find, or 0 to return first child regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found child, or 0 if not found.
+ xml_node<Ch> *first_node(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_node<Ch> *child = m_first_node; child; child = child->next_sibling())
+ if (internal::compare(child->name(), child->name_size(), name, name_size, case_sensitive))
+ return child;
+ return 0;
+ }
+ else
+ return m_first_node;
+ }
+
+ //! Gets last child node, optionally matching node name.
+ //! Behaviour is undefined if node has no children.
+ //! Use first_node() to test if node has children.
+ //! \param name Name of child to find, or 0 to return last child regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found child, or 0 if not found.
+ xml_node<Ch> *last_node(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ assert(m_first_node); // Cannot query for last child if node has no children
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_node<Ch> *child = m_last_node; child; child = child->previous_sibling())
+ if (internal::compare(child->name(), child->name_size(), name, name_size, case_sensitive))
+ return child;
+ return 0;
+ }
+ else
+ return m_last_node;
+ }
+
+ //! Gets previous sibling node, optionally matching node name.
+ //! Behaviour is undefined if node has no parent.
+ //! Use parent() to test if node has a parent.
+ //! \param name Name of sibling to find, or 0 to return previous sibling regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found sibling, or 0 if not found.
+ xml_node<Ch> *previous_sibling(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ assert(this->m_parent); // Cannot query for siblings if node has no parent
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_node<Ch> *sibling = m_prev_sibling; sibling; sibling = sibling->m_prev_sibling)
+ if (internal::compare(sibling->name(), sibling->name_size(), name, name_size, case_sensitive))
+ return sibling;
+ return 0;
+ }
+ else
+ return m_prev_sibling;
+ }
+
+ //! Gets next sibling node, optionally matching node name.
+ //! Behaviour is undefined if node has no parent.
+ //! Use parent() to test if node has a parent.
+ //! \param name Name of sibling to find, or 0 to return next sibling regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found sibling, or 0 if not found.
+ xml_node<Ch> *next_sibling(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ assert(this->m_parent); // Cannot query for siblings if node has no parent
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_node<Ch> *sibling = m_next_sibling; sibling; sibling = sibling->m_next_sibling)
+ if (internal::compare(sibling->name(), sibling->name_size(), name, name_size, case_sensitive))
+ return sibling;
+ return 0;
+ }
+ else
+ return m_next_sibling;
+ }
+
+ //! Gets first attribute of node, optionally matching attribute name.
+ //! \param name Name of attribute to find, or 0 to return first attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found attribute, or 0 if not found.
+ xml_attribute<Ch> *first_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_attribute<Ch> *attribute = m_first_attribute; attribute; attribute = attribute->m_next_attribute)
+ if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive))
+ return attribute;
+ return 0;
+ }
+ else
+ return m_first_attribute;
+ }
+
+ //! Gets last attribute of node, optionally matching attribute name.
+ //! \param name Name of attribute to find, or 0 to return last attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Pointer to found attribute, or 0 if not found.
+ xml_attribute<Ch> *last_attribute(const Ch *name = 0, std::size_t name_size = 0, bool case_sensitive = default_case_sensitivity) const
+ {
+ if (name)
+ {
+ if (name_size == 0)
+ name_size = internal::measure(name);
+ for (xml_attribute<Ch> *attribute = m_last_attribute; attribute; attribute = attribute->m_prev_attribute)
+ if (internal::compare(attribute->name(), attribute->name_size(), name, name_size, case_sensitive))
+ return attribute;
+ return 0;
+ }
+ else
+ return m_first_attribute ? m_last_attribute : 0;
+ }
+
+ //! Gets first attribute of node, optionally matching attribute name, converting to type T
+ //! \param name Name of attribute to find, or 0 to return first attribute regardless of its name; this string doesn't have to be zero-terminated if name_size is non-zero
+ //! \param name_size Size of name, in characters, or 0 to have size calculated automatically from string
+ //! \param default_val value to return if attribute is not found or if it has no value
+ //! \param case_sensitive Should name comparison be case-sensitive; non case-sensitive comparison works properly only for ASCII characters
+ //! \return Converted value of attribute, or default_val if attribute not found or it has no value
+ template<typename T>
+ T first_attribute_value(const Ch *name = 0, std::size_t name_size = 0, T default_val = T(), bool case_sensitive = default_case_sensitivity )
+ {
+ rapidxml::xml_attribute<>* a = first_attribute( name, name_size, case_sensitive );
+ return a ? a->valueT<T>(default_val) : default_val;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Node modification
+
+ //! Sets type of node.
+ //! \param type Type of node to set.
+ void type(node_type type)
+ {
+ m_type = type;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Node manipulation
+
+ //! Prepends a new child node.
+ //! The prepended child becomes the first child, and all existing children are moved one position back.
+ //! \param child Node to prepend.
+ void prepend_node(xml_node<Ch> *child)
+ {
+ assert(child && !child->parent() && child->type() != node_document);
+ if (first_node())
+ {
+ child->m_next_sibling = m_first_node;
+ m_first_node->m_prev_sibling = child;
+ }
+ else
+ {
+ child->m_next_sibling = 0;
+ m_last_node = child;
+ }
+ m_first_node = child;
+ child->m_parent = this;
+ child->m_prev_sibling = 0;
+ }
+
+ //! Appends a new child node.
+ //! The appended child becomes the last child.
+ //! \param child Node to append.
+ void append_node(xml_node<Ch> *child)
+ {
+ assert(child && !child->parent() && child->type() != node_document);
+ if (first_node())
+ {
+ child->m_prev_sibling = m_last_node;
+ m_last_node->m_next_sibling = child;
+ }
+ else
+ {
+ child->m_prev_sibling = 0;
+ m_first_node = child;
+ }
+ m_last_node = child;
+ child->m_parent = this;
+ child->m_next_sibling = 0;
+ }
+
+ //! Inserts a new child node at specified place inside the node.
+ //! All children after and including the specified node are moved one position back.
+ //! \param where Place where to insert the child, or 0 to insert at the back.
+ //! \param child Node to insert.
+ void insert_node(xml_node<Ch> *where, xml_node<Ch> *child)
+ {
+ assert(!where || where->parent() == this);
+ assert(child && !child->parent() && child->type() != node_document);
+ if (where == m_first_node)
+ prepend_node(child);
+ else if (where == 0)
+ append_node(child);
+ else
+ {
+ child->m_prev_sibling = where->m_prev_sibling;
+ child->m_next_sibling = where;
+ where->m_prev_sibling->m_next_sibling = child;
+ where->m_prev_sibling = child;
+ child->m_parent = this;
+ }
+ }
+
+ //! Removes first child node.
+ //! If node has no children, behaviour is undefined.
+ //! Use first_node() to test if node has children.
+ void remove_first_node()
+ {
+ assert(first_node());
+ xml_node<Ch> *child = m_first_node;
+ m_first_node = child->m_next_sibling;
+ if (child->m_next_sibling)
+ child->m_next_sibling->m_prev_sibling = 0;
+ else
+ m_last_node = 0;
+ child->m_parent = 0;
+ }
+
+ //! Removes last child of the node.
+ //! If node has no children, behaviour is undefined.
+ //! Use first_node() to test if node has children.
+ void remove_last_node()
+ {
+ assert(first_node());
+ xml_node<Ch> *child = m_last_node;
+ if (child->m_prev_sibling)
+ {
+ m_last_node = child->m_prev_sibling;
+ child->m_prev_sibling->m_next_sibling = 0;
+ }
+ else
+ m_first_node = 0;
+ child->m_parent = 0;
+ }
+
+ //! Removes specified child from the node
+ // \param where Pointer to child to be removed.
+ void remove_node(xml_node<Ch> *where)
+ {
+ assert(where && where->parent() == this);
+ assert(first_node());
+ if (where == m_first_node)
+ remove_first_node();
+ else if (where == m_last_node)
+ remove_last_node();
+ else
+ {
+ where->m_prev_sibling->m_next_sibling = where->m_next_sibling;
+ where->m_next_sibling->m_prev_sibling = where->m_prev_sibling;
+ where->m_parent = 0;
+ }
+ }
+
+ //! Removes all child nodes (but not attributes).
+ void remove_all_nodes()
+ {
+ for (xml_node<Ch> *node = first_node(); node; node = node->m_next_sibling)
+ node->m_parent = 0;
+ m_first_node = 0;
+ }
+
+ //! Prepends a new attribute to the node.
+ //! \param attribute Attribute to prepend.
+ void prepend_attribute(xml_attribute<Ch> *attribute)
+ {
+ assert(attribute && !attribute->parent());
+ if (first_attribute())
+ {
+ attribute->m_next_attribute = m_first_attribute;
+ m_first_attribute->m_prev_attribute = attribute;
+ }
+ else
+ {
+ attribute->m_next_attribute = 0;
+ m_last_attribute = attribute;
+ }
+ m_first_attribute = attribute;
+ attribute->m_parent = this;
+ attribute->m_prev_attribute = 0;
+ }
+
+ //! Appends a new attribute to the node.
+ //! \param attribute Attribute to append.
+ void append_attribute(xml_attribute<Ch> *attribute)
+ {
+ assert(attribute && !attribute->parent());
+ if (first_attribute())
+ {
+ attribute->m_prev_attribute = m_last_attribute;
+ m_last_attribute->m_next_attribute = attribute;
+ }
+ else
+ {
+ attribute->m_prev_attribute = 0;
+ m_first_attribute = attribute;
+ }
+ m_last_attribute = attribute;
+ attribute->m_parent = this;
+ attribute->m_next_attribute = 0;
+ }
+
+ //! Inserts a new attribute at specified place inside the node.
+ //! All attributes after and including the specified attribute are moved one position back.
+ //! \param where Place where to insert the attribute, or 0 to insert at the back.
+ //! \param attribute Attribute to insert.
+ void insert_attribute(xml_attribute<Ch> *where, xml_attribute<Ch> *attribute)
+ {
+ assert(!where || where->parent() == this);
+ assert(attribute && !attribute->parent());
+ if (where == m_first_attribute)
+ prepend_attribute(attribute);
+ else if (where == 0)
+ append_attribute(attribute);
+ else
+ {
+ attribute->m_prev_attribute = where->m_prev_attribute;
+ attribute->m_next_attribute = where;
+ where->m_prev_attribute->m_next_attribute = attribute;
+ where->m_prev_attribute = attribute;
+ attribute->m_parent = this;
+ }
+ }
+
+ //! Removes first attribute of the node.
+ //! If node has no attributes, behaviour is undefined.
+ //! Use first_attribute() to test if node has attributes.
+ void remove_first_attribute()
+ {
+ assert(first_attribute());
+ xml_attribute<Ch> *attribute = m_first_attribute;
+ if (attribute->m_next_attribute)
+ {
+ attribute->m_next_attribute->m_prev_attribute = 0;
+ }
+ else
+ m_last_attribute = 0;
+ attribute->m_parent = 0;
+ m_first_attribute = attribute->m_next_attribute;
+ }
+
+ //! Removes last attribute of the node.
+ //! If node has no attributes, behaviour is undefined.
+ //! Use first_attribute() to test if node has attributes.
+ void remove_last_attribute()
+ {
+ assert(first_attribute());
+ xml_attribute<Ch> *attribute = m_last_attribute;
+ if (attribute->m_prev_attribute)
+ {
+ attribute->m_prev_attribute->m_next_attribute = 0;
+ m_last_attribute = attribute->m_prev_attribute;
+ }
+ else
+ m_first_attribute = 0;
+ attribute->m_parent = 0;
+ }
+
+ //! Removes specified attribute from node.
+ //! \param where Pointer to attribute to be removed.
+ void remove_attribute(xml_attribute<Ch> *where)
+ {
+ assert(first_attribute() && where->parent() == this);
+ if (where == m_first_attribute)
+ remove_first_attribute();
+ else if (where == m_last_attribute)
+ remove_last_attribute();
+ else
+ {
+ where->m_prev_attribute->m_next_attribute = where->m_next_attribute;
+ where->m_next_attribute->m_prev_attribute = where->m_prev_attribute;
+ where->m_parent = 0;
+ }
+ }
+
+ //! Removes all attributes of node.
+ void remove_all_attributes()
+ {
+ for (xml_attribute<Ch> *attribute = first_attribute(); attribute; attribute = attribute->m_next_attribute)
+ attribute->m_parent = 0;
+ m_first_attribute = 0;
+ }
+
+ private:
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Restrictions
+
+ // No copying
+ xml_node(const xml_node &);
+ void operator =(const xml_node &);
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Data members
+
+ // Note that some of the pointers below have UNDEFINED values if certain other pointers are 0.
+ // This is required for maximum performance, as it allows the parser to omit initialization of
+ // unneded/redundant values.
+ //
+ // The rules are as follows:
+ // 1. first_node and first_attribute contain valid pointers, or 0 if node has no children/attributes respectively
+ // 2. last_node and last_attribute are valid only if node has at least one child/attribute respectively, otherwise they contain garbage
+ // 3. prev_sibling and next_sibling are valid only if node has a parent, otherwise they contain garbage
+
+ node_type m_type; // Type of node; always valid
+ xml_node<Ch> *m_first_node; // Pointer to first child node, or 0 if none; always valid
+ xml_node<Ch> *m_last_node; // Pointer to last child node, or 0 if none; this value is only valid if m_first_node is non-zero
+ xml_attribute<Ch> *m_first_attribute; // Pointer to first attribute of node, or 0 if none; always valid
+ xml_attribute<Ch> *m_last_attribute; // Pointer to last attribute of node, or 0 if none; this value is only valid if m_first_attribute is non-zero
+ xml_node<Ch> *m_prev_sibling; // Pointer to previous sibling of node, or 0 if none; this value is only valid if m_parent is non-zero
+ xml_node<Ch> *m_next_sibling; // Pointer to next sibling of node, or 0 if none; this value is only valid if m_parent is non-zero
+
+ };
+
+ ///////////////////////////////////////////////////////////////////////////
+ // XML document
+
+ //! This class represents root of the DOM hierarchy.
+ //! It is also an xml_node and a memory_pool through public inheritance.
+ //! Use parse() function to build a DOM tree from a zero-terminated XML text string.
+ //! parse() function allocates memory for nodes and attributes by using functions of xml_document,
+ //! which are inherited from memory_pool.
+ //! To access root node of the document, use the document itself, as if it was an xml_node.
+ //! \param Ch Character type to use.
+ template<class Ch = char>
+ class xml_document: public xml_node<Ch>, public memory_pool<Ch>
+ {
+
+ public:
+
+ //! Constructs empty XML document
+ xml_document()
+ : xml_node<Ch>(node_document)
+ {
+ }
+
+ //! Parses zero-terminated XML string according to given flags.
+ //! Passed string will be modified by the parser, unless rapidxml::parse_non_destructive flag is used.
+ //! The string must persist for the lifetime of the document.
+ //! In case of error, rapidxml::parse_error exception will be thrown.
+ //! <br><br>
+ //! If you want to parse contents of a file, you must first load the file into the memory, and pass pointer to its beginning.
+ //! Make sure that data is zero-terminated.
+ //! <br><br>
+ //! Document can be parsed into multiple times.
+ //! Each new call to parse removes previous nodes and attributes (if any), but does not clear memory pool.
+ //! \param text XML data to parse; pointer is non-const to denote fact that this data may be modified by the parser.
+ template<int Flags>
+ void parse(Ch *text)
+ {
+ assert(text);
+
+ // Remove current contents
+ this->remove_all_nodes();
+ this->remove_all_attributes();
+
+ // Parse BOM, if any
+ parse_bom<Flags>(text);
+
+ // Parse children
+ while (1)
+ {
+ // Skip whitespace before node
+ skip<whitespace_pred, Flags>(text);
+ if (*text == 0)
+ break;
+
+ // Parse and append new child
+ if (*text == Ch('<'))
+ {
+ ++text; // Skip '<'
+ if (xml_node<Ch> *node = parse_node<Flags>(text))
+ this->append_node(node);
+ }
+ else
+ RAPIDXML_PARSE_ERROR("expected <", text);
+ }
+
+ }
+
+ //! Clears the document by deleting all nodes and clearing the memory pool.
+ //! All nodes owned by document pool are destroyed.
+ void clear()
+ {
+ this->remove_all_nodes();
+ this->remove_all_attributes();
+ memory_pool<Ch>::clear();
+ }
+
+ private:
+
+ ///////////////////////////////////////////////////////////////////////
+ // Internal character utility functions
+
+ // Detect whitespace character
+ struct whitespace_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_whitespace[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect node name character
+ struct node_name_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_node_name[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect attribute name character
+ struct attribute_name_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_attribute_name[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect text character (PCDATA)
+ struct text_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_text[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect text character (PCDATA) that does not require processing
+ struct text_pure_no_ws_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_text_pure_no_ws[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect text character (PCDATA) that does not require processing
+ struct text_pure_with_ws_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ return internal::lookup_tables<0>::lookup_text_pure_with_ws[static_cast<unsigned char>(ch)];
+ }
+ };
+
+ // Detect attribute value character
+ template<Ch Quote>
+ struct attribute_value_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ if (Quote == Ch('\''))
+ return internal::lookup_tables<0>::lookup_attribute_data_1[static_cast<unsigned char>(ch)];
+ if (Quote == Ch('\"'))
+ return internal::lookup_tables<0>::lookup_attribute_data_2[static_cast<unsigned char>(ch)];
+ return 0; // Should never be executed, to avoid warnings on Comeau
+ }
+ };
+
+ // Detect attribute value character
+ template<Ch Quote>
+ struct attribute_value_pure_pred
+ {
+ static unsigned char test(Ch ch)
+ {
+ if (Quote == Ch('\''))
+ return internal::lookup_tables<0>::lookup_attribute_data_1_pure[static_cast<unsigned char>(ch)];
+ if (Quote == Ch('\"'))
+ return internal::lookup_tables<0>::lookup_attribute_data_2_pure[static_cast<unsigned char>(ch)];
+ return 0; // Should never be executed, to avoid warnings on Comeau
+ }
+ };
+
+ // Insert coded character, using UTF8 or 8-bit ASCII
+ template<int Flags>
+ static void insert_coded_character(Ch *&text, unsigned long code)
+ {
+ if (Flags & parse_no_utf8)
+ {
+ // Insert 8-bit ASCII character
+ // Todo: possibly verify that code is less than 256 and use replacement char otherwise?
+ text[0] = static_cast<unsigned char>(code);
+ text += 1;
+ }
+ else
+ {
+ // Insert UTF8 sequence
+ if (code < 0x80) // 1 byte sequence
+ {
+ text[0] = static_cast<unsigned char>(code);
+ text += 1;
+ }
+ else if (code < 0x800) // 2 byte sequence
+ {
+ text[1] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[0] = static_cast<unsigned char>(code | 0xC0);
+ text += 2;
+ }
+ else if (code < 0x10000) // 3 byte sequence
+ {
+ text[2] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[1] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[0] = static_cast<unsigned char>(code | 0xE0);
+ text += 3;
+ }
+ else if (code < 0x110000) // 4 byte sequence
+ {
+ text[3] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[2] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[1] = static_cast<unsigned char>((code | 0x80) & 0xBF); code >>= 6;
+ text[0] = static_cast<unsigned char>(code | 0xF0);
+ text += 4;
+ }
+ else // Invalid, only codes up to 0x10FFFF are allowed in Unicode
+ {
+ RAPIDXML_PARSE_ERROR("invalid numeric character entity", text);
+ }
+ }
+ }
+
+ // Skip characters until predicate evaluates to true
+ template<class StopPred, int Flags>
+ static void skip(Ch *&text)
+ {
+ Ch *tmp = text;
+ while (StopPred::test(*tmp))
+ ++tmp;
+ text = tmp;
+ }
+
+ // Skip characters until predicate evaluates to true while doing the following:
+ // - replacing XML character entity references with proper characters (' & " < > &#...;)
+ // - condensing whitespace sequences to single space character
+ template<class StopPred, class StopPredPure, int Flags>
+ static Ch *skip_and_expand_character_refs(Ch *&text)
+ {
+ // If entity translation, whitespace condense and whitespace trimming is disabled, use plain skip
+ if (Flags & parse_no_entity_translation &&
+ !(Flags & parse_normalize_whitespace) &&
+ !(Flags & parse_trim_whitespace))
+ {
+ skip<StopPred, Flags>(text);
+ return text;
+ }
+
+ // Use simple skip until first modification is detected
+ skip<StopPredPure, Flags>(text);
+
+ // Use translation skip
+ Ch *src = text;
+ Ch *dest = src;
+ while (StopPred::test(*src))
+ {
+ // If entity translation is enabled
+ if (!(Flags & parse_no_entity_translation))
+ {
+ // Test if replacement is needed
+ if (src[0] == Ch('&'))
+ {
+ switch (src[1])
+ {
+
+ // & '
+ case Ch('a'):
+ if (src[2] == Ch('m') && src[3] == Ch('p') && src[4] == Ch(';'))
+ {
+ *dest = Ch('&');
+ ++dest;
+ src += 5;
+ continue;
+ }
+ if (src[2] == Ch('p') && src[3] == Ch('o') && src[4] == Ch('s') && src[5] == Ch(';'))
+ {
+ *dest = Ch('\'');
+ ++dest;
+ src += 6;
+ continue;
+ }
+ break;
+
+ // "
+ case Ch('q'):
+ if (src[2] == Ch('u') && src[3] == Ch('o') && src[4] == Ch('t') && src[5] == Ch(';'))
+ {
+ *dest = Ch('"');
+ ++dest;
+ src += 6;
+ continue;
+ }
+ break;
+
+ // >
+ case Ch('g'):
+ if (src[2] == Ch('t') && src[3] == Ch(';'))
+ {
+ *dest = Ch('>');
+ ++dest;
+ src += 4;
+ continue;
+ }
+ break;
+
+ // <
+ case Ch('l'):
+ if (src[2] == Ch('t') && src[3] == Ch(';'))
+ {
+ *dest = Ch('<');
+ ++dest;
+ src += 4;
+ continue;
+ }
+ break;
+
+ // &#...; - assumes ASCII
+ case Ch('#'):
+ if (src[2] == Ch('x'))
+ {
+ unsigned long code = 0;
+ src += 3; // Skip &#x
+ while (1)
+ {
+ unsigned char digit = internal::lookup_tables<0>::lookup_digits[static_cast<unsigned char>(*src)];
+ if (digit == 0xFF)
+ break;
+ code = code * 16 + digit;
+ ++src;
+ }
+ insert_coded_character<Flags>(dest, code); // Put character in output
+ }
+ else
+ {
+ unsigned long code = 0;
+ src += 2; // Skip &#
+ while (1)
+ {
+ unsigned char digit = internal::lookup_tables<0>::lookup_digits[static_cast<unsigned char>(*src)];
+ if (digit == 0xFF)
+ break;
+ code = code * 10 + digit;
+ ++src;
+ }
+ insert_coded_character<Flags>(dest, code); // Put character in output
+ }
+ if (*src == Ch(';'))
+ ++src;
+ else
+ RAPIDXML_PARSE_ERROR("expected ;", src);
+ continue;
+
+ // Something else
+ default:
+ // Ignore, just copy '&' verbatim
+ break;
+
+ }
+ }
+ }
+
+ // If whitespace condensing is enabled
+ if (Flags & parse_normalize_whitespace)
+ {
+ // Test if condensing is needed
+ if (whitespace_pred::test(*src))
+ {
+ *dest = Ch(' '); ++dest; // Put single space in dest
+ ++src; // Skip first whitespace char
+ // Skip remaining whitespace chars
+ while (whitespace_pred::test(*src))
+ ++src;
+ continue;
+ }
+ }
+
+ // No replacement, only copy character
+ *dest++ = *src++;
+
+ }
+
+ // Return new end
+ text = src;
+ return dest;
+
+ }
+
+ ///////////////////////////////////////////////////////////////////////
+ // Internal parsing functions
+
+ // Parse BOM, if any
+ template<int Flags>
+ void parse_bom(Ch *&text)
+ {
+ // UTF-8?
+ if (static_cast<unsigned char>(text[0]) == 0xEF &&
+ static_cast<unsigned char>(text[1]) == 0xBB &&
+ static_cast<unsigned char>(text[2]) == 0xBF)
+ {
+ text += 3; // Skup utf-8 bom
+ }
+ }
+
+ // Parse XML declaration (<?xml...)
+ template<int Flags>
+ xml_node<Ch> *parse_xml_declaration(Ch *&text)
+ {
+ // If parsing of declaration is disabled
+ if (!(Flags & parse_declaration_node))
+ {
+ // Skip until end of declaration
+ while (text[0] != Ch('?') || text[1] != Ch('>'))
+ {
+ if (!text[0])
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+ text += 2; // Skip '?>'
+ return 0;
+ }
+
+ // Create declaration
+ xml_node<Ch> *declaration = this->allocate_node(node_declaration);
+
+ // Skip whitespace before attributes or ?>
+ skip<whitespace_pred, Flags>(text);
+
+ // Parse declaration attributes
+ parse_node_attributes<Flags>(text, declaration);
+
+ // Skip ?>
+ if (text[0] != Ch('?') || text[1] != Ch('>'))
+ RAPIDXML_PARSE_ERROR("expected ?>", text);
+ text += 2;
+
+ return declaration;
+ }
+
+ // Parse XML comment (<!--...)
+ template<int Flags>
+ xml_node<Ch> *parse_comment(Ch *&text)
+ {
+ // If parsing of comments is disabled
+ if (!(Flags & parse_comment_nodes))
+ {
+ // Skip until end of comment
+ while (text[0] != Ch('-') || text[1] != Ch('-') || text[2] != Ch('>'))
+ {
+ if (!text[0])
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+ text += 3; // Skip '-->'
+ return 0; // Do not produce comment node
+ }
+
+ // Remember value start
+ Ch *value = text;
+
+ // Skip until end of comment
+ while (text[0] != Ch('-') || text[1] != Ch('-') || text[2] != Ch('>'))
+ {
+ if (!text[0])
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+
+ // Create comment node
+ xml_node<Ch> *comment = this->allocate_node(node_comment);
+ comment->value(value, text - value);
+
+ // Place zero terminator after comment value
+ if (!(Flags & parse_no_string_terminators))
+ *text = Ch('\0');
+
+ text += 3; // Skip '-->'
+ return comment;
+ }
+
+ // Parse DOCTYPE
+ template<int Flags>
+ xml_node<Ch> *parse_doctype(Ch *&text)
+ {
+ // Remember value start
+ Ch *value = text;
+
+ // Skip to >
+ while (*text != Ch('>'))
+ {
+ // Determine character type
+ switch (*text)
+ {
+
+ // If '[' encountered, scan for matching ending ']' using naive algorithm with depth
+ // This works for all W3C test files except for 2 most wicked
+ case Ch('['):
+ {
+ ++text; // Skip '['
+ int depth = 1;
+ while (depth > 0)
+ {
+ switch (*text)
+ {
+ case Ch('['): ++depth; break;
+ case Ch(']'): --depth; break;
+ case 0: RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ }
+ ++text;
+ }
+ break;
+ }
+
+ // Error on end of text
+ case Ch('\0'):
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+
+ // Other character, skip it
+ default:
+ ++text;
+
+ }
+ }
+
+ // If DOCTYPE nodes enabled
+ if (Flags & parse_doctype_node)
+ {
+ // Create a new doctype node
+ xml_node<Ch> *doctype = this->allocate_node(node_doctype);
+ doctype->value(value, text - value);
+
+ // Place zero terminator after value
+ if (!(Flags & parse_no_string_terminators))
+ *text = Ch('\0');
+
+ text += 1; // skip '>'
+ return doctype;
+ }
+ else
+ {
+ text += 1; // skip '>'
+ return 0;
+ }
+
+ }
+
+ // Parse PI
+ template<int Flags>
+ xml_node<Ch> *parse_pi(Ch *&text)
+ {
+ // If creation of PI nodes is enabled
+ if (Flags & parse_pi_nodes)
+ {
+ // Create pi node
+ xml_node<Ch> *pi = this->allocate_node(node_pi);
+
+ // Extract PI target name
+ Ch *name = text;
+ skip<node_name_pred, Flags>(text);
+ if (text == name)
+ RAPIDXML_PARSE_ERROR("expected PI target", text);
+ pi->name(name, text - name);
+
+ // Skip whitespace between pi target and pi
+ skip<whitespace_pred, Flags>(text);
+
+ // Remember start of pi
+ Ch *value = text;
+
+ // Skip to '?>'
+ while (text[0] != Ch('?') || text[1] != Ch('>'))
+ {
+ if (*text == Ch('\0'))
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+
+ // Set pi value (verbatim, no entity expansion or whitespace normalization)
+ pi->value(value, text - value);
+
+ // Place zero terminator after name and value
+ if (!(Flags & parse_no_string_terminators))
+ {
+ pi->name()[pi->name_size()] = Ch('\0');
+ pi->value()[pi->value_size()] = Ch('\0');
+ }
+
+ text += 2; // Skip '?>'
+ return pi;
+ }
+ else
+ {
+ // Skip to '?>'
+ while (text[0] != Ch('?') || text[1] != Ch('>'))
+ {
+ if (*text == Ch('\0'))
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+ text += 2; // Skip '?>'
+ return 0;
+ }
+ }
+
+ // Parse and append data
+ // Return character that ends data.
+ // This is necessary because this character might have been overwritten by a terminating 0
+ template<int Flags>
+ Ch parse_and_append_data(xml_node<Ch> *node, Ch *&text, Ch *contents_start)
+ {
+ // Backup to contents start if whitespace trimming is disabled
+ if (!(Flags & parse_trim_whitespace))
+ text = contents_start;
+
+ // Skip until end of data
+ Ch *value = text, *end;
+ if (Flags & parse_normalize_whitespace)
+ end = skip_and_expand_character_refs<text_pred, text_pure_with_ws_pred, Flags>(text);
+ else
+ end = skip_and_expand_character_refs<text_pred, text_pure_no_ws_pred, Flags>(text);
+
+ // Trim trailing whitespace if flag is set; leading was already trimmed by whitespace skip after >
+ if (Flags & parse_trim_whitespace)
+ {
+ if (Flags & parse_normalize_whitespace)
+ {
+ // Whitespace is already condensed to single space characters by skipping function, so just trim 1 char off the end
+ if (*(end - 1) == Ch(' '))
+ --end;
+ }
+ else
+ {
+ // Backup until non-whitespace character is found
+ while (whitespace_pred::test(*(end - 1)))
+ --end;
+ }
+ }
+
+ // If characters are still left between end and value (this test is only necessary if normalization is enabled)
+ // Create new data node
+ if (!(Flags & parse_no_data_nodes))
+ {
+ xml_node<Ch> *data = this->allocate_node(node_data);
+ data->value(value, end - value);
+ node->append_node(data);
+ }
+
+ // Add data to parent node if no data exists yet
+ if (!(Flags & parse_no_element_values))
+ if (*node->value() == Ch('\0'))
+ node->value(value, end - value);
+
+ // Place zero terminator after value
+ if (!(Flags & parse_no_string_terminators))
+ {
+ Ch ch = *text;
+ *end = Ch('\0');
+ return ch; // Return character that ends data; this is required because zero terminator overwritten it
+ }
+
+ // Return character that ends data
+ return *text;
+ }
+
+ // Parse CDATA
+ template<int Flags>
+ xml_node<Ch> *parse_cdata(Ch *&text)
+ {
+ // If CDATA is disabled
+ if (Flags & parse_no_data_nodes)
+ {
+ // Skip until end of cdata
+ while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>'))
+ {
+ if (!text[0])
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+ text += 3; // Skip ]]>
+ return 0; // Do not produce CDATA node
+ }
+
+ // Skip until end of cdata
+ Ch *value = text;
+ while (text[0] != Ch(']') || text[1] != Ch(']') || text[2] != Ch('>'))
+ {
+ if (!text[0])
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+
+ // Create new cdata node
+ xml_node<Ch> *cdata = this->allocate_node(node_cdata);
+ cdata->value(value, text - value);
+
+ // Place zero terminator after value
+ if (!(Flags & parse_no_string_terminators))
+ *text = Ch('\0');
+
+ text += 3; // Skip ]]>
+ return cdata;
+ }
+
+ // Parse element node
+ template<int Flags>
+ xml_node<Ch> *parse_element(Ch *&text)
+ {
+ // Create element node
+ xml_node<Ch> *element = this->allocate_node(node_element);
+
+ // Extract element name
+ Ch *name = text;
+ skip<node_name_pred, Flags>(text);
+ if (text == name)
+ RAPIDXML_PARSE_ERROR("expected element name", text);
+ element->name(name, text - name);
+
+ // Skip whitespace between element name and attributes or >
+ skip<whitespace_pred, Flags>(text);
+
+ // Parse attributes, if any
+ parse_node_attributes<Flags>(text, element);
+
+ // Determine ending type
+ if (*text == Ch('>'))
+ {
+ ++text;
+ parse_node_contents<Flags>(text, element);
+ }
+ else if (*text == Ch('/'))
+ {
+ ++text;
+ if (*text != Ch('>'))
+ RAPIDXML_PARSE_ERROR("expected >", text);
+ ++text;
+ }
+ else
+ RAPIDXML_PARSE_ERROR("expected >", text);
+
+ // Place zero terminator after name
+ if (!(Flags & parse_no_string_terminators))
+ element->name()[element->name_size()] = Ch('\0');
+
+ // Return parsed element
+ return element;
+ }
+
+ // Determine node type, and parse it
+ template<int Flags>
+ xml_node<Ch> *parse_node(Ch *&text)
+ {
+ // Parse proper node type
+ switch (text[0])
+ {
+
+ // <...
+ default:
+ // Parse and append element node
+ return parse_element<Flags>(text);
+
+ // <?...
+ case Ch('?'):
+ ++text; // Skip ?
+ if ((text[0] == Ch('x') || text[0] == Ch('X')) &&
+ (text[1] == Ch('m') || text[1] == Ch('M')) &&
+ (text[2] == Ch('l') || text[2] == Ch('L')) &&
+ whitespace_pred::test(text[3]))
+ {
+ // '<?xml ' - xml declaration
+ text += 4; // Skip 'xml '
+ return parse_xml_declaration<Flags>(text);
+ }
+ else
+ {
+ // Parse PI
+ return parse_pi<Flags>(text);
+ }
+
+ // <!...
+ case Ch('!'):
+
+ // Parse proper subset of <! node
+ switch (text[1])
+ {
+
+ // <!-
+ case Ch('-'):
+ if (text[2] == Ch('-'))
+ {
+ // '<!--' - xml comment
+ text += 3; // Skip '!--'
+ return parse_comment<Flags>(text);
+ }
+ break;
+
+ // <![
+ case Ch('['):
+ if (text[2] == Ch('C') && text[3] == Ch('D') && text[4] == Ch('A') &&
+ text[5] == Ch('T') && text[6] == Ch('A') && text[7] == Ch('['))
+ {
+ // '<![CDATA[' - cdata
+ text += 8; // Skip '![CDATA['
+ return parse_cdata<Flags>(text);
+ }
+ break;
+
+ // <!D
+ case Ch('D'):
+ if (text[2] == Ch('O') && text[3] == Ch('C') && text[4] == Ch('T') &&
+ text[5] == Ch('Y') && text[6] == Ch('P') && text[7] == Ch('E') &&
+ whitespace_pred::test(text[8]))
+ {
+ // '<!DOCTYPE ' - doctype
+ text += 9; // skip '!DOCTYPE '
+ return parse_doctype<Flags>(text);
+ }
+
+ } // switch
+
+ // Attempt to skip other, unrecognized node types starting with <!
+ ++text; // Skip !
+ while (*text != Ch('>'))
+ {
+ if (*text == 0)
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+ ++text;
+ }
+ ++text; // Skip '>'
+ return 0; // No node recognized
+
+ }
+ }
+
+ // Parse contents of the node - children, data etc.
+ template<int Flags>
+ void parse_node_contents(Ch *&text, xml_node<Ch> *node)
+ {
+ // For all children and text
+ while (1)
+ {
+ // Skip whitespace between > and node contents
+ Ch *contents_start = text; // Store start of node contents before whitespace is skipped
+ skip<whitespace_pred, Flags>(text);
+ Ch next_char = *text;
+
+ // After data nodes, instead of continuing the loop, control jumps here.
+ // This is because zero termination inside parse_and_append_data() function
+ // would wreak havoc with the above code.
+ // Also, skipping whitespace after data nodes is unnecessary.
+ after_data_node:
+
+ // Determine what comes next: node closing, child node, data node, or 0?
+ switch (next_char)
+ {
+
+ // Node closing or child node
+ case Ch('<'):
+ if (text[1] == Ch('/'))
+ {
+ // Node closing
+ text += 2; // Skip '</'
+ if (Flags & parse_validate_closing_tags)
+ {
+ // Skip and validate closing tag name
+ Ch *closing_name = text;
+ skip<node_name_pred, Flags>(text);
+ if (!internal::compare(node->name(), node->name_size(), closing_name, text - closing_name, true))
+ RAPIDXML_PARSE_ERROR("invalid closing tag name", text);
+ }
+ else
+ {
+ // No validation, just skip name
+ skip<node_name_pred, Flags>(text);
+ }
+ // Skip remaining whitespace after node name
+ skip<whitespace_pred, Flags>(text);
+ if (*text != Ch('>'))
+ RAPIDXML_PARSE_ERROR("expected >", text);
+ ++text; // Skip '>'
+ return; // Node closed, finished parsing contents
+ }
+ else
+ {
+ // Child node
+ ++text; // Skip '<'
+ if (xml_node<Ch> *child = parse_node<Flags>(text))
+ node->append_node(child);
+ }
+ break;
+
+ // End of data - error
+ case Ch('\0'):
+ RAPIDXML_PARSE_ERROR("unexpected end of data", text);
+
+ // Data node
+ default:
+ next_char = parse_and_append_data<Flags>(node, text, contents_start);
+ goto after_data_node; // Bypass regular processing after data nodes
+
+ }
+ }
+ }
+
+ // Parse XML attributes of the node
+ template<int Flags>
+ void parse_node_attributes(Ch *&text, xml_node<Ch> *node)
+ {
+ // For all attributes
+ while (attribute_name_pred::test(*text))
+ {
+ // Extract attribute name
+ Ch *name = text;
+ ++text; // Skip first character of attribute name
+ skip<attribute_name_pred, Flags>(text);
+ if (text == name)
+ RAPIDXML_PARSE_ERROR("expected attribute name", name);
+
+ // Create new attribute
+ xml_attribute<Ch> *attribute = this->allocate_attribute();
+ attribute->name(name, text - name);
+ node->append_attribute(attribute);
+
+ // Skip whitespace after attribute name
+ skip<whitespace_pred, Flags>(text);
+
+ // Skip =
+ if (*text != Ch('='))
+ RAPIDXML_PARSE_ERROR("expected =", text);
+ ++text;
+
+ // Add terminating zero after name
+ if (!(Flags & parse_no_string_terminators))
+ attribute->name()[attribute->name_size()] = 0;
+
+ // Skip whitespace after =
+ skip<whitespace_pred, Flags>(text);
+
+ // Skip quote and remember if it was ' or "
+ Ch quote = *text;
+ if (quote != Ch('\'') && quote != Ch('"'))
+ RAPIDXML_PARSE_ERROR("expected ' or \"", text);
+ ++text;
+
+ // Extract attribute value and expand char refs in it
+ Ch *value = text, *end;
+ const int AttFlags = Flags & ~parse_normalize_whitespace; // No whitespace normalization in attributes
+ if (quote == Ch('\''))
+ end = skip_and_expand_character_refs<attribute_value_pred<Ch('\'')>, attribute_value_pure_pred<Ch('\'')>, AttFlags>(text);
+ else
+ end = skip_and_expand_character_refs<attribute_value_pred<Ch('"')>, attribute_value_pure_pred<Ch('"')>, AttFlags>(text);
+
+ // Set attribute value
+ attribute->value(value, end - value);
+
+ // Make sure that end quote is present
+ if (*text != quote)
+ RAPIDXML_PARSE_ERROR("expected ' or \"", text);
+ ++text; // Skip quote
+
+ // Add terminating zero after value
+ if (!(Flags & parse_no_string_terminators))
+ attribute->value()[attribute->value_size()] = 0;
+
+ // Skip whitespace after attribute value
+ skip<whitespace_pred, Flags>(text);
+ }
+ }
+
+ };
+
+ //! \cond internal
+ namespace internal
+ {
+
+ // Whitespace (space \n \r \t)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_whitespace[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 1, 1, 0, 0, 1, 0, 0, // 0
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 1
+ 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 2
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 3
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 4
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 5
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 6
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 7
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 8
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // 9
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // A
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // B
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // C
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // D
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // E
+ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 // F
+ };
+
+ // Node name (anything but space \n \r \t / > ? \0)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_node_name[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Text (i.e. PCDATA) (anything but < \0)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_text[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Text (i.e. PCDATA) that does not require processing when ws normalization is disabled
+ // (anything but < \0 &)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_text_pure_no_ws[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Text (i.e. PCDATA) that does not require processing when ws normalizationis is enabled
+ // (anything but < \0 & space \n \r \t)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_text_pure_with_ws[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 0, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Attribute name (anything but space \n \r \t / < > = ? ! \0)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_attribute_name[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 0, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 0, 0, 0, 0, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Attribute data with single quote (anything but ' \0)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_attribute_data_1[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 1, 1, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Attribute data with single quote that does not require processing (anything but ' \0 &)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_attribute_data_1_pure[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 1, 1, 1, 1, 0, 0, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Attribute data with double quote (anything but " \0)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_attribute_data_2[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Attribute data with double quote that does not require processing (anything but " \0 &)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_attribute_data_2_pure[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 0
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 1
+ 1, 1, 0, 1, 1, 1, 0, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 2
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 3
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 4
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 5
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 6
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 7
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 8
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // 9
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // A
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // B
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // C
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // D
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, // E
+ 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 // F
+ };
+
+ // Digits (dec and hex, 255 denotes end of numeric character reference)
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_digits[256] =
+ {
+ // 0 1 2 3 4 5 6 7 8 9 A B C D E F
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 0
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 1
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 2
+ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9,255,255,255,255,255,255, // 3
+ 255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255, // 4
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 5
+ 255, 10, 11, 12, 13, 14, 15,255,255,255,255,255,255,255,255,255, // 6
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 7
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 8
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // 9
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // A
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // B
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // C
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // D
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255, // E
+ 255,255,255,255,255,255,255,255,255,255,255,255,255,255,255,255 // F
+ };
+
+ // Upper case conversion
+ template<int Dummy>
+ const unsigned char lookup_tables<Dummy>::lookup_upcase[256] =
+ {
+ // 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, A B C D E F
+ 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, // 0
+ 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, // 1
+ 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, // 2
+ 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, // 3
+ 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, // 4
+ 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, // 5
+ 96, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, // 6
+ 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 123,124,125,126,127, // 7
+ 128,129,130,131,132,133,134,135,136,137,138,139,140,141,142,143, // 8
+ 144,145,146,147,148,149,150,151,152,153,154,155,156,157,158,159, // 9
+ 160,161,162,163,164,165,166,167,168,169,170,171,172,173,174,175, // A
+ 176,177,178,179,180,181,182,183,184,185,186,187,188,189,190,191, // B
+ 192,193,194,195,196,197,198,199,200,201,202,203,204,205,206,207, // C
+ 208,209,210,211,212,213,214,215,216,217,218,219,220,221,222,223, // D
+ 224,225,226,227,228,229,230,231,232,233,234,235,236,237,238,239, // E
+ 240,241,242,243,244,245,246,247,248,249,250,251,252,253,254,255 // F
+ };
+ }
+ //! \endcond
+
+}
+
+// Undefine internal macros
+#undef RAPIDXML_PARSE_ERROR
+
+// On MSVC, restore warnings state
+#ifdef _MSC_VER
+ #pragma warning(pop)
+#endif
+
+#endif
diff --git a/externals/Pangolin/include/pangolin/utils/xml/rapidxml_iterators.hpp b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_iterators.hpp
new file mode 100755
index 0000000000000000000000000000000000000000..85c58946d31d86e2c994a6d9a61566438dd44c30
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_iterators.hpp
@@ -0,0 +1,174 @@
+#ifndef RAPIDXML_ITERATORS_HPP_INCLUDED
+#define RAPIDXML_ITERATORS_HPP_INCLUDED
+
+// Copyright (C) 2006, 2009 Marcin Kalicinski
+// Version 1.13
+// Revision $DateTime: 2009/05/13 01:46:17 $
+//! \file rapidxml_iterators.hpp This file contains rapidxml iterators
+
+#include "rapidxml.hpp"
+
+namespace rapidxml
+{
+
+ //! Iterator of child nodes of xml_node
+ template<class Ch>
+ class node_iterator
+ {
+
+ public:
+
+ typedef typename xml_node<Ch> value_type;
+ typedef typename xml_node<Ch> &reference;
+ typedef typename xml_node<Ch> *pointer;
+ typedef std::ptrdiff_t difference_type;
+ typedef std::bidirectional_iterator_tag iterator_category;
+
+ node_iterator()
+ : m_node(0)
+ {
+ }
+
+ node_iterator(xml_node<Ch> *node)
+ : m_node(node->first_node())
+ {
+ }
+
+ reference operator *() const
+ {
+ assert(m_node);
+ return *m_node;
+ }
+
+ pointer operator->() const
+ {
+ assert(m_node);
+ return m_node;
+ }
+
+ node_iterator& operator++()
+ {
+ assert(m_node);
+ m_node = m_node->next_sibling();
+ return *this;
+ }
+
+ node_iterator operator++(int)
+ {
+ node_iterator tmp = *this;
+ ++this;
+ return tmp;
+ }
+
+ node_iterator& operator--()
+ {
+ assert(m_node && m_node->previous_sibling());
+ m_node = m_node->previous_sibling();
+ return *this;
+ }
+
+ node_iterator operator--(int)
+ {
+ node_iterator tmp = *this;
+ ++this;
+ return tmp;
+ }
+
+ bool operator ==(const node_iterator<Ch> &rhs)
+ {
+ return m_node == rhs.m_node;
+ }
+
+ bool operator !=(const node_iterator<Ch> &rhs)
+ {
+ return m_node != rhs.m_node;
+ }
+
+ private:
+
+ xml_node<Ch> *m_node;
+
+ };
+
+ //! Iterator of child attributes of xml_node
+ template<class Ch>
+ class attribute_iterator
+ {
+
+ public:
+
+ typedef typename xml_attribute<Ch> value_type;
+ typedef typename xml_attribute<Ch> &reference;
+ typedef typename xml_attribute<Ch> *pointer;
+ typedef std::ptrdiff_t difference_type;
+ typedef std::bidirectional_iterator_tag iterator_category;
+
+ attribute_iterator()
+ : m_attribute(0)
+ {
+ }
+
+ attribute_iterator(xml_node<Ch> *node)
+ : m_attribute(node->first_attribute())
+ {
+ }
+
+ reference operator *() const
+ {
+ assert(m_attribute);
+ return *m_attribute;
+ }
+
+ pointer operator->() const
+ {
+ assert(m_attribute);
+ return m_attribute;
+ }
+
+ attribute_iterator& operator++()
+ {
+ assert(m_attribute);
+ m_attribute = m_attribute->next_attribute();
+ return *this;
+ }
+
+ attribute_iterator operator++(int)
+ {
+ attribute_iterator tmp = *this;
+ ++this;
+ return tmp;
+ }
+
+ attribute_iterator& operator--()
+ {
+ assert(m_attribute && m_attribute->previous_attribute());
+ m_attribute = m_attribute->previous_attribute();
+ return *this;
+ }
+
+ attribute_iterator operator--(int)
+ {
+ attribute_iterator tmp = *this;
+ ++this;
+ return tmp;
+ }
+
+ bool operator ==(const attribute_iterator<Ch> &rhs)
+ {
+ return m_attribute == rhs.m_attribute;
+ }
+
+ bool operator !=(const attribute_iterator<Ch> &rhs)
+ {
+ return m_attribute != rhs.m_attribute;
+ }
+
+ private:
+
+ xml_attribute<Ch> *m_attribute;
+
+ };
+
+}
+
+#endif
diff --git a/externals/Pangolin/include/pangolin/utils/xml/rapidxml_print.hpp b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_print.hpp
new file mode 100755
index 0000000000000000000000000000000000000000..d03d5f5b06a66d3042ae58cfe6e7c8310eaf9d8e
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_print.hpp
@@ -0,0 +1,421 @@
+#ifndef RAPIDXML_PRINT_HPP_INCLUDED
+#define RAPIDXML_PRINT_HPP_INCLUDED
+
+// Copyright (C) 2006, 2009 Marcin Kalicinski
+// Version 1.13
+// Revision $DateTime: 2009/05/13 01:46:17 $
+//! \file rapidxml_print.hpp This file contains rapidxml printer implementation
+
+#include "rapidxml.hpp"
+
+// Only include streams if not disabled
+#ifndef RAPIDXML_NO_STREAMS
+ #include <ostream>
+ #include <iterator>
+#endif
+
+namespace rapidxml
+{
+
+ ///////////////////////////////////////////////////////////////////////
+ // Printing flags
+
+ const int print_no_indenting = 0x1; //!< Printer flag instructing the printer to suppress indenting of XML. See print() function.
+
+ ///////////////////////////////////////////////////////////////////////
+ // Internal
+
+ //! \cond internal
+ namespace internal
+ {
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Internal character operations
+
+ // Copy characters from given range to given output iterator
+ template<class OutIt, class Ch>
+ inline OutIt copy_chars(const Ch *begin, const Ch *end, OutIt out)
+ {
+ while (begin != end)
+ *out++ = *begin++;
+ return out;
+ }
+
+ // Copy characters from given range to given output iterator and expand
+ // characters into references (< > ' " &)
+ template<class OutIt, class Ch>
+ inline OutIt copy_and_expand_chars(const Ch *begin, const Ch *end, Ch noexpand, OutIt out)
+ {
+ while (begin != end)
+ {
+ if (*begin == noexpand)
+ {
+ *out++ = *begin; // No expansion, copy character
+ }
+ else
+ {
+ switch (*begin)
+ {
+ case Ch('<'):
+ *out++ = Ch('&'); *out++ = Ch('l'); *out++ = Ch('t'); *out++ = Ch(';');
+ break;
+ case Ch('>'):
+ *out++ = Ch('&'); *out++ = Ch('g'); *out++ = Ch('t'); *out++ = Ch(';');
+ break;
+ case Ch('\''):
+ *out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('p'); *out++ = Ch('o'); *out++ = Ch('s'); *out++ = Ch(';');
+ break;
+ case Ch('"'):
+ *out++ = Ch('&'); *out++ = Ch('q'); *out++ = Ch('u'); *out++ = Ch('o'); *out++ = Ch('t'); *out++ = Ch(';');
+ break;
+ case Ch('&'):
+ *out++ = Ch('&'); *out++ = Ch('a'); *out++ = Ch('m'); *out++ = Ch('p'); *out++ = Ch(';');
+ break;
+ default:
+ *out++ = *begin; // No expansion, copy character
+ }
+ }
+ ++begin; // Step to next character
+ }
+ return out;
+ }
+
+ // Fill given output iterator with repetitions of the same character
+ template<class OutIt, class Ch>
+ inline OutIt fill_chars(OutIt out, int n, Ch ch)
+ {
+ for (int i = 0; i < n; ++i)
+ *out++ = ch;
+ return out;
+ }
+
+ // Find character
+ template<class Ch, Ch ch>
+ inline bool find_char(const Ch *begin, const Ch *end)
+ {
+ while (begin != end)
+ if (*begin++ == ch)
+ return true;
+ return false;
+ }
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Internal printing operations
+
+ // Print node
+ template<class OutIt, class Ch>
+ inline OutIt print_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ // Print proper node type
+ switch (node->type())
+ {
+
+ // Document
+ case node_document:
+ out = print_children(out, node, flags, indent);
+ break;
+
+ // Element
+ case node_element:
+ out = print_element_node(out, node, flags, indent);
+ break;
+
+ // Data
+ case node_data:
+ out = print_data_node(out, node, flags, indent);
+ break;
+
+ // CDATA
+ case node_cdata:
+ out = print_cdata_node(out, node, flags, indent);
+ break;
+
+ // Declaration
+ case node_declaration:
+ out = print_declaration_node(out, node, flags, indent);
+ break;
+
+ // Comment
+ case node_comment:
+ out = print_comment_node(out, node, flags, indent);
+ break;
+
+ // Doctype
+ case node_doctype:
+ out = print_doctype_node(out, node, flags, indent);
+ break;
+
+ // Pi
+ case node_pi:
+ out = print_pi_node(out, node, flags, indent);
+ break;
+
+ // Unknown
+ default:
+ assert(0);
+ break;
+ }
+
+ // If indenting not disabled, add line break after node
+ if (!(flags & print_no_indenting))
+ *out = Ch('\n'), ++out;
+
+ // Return modified iterator
+ return out;
+ }
+
+ // Print children of the node
+ template<class OutIt, class Ch>
+ inline OutIt print_children(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ for (xml_node<Ch> *child = node->first_node(); child; child = child->next_sibling())
+ out = print_node(out, child, flags, indent);
+ return out;
+ }
+
+ // Print attributes of the node
+ template<class OutIt, class Ch>
+ inline OutIt print_attributes(OutIt out, const xml_node<Ch> *node, int flags)
+ {
+ for (xml_attribute<Ch> *attribute = node->first_attribute(); attribute; attribute = attribute->next_attribute())
+ {
+ if (attribute->name() && attribute->value())
+ {
+ // Print attribute name
+ *out = Ch(' '), ++out;
+ out = copy_chars(attribute->name(), attribute->name() + attribute->name_size(), out);
+ *out = Ch('='), ++out;
+ // Print attribute value using appropriate quote type
+ if (find_char<Ch, Ch('"')>(attribute->value(), attribute->value() + attribute->value_size()))
+ {
+ *out = Ch('\''), ++out;
+ out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('"'), out);
+ *out = Ch('\''), ++out;
+ }
+ else
+ {
+ *out = Ch('"'), ++out;
+ out = copy_and_expand_chars(attribute->value(), attribute->value() + attribute->value_size(), Ch('\''), out);
+ *out = Ch('"'), ++out;
+ }
+ }
+ }
+ return out;
+ }
+
+ // Print data node
+ template<class OutIt, class Ch>
+ inline OutIt print_data_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_data);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ out = copy_and_expand_chars(node->value(), node->value() + node->value_size(), Ch(0), out);
+ return out;
+ }
+
+ // Print data node
+ template<class OutIt, class Ch>
+ inline OutIt print_cdata_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_cdata);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'); ++out;
+ *out = Ch('!'); ++out;
+ *out = Ch('['); ++out;
+ *out = Ch('C'); ++out;
+ *out = Ch('D'); ++out;
+ *out = Ch('A'); ++out;
+ *out = Ch('T'); ++out;
+ *out = Ch('A'); ++out;
+ *out = Ch('['); ++out;
+ out = copy_chars(node->value(), node->value() + node->value_size(), out);
+ *out = Ch(']'); ++out;
+ *out = Ch(']'); ++out;
+ *out = Ch('>'); ++out;
+ return out;
+ }
+
+ // Print element node
+ template<class OutIt, class Ch>
+ inline OutIt print_element_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_element);
+
+ // Print element name and attributes, if any
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'), ++out;
+ out = copy_chars(node->name(), node->name() + node->name_size(), out);
+ out = print_attributes(out, node, flags);
+
+ // If node is childless
+ if (node->value_size() == 0 && !node->first_node())
+ {
+ // Print childless node tag ending
+ *out = Ch('/'), ++out;
+ *out = Ch('>'), ++out;
+ }
+ else
+ {
+ // Print normal node tag ending
+ *out = Ch('>'), ++out;
+
+ // Test if node contains a single data node only (and no other nodes)
+ xml_node<Ch> *child = node->first_node();
+ if (!child)
+ {
+ // If node has no children, only print its value without indenting
+ out = copy_and_expand_chars(node->value(), node->value() + node->value_size(), Ch(0), out);
+ }
+ else if (child->next_sibling() == 0 && child->type() == node_data)
+ {
+ // If node has a sole data child, only print its value without indenting
+ out = copy_and_expand_chars(child->value(), child->value() + child->value_size(), Ch(0), out);
+ }
+ else
+ {
+ // Print all children with full indenting
+ if (!(flags & print_no_indenting))
+ *out = Ch('\n'), ++out;
+ out = print_children(out, node, flags, indent + 1);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ }
+
+ // Print node end
+ *out = Ch('<'), ++out;
+ *out = Ch('/'), ++out;
+ out = copy_chars(node->name(), node->name() + node->name_size(), out);
+ *out = Ch('>'), ++out;
+ }
+ return out;
+ }
+
+ // Print declaration node
+ template<class OutIt, class Ch>
+ inline OutIt print_declaration_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ // Print declaration start
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'), ++out;
+ *out = Ch('?'), ++out;
+ *out = Ch('x'), ++out;
+ *out = Ch('m'), ++out;
+ *out = Ch('l'), ++out;
+
+ // Print attributes
+ out = print_attributes(out, node, flags);
+
+ // Print declaration end
+ *out = Ch('?'), ++out;
+ *out = Ch('>'), ++out;
+
+ return out;
+ }
+
+ // Print comment node
+ template<class OutIt, class Ch>
+ inline OutIt print_comment_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_comment);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'), ++out;
+ *out = Ch('!'), ++out;
+ *out = Ch('-'), ++out;
+ *out = Ch('-'), ++out;
+ out = copy_chars(node->value(), node->value() + node->value_size(), out);
+ *out = Ch('-'), ++out;
+ *out = Ch('-'), ++out;
+ *out = Ch('>'), ++out;
+ return out;
+ }
+
+ // Print doctype node
+ template<class OutIt, class Ch>
+ inline OutIt print_doctype_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_doctype);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'), ++out;
+ *out = Ch('!'), ++out;
+ *out = Ch('D'), ++out;
+ *out = Ch('O'), ++out;
+ *out = Ch('C'), ++out;
+ *out = Ch('T'), ++out;
+ *out = Ch('Y'), ++out;
+ *out = Ch('P'), ++out;
+ *out = Ch('E'), ++out;
+ *out = Ch(' '), ++out;
+ out = copy_chars(node->value(), node->value() + node->value_size(), out);
+ *out = Ch('>'), ++out;
+ return out;
+ }
+
+ // Print pi node
+ template<class OutIt, class Ch>
+ inline OutIt print_pi_node(OutIt out, const xml_node<Ch> *node, int flags, int indent)
+ {
+ assert(node->type() == node_pi);
+ if (!(flags & print_no_indenting))
+ out = fill_chars(out, indent, Ch('\t'));
+ *out = Ch('<'), ++out;
+ *out = Ch('?'), ++out;
+ out = copy_chars(node->name(), node->name() + node->name_size(), out);
+ *out = Ch(' '), ++out;
+ out = copy_chars(node->value(), node->value() + node->value_size(), out);
+ *out = Ch('?'), ++out;
+ *out = Ch('>'), ++out;
+ return out;
+ }
+
+ }
+ //! \endcond
+
+ ///////////////////////////////////////////////////////////////////////////
+ // Printing
+
+ //! Prints XML to given output iterator.
+ //! \param out Output iterator to print to.
+ //! \param node Node to be printed. Pass xml_document to print entire document.
+ //! \param flags Flags controlling how XML is printed.
+ //! \return Output iterator pointing to position immediately after last character of printed text.
+ template<class OutIt, class Ch>
+ inline OutIt print(OutIt out, const xml_node<Ch> &node, int flags = 0)
+ {
+ return internal::print_node(out, &node, flags, 0);
+ }
+
+#ifndef RAPIDXML_NO_STREAMS
+
+ //! Prints XML to given output stream.
+ //! \param out Output stream to print to.
+ //! \param node Node to be printed. Pass xml_document to print entire document.
+ //! \param flags Flags controlling how XML is printed.
+ //! \return Output stream.
+ template<class Ch>
+ inline std::basic_ostream<Ch> &print(std::basic_ostream<Ch> &out, const xml_node<Ch> &node, int flags = 0)
+ {
+ print(std::ostream_iterator<Ch>(out), node, flags);
+ return out;
+ }
+
+ //! Prints formatted XML to given output stream. Uses default printing flags. Use print() function to customize printing process.
+ //! \param out Output stream to print to.
+ //! \param node Node to be printed.
+ //! \return Output stream.
+ template<class Ch>
+ inline std::basic_ostream<Ch> &operator <<(std::basic_ostream<Ch> &out, const xml_node<Ch> &node)
+ {
+ return print(out, node);
+ }
+
+#endif
+
+}
+
+#endif
diff --git a/externals/Pangolin/include/pangolin/utils/xml/rapidxml_utils.hpp b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_utils.hpp
new file mode 100755
index 0000000000000000000000000000000000000000..f2824bf514f28c539bb0e7ac699f8a03a1a0ce83
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/utils/xml/rapidxml_utils.hpp
@@ -0,0 +1,122 @@
+#ifndef RAPIDXML_UTILS_HPP_INCLUDED
+#define RAPIDXML_UTILS_HPP_INCLUDED
+
+// Copyright (C) 2006, 2009 Marcin Kalicinski
+// Version 1.13
+// Revision $DateTime: 2009/05/13 01:46:17 $
+//! \file rapidxml_utils.hpp This file contains high-level rapidxml utilities that can be useful
+//! in certain simple scenarios. They should probably not be used if maximizing performance is the main objective.
+
+#include "rapidxml.hpp"
+#include <vector>
+#include <string>
+#include <fstream>
+#include <stdexcept>
+
+namespace rapidxml
+{
+
+ //! Represents data loaded from a file
+ template<class Ch = char>
+ class file
+ {
+
+ public:
+
+ //! Loads file into the memory. Data will be automatically destroyed by the destructor.
+ //! \param filename Filename to load.
+ file(const char *filename)
+ {
+ using namespace std;
+
+ // Open stream
+ basic_ifstream<Ch> stream(filename, ios::binary);
+ if (!stream)
+ throw runtime_error(string("cannot open file ") + filename);
+ stream.unsetf(ios::skipws);
+
+ // Determine stream size
+ stream.seekg(0, ios::end);
+ size_t size = (size_t)stream.tellg();
+ stream.seekg(0);
+
+ // Load data and add terminating 0
+ m_data.resize(size + 1);
+ stream.read(&m_data.front(), static_cast<streamsize>(size));
+ m_data[size] = 0;
+ }
+
+ //! Loads file into the memory. Data will be automatically destroyed by the destructor
+ //! \param stream Stream to load from
+ file(std::basic_istream<Ch> &stream)
+ {
+ using namespace std;
+
+ // Load data and add terminating 0
+ stream.unsetf(ios::skipws);
+ m_data.assign(istreambuf_iterator<Ch>(stream), istreambuf_iterator<Ch>());
+ if (stream.fail() || stream.bad())
+ throw runtime_error("error reading stream");
+ m_data.push_back(0);
+ }
+
+ //! Gets file data.
+ //! \return Pointer to data of file.
+ Ch *data()
+ {
+ return &m_data.front();
+ }
+
+ //! Gets file data.
+ //! \return Pointer to data of file.
+ const Ch *data() const
+ {
+ return &m_data.front();
+ }
+
+ //! Gets file data size.
+ //! \return Size of file data, in characters.
+ std::size_t size() const
+ {
+ return m_data.size();
+ }
+
+ private:
+
+ std::vector<Ch> m_data; // File data
+
+ };
+
+ //! Counts children of node. Time complexity is O(n).
+ //! \return Number of children of node
+ template<class Ch>
+ inline std::size_t count_children(xml_node<Ch> *node)
+ {
+ xml_node<Ch> *child = node->first_node();
+ std::size_t count = 0;
+ while (child)
+ {
+ ++count;
+ child = child->next_sibling();
+ }
+ return count;
+ }
+
+ //! Counts attributes of node. Time complexity is O(n).
+ //! \return Number of attributes of node
+ template<class Ch>
+ inline std::size_t count_attributes(xml_node<Ch> *node)
+ {
+ xml_attribute<Ch> *attr = node->first_attribute();
+ std::size_t count = 0;
+ while (attr)
+ {
+ ++count;
+ attr = attr->next_attribute();
+ }
+ return count;
+ }
+
+}
+
+#endif
diff --git a/externals/Pangolin/include/pangolin/var/input_record_repeat.h b/externals/Pangolin/include/pangolin/var/input_record_repeat.h
new file mode 100644
index 0000000000000000000000000000000000000000..b8df0c5182969d76c5a96223800c886142e70b5b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/input_record_repeat.h
@@ -0,0 +1,86 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+
+#include <list>
+#include <string>
+#include <fstream>
+
+namespace pangolin
+{
+
+struct FrameInput
+{
+ int index;
+ std::string var;
+ std::string val;
+};
+
+struct PANGOLIN_EXPORT InputRecordRepeat
+{
+ InputRecordRepeat(const std::string& var_record_prefix);
+ ~InputRecordRepeat();
+
+ void SetIndex(int id);
+
+ void Record();
+ void Stop();
+
+ void LoadBuffer(const std::string& filename);
+ void SaveBuffer(const std::string& filename);
+ void ClearBuffer();
+
+ void PlayBuffer();
+ void PlayBuffer(size_t start, size_t end);
+
+ void UpdateVariable(const std::string& name );
+
+ template<typename T>
+ inline void UpdateVariable(const Var<T>& var ) {
+ GuiVarChanged((void*)this, var.var->Meta().full_name, *var.var);
+ }
+
+ size_t Size();
+
+protected:
+ bool record;
+ bool play;
+
+ int index;
+ std::ofstream file;
+ std::string filename;
+
+ std::list<FrameInput> play_queue;
+ std::list<FrameInput> record_queue;
+
+ static void GuiVarChanged(void* data, const std::string& name, VarValueGeneric& var);
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/var.h b/externals/Pangolin/include/pangolin/var/var.h
new file mode 100644
index 0000000000000000000000000000000000000000..3c23e3ddbc61950570527dbe685762c67498d1f7
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/var.h
@@ -0,0 +1,340 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <stdexcept>
+#include <string.h>
+#include <cmath>
+
+#include <pangolin/var/varvalue.h>
+#include <pangolin/var/varwrapper.h>
+#include <pangolin/var/varstate.h>
+
+namespace pangolin
+{
+
+template<typename T>
+inline void InitialiseNewVarMetaGeneric(
+ VarValue<T>& v, const std::string& name
+) {
+ // Initialise meta parameters
+ const std::vector<std::string> parts = pangolin::Split(name,'.');
+ v.Meta().full_name = name;
+ v.Meta().friendly = parts.size() > 0 ? parts[parts.size()-1] : "";
+ v.Meta().range[0] = 0.0;
+ v.Meta().range[1] = 0.0;
+ v.Meta().increment = 0.0;
+ v.Meta().flags = META_FLAG_NONE;
+ v.Meta().logscale = false;
+ v.Meta().generic = true;
+
+ VarState::I().NotifyNewVar<T>(name, v);
+}
+
+template<typename T>
+inline void InitialiseNewVarMeta(
+ VarValue<T>& v, const std::string& name,
+ double min = 0, double max = 0, int flags = META_FLAG_TOGGLE,
+ bool logscale = false
+) {
+ // Initialise meta parameters
+ const std::vector<std::string> parts = pangolin::Split(name,'.');
+ v.Meta().full_name = name;
+ v.Meta().friendly = parts.size() > 0 ? parts[parts.size()-1] : "";
+ v.Meta().range[0] = min;
+ v.Meta().range[1] = max;
+ if (std::is_integral<T>::value) {
+ v.Meta().increment = 1.0;
+ } else {
+ v.Meta().increment = (max - min) / 100.0;
+ }
+ v.Meta().flags = flags;
+ v.Meta().logscale = logscale;
+ v.Meta().generic = false;
+
+ VarState::I().NotifyNewVar<T>(name, v);
+}
+
+template<typename T>
+class Var
+{
+public:
+ static T& Attach(
+ const std::string& name, T& variable,
+ double min, double max, bool logscale = false
+ ) {
+ // Find name in VarStore
+ VarValueGeneric*& v = VarState::I()[name];
+ if(v) {
+ throw std::runtime_error("Var with that name already exists.");
+ }else{
+ // new VarRef<T> (owned by VarStore)
+ VarValue<T&>* nv = new VarValue<T&>(variable);
+ v = nv;
+ if(logscale) {
+ if (min <= 0 || max <= 0) {
+ throw std::runtime_error("LogScale: range of numbers must be positive!");
+ }
+ InitialiseNewVarMeta<T&>(*nv, name, std::log(min), std::log(max), META_FLAG_TOGGLE, logscale);
+ }else{
+ InitialiseNewVarMeta<T&>(*nv, name, min, max, META_FLAG_TOGGLE, logscale);
+ }
+ }
+ return variable;
+ }
+
+ static T& Attach(
+ const std::string& name, T& variable, int flags = META_FLAG_NONE
+ ) {
+ // Find name in VarStore
+ VarValueGeneric*& v = VarState::I()[name];
+ if (v) {
+ throw std::runtime_error("Var with that name already exists.");
+ }
+ else{
+ // new VarRef<T> (owned by VarStore)
+ VarValue<T&>* nv = new VarValue<T&>(variable);
+ v = nv;
+ InitialiseNewVarMeta<T&>(*nv, name, 0.0, 0.0, flags);
+ }
+ return variable;
+ }
+
+ static T& Attach(
+ const std::string& name, T& variable, bool toggle
+ ) {
+ return Attach(name, variable, toggle ? META_FLAG_TOGGLE : META_FLAG_NONE);
+ }
+
+ ~Var()
+ {
+ delete ptr;
+ }
+
+ Var( VarValueGeneric& v )
+ : ptr(0)
+ {
+ InitialiseFromGeneric(&v);
+ }
+
+
+ Var( const std::string& name )
+ : ptr(0)
+ {
+ // Find name in VarStore
+ VarValueGeneric*& v = VarState::I()[name];
+ if(v && !v->Meta().generic) {
+ InitialiseFromGeneric(v);
+ }else{
+ // new VarValue<T> (owned by VarStore)
+ VarValue<T>* nv;
+ if(v) {
+ // Specialise generic variable
+ nv = new VarValue<T>( Convert<T,std::string>::Do( v->str->Get() ) );
+ delete v;
+ }else{
+ nv = new VarValue<T>( T() );
+ }
+ v = nv;
+ var = nv;
+ InitialiseNewVarMeta(*nv, name);
+ }
+ }
+
+ Var(const std::string& name, const T& value, int flags = META_FLAG_NONE)
+ : ptr(0)
+ {
+ // Find name in VarStore
+ VarValueGeneric*& v = VarState::I()[name];
+ if(v && !v->Meta().generic) {
+ InitialiseFromGeneric(v);
+ }else{
+ // new VarValue<T> (owned by VarStore)
+ VarValue<T>* nv;
+ if(v) {
+ // Specialise generic variable
+ nv = new VarValue<T>( Convert<T,std::string>::Do( v->str->Get() ) );
+ delete v;
+ }else{
+ nv = new VarValue<T>(value);
+ }
+ v = nv;
+ var = nv;
+ InitialiseNewVarMeta(*nv, name, 0, 1, flags);
+ }
+ }
+
+ Var(const std::string& name, const T& value, bool toggle)
+ : Var(name, value, toggle ? META_FLAG_TOGGLE : META_FLAG_NONE)
+ {
+ }
+
+ Var(
+ const std::string& name, const T& value,
+ double min, double max, bool logscale = false
+ ) : ptr(0)
+ {
+ // Find name in VarStore
+ VarValueGeneric*& v = VarState::I()[name];
+ if(v && !v->Meta().generic) {
+ InitialiseFromGeneric(v);
+ }else{
+ // new VarValue<T> (owned by VarStore)
+ VarValue<T>* nv;
+ if(v) {
+ // Specialise generic variable
+ nv = new VarValue<T>( Convert<T,std::string>::Do( v->str->Get() ) );
+ delete v;
+ }else{
+ nv = new VarValue<T>(value);
+ }
+ var = nv;
+ v = nv;
+ if(logscale) {
+ if (min <= 0 || max <= 0) {
+ throw std::runtime_error("LogScale: range of numbers must be positive!");
+ }
+ InitialiseNewVarMeta(*nv, name, std::log(min), std::log(max), META_FLAG_TOGGLE, true);
+ }else{
+ InitialiseNewVarMeta(*nv, name, min, max);
+ }
+ }
+ }
+
+ void Reset()
+ {
+ var->Reset();
+ }
+
+ const T& Get() const
+ {
+ try{
+ return var->Get();
+ }catch(const BadInputException &e)
+ {
+ const_cast<Var<T> *>(this)->Reset();
+ return var->Get();
+ }
+ }
+
+ operator const T& () const
+ {
+ return Get();
+ }
+
+ const T* operator->()
+ {
+ try{
+ return &(var->Get());
+ }catch(BadInputException)
+ {
+ Reset();
+ return &(var->Get());
+ }
+ }
+
+ void operator=(const T& val)
+ {
+ var->Set(val);
+ }
+
+ void operator=(const Var<T>& v)
+ {
+ var->Set(v.var->Get());
+ }
+
+ VarMeta& Meta()
+ {
+ return var->Meta();
+ }
+
+ bool GuiChanged()
+ {
+ if(var->Meta().gui_changed) {
+ var->Meta().gui_changed = false;
+ return true;
+ }
+ return false;
+ }
+
+ VarValueT<T>& Ref()
+ {
+ return *var;
+ }
+
+protected:
+ // Initialise from existing variable, obtain data / accessor
+ void InitialiseFromGeneric(VarValueGeneric* v)
+ {
+ if( !strcmp(v->TypeId(), typeid(T).name()) ) {
+ // Same type
+ var = (VarValueT<T>*)(v);
+ }else if( std::is_same<T,std::string>::value ) {
+ // Use types string accessor
+ var = (VarValueT<T>*)(v->str);
+ }else if( !strcmp(v->TypeId(), typeid(bool).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,bool>( *(VarValueT<bool>*)v );
+ var = ptr;
+ }else if( !strcmp(v->TypeId(), typeid(short).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,short>( *(VarValueT<short>*)v );
+ var = ptr;
+ }else if( !strcmp(v->TypeId(), typeid(int).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,int>( *(VarValueT<int>*)v );
+ var = ptr;
+ }else if( !strcmp(v->TypeId(), typeid(long).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,long>( *(VarValueT<long>*)v );
+ var = ptr;
+ }else if( !strcmp(v->TypeId(), typeid(float).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,float>( *(VarValueT<float>*)v );
+ var = ptr;
+ }else if( !strcmp(v->TypeId(), typeid(double).name() ) ) {
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,double>( *(VarValueT<double>*)v );
+ var = ptr;
+ }else{
+ // other types: have to go via string
+ // Wrapper, owned by this object
+ ptr = new VarWrapper<T,std::string>( *(v->str) );
+ var = ptr;
+ }
+ }
+
+ // Holds reference to stored variable object
+ // N.B. mutable because it is a cached value and Get() is advertised as const.
+ mutable VarValueT<T>* var;
+
+ // ptr is non-zero if this object owns the object variable (a wrapper)
+ VarValueT<T>* ptr;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varextra.h b/externals/Pangolin/include/pangolin/var/varextra.h
new file mode 100644
index 0000000000000000000000000000000000000000..638826235e2e52899ce7244d5f12c670613321c9
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varextra.h
@@ -0,0 +1,92 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/platform.h>
+#include <pangolin/var/var.h>
+#include <vector>
+
+namespace pangolin
+{
+
+PANGOLIN_EXPORT
+void ParseVarsFile(const std::string& filename);
+
+PANGOLIN_EXPORT
+void LoadJsonFile(const std::string& filename, const std::string& prefix="");
+
+PANGOLIN_EXPORT
+void SaveJsonFile(const std::string& filename, const std::string& prefix="");
+
+PANGOLIN_EXPORT
+void ProcessHistoricCallbacks(NewVarCallbackFn callback, void* data, const std::string& filter = "");
+
+PANGOLIN_EXPORT
+void RegisterNewVarCallback(NewVarCallbackFn callback, void* data, const std::string& filter = "");
+
+PANGOLIN_EXPORT
+void RegisterGuiVarChangedCallback(GuiVarChangedCallbackFn callback, void* data, const std::string& filter = "");
+
+template<typename T>
+struct SetVarFunctor
+{
+ SetVarFunctor(const std::string& name, T val) : varName(name), setVal(val) {}
+ void operator()() { Var<T>(varName).Ref().Set(setVal); }
+ std::string varName;
+ T setVal;
+};
+
+struct ToggleVarFunctor
+{
+ ToggleVarFunctor(const std::string& name) : varName(name) {}
+ void operator()() { Var<bool> val(varName,false); val = !val; }
+ std::string varName;
+};
+
+inline bool Pushed(Var<bool>& button)
+{
+ bool val = button;
+ button = false;
+ return val;
+}
+
+inline bool Pushed(bool& button)
+{
+ bool val = button;
+ button = false;
+ return val;
+}
+
+template<typename T>
+inline std::ostream& operator<<(std::ostream& s, Var<T>& rhs)
+{
+ s << rhs.operator const T &();
+ return s;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varstate.h b/externals/Pangolin/include/pangolin/var/varstate.h
new file mode 100644
index 0000000000000000000000000000000000000000..90d7e2cdb182b4ddbbff8fa234d76c097748affb
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varstate.h
@@ -0,0 +1,130 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <map>
+#include <vector>
+#include <pangolin/platform.h>
+#include <pangolin/var/varvalue.h>
+#include <pangolin/utils/file_utils.h>
+
+namespace pangolin
+{
+
+typedef void (*NewVarCallbackFn)(void* data, const std::string& name, VarValueGeneric& var, bool brand_new);
+typedef void (*GuiVarChangedCallbackFn)(void* data, const std::string& name, VarValueGeneric& var);
+
+struct PANGOLIN_EXPORT NewVarCallback
+{
+ NewVarCallback(const std::string& filter, NewVarCallbackFn fn, void* data)
+ :filter(filter),fn(fn),data(data) {}
+ std::string filter;
+ NewVarCallbackFn fn;
+ void* data;
+};
+
+struct PANGOLIN_EXPORT GuiVarChangedCallback
+{
+ GuiVarChangedCallback(const std::string& filter, GuiVarChangedCallbackFn fn, void* data)
+ :filter(filter),fn(fn),data(data) {}
+ std::string filter;
+ GuiVarChangedCallbackFn fn;
+ void* data;
+};
+
+class PANGOLIN_EXPORT VarState
+{
+public:
+ static VarState& I();
+
+ VarState();
+ ~VarState();
+
+ void Clear();
+
+ template<typename T>
+ void NotifyNewVar(const std::string& name, VarValue<T>& var )
+ {
+ var_adds.push_back(name);
+
+ // notify those watching new variables
+ for(std::vector<NewVarCallback>::iterator invc = new_var_callbacks.begin(); invc != new_var_callbacks.end(); ++invc) {
+ if( StartsWith(name,invc->filter) ) {
+ invc->fn( invc->data, name, var, true);
+ }
+ }
+ }
+
+ VarValueGeneric*& operator[](const std::string& str)
+ {
+ VarStoreContainer::iterator it = vars.find(str);
+ if (it == vars.end()) {
+ vars[str] = nullptr;
+ }
+ return vars[str];
+ }
+
+ bool Exists(const std::string& str) const
+ {
+ return vars.find(str) != vars.end();
+ }
+
+ void FlagVarChanged()
+ {
+ varHasChanged = true;
+ }
+
+ bool VarHasChanged()
+ {
+ const bool has_changed = varHasChanged;
+ varHasChanged = false;
+ return has_changed;
+ }
+
+//protected:
+ typedef std::map<std::string, VarValueGeneric*> VarStoreContainer;
+ typedef std::vector<std::string> VarStoreAdditions;
+
+ VarStoreContainer vars;
+ VarStoreAdditions var_adds;
+
+ std::vector<NewVarCallback> new_var_callbacks;
+ std::vector<GuiVarChangedCallback> gui_var_changed_callbacks;
+
+ bool varHasChanged;
+};
+
+inline bool GuiVarHasChanged() {
+ return VarState::I().VarHasChanged();
+}
+
+inline void FlagVarChanged() {
+ VarState::I().FlagVarChanged();
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varvalue.h b/externals/Pangolin/include/pangolin/var/varvalue.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee3193dc2be6c8c0b8a279c4e3a762ab5977a09a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varvalue.h
@@ -0,0 +1,114 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/var/varvaluet.h>
+#include <pangolin/var/varwrapper.h>
+
+namespace pangolin
+{
+
+template<typename T>
+class VarValue : public VarValueT<typename std::remove_reference<T>::type>
+{
+public:
+ typedef typename std::remove_reference<T>::type VarT;
+
+ ~VarValue()
+ {
+ delete str_ptr;
+ }
+
+ VarValue()
+ {
+ Init();
+ }
+
+ VarValue(const T& value)
+ : value(value), default_value(value)
+ {
+ Init();
+ }
+
+ VarValue(const T& value, const VarT& default_value)
+ : value(value), default_value(default_value)
+ {
+ Init();
+ }
+
+ const char* TypeId() const
+ {
+ return typeid(VarT).name();
+ }
+
+ void Reset()
+ {
+ value = default_value;
+ }
+
+ VarMeta& Meta()
+ {
+ return meta;
+ }
+
+ const VarT& Get() const
+ {
+ return value;
+ }
+
+ VarT& Get()
+ {
+ return value;
+ }
+
+ void Set(const VarT& val)
+ {
+ value = val;
+ }
+
+protected:
+ void Init()
+ {
+ if(std::is_same<VarT,std::string>::value) {
+ str_ptr = 0;
+ this->str = (VarValueT<std::string>*)this;
+ }else{
+ str_ptr = new VarWrapper<std::string,VarT>(*this);
+ this->str = str_ptr;
+ }
+ }
+
+ // If non-zero, this class owns this str pointer in the base-class.
+ VarValueT<std::string>* str_ptr;
+
+ T value;
+ VarT default_value;
+ VarMeta meta;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varvaluegeneric.h b/externals/Pangolin/include/pangolin/var/varvaluegeneric.h
new file mode 100644
index 0000000000000000000000000000000000000000..fe81402981209ca0590df7a48aaf43473493e19f
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varvaluegeneric.h
@@ -0,0 +1,87 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <string>
+
+namespace pangolin
+{
+constexpr int META_FLAG_NONE = 0x0000;
+constexpr int META_FLAG_TOGGLE = 0x0001;
+constexpr int META_FLAG_READONLY = 0x0002;
+
+struct VarMeta
+{
+ VarMeta() :
+ increment(0.),
+ flags(META_FLAG_NONE),
+ gui_changed(false),
+ logscale(false),
+ generic(false)
+ {
+ range[0] = 0.;
+ range[1] = 0.;
+ }
+
+ std::string full_name;
+ std::string friendly;
+ double range[2];
+ double increment;
+ int flags;
+ bool gui_changed;
+ bool logscale;
+ bool generic;
+};
+
+// Forward declaration
+template<typename T>
+class VarValueT;
+
+//! Abstract base class for named Pangolin variables
+class VarValueGeneric
+{
+public:
+ VarValueGeneric()
+ : str(0)
+ {
+ }
+
+ virtual ~VarValueGeneric()
+ {
+ }
+
+ virtual const char* TypeId() const = 0;
+ virtual void Reset() = 0;
+ virtual VarMeta& Meta() = 0;
+
+//protected:
+ // String serialisation object.
+ VarValueT<std::string>* str;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varvaluet.h b/externals/Pangolin/include/pangolin/var/varvaluet.h
new file mode 100644
index 0000000000000000000000000000000000000000..5b7a45c54fdff10135d0a30e86eedf9a0e4c0b92
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varvaluet.h
@@ -0,0 +1,46 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/var/varvaluegeneric.h>
+#include <pangolin/compat/type_traits.h>
+
+namespace pangolin
+{
+
+template<typename T>
+class VarValueT : public VarValueGeneric
+{
+public:
+ typedef typename std::remove_reference<T>::type VarT;
+
+ virtual const VarT& Get() const = 0;
+ virtual void Set(const VarT& val) = 0;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/var/varwrapper.h b/externals/Pangolin/include/pangolin/var/varwrapper.h
new file mode 100644
index 0000000000000000000000000000000000000000..6033c74c9a71a351df708a9d04ddad6753bdfbf9
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/var/varwrapper.h
@@ -0,0 +1,91 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/var/varvaluegeneric.h>
+#include <pangolin/compat/type_traits.h>
+#include <pangolin/utils/type_convert.h>
+
+namespace pangolin
+{
+
+template<typename T, typename S>
+class VarWrapper : public VarValueT<T>
+{
+public:
+ typedef typename std::remove_reference<S>::type VarS;
+
+ VarWrapper(VarValueT<S>& src)
+ : src(src)
+ {
+ this->str = src.str;
+ }
+
+ const char* TypeId() const
+ {
+ return typeid(T).name();
+ }
+
+ void Reset()
+ {
+ src.Reset();
+
+ // If reset throws, it will go up to the user, since there is nothing
+ // more we can do
+ cache = Convert<T,VarS>::Do(src.Get());
+ }
+
+ VarMeta& Meta()
+ {
+ return src.Meta();
+ }
+
+ const T& Get() const
+ {
+ // This might throw, but we can't reset because this is a const method
+ cache = Convert<T,VarS>::Do(src.Get());
+ return cache;
+ }
+
+ void Set(const T& val)
+ {
+ cache = val;
+ try {
+ src.Set( Convert<VarS, T>::Do(val) );
+ }catch(const BadInputException &e) {
+ pango_print_warn("Unable to set variable with type %s from %s. Resetting.", typeid(VarS).name(), typeid(T).name() );
+ Reset();
+ }
+ }
+
+protected:
+ mutable T cache;
+ VarValueT<S>& src;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/debayer.h b/externals/Pangolin/include/pangolin/video/drivers/debayer.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d4d9bfc40610fdbb4072630fee1d38d6ef2f382
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/debayer.h
@@ -0,0 +1,116 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+// Enum to match libdc1394's dc1394_bayer_method_t
+typedef enum {
+ BAYER_METHOD_NEAREST = 0,
+ BAYER_METHOD_SIMPLE,
+ BAYER_METHOD_BILINEAR,
+ BAYER_METHOD_HQLINEAR,
+ BAYER_METHOD_DOWNSAMPLE_,
+ BAYER_METHOD_EDGESENSE,
+ BAYER_METHOD_VNG,
+ BAYER_METHOD_AHD,
+
+ // Pangolin custom defines
+ BAYER_METHOD_NONE = 512,
+ BAYER_METHOD_DOWNSAMPLE,
+ BAYER_METHOD_DOWNSAMPLE_MONO
+} bayer_method_t;
+
+// Enum to match libdc1394's dc1394_color_filter_t
+typedef enum {
+ DC1394_COLOR_FILTER_RGGB = 512,
+ DC1394_COLOR_FILTER_GBRG,
+ DC1394_COLOR_FILTER_GRBG,
+ DC1394_COLOR_FILTER_BGGR
+} color_filter_t;
+
+// Video class that debayers its video input using the given method.
+class PANGOLIN_EXPORT DebayerVideo :
+ public VideoInterface,
+ public VideoFilterInterface,
+ public BufferAwareVideoInterface
+{
+public:
+ DebayerVideo(std::unique_ptr<VideoInterface>& videoin, const std::vector<bayer_method_t> &method, color_filter_t tile);
+ ~DebayerVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::vector<VideoInterface*>& InputStreams();
+
+ static color_filter_t ColorFilterFromString(std::string str);
+
+ static bayer_method_t BayerMethodFromString(std::string str);
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+protected:
+ void ProcessStreams(unsigned char* out, const unsigned char* in);
+
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+
+ size_t size_bytes;
+ std::unique_ptr<unsigned char[]> buffer;
+
+ std::vector<bayer_method_t> methods;
+ color_filter_t tile;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/deinterlace.h b/externals/Pangolin/include/pangolin/video/drivers/deinterlace.h
new file mode 100644
index 0000000000000000000000000000000000000000..4098ef34e086b734274414e7b062d09283d942eb
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/deinterlace.h
@@ -0,0 +1,62 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/video.h>
+#include <vector>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT DeinterlaceVideo
+ : public VideoInterface
+{
+public:
+ DeinterlaceVideo(std::unique_ptr<VideoInterface>& videoin);
+ ~DeinterlaceVideo();
+
+ size_t SizeBytes() const;
+
+ const std::vector<StreamInfo>& Streams() const;
+
+ void Start();
+
+ void Stop();
+
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+protected:
+ std::unique_ptr<VideoInterface> videoin;
+ std::vector<StreamInfo> streams;
+ unsigned char* buffer;
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/depthsense.h b/externals/Pangolin/include/pangolin/video/drivers/depthsense.h
new file mode 100644
index 0000000000000000000000000000000000000000..13a90c979cf560cba70769a830bf26bcc8ff9b99
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/depthsense.h
@@ -0,0 +1,171 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/iostream_operators.h>
+#include <pangolin/video/video.h>
+
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+
+// DepthSense SDK for SoftKinetic cameras from Creative
+#include <DepthSense.hxx>
+
+namespace pangolin
+{
+
+enum DepthSenseSensorType
+{
+ DepthSenseUnassigned = -1,
+ DepthSenseRgb = 0,
+ DepthSenseDepth
+};
+
+// Video class that outputs test video signal.
+class PANGOLIN_EXPORT DepthSenseVideo :
+ public VideoInterface, public VideoPropertiesInterface
+{
+public:
+ DepthSenseVideo(DepthSense::Device device, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri);
+ ~DepthSenseVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::DeviceProperties()
+ const picojson::value& DeviceProperties() const {
+ return device_properties;
+ }
+
+ //! Implement VideoInput::DeviceProperties()
+ const picojson::value& FrameProperties() const {
+ return frame_properties;
+ }
+protected:
+ void onNewColorSample(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data);
+ void onNewDepthSample(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data);
+
+ struct SensorConfig
+ {
+ DepthSenseSensorType type;
+ ImageDim dim;
+ unsigned int fps;
+ };
+
+ void UpdateParameters(const DepthSense::Node& node, const Uri& uri);
+ void ConfigureNodes(const Uri& uri);
+ void ConfigureDepthNode(const SensorConfig& sensorConfig, const Uri& uri);
+ void ConfigureColorNode(const SensorConfig& sensorConfig, const Uri& uri);
+
+ double GetDeltaTime() const;
+
+ std::vector<StreamInfo> streams;
+ picojson::value device_properties;
+ picojson::value frame_properties;
+ picojson::value* streams_properties;
+
+
+ DepthSense::Device device;
+ DepthSense::DepthNode g_dnode;
+ DepthSense::ColorNode g_cnode;
+
+ unsigned char* fill_image;
+
+ int depthmap_stream;
+ int rgb_stream;
+
+ int gotDepth;
+ int gotColor;
+ std::mutex update_mutex;
+ std::condition_variable cond_image_filled;
+ std::condition_variable cond_image_requested;
+
+ SensorConfig sensorConfig[2];
+
+ bool enableDepth;
+ bool enableColor;
+ double depthTs;
+ double colorTs;
+
+ size_t size_bytes;
+};
+
+class DepthSenseContext
+{
+ friend class DepthSenseVideo;
+
+public:
+ // Singleton Instance
+ static DepthSenseContext& I();
+
+ DepthSenseVideo* GetDepthSenseVideo(size_t device_num, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri);
+
+protected:
+ // Protected Constructor
+ DepthSenseContext();
+ ~DepthSenseContext();
+
+ DepthSense::Context& Context();
+
+ void NewDeviceRunning();
+ void DeviceClosing();
+
+ void StartNodes();
+ void StopNodes();
+
+ void EventLoop();
+
+ void onDeviceConnected(DepthSense::Context context, DepthSense::Context::DeviceAddedData data);
+ void onDeviceDisconnected(DepthSense::Context context, DepthSense::Context::DeviceRemovedData data);
+
+ DepthSense::Context g_context;
+
+ std::thread event_thread;
+ bool is_running;
+
+ int running_devices;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/ffmpeg.h b/externals/Pangolin/include/pangolin/video/drivers/ffmpeg.h
new file mode 100644
index 0000000000000000000000000000000000000000..8f0e67282607c69999e47141d384f429236d83a5
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/ffmpeg.h
@@ -0,0 +1,206 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+extern "C"
+{
+
+// HACK for some versions of FFMPEG
+#ifndef INT64_C
+#define INT64_C(c) (c ## LL)
+#define UINT64_C(c) (c ## ULL)
+#endif
+
+#include <libavformat/avformat.h>
+#include <libswscale/swscale.h>
+#include <libavutil/pixdesc.h>
+#include <libavcodec/avcodec.h>
+}
+
+#ifndef HAVE_FFMPEG_AVPIXELFORMAT
+# define AVPixelFormat PixelFormat
+#endif
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT FfmpegVideo : public VideoInterface
+{
+public:
+ FfmpegVideo(const std::string filename, const std::string fmtout = "RGB24", const std::string codec_hint = "", bool dump_info = false, int user_video_stream = -1, ImageDim size = ImageDim(0,0));
+ ~FfmpegVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+protected:
+ void InitUrl(const std::string filename, const std::string fmtout = "RGB24", const std::string codec_hint = "", bool dump_info = false , int user_video_stream = -1, ImageDim size= ImageDim(0,0));
+
+ std::vector<StreamInfo> streams;
+
+ SwsContext *img_convert_ctx;
+ AVFormatContext *pFormatCtx;
+ int videoStream;
+ int audioStream;
+ AVCodecContext *pVidCodecCtx;
+ AVCodecContext *pAudCodecCtx;
+ AVCodec *pVidCodec;
+ AVCodec *pAudCodec;
+ AVFrame *pFrame;
+ AVFrame *pFrameOut;
+ AVPacket packet;
+ int numBytesOut;
+ uint8_t *buffer;
+ AVPixelFormat fmtout;
+};
+
+enum FfmpegMethod
+{
+ FFMPEG_FAST_BILINEAR = 1,
+ FFMPEG_BILINEAR = 2,
+ FFMPEG_BICUBIC = 4,
+ FFMPEG_X = 8,
+ FFMPEG_POINT = 0x10,
+ FFMPEG_AREA = 0x20,
+ FFMPEG_BICUBLIN = 0x40,
+ FFMPEG_GAUSS = 0x80,
+ FFMPEG_SINC =0x100,
+ FFMPEG_LANCZOS =0x200,
+ FFMPEG_SPLINE =0x400
+};
+
+class PANGOLIN_EXPORT FfmpegConverter : public VideoInterface
+{
+public:
+ FfmpegConverter(std::unique_ptr<VideoInterface>& videoin, const std::string pixelfmtout = "RGB24", FfmpegMethod method = FFMPEG_POINT);
+ ~FfmpegConverter();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+protected:
+ std::vector<StreamInfo> streams;
+
+ struct ConvertContext
+ {
+ SwsContext* img_convert_ctx;
+ AVPixelFormat fmtsrc;
+ AVPixelFormat fmtdst;
+ AVFrame* avsrc;
+ AVFrame* avdst;
+ size_t w,h;
+ size_t src_buffer_offset;
+ size_t dst_buffer_offset;
+
+ void convert(const unsigned char * src, unsigned char* dst);
+
+ };
+
+ std::unique_ptr<VideoInterface> videoin;
+ std::unique_ptr<unsigned char[]> input_buffer;
+
+ std::vector<ConvertContext> converters;
+ //size_t src_buffer_size;
+ size_t dst_buffer_size;
+};
+
+#if (LIBAVFORMAT_VERSION_MAJOR > 55) || ((LIBAVFORMAT_VERSION_MAJOR == 55) && (LIBAVFORMAT_VERSION_MINOR >= 7))
+typedef AVCodecID CodecID;
+#endif
+
+// Forward declaration
+class FfmpegVideoOutputStream;
+
+class PANGOLIN_EXPORT FfmpegVideoOutput
+ : public VideoOutputInterface
+{
+ friend class FfmpegVideoOutputStream;
+public:
+ FfmpegVideoOutput( const std::string& filename, int base_frame_rate, int bit_rate );
+ ~FfmpegVideoOutput();
+
+ const std::vector<StreamInfo>& Streams() const override;
+
+ void SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri, const picojson::value& properties) override;
+
+ int WriteStreams(const unsigned char* data, const picojson::value& frame_properties) override;
+
+ bool IsPipe() const override;
+
+protected:
+ void Initialise(std::string filename);
+ void StartStream();
+ void Close();
+
+ std::string filename;
+ bool started;
+ AVFormatContext *oc;
+ std::vector<FfmpegVideoOutputStream*> streams;
+ std::vector<StreamInfo> strs;
+
+ int frame_count;
+
+ int base_frame_rate;
+ int bit_rate;
+ bool is_pipe;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/firewire.h b/externals/Pangolin/include/pangolin/video/drivers/firewire.h
new file mode 100644
index 0000000000000000000000000000000000000000..a631a220b405e6888012b9e6aea39a1a3631588c
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/firewire.h
@@ -0,0 +1,254 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+#include <dc1394/dc1394.h>
+
+#ifndef _WIN32
+#include <unistd.h>
+#endif
+
+
+
+namespace pangolin
+{
+
+PANGOLIN_EXPORT
+std::string Dc1394ColorCodingToString(dc1394color_coding_t coding);
+
+PANGOLIN_EXPORT
+dc1394color_coding_t Dc1394ColorCodingFromString(std::string coding);
+
+PANGOLIN_EXPORT
+void Dc1394ModeDetails(dc1394video_mode_t mode, unsigned& w, unsigned& h, std::string& format );
+
+class PANGOLIN_EXPORT FirewireFrame
+{
+ friend class FirewireVideo;
+public:
+ bool isValid() { return frame; }
+ unsigned char* Image() { return frame ? frame->image : 0; }
+ unsigned Width() const { return frame ? frame->size[0] : 0; }
+ unsigned Height() const { return frame ? frame->size[1] : 0; }
+ unsigned ImageBytes() const { return frame ? frame->image_bytes : 0; }
+ int FramesBehind() const { return frame ? frame->frames_behind : -1; }
+ unsigned Timestamp() const { return frame ? frame->timestamp : 0; }
+ int Id() const { return frame ? frame->id : -1; }
+protected:
+ FirewireFrame(dc1394video_frame_t* frame) : frame(frame) {}
+ dc1394video_frame_t *frame;
+};
+
+struct PANGOLIN_EXPORT Guid
+{
+ Guid(uint64_t guid):guid(guid){}
+ uint64_t guid;
+};
+
+class PANGOLIN_EXPORT FirewireVideo : public VideoInterface
+{
+public:
+ const static int MAX_FR = -1;
+ const static int EXT_TRIG = -1;
+ const static uint32_t GPIO_CTRL_PIN0 = 0x1110;
+ const static uint32_t GPIO_CTRL_PIN1 = 0x1120;
+ const static uint32_t GPIO_CTRL_PIN2 = 0x1130;
+ const static uint32_t GPIO_CTRL_PIN3 = 0x1140;
+
+ FirewireVideo(
+ unsigned deviceid = 0,
+ dc1394video_mode_t video_mode = DC1394_VIDEO_MODE_640x480_RGB8,
+ dc1394framerate_t framerate = DC1394_FRAMERATE_30,
+ dc1394speed_t iso_speed = DC1394_ISO_SPEED_400,
+ int dma_buffers = 10
+ );
+
+ FirewireVideo(
+ Guid guid,
+ dc1394video_mode_t video_mode = DC1394_VIDEO_MODE_640x480_RGB8,
+ dc1394framerate_t framerate = DC1394_FRAMERATE_30,
+ dc1394speed_t iso_speed = DC1394_ISO_SPEED_400,
+ int dma_buffers = 10
+ );
+
+ FirewireVideo(
+ Guid guid,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top,
+ dc1394speed_t iso_speed,
+ int dma_buffers, bool reset_at_boot=false
+ );
+
+ FirewireVideo(
+ unsigned deviceid,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top,
+ dc1394speed_t iso_speed,
+ int dma_buffers, bool reset_at_boot=false
+ );
+
+ ~FirewireVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! (deprecated: use Streams[i].Width())
+ //! Return image width
+ unsigned Width() const { return width; }
+
+ //! (deprecated: use Streams[i].Height())
+ //! Return image height
+ unsigned Height() const { return height; }
+
+ //! Return object containing reference to image data within
+ //! DMA buffer. The FirewireFrame must be returned to
+ //! signal that it can be reused with a corresponding PutFrame()
+ FirewireFrame GetNext(bool wait = true);
+
+ //! Return object containing reference to newest image data within
+ //! DMA buffer discarding old images. The FirewireFrame must be
+ //! returned to signal that it can be reused with a corresponding PutFrame()
+ FirewireFrame GetNewest(bool wait = true);
+
+ //! Return FirewireFrame object. Data held by FirewireFrame is
+ //! invalidated on return.
+ void PutFrame(FirewireFrame& frame);
+
+ //! return absolute shutter value
+ float GetShutterTime() const;
+
+ //! set absolute shutter value
+ void SetShutterTime(float val);
+
+ //! set auto shutter value
+ void SetAutoShutterTime();
+
+ //! return absolute gain value
+ float GetGain() const;
+
+ //! set absolute gain value
+ void SetGain(float val);
+
+ //! set auto gain value
+ void SetAutoGain();
+
+ //! return absolute brightness value
+ float GetBrightness() const;
+
+ //! set absolute brightness value
+ void SetBrightness(float val);
+
+ //! set auto brightness
+ void SetAutoBrightness();
+
+ //! return absolute gamma value
+ float GetGamma() const;
+
+ //! return quantised shutter value
+ void SetShutterTimeQuant(int shutter);
+
+ //! set the trigger to internal, i.e. determined by video mode
+ void SetInternalTrigger();
+
+ //! set the trigger to external
+ void SetExternalTrigger(
+ dc1394trigger_mode_t mode=DC1394_TRIGGER_MODE_0,
+ dc1394trigger_polarity_t polarity=DC1394_TRIGGER_ACTIVE_HIGH,
+ dc1394trigger_source_t source=DC1394_TRIGGER_SOURCE_0
+ );
+
+ //! set a camera register
+ void SetRegister(uint64_t offset, uint32_t value);
+
+ //! read camera register
+ uint32_t GetRegister(uint64_t offset);
+
+ //! set a camera control register
+ void SetControlRegister(uint64_t offset, uint32_t value);
+
+ //! read camera control register
+ uint32_t GetControlRegister(uint64_t offset);
+
+protected:
+ void init_stream_info();
+
+ void init_camera(
+ uint64_t guid, int dma_frames,
+ dc1394speed_t iso_speed,
+ dc1394video_mode_t video_mode,
+ dc1394framerate_t framerate
+ );
+
+ void init_format7_camera(
+ uint64_t guid, int dma_frames,
+ dc1394speed_t iso_speed,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top, bool reset_at_boot
+ );
+
+ static int nearest_value(int value, int step, int min, int max);
+ static double bus_period_from_iso_speed(dc1394speed_t iso_speed);
+
+ size_t frame_size_bytes;
+ std::vector<StreamInfo> streams;
+
+ bool running;
+ dc1394camera_t *camera;
+ unsigned width, height, top, left;
+ //dc1394featureset_t features;
+ dc1394_t * d;
+ dc1394camera_list_t * list;
+ mutable dc1394error_t err;
+
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/images.h b/externals/Pangolin/include/pangolin/video/drivers/images.h
new file mode 100644
index 0000000000000000000000000000000000000000..2ba4e0ef2647e56a9708198526b6097035b24f6d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/images.h
@@ -0,0 +1,121 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/image/image_io.h>
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+#include <deque>
+#include <vector>
+
+namespace pangolin
+{
+
+// Video class that outputs test video signal.
+class PANGOLIN_EXPORT ImagesVideo : public VideoInterface, public VideoPlaybackInterface, public VideoPropertiesInterface
+{
+public:
+ ImagesVideo(const std::string& wildcard_path);
+ ImagesVideo(const std::string& wildcard_path, const PixelFormat& raw_fmt, size_t raw_width, size_t raw_height);
+
+ // Explicitly delete copy ctor and assignment operator.
+ // See http://stackoverflow.com/questions/29565299/how-to-use-a-vector-of-unique-pointers-in-a-dll-exported-class-with-visual-studi
+ // >> It appears adding __declspec(dllexport) forces the compiler to define the implicitly-declared copy constructor and copy assignment operator
+ ImagesVideo(const ImagesVideo&) = delete;
+ ImagesVideo& operator=(const ImagesVideo&) = delete;
+
+ ~ImagesVideo();
+
+ ///////////////////////////////////
+ // Implement VideoInterface
+
+ void Start() override;
+
+ void Stop() override;
+
+ size_t SizeBytes() const override;
+
+ const std::vector<StreamInfo>& Streams() const override;
+
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ ///////////////////////////////////
+ // Implement VideoPlaybackInterface
+
+ size_t GetCurrentFrameId() const override;
+
+ size_t GetTotalFrames() const override;
+
+ size_t Seek(size_t frameid) override;
+
+ ///////////////////////////////////
+ // Implement VideoPropertiesInterface
+
+ const picojson::value& DeviceProperties() const override;
+
+ const picojson::value& FrameProperties() const override;
+
+protected:
+ typedef std::vector<TypedImage> Frame;
+
+ const std::string& Filename(size_t frameNum, size_t channelNum) {
+ return filenames[channelNum][frameNum];
+ }
+
+ void PopulateFilenames(const std::string& wildcard_path);
+
+ void PopulateFilenamesFromJson(const std::string& filename);
+
+ bool LoadFrame(size_t i);
+
+ void ConfigureStreamSizes();
+
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+
+ size_t num_files;
+ size_t num_channels;
+ size_t next_frame_id;
+ std::vector<std::vector<std::string> > filenames;
+ std::vector<Frame> loaded;
+
+ bool unknowns_are_raw;
+ PixelFormat raw_fmt;
+ size_t raw_width;
+ size_t raw_height;
+
+ // Load any json properties if they are defined
+ picojson::value device_properties;
+ picojson::value json_frames;
+ picojson::value null_props;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/images_out.h b/externals/Pangolin/include/pangolin/video/drivers/images_out.h
new file mode 100644
index 0000000000000000000000000000000000000000..ad6f662bcc42050d5a0fad278dcfb95cc2f45387
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/images_out.h
@@ -0,0 +1,60 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <fstream>
+#include <pangolin/video/video_output.h>
+#include <pangolin/log/packetstream_writer.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT ImagesVideoOutput : public VideoOutputInterface
+{
+public:
+ ImagesVideoOutput(const std::string& image_folder, const std::string& json_file_out, const std::string &image_file_extension);
+ ~ImagesVideoOutput();
+
+ const std::vector<StreamInfo>& Streams() const override;
+ void SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri, const picojson::value& device_properties) override;
+ int WriteStreams(const unsigned char* data, const picojson::value& frame_properties) override;
+ bool IsPipe() const override;
+
+protected:
+ std::vector<StreamInfo> streams;
+ std::string input_uri;
+ picojson::value device_properties;
+ picojson::value json_frames;
+
+ size_t image_index;
+ std::string image_folder;
+ std::string image_file_extension;
+ std::ofstream file;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/join.h b/externals/Pangolin/include/pangolin/video/drivers/join.h
new file mode 100644
index 0000000000000000000000000000000000000000..8eb383da4fc90040cc85c342aa271053c3298775
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/join.h
@@ -0,0 +1,78 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT JoinVideo
+ : public VideoInterface, public VideoFilterInterface
+{
+public:
+ JoinVideo(std::vector<std::unique_ptr<VideoInterface>> &src);
+
+ ~JoinVideo();
+
+ // Explicitly delete copy ctor and assignment operator.
+ // See http://stackoverflow.com/questions/29565299/how-to-use-a-vector-of-unique-pointers-in-a-dll-exported-class-with-visual-studi
+ // >> It appears adding __declspec(dllexport) forces the compiler to define the implicitly-declared copy constructor and copy assignment operator
+ JoinVideo(const JoinVideo&) = delete;
+ JoinVideo& operator=(const JoinVideo&) = delete;
+
+ size_t SizeBytes() const;
+
+ const std::vector<StreamInfo>& Streams() const;
+
+ void Start();
+
+ void Stop();
+
+ bool Sync(int64_t tolerance_us, double transfer_bandwidth_gbps = 0);
+
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::vector<VideoInterface*>& InputStreams();
+
+protected:
+ int64_t GetAdjustedCaptureTime(size_t src_index);
+
+ std::vector<std::unique_ptr<VideoInterface>> storage;
+ std::vector<VideoInterface*> src;
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+
+ int64_t sync_tolerance_us;
+ int64_t transfer_bandwidth_bytes_per_us;
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/merge.h b/externals/Pangolin/include/pangolin/video/drivers/merge.h
new file mode 100644
index 0000000000000000000000000000000000000000..fd3cc0716b339acc50475ba4161267a009707318
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/merge.h
@@ -0,0 +1,70 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+// Take N streams, and place them into one big buffer.
+class PANGOLIN_EXPORT MergeVideo : public VideoInterface, public VideoFilterInterface
+{
+public:
+ MergeVideo(std::unique_ptr<VideoInterface>& src, const std::vector<Point>& stream_pos, size_t w, size_t h);
+ ~MergeVideo();
+
+ void Start() override;
+
+ void Stop() override;
+
+ size_t SizeBytes() const override;
+
+ const std::vector<StreamInfo>& Streams() const override;
+
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ std::vector<VideoInterface*>& InputStreams() override;
+
+protected:
+ void CopyBuffer(unsigned char* dst_bytes, unsigned char* src_bytes);
+
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::unique_ptr<uint8_t[]> buffer;
+ std::vector<Point> stream_pos;
+
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/mirror.h b/externals/Pangolin/include/pangolin/video/drivers/mirror.h
new file mode 100644
index 0000000000000000000000000000000000000000..5d04054695b7235673a3b9946ffefd5af4f6c882
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/mirror.h
@@ -0,0 +1,96 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+enum MirrorOptions
+{
+ MirrorOptionsNone = 0,
+ MirrorOptionsFlipX,
+ MirrorOptionsFlipY,
+ MirrorOptionsFlipXY,
+ MirrorOptionsTranspose,
+ MirrorOptionsRotateCW,
+ MirrorOptionsRotateCCW,
+};
+
+// Video class that debayers its video input using the given method.
+class PANGOLIN_EXPORT MirrorVideo :
+ public VideoInterface,
+ public VideoFilterInterface,
+ public BufferAwareVideoInterface
+{
+public:
+ MirrorVideo(std::unique_ptr<VideoInterface>& videoin, const std::vector<MirrorOptions>& flips);
+ ~MirrorVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoFilterInterface method
+ std::vector<VideoInterface*>& InputStreams();
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+protected:
+ void Process(unsigned char* image, const unsigned char* buffer);
+
+ std::unique_ptr<VideoInterface> videoin;
+ std::vector<VideoInterface*> inputs;
+ std::vector<StreamInfo> streams;
+ std::vector<MirrorOptions> flips;
+ size_t size_bytes;
+ unsigned char* buffer;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/openni.h b/externals/Pangolin/include/pangolin/video/drivers/openni.h
new file mode 100644
index 0000000000000000000000000000000000000000..195f1d9b2bf9e2064514700167d67d2700f60893
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/openni.h
@@ -0,0 +1,72 @@
+#pragma once
+
+#include <pangolin/pangolin.h>
+
+#include <pangolin/video/video.h>
+#include <pangolin/video/drivers/openni_common.h>
+
+// Workaround poor OpenNI Platform test on Linux before including XnCppWrapper.h
+// See https://github.com/dennishamester/OpenNI/commit/ca99f6181234c682bba42a6ba
+#ifdef _LINUX_
+#define linux 1
+#endif // _LINUX_
+
+// OpenNI generates SO MANY warnings, we'll just disable all for this header(!)
+// GCC and clang will listen to this pramga.
+#ifndef _MSVC_
+#pragma GCC system_header
+#endif
+#include <XnCppWrapper.h>
+
+namespace pangolin
+{
+
+//! Interface to video capture sources
+struct PANGOLIN_EXPORT OpenNiVideo : public VideoInterface
+{
+public:
+ OpenNiVideo(OpenNiSensorType s1, OpenNiSensorType s2, ImageDim dim = ImageDim(640,480), int fps = 30);
+ ~OpenNiVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ void SetAutoExposure(bool enabled)
+ {
+#if XN_MINOR_VERSION > 5 || (XN_MINOR_VERSION == 5 && XN_BUILD_VERSION >= 7)
+ if(imageNode.IsValid()) {
+ imageNode.GetAutoExposureCap().Set(enabled ? 1 : 0);
+ }
+#else
+ throw pangolin::VideoException("SetAutoExposure Not supported for this version of OpenNI.");
+#endif
+ }
+
+protected:
+ std::vector<StreamInfo> streams;
+ OpenNiSensorType sensor_type[2];
+
+ xn::Context context;
+ xn::DepthGenerator depthNode;
+ xn::ImageGenerator imageNode;
+ xn::IRGenerator irNode;
+
+ size_t sizeBytes;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/openni2.h b/externals/Pangolin/include/pangolin/video/drivers/openni2.h
new file mode 100644
index 0000000000000000000000000000000000000000..d3874aa8c21db822cda711e3237489adb8ad9749
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/openni2.h
@@ -0,0 +1,147 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Richard Newcombe
+ * 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+
+#include <pangolin/video/video.h>
+#include <pangolin/video/drivers/openni_common.h>
+
+#include <OpenNI.h>
+
+namespace pangolin
+{
+const int MAX_OPENNI2_STREAMS = 2 * ONI_MAX_SENSORS;
+
+//! Interface to video capture sources
+struct OpenNi2Video : public VideoInterface, public VideoPropertiesInterface, public VideoPlaybackInterface
+{
+public:
+
+ // Open all RGB and Depth streams from all devices
+ OpenNi2Video(ImageDim dim=ImageDim(640,480), ImageRoi roi=ImageRoi(0,0,0,0), int fps=30);
+
+ // Open streams specified
+ OpenNi2Video(std::vector<OpenNiStreamMode>& stream_modes);
+
+ // Open openni file
+ OpenNi2Video(const std::string& filename);
+
+ // Open openni file with certain params
+ OpenNi2Video(const std::string& filename, std::vector<OpenNiStreamMode>& stream_modes);
+
+ void UpdateProperties();
+
+ void SetMirroring(bool enable);
+ void SetAutoExposure(bool enable);
+ void SetAutoWhiteBalance(bool enable);
+ void SetDepthCloseRange(bool enable);
+ void SetDepthHoleFilter(bool enable);
+ void SetDepthColorSyncEnabled(bool enable);
+ void SetFastCrop(bool enable);
+ void SetRegisterDepthToImage(bool enable);
+ void SetPlaybackSpeed(float speed);
+ void SetPlaybackRepeat(bool enabled);
+
+ ~OpenNi2Video();
+
+ //! Implement VideoInput::Start()
+ void Start() override;
+
+ //! Implement VideoInput::Stop()
+ void Stop() override;
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const override;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const override;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& DeviceProperties() const override{
+ return device_properties;
+ }
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& FrameProperties() const override{
+ return frame_properties;
+ }
+
+ //! Implement VideoPlaybackInterface::GetCurrentFrameId
+ size_t GetCurrentFrameId() const override;
+
+ //! Implement VideoPlaybackInterface::GetTotalFrames
+ size_t GetTotalFrames() const override;
+
+ //! Implement VideoPlaybackInterface::Seek
+ size_t Seek(size_t frameid) override;
+
+ openni::VideoStream* GetVideoStream(int stream);
+
+protected:
+ void InitialiseOpenNI();
+ int AddDevice(const std::string& device_uri);
+ void AddStream(const OpenNiStreamMode& mode);
+ void SetupStreamModes();
+ void PrintOpenNI2Modes(openni::SensorType sensorType);
+ openni::VideoMode FindOpenNI2Mode(openni::Device &device, openni::SensorType sensorType, int width, int height, int fps, openni::PixelFormat fmt );
+
+ size_t numDevices;
+ size_t numStreams;
+
+ openni::Device devices[ONI_MAX_SENSORS];
+ OpenNiStreamMode sensor_type[ONI_MAX_SENSORS];
+
+ openni::VideoStream video_stream[ONI_MAX_SENSORS];
+ openni::VideoFrameRef video_frame[ONI_MAX_SENSORS];
+
+ std::vector<StreamInfo> streams;
+ size_t sizeBytes;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+ picojson::value* streams_properties;
+
+ bool use_depth;
+ bool use_ir;
+ bool use_rgb;
+ bool depth_to_color;
+ bool use_ir_and_rgb;
+
+ size_t current_frame_index;
+ size_t total_frames;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/openni_common.h b/externals/Pangolin/include/pangolin/video/drivers/openni_common.h
new file mode 100644
index 0000000000000000000000000000000000000000..c93183ee11a47b001ce5507dba734324787d3e41
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/openni_common.h
@@ -0,0 +1,153 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ * 2015 Richard Newcombe
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+enum OpenNiSensorType
+{
+ OpenNiUnassigned = -1,
+ OpenNiRgb = 0,
+ OpenNiIr,
+ OpenNiDepth_1mm,
+ OpenNiDepth_1mm_Registered,
+ OpenNiDepth_100um,
+ OpenNiIr8bit,
+ OpenNiIr24bit,
+ OpenNiIrProj,
+ OpenNiIr8bitProj,
+ OpenNiGrey
+};
+
+struct PANGOLIN_EXPORT OpenNiStreamMode
+{
+ OpenNiStreamMode(
+ OpenNiSensorType sensor_type=OpenNiUnassigned,
+ ImageDim dim=ImageDim(640,480), ImageRoi roi=ImageRoi(0,0,0,0), int fps=30, int device=0
+ )
+ : sensor_type(sensor_type), dim(dim), roi(roi), fps(fps), device(device)
+ {
+
+ }
+
+ OpenNiSensorType sensor_type;
+ ImageDim dim;
+ ImageRoi roi;
+ int fps;
+ int device;
+};
+
+inline OpenNiSensorType openni_sensor(const std::string& str)
+{
+ if( !str.compare("grey") || !str.compare("gray") ) {
+ return OpenNiGrey;
+ }else if( !str.compare("rgb") ) {
+ return OpenNiRgb;
+ }else if( !str.compare("ir") ) {
+ return OpenNiIr;
+ }else if( !str.compare("depth1mm") || !str.compare("depth") ) {
+ return OpenNiDepth_1mm;
+ }else if( !str.compare("depth100um") ) {
+ return OpenNiDepth_100um;
+ }else if( !str.compare("depth_reg") || !str.compare("reg_depth")) {
+ return OpenNiDepth_1mm_Registered;
+ }else if( !str.compare("ir8") ) {
+ return OpenNiIr8bit;
+ }else if( !str.compare("ir24") ) {
+ return OpenNiIr24bit;
+ }else if( !str.compare("ir+") ) {
+ return OpenNiIrProj;
+ }else if( !str.compare("ir8+") ) {
+ return OpenNiIr8bitProj;
+ }else if( str.empty() ) {
+ return OpenNiUnassigned;
+ }else{
+ throw pangolin::VideoException("Unknown OpenNi sensor", str );
+ }
+}
+
+// Find prefix character key
+// Given arguments "depth!5:320x240@15", "!:@", would return map
+// \0->"depth", !->"5", :->"320x240", @->"15"
+inline std::map<char,std::string> GetTokenSplits(const std::string& str, const std::string& tokens)
+{
+ std::map<char,std::string> splits;
+
+ char last_token = 0;
+ size_t last_start = 0;
+ for(unsigned int i=0; i<str.size(); ++i) {
+ size_t token_pos = tokens.find(str[i]);
+ if(token_pos != std::string::npos) {
+ splits[last_token] = str.substr(last_start, i-last_start);
+ last_token = tokens[token_pos];
+ last_start = i+1;
+ }
+ }
+
+ if(last_start < str.size()) {
+ splits[last_token] = str.substr(last_start);
+ }
+
+ return splits;
+}
+
+inline std::istream& operator>> (std::istream &is, OpenNiStreamMode& fmt)
+{
+ std::string str;
+ is >> str;
+
+ std::map<char,std::string> splits = GetTokenSplits(str, "!:@#");
+
+ if(splits.count(0)) {
+ fmt.sensor_type = openni_sensor(splits[0]);
+ }
+
+ if(splits.count('@')) {
+ fmt.fps = pangolin::Convert<int,std::string>::Do(splits['@']);
+ }
+
+ if(splits.count(':')) {
+ fmt.dim = pangolin::Convert<ImageDim,std::string>::Do(splits[':']);
+ }
+
+ if(splits.count('!')) {
+ fmt.device = pangolin::Convert<int,std::string>::Do(splits['!']);
+ }
+
+ if(splits.count('#')) {
+ fmt.roi = pangolin::Convert<ImageRoi,std::string>::Do(splits['#']);
+ }
+
+ return is;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/pack.h b/externals/Pangolin/include/pangolin/video/drivers/pack.h
new file mode 100644
index 0000000000000000000000000000000000000000..3bfb1f87ca0ab1c4152ae5e34f2ae10e062ba5ac
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/pack.h
@@ -0,0 +1,84 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+// Video class that packs its video input using the given method.
+class PANGOLIN_EXPORT PackVideo :
+ public VideoInterface,
+ public VideoFilterInterface,
+ public BufferAwareVideoInterface
+{
+public:
+ PackVideo(std::unique_ptr<VideoInterface>& videoin, PixelFormat new_fmt);
+ ~PackVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoFilterInterface method
+ std::vector<VideoInterface*>& InputStreams();
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+protected:
+ void Process(unsigned char* image, const unsigned char* buffer);
+
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+ unsigned char* buffer;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/pango.h b/externals/Pangolin/include/pangolin/video/drivers/pango.h
new file mode 100644
index 0000000000000000000000000000000000000000..f6fecbe93406a8686ba3b465ea949c118e397171
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/pango.h
@@ -0,0 +1,104 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/log/packetstream_reader.h>
+#include <pangolin/log/playback_session.h>
+#include <pangolin/video/stream_encoder_factory.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT PangoVideo
+ : public VideoInterface, public VideoPropertiesInterface, public VideoPlaybackInterface
+{
+public:
+ PangoVideo(const std::string& filename, std::shared_ptr<PlaybackSession> playback_session);
+ ~PangoVideo();
+
+ // Implement VideoInterface
+
+ size_t SizeBytes() const override;
+
+ const std::vector<StreamInfo>& Streams() const override;
+
+ void Start() override;
+
+ void Stop() override;
+
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ // Implement VideoPropertiesInterface
+ const picojson::value& DeviceProperties() const override {
+ if (-1 == _src_id) throw std::runtime_error("Not initialised");
+ return _device_properties;
+ }
+
+ const picojson::value& FrameProperties() const override {
+ return _frame_properties;
+ }
+
+ // Implement VideoPlaybackInterface
+
+ size_t GetCurrentFrameId() const override;
+
+ size_t GetTotalFrames() const override;
+
+ size_t Seek(size_t frameid) override;
+
+ std::string GetSourceUri();
+
+private:
+ void HandlePipeClosed();
+
+protected:
+ int FindPacketStreamSource();
+ void SetupStreams(const PacketStreamSource& src);
+
+ const std::string _filename;
+ std::shared_ptr<PlaybackSession> _playback_session;
+ std::shared_ptr<PacketStreamReader> _reader;
+ SyncTimeEventPromise _event_promise;
+ int _src_id;
+ const PacketStreamSource* _source;
+
+ size_t _size_bytes;
+ bool _fixed_size;
+ std::vector<StreamInfo> _streams;
+ std::vector<ImageDecoderFunc> stream_decoder;
+ picojson::value _device_properties;
+ picojson::value _frame_properties;
+ std::string _source_uri;
+
+ Registration<size_t> session_seek;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/pango_video_output.h b/externals/Pangolin/include/pangolin/video/drivers/pango_video_output.h
new file mode 100644
index 0000000000000000000000000000000000000000..a986a70bb32c92156e1a694156fe9f30ac9025ca
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/pango_video_output.h
@@ -0,0 +1,70 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/log/packetstream_writer.h>
+#include <pangolin/video/video_output.h>
+
+#include <pangolin/video/stream_encoder_factory.h>
+
+#include <functional>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT PangoVideoOutput : public VideoOutputInterface
+{
+public:
+ PangoVideoOutput(const std::string& filename, size_t buffer_size_bytes, const std::map<size_t, std::string> &stream_encoder_uris);
+ ~PangoVideoOutput();
+
+ const std::vector<StreamInfo>& Streams() const override;
+ void SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri, const picojson::value& device_properties) override;
+ int WriteStreams(const unsigned char* data, const picojson::value& frame_properties) override;
+ bool IsPipe() const override;
+
+protected:
+// void WriteHeader();
+
+ std::vector<StreamInfo> streams;
+ std::string input_uri;
+ const std::string filename;
+ picojson::value device_properties;
+
+ PacketStreamWriter packetstream;
+ size_t packetstream_buffer_size_bytes;
+ int packetstreamsrcid;
+ size_t total_frame_size;
+ bool is_pipe;
+
+ bool fixed_size;
+ std::map<size_t, std::string> stream_encoder_uris;
+ std::vector<ImageEncoderFunc> stream_encoders;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/pleora.h b/externals/Pangolin/include/pangolin/video/drivers/pleora.h
new file mode 100644
index 0000000000000000000000000000000000000000..4b4574415af72489ad2bff43b281e37a33538a93
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/pleora.h
@@ -0,0 +1,196 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+#include <PvDevice.h>
+#include <PvDeviceGEV.h>
+#include <PvDeviceU3V.h>
+#include <PvStream.h>
+#include <PvStreamGEV.h>
+#include <PvStreamU3V.h>
+#include <PvBuffer.h>
+
+#include <PvSystem.h>
+
+#include <stdlib.h>
+#include <list>
+
+namespace pangolin
+{
+
+struct GrabbedBuffer {
+
+ inline GrabbedBuffer(PvBuffer* b,PvResult r,bool v)
+ : buff(b), res(r), valid(v)
+ {
+ }
+
+ PvBuffer* buff;
+ PvResult res;
+ bool valid;
+
+};
+
+typedef std::list<GrabbedBuffer> GrabbedBufferList;
+
+typedef std::list<PvBuffer *> BufferList;
+
+class PANGOLIN_EXPORT PleoraVideo : public VideoInterface, public VideoPropertiesInterface,
+ public BufferAwareVideoInterface, public GenicamVideoInterface
+{
+public:
+
+ static const size_t DEFAULT_BUFFER_COUNT = 30;
+
+ PleoraVideo(const Params& p);
+
+ ~PleoraVideo();
+
+ void Start();
+
+ void Stop();
+
+ size_t SizeBytes() const;
+
+ const std::vector<StreamInfo>& Streams() const;
+
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::string GetParameter(const std::string& name);
+
+ void SetParameter(const std::string& name, const std::string& value);
+
+ void SetGain(int64_t val);
+
+ int64_t GetGain();
+
+ void SetAnalogBlackLevel(int64_t val);
+
+ int64_t GetAnalogBlackLevel();
+
+ void SetExposure(double val);
+
+ double GetExposure();
+
+ void SetGamma(double val);
+
+ double GetGamma();
+
+
+
+ void SetupTrigger(bool triggerActive, int64_t triggerSource, int64_t acquisitionMode);
+
+ const picojson::value& DeviceProperties() const {
+ return device_properties;
+ }
+
+ const picojson::value& FrameProperties() const {
+ return frame_properties;
+ }
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+protected:
+
+ void InitDevice(const char *model_name, const char *serial_num, size_t index);
+
+ void DeinitDevice();
+
+ void SetDeviceParams(Params& p);
+
+ void InitStream();
+
+ void DeinitStream();
+
+ void InitPangoStreams();
+
+ void InitPangoDeviceProperties();
+
+ void InitBuffers(size_t buffer_count);
+
+ void DeinitBuffers();
+
+ template<typename T>
+ T DeviceParam(const char* name);
+
+ template<typename T>
+ bool SetDeviceParam(const char* name, T val);
+
+ template<typename T>
+ T StreamParam(const char* name);
+
+ template<typename T>
+ bool SetStreamParam(const char* name, T val);
+
+ bool ParseBuffer(PvBuffer* lBuffer, unsigned char* image);
+
+ void RetriveAllAvailableBuffers(uint32_t timeout);
+
+ std::vector<StreamInfo> streams;
+ picojson::value device_properties;
+ picojson::value frame_properties;
+
+ size_t size_bytes;
+
+ // Pleora handles
+ PvSystem* lPvSystem;
+ const PvDeviceInfo* lDeviceInfo;
+ PvDevice* lDevice;
+ PvStream* lStream;
+
+ // Genicam device parameters
+ PvGenParameterArray* lDeviceParams;
+ PvGenCommand* lStart;
+ PvGenCommand* lStop;
+
+ PvGenInteger* lAnalogGain;
+ PvGenInteger* lAnalogBlackLevel;
+ PvGenFloat* lExposure;
+ PvGenFloat* lGamma;
+ PvGenEnum* lAquisitionMode;
+ PvGenEnum* lTriggerSource;
+ PvGenEnum* lTriggerMode;
+ PvGenFloat* lTemperatureCelcius;
+ bool getTemp;
+
+ // Genicam stream parameters
+ PvGenParameterArray* lStreamParams;
+
+ BufferList lBufferList;
+ GrabbedBufferList lGrabbedBuffList;
+ uint32_t validGrabbedBuffers;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/pvn.h b/externals/Pangolin/include/pangolin/video/drivers/pvn.h
new file mode 100644
index 0000000000000000000000000000000000000000..8e47e5468def8a3c6aec689cb50097f08ec1444c
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/pvn.h
@@ -0,0 +1,77 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+#include <pangolin/utils/timer.h>
+
+#include <fstream>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT PvnVideo : public VideoInterface
+{
+public:
+ PvnVideo(const std::string& filename, bool realtime = false);
+ ~PvnVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+protected:
+ int frames;
+ std::ifstream file;
+
+ std::vector<StreamInfo> streams;
+ size_t frame_size_bytes;
+
+ bool realtime;
+ pangolin::basetime frame_interval;
+ pangolin::basetime last_frame;
+
+ void ReadFileHeader();
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/realsense.h b/externals/Pangolin/include/pangolin/video/drivers/realsense.h
new file mode 100644
index 0000000000000000000000000000000000000000..064bc7a43bb1b3977625874e3f1a583a2b0ddded
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/realsense.h
@@ -0,0 +1,87 @@
+#pragma once
+
+#include <pangolin/pangolin.h>
+
+#include <pangolin/video/video.h>
+
+#include <pangolin/video/iostream_operators.h>
+
+namespace rs {
+class context;
+class device;
+}
+
+namespace pangolin
+{
+
+//! Interface to video capture sources
+struct RealSenseVideo : public VideoInterface, public VideoPropertiesInterface, public VideoPlaybackInterface
+{
+public:
+
+ // Open all RGB and Depth streams from all devices
+ RealSenseVideo(ImageDim dim=ImageDim(640,480), int fps=30);
+
+ // Open streams specified
+ // TODO
+ //RealSenseVideo(std::vector<OpenNiStreamMode>& stream_modes);
+
+ ~RealSenseVideo();
+
+ //! Implement VideoInput::Start()
+ void Start() override;
+
+ //! Implement VideoInput::Stop()
+ void Stop() override;
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const override;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const override;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& DeviceProperties() const override {
+ return device_properties;
+ }
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& FrameProperties() const override {
+ return frame_properties;
+ }
+
+ //! Implement VideoPlaybackInterface::GetCurrentFrameId
+ size_t GetCurrentFrameId() const override;
+
+ //! Implement VideoPlaybackInterface::GetTotalFrames
+ size_t GetTotalFrames() const override;
+
+ //! Implement VideoPlaybackInterface::Seek
+ size_t Seek(size_t frameid) override;
+
+protected:
+ size_t sizeBytes;
+
+ std::vector<StreamInfo> streams;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+ picojson::value* streams_properties;
+
+ size_t current_frame_index;
+ size_t total_frames;
+
+ rs::context* ctx_;
+ std::vector<rs::device*> devs_;
+
+ ImageDim dim_;
+ size_t fps_;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/realsense2.h b/externals/Pangolin/include/pangolin/video/drivers/realsense2.h
new file mode 100644
index 0000000000000000000000000000000000000000..589f73cb711ceb87dd131fa6f0dfce0ba04ee6ff
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/realsense2.h
@@ -0,0 +1,87 @@
+#pragma once
+
+#include <pangolin/pangolin.h>
+
+#include <pangolin/video/video.h>
+
+#include <pangolin/video/iostream_operators.h>
+
+namespace rs2 {
+class pipeline;
+class config;
+}
+
+namespace pangolin
+{
+
+//! Interface to video capture sources
+struct RealSense2Video : public VideoInterface, public VideoPropertiesInterface, public VideoPlaybackInterface
+{
+public:
+
+ // Open all RGB and Depth streams from all devices
+ RealSense2Video(ImageDim dim=ImageDim(640,480), int fps=30);
+
+ // Open streams specified
+ // TODO
+ //RealSense2Video(std::vector<OpenNiStreamMode>& stream_modes);
+
+ ~RealSense2Video();
+
+ //! Implement VideoInput::Start()
+ void Start() override;
+
+ //! Implement VideoInput::Stop()
+ void Stop() override;
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const override;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const override;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& DeviceProperties() const override {
+ return device_properties;
+ }
+
+ //! Implement VideoPropertiesInterface::Properties()
+ const picojson::value& FrameProperties() const override {
+ return frame_properties;
+ }
+
+ //! Implement VideoPlaybackInterface::GetCurrentFrameId
+ size_t GetCurrentFrameId() const override;
+
+ //! Implement VideoPlaybackInterface::GetTotalFrames
+ size_t GetTotalFrames() const override;
+
+ //! Implement VideoPlaybackInterface::Seek
+ size_t Seek(size_t frameid) override;
+
+protected:
+ size_t sizeBytes;
+
+ std::vector<StreamInfo> streams;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+ picojson::value* streams_properties;
+
+ size_t current_frame_index;
+ size_t total_frames;
+
+ rs2::pipeline* pipe;
+ rs2::config* cfg;
+
+ ImageDim dim_;
+ size_t fps_;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/shared_memory.h b/externals/Pangolin/include/pangolin/video/drivers/shared_memory.h
new file mode 100644
index 0000000000000000000000000000000000000000..66f27dccbe53b1939288503882be565bcb226596
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/shared_memory.h
@@ -0,0 +1,37 @@
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+#include <pangolin/utils/posix/condition_variable.h>
+#include <pangolin/utils/posix/shared_memory_buffer.h>
+
+#include <memory>
+#include <vector>
+
+namespace pangolin
+{
+
+class SharedMemoryVideo : public VideoInterface
+{
+public:
+ SharedMemoryVideo(size_t w, size_t h, std::string pix_fmt,
+ const std::shared_ptr<SharedMemoryBufferInterface>& shared_memory,
+ const std::shared_ptr<ConditionVariableInterface>& buffer_full);
+ ~SharedMemoryVideo();
+
+ size_t SizeBytes() const;
+ const std::vector<StreamInfo>& Streams() const;
+ void Start();
+ void Stop();
+ bool GrabNext(unsigned char *image, bool wait);
+ bool GrabNewest(unsigned char *image, bool wait);
+
+private:
+ PixelFormat _fmt;
+ size_t _frame_size;
+ std::vector<StreamInfo> _streams;
+ std::shared_ptr<SharedMemoryBufferInterface> _shared_memory;
+ std::shared_ptr<ConditionVariableInterface> _buffer_full;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/shift.h b/externals/Pangolin/include/pangolin/video/drivers/shift.h
new file mode 100644
index 0000000000000000000000000000000000000000..ed7f5104aa3596bbed37b9c5d1b1d2f4fcd1c85c
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/shift.h
@@ -0,0 +1,73 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+// Video class that debayers its video input using the given method.
+class PANGOLIN_EXPORT ShiftVideo : public VideoInterface, public VideoFilterInterface
+{
+public:
+ ShiftVideo(std::unique_ptr<VideoInterface>& videoin, PixelFormat new_fmt, int shift_right_bits = 0, unsigned int mask = 0xFFFF);
+ ~ShiftVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::vector<VideoInterface*>& InputStreams();
+
+protected:
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+ unsigned char* buffer;
+ int shift_right_bits;
+ unsigned int mask;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/split.h b/externals/Pangolin/include/pangolin/video/drivers/split.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5674580a78e8b2f491b2ce99f3ebf89f9290fe0
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/split.h
@@ -0,0 +1,65 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/video.h>
+#include <vector>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT SplitVideo
+ : public VideoInterface, public VideoFilterInterface
+{
+public:
+ SplitVideo(std::unique_ptr<VideoInterface>& videoin, const std::vector<StreamInfo>& streams);
+
+ ~SplitVideo();
+
+ size_t SizeBytes() const;
+
+ const std::vector<StreamInfo>& Streams() const;
+
+ void Start();
+
+ void Stop();
+
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::vector<VideoInterface*>& InputStreams();
+
+protected:
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/teli.h b/externals/Pangolin/include/pangolin/video/drivers/teli.h
new file mode 100644
index 0000000000000000000000000000000000000000..e0f851d047cfba829fc37a00fec8c8d0979e074d
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/teli.h
@@ -0,0 +1,118 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/video/video.h>
+
+#include <TeliCamApi.h>
+
+namespace pangolin
+{
+
+// Video class that outputs test video signal.
+class PANGOLIN_EXPORT TeliVideo : public VideoInterface, public VideoPropertiesInterface,
+ public BufferAwareVideoInterface, public GenicamVideoInterface
+{
+public:
+ TeliVideo(const Params &p);
+ ~TeliVideo();
+
+ Params OpenCameraAndGetRemainingParameters(Params ¶ms);
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ inline Teli::CAM_HANDLE GetCameraHandle() {
+ return cam;
+ }
+
+ inline Teli::CAM_STRM_HANDLE GetCameraStreamHandle() {
+ return strm;
+ }
+
+ bool GetParameter(const std::string& name, std::string& result);
+
+ bool SetParameter(const std::string& name, const std::string& value);
+
+ //! Returns number of available frames
+ uint32_t AvailableFrames() const;
+
+ //! Drops N frames in the queue starting from the oldest
+ //! returns false if less than n frames arae available
+ bool DropNFrames(uint32_t n);
+
+ //! Access JSON properties of device
+ const picojson::value& DeviceProperties() const;
+
+ //! Access JSON properties of most recently captured frame
+ const picojson::value& FrameProperties() const;
+
+ void PopulateEstimatedCenterCaptureTime(pangolin::basetime host_reception_time);
+
+protected:
+ void Initialise();
+ void InitPangoDeviceProperties();
+ void SetDeviceParams(const Params &p);
+ void SetNodeValStr(Teli::CAM_HANDLE cam, Teli::CAM_NODE_HANDLE node, std::string node_str, std::string val_str);
+
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+
+ Teli::CAM_HANDLE cam;
+ Teli::CAM_STRM_HANDLE strm;
+
+#ifdef _WIN_
+ HANDLE hStrmCmpEvt;
+#endif
+#ifdef _LINUX_
+ Teli::SIGNAL_HANDLE hStrmCmpEvt;
+#endif
+ double transfer_bandwidth_gbps;
+ int exposure_us;
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/test.h b/externals/Pangolin/include/pangolin/video/drivers/test.h
new file mode 100644
index 0000000000000000000000000000000000000000..02646f6b1004ebab1c8e02f9b6b4b657dc59c8e4
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/test.h
@@ -0,0 +1,66 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+// Video class that outputs test video signal.
+class PANGOLIN_EXPORT TestVideo : public VideoInterface
+{
+public:
+ TestVideo(size_t w, size_t h, size_t n, std::string pix_fmt);
+ ~TestVideo();
+
+ //! Implement VideoInput::Start()
+ void Start() override;
+
+ //! Implement VideoInput::Stop()
+ void Stop() override;
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const override;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const override;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+protected:
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/thread.h b/externals/Pangolin/include/pangolin/video/drivers/thread.h
new file mode 100644
index 0000000000000000000000000000000000000000..ddd02421c2b53c34053ce1f3562dd69cccde960a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/thread.h
@@ -0,0 +1,112 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+#include <memory>
+#include <pangolin/utils/fix_size_buffer_queue.h>
+
+namespace pangolin
+{
+
+
+// Video class that creates a thread that keeps pulling frames and processing from its children.
+class PANGOLIN_EXPORT ThreadVideo : public VideoInterface, public VideoPropertiesInterface,
+ public BufferAwareVideoInterface, public VideoFilterInterface
+{
+public:
+ ThreadVideo(std::unique_ptr<VideoInterface>& videoin, size_t num_buffers);
+ ~ThreadVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ const picojson::value& DeviceProperties() const;
+
+ const picojson::value& FrameProperties() const;
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+ void operator()();
+
+ std::vector<VideoInterface*>& InputStreams();
+
+protected:
+ struct GrabResult
+ {
+ GrabResult(const size_t buffer_size)
+ : return_status(false),
+ buffer(new unsigned char[buffer_size])
+ {
+ }
+
+ // No copy constructor.
+ GrabResult(const GrabResult& o) = delete;
+
+ // Default move constructor
+ GrabResult(GrabResult&& o) = default;
+
+ bool return_status;
+ std::unique_ptr<unsigned char[]> buffer;
+ picojson::value frame_properties;
+ };
+
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+
+ bool quit_grab_thread;
+ FixSizeBuffersQueue<GrabResult> queue;
+
+ std::condition_variable cv;
+ std::mutex cvMtx;
+ std::thread grab_thread;
+
+ mutable picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/truncate.h b/externals/Pangolin/include/pangolin/video/drivers/truncate.h
new file mode 100644
index 0000000000000000000000000000000000000000..cfdd3c0a540b39cdd8a60e25b077c2583b0d7cf3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/truncate.h
@@ -0,0 +1,71 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/video.h>
+#include <vector>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT TruncateVideo
+ : public VideoInterface, public VideoFilterInterface
+{
+public:
+ TruncateVideo(std::unique_ptr<VideoInterface>& videoin, size_t begin, size_t end);
+
+ ~TruncateVideo();
+
+ size_t SizeBytes() const;
+
+ const std::vector<StreamInfo>& Streams() const;
+
+ void Start();
+
+ void Stop();
+
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ std::vector<VideoInterface*>& InputStreams();
+
+protected:
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+
+ size_t begin;
+ size_t end;
+ size_t next_frame_to_grab;
+
+ inline VideoPlaybackInterface* GetVideoPlaybackInterface(){ return dynamic_cast<VideoPlaybackInterface*>(src.get()); }
+};
+
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/unpack.h b/externals/Pangolin/include/pangolin/video/drivers/unpack.h
new file mode 100644
index 0000000000000000000000000000000000000000..4f0826c4a93847f67670f0914ff3d08f7b178591
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/unpack.h
@@ -0,0 +1,84 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+namespace pangolin
+{
+
+// Video class that debayers its video input using the given method.
+class PANGOLIN_EXPORT UnpackVideo :
+ public VideoInterface,
+ public VideoFilterInterface,
+ public BufferAwareVideoInterface
+{
+public:
+ UnpackVideo(std::unique_ptr<VideoInterface>& videoin, PixelFormat new_fmt);
+ ~UnpackVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoFilterInterface method
+ std::vector<VideoInterface*>& InputStreams();
+
+ uint32_t AvailableFrames() const;
+
+ bool DropNFrames(uint32_t n);
+
+protected:
+ void Process(unsigned char* image, const unsigned char* buffer);
+
+ std::unique_ptr<VideoInterface> src;
+ std::vector<VideoInterface*> videoin;
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+ unsigned char* buffer;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/uvc.h b/externals/Pangolin/include/pangolin/video/drivers/uvc.h
new file mode 100755
index 0000000000000000000000000000000000000000..148a6f7a0e3c000983ea7b0d79f93b27a0e49cc5
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/uvc.h
@@ -0,0 +1,115 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+#include <pangolin/utils/timer.h>
+
+#ifdef _MSC_VER
+// Define missing timeval struct
+typedef struct timeval {
+ long tv_sec;
+ long tv_usec;
+} timeval;
+#endif // _MSC_VER
+
+#include <libuvc/libuvc.h>
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT UvcVideo : public VideoInterface, public VideoUvcInterface, public VideoPropertiesInterface
+{
+public:
+ UvcVideo(int vendor_id, int product_id, const char* sn, int deviceid, int width, int height, int fps);
+ ~UvcVideo();
+
+ void InitDevice(int vid, int pid, const char* sn, int deviceid, int width, int height, int fps);
+ void DeinitDevice();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoUvcInterface::GetCtrl()
+ int IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code);
+
+ //! Implement VideoUvcInterface::GetExposure()
+ bool GetExposure(int& exp_us);
+
+ //! Implement VideoUvcInterface::SetExposure()
+ bool SetExposure(int exp_us);
+
+ //! Implement VideoUvcInterface::GetGain()
+ bool GetGain(float& gain);
+
+ //! Implement VideoUvcInterface::SetGain()
+ bool SetGain(float gain);
+
+ //! Access JSON properties of device
+ const picojson::value& DeviceProperties() const;
+
+ //! Access JSON properties of most recently captured frame
+ const picojson::value& FrameProperties() const;
+
+protected:
+ void InitPangoDeviceProperties();
+ static uvc_error_t FindDevice(
+ uvc_context_t *ctx, uvc_device_t **dev,
+ int vid, int pid, const char *sn, int device_id);
+
+ std::vector<StreamInfo> streams;
+ size_t size_bytes;
+
+ uvc_context* ctx_;
+ uvc_device* dev_;
+ uvc_device_handle* devh_;
+ uvc_stream_handle* strm_;
+ uvc_stream_ctrl_t ctrl_;
+ uvc_frame_t* frame_;
+ picojson::value device_properties;
+ picojson::value frame_properties;
+ bool is_streaming;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/uvc_mediafoundation.h b/externals/Pangolin/include/pangolin/video/drivers/uvc_mediafoundation.h
new file mode 100644
index 0000000000000000000000000000000000000000..dfa6f1260561753b3dbd46ae6d35a0a77a6447f3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/uvc_mediafoundation.h
@@ -0,0 +1,82 @@
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+#include <pangolin/video/video_interface.h>
+
+struct IMFActivate;
+struct IMFMediaSource;
+struct IMFSourceReader;
+struct IBaseFilter;
+struct IKsControl;
+
+namespace pangolin
+{
+
+class PANGOLIN_EXPORT UvcMediaFoundationVideo
+ : public pangolin::VideoInterface, public pangolin::VideoUvcInterface, public pangolin::VideoPropertiesInterface
+{
+ public:
+ UvcMediaFoundationVideo(int vendorId, int productId, int deviceId, size_t width, size_t height, int fps);
+ ~UvcMediaFoundationVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<pangolin::StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext(unsigned char* image, bool wait = true);
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest(unsigned char* image, bool wait = true);
+
+ //! Implement VideoUvcInterface::GetCtrl()
+ int IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, pangolin::UvcRequestCode req_code);
+
+ //! Implement VideoUvcInterface::GetExposure()
+ bool GetExposure(int& exp_us);
+
+ //! Implement VideoUvcInterface::SetExposure()
+ bool SetExposure(int exp_us);
+
+ //! Implement VideoUvcInterface::GetGain()
+ bool GetGain(float& gain);
+
+ //! Implement VideoUvcInterface::SetGain()
+ bool SetGain(float gain);
+
+ //! Access JSON properties of device
+ const picojson::value& DeviceProperties() const;
+
+ //! Access JSON properties of most recently captured frame
+ const picojson::value& FrameProperties() const;
+
+ protected:
+ bool FindDevice(int vendorId, int productId, int deviceId);
+ void InitDevice(size_t width, size_t height, int fps);
+ void DeinitDevice();
+
+ static bool DeviceMatches(const std::wstring& symLink, int vendorId, int productId);
+ static bool SymLinkIDMatches(const std::wstring& symLink, const wchar_t* idStr, int id);
+
+ std::vector<pangolin::StreamInfo> streams;
+ size_t size_bytes;
+
+ IMFMediaSource* mediaSource;
+ IMFSourceReader* sourceReader;
+ IBaseFilter* baseFilter;
+ IKsControl* ksControl;
+ DWORD ksControlNodeId;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+}
diff --git a/externals/Pangolin/include/pangolin/video/drivers/v4l.h b/externals/Pangolin/include/pangolin/video/drivers/v4l.h
new file mode 100755
index 0000000000000000000000000000000000000000..a2ed55a294ba3db7f13e626cd7b782ca80b6057b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/drivers/v4l.h
@@ -0,0 +1,128 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/pangolin.h>
+#include <pangolin/video/video.h>
+
+#include <asm/types.h>
+#include <linux/videodev2.h>
+
+namespace pangolin
+{
+
+typedef enum {
+ IO_METHOD_READ,
+ IO_METHOD_MMAP,
+ IO_METHOD_USERPTR,
+} io_method;
+
+struct buffer {
+ void* start;
+ size_t length;
+};
+
+class PANGOLIN_EXPORT V4lVideo : public VideoInterface, public VideoUvcInterface, public VideoPropertiesInterface
+{
+public:
+ V4lVideo(const char* dev_name, io_method io = IO_METHOD_MMAP, unsigned iwidth=0, unsigned iheight=0);
+ ~V4lVideo();
+
+ //! Implement VideoInput::Start()
+ void Start();
+
+ //! Implement VideoInput::Stop()
+ void Stop();
+
+ //! Implement VideoInput::SizeBytes()
+ size_t SizeBytes() const;
+
+ //! Implement VideoInput::Streams()
+ const std::vector<StreamInfo>& Streams() const;
+
+ //! Implement VideoInput::GrabNext()
+ bool GrabNext( unsigned char* image, bool wait = true );
+
+ //! Implement VideoInput::GrabNewest()
+ bool GrabNewest( unsigned char* image, bool wait = true );
+
+ //! Implement VideoUvcInterface::IoCtrl()
+ int IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code);
+
+ bool GetExposure(int& exp_us);
+
+ bool SetExposure(int exp_us);
+
+ bool GetGain(float& gain);
+
+ bool SetGain(float gain);
+
+ int GetFileDescriptor() const{
+ return fd;
+ }
+
+ //! Access JSON properties of device
+ const picojson::value& DeviceProperties() const;
+
+ //! Access JSON properties of most recently captured frame
+ const picojson::value& FrameProperties() const;
+
+protected:
+ void InitPangoDeviceProperties();
+
+
+ int ReadFrame(unsigned char* image);
+ void Mainloop();
+
+ void init_read(unsigned int buffer_size);
+ void init_mmap(const char* dev_name);
+ void init_userp(const char* dev_name, unsigned int buffer_size);
+
+ void init_device(const char* dev_name, unsigned iwidth, unsigned iheight, unsigned ifps, unsigned v4l_format = V4L2_PIX_FMT_YUYV, v4l2_field field = V4L2_FIELD_INTERLACED);
+ void uninit_device();
+
+ void open_device(const char* dev_name);
+ void close_device();
+
+ std::vector<StreamInfo> streams;
+
+ io_method io;
+ int fd;
+ buffer* buffers;
+ unsigned int n_buffers;
+ bool running;
+ unsigned width;
+ unsigned height;
+ float fps;
+ size_t image_size;
+
+ picojson::value device_properties;
+ picojson::value frame_properties;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/iostream_operators.h b/externals/Pangolin/include/pangolin/video/iostream_operators.h
new file mode 100644
index 0000000000000000000000000000000000000000..600e06e17dad1402eb0d3e897b705be833016edc
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/iostream_operators.h
@@ -0,0 +1,132 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <iostream>
+#include <cctype>
+
+#include <pangolin/video/video_exception.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/video/stream_info.h>
+
+namespace pangolin
+{
+
+struct PANGOLIN_EXPORT Point
+{
+ inline Point() : x(0), y(0) {}
+ inline Point(size_t x, size_t y) : x(x), y(y) {}
+ size_t x;
+ size_t y;
+};
+
+typedef Point ImageDim;
+
+struct PANGOLIN_EXPORT ImageRoi
+{
+ inline ImageRoi() : x(0), y(0), w(0), h(0) {}
+ inline ImageRoi(size_t x, size_t y, size_t w, size_t h) : x(x), y(y), w(w), h(h) {}
+ size_t x; size_t y;
+ size_t w; size_t h;
+};
+
+inline std::istream& operator>> (std::istream &is, ImageDim &dim)
+{
+ if(std::isdigit(is.peek()) ) {
+ // Expect 640x480, 640*480, ...
+ is >> dim.x; is.get(); is >> dim.y;
+ }else{
+ // Expect 'VGA', 'QVGA', etc
+ std::string sdim;
+ is >> sdim;
+ ToUpper(sdim);
+
+ if( !sdim.compare("QQVGA") ) {
+ dim = ImageDim(160,120);
+ }else if( !sdim.compare("HQVGA") ) {
+ dim = ImageDim(240,160);
+ }else if( !sdim.compare("QVGA") ) {
+ dim = ImageDim(320,240);
+ }else if( !sdim.compare("WQVGA") ) {
+ dim = ImageDim(360,240);
+ }else if( !sdim.compare("HVGA") ) {
+ dim = ImageDim(480,320);
+ }else if( !sdim.compare("VGA") ) {
+ dim = ImageDim(640,480);
+ }else if( !sdim.compare("WVGA") ) {
+ dim = ImageDim(720,480);
+ }else if( !sdim.compare("SVGA") ) {
+ dim = ImageDim(800,600);
+ }else if( !sdim.compare("DVGA") ) {
+ dim = ImageDim(960,640);
+ }else if( !sdim.compare("WSVGA") ) {
+ dim = ImageDim(1024,600);
+ }else{
+ throw VideoException("Unrecognised image-size string.");
+ }
+ }
+ return is;
+}
+
+inline std::istream& operator>> (std::istream &is, ImageRoi &roi)
+{
+ is >> roi.x; is.get(); is >> roi.y; is.get();
+ is >> roi.w; is.get(); is >> roi.h;
+ return is;
+}
+
+inline std::istream& operator>> (std::istream &is, PixelFormat& fmt)
+{
+ std::string sfmt;
+ is >> sfmt;
+ fmt = PixelFormatFromString(sfmt);
+ return is;
+}
+
+inline std::istream& operator>> (std::istream &is, Image<unsigned char>& img)
+{
+ size_t offset;
+ is >> offset; is.get();
+ img.ptr = (unsigned char*)(offset);
+ is >> img.w; is.get();
+ is >> img.h; is.get();
+ is >> img.pitch;
+ return is;
+}
+
+inline std::istream& operator>> (std::istream &is, StreamInfo &stream)
+{
+ PixelFormat fmt;
+ Image<unsigned char> img_offset;
+ is >> img_offset; is.get();
+ is >> fmt;
+ stream = StreamInfo(fmt, img_offset);
+ return is;
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/stream_encoder_factory.h b/externals/Pangolin/include/pangolin/video/stream_encoder_factory.h
new file mode 100644
index 0000000000000000000000000000000000000000..b6f7efe8a44eb85a206882d7a3951ca7cd3f0a90
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/stream_encoder_factory.h
@@ -0,0 +1,22 @@
+#pragma once
+
+#include <memory>
+
+#include <pangolin/image/image_io.h>
+
+namespace pangolin {
+
+using ImageEncoderFunc = std::function<void(std::ostream&, const Image<unsigned char>&)>;
+using ImageDecoderFunc = std::function<TypedImage(std::istream&)>;
+
+class StreamEncoderFactory
+{
+public:
+ static StreamEncoderFactory& I();
+
+ ImageEncoderFunc GetEncoder(const std::string& encoder_spec, const PixelFormat& fmt);
+
+ ImageDecoderFunc GetDecoder(const std::string& encoder_spec, const PixelFormat& fmt);
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/stream_info.h b/externals/Pangolin/include/pangolin/video/stream_info.h
new file mode 100644
index 0000000000000000000000000000000000000000..d141d7a4a8bb2d1d01276c99315cecdde524f54b
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/stream_info.h
@@ -0,0 +1,100 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/image/image.h>
+#include <pangolin/image/pixel_format.h>
+
+namespace pangolin {
+
+class PANGOLIN_EXPORT StreamInfo
+{
+public:
+ inline StreamInfo()
+ : fmt(PixelFormatFromString("GRAY8")) {}
+
+ inline StreamInfo(PixelFormat fmt, const Image<unsigned char> img_offset )
+ : fmt(fmt), img_offset(img_offset) {}
+
+ inline StreamInfo(PixelFormat fmt, size_t w, size_t h, size_t pitch, unsigned char* offset = 0)
+ : fmt(fmt), img_offset(offset,w,h,pitch) {}
+
+ //! Format representing how image is laid out in memory
+ inline const PixelFormat &PixFormat() const { return fmt; }
+
+ //! Image width in pixels
+ inline size_t Width() const { return img_offset.w; }
+
+ //! Image height in pixels
+ inline size_t Height() const { return img_offset.h; }
+
+ inline double Aspect() const { return (double)Width() / (double)Height(); }
+
+ //! Pitch: Number of bytes between one image row and the next
+ inline size_t Pitch() const { return img_offset.pitch; }
+
+ //! Number of contiguous bytes in memory that the image occupies
+ inline size_t RowBytes() const {
+ // Row size without padding
+ return (fmt.bpp*img_offset.w)/8;
+ }
+
+ //! Returns true iff image contains padding or stridded access
+ //! This implies that the image data is not contiguous in memory.
+ inline bool IsPitched() const {
+ return Pitch() != RowBytes();
+ }
+
+ //! Number of contiguous bytes in memory that the image occupies
+ inline size_t SizeBytes() const {
+ return (img_offset.h-1) * img_offset.pitch + RowBytes();
+ }
+
+ //! Offset in bytes relative to start of frame buffer
+ inline unsigned char* Offset() const { return img_offset.ptr; }
+
+ //! Return Image wrapper around raw base pointer
+ inline Image<unsigned char> StreamImage(unsigned char* base_ptr) const {
+ Image<unsigned char> img = img_offset;
+ img.ptr += (size_t)base_ptr;
+ return img;
+ }
+
+ //! Return Image wrapper around raw base pointer
+ inline const Image<unsigned char> StreamImage(const unsigned char* base_ptr) const {
+ Image<unsigned char> img = img_offset;
+ img.ptr += (size_t)base_ptr;
+ return img;
+ }
+
+protected:
+ PixelFormat fmt;
+ Image<unsigned char> img_offset;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video.h b/externals/Pangolin/include/pangolin/video/video.h
new file mode 100644
index 0000000000000000000000000000000000000000..46e982ddbc663418054d83f320c995873ef3314a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video.h
@@ -0,0 +1,261 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Pangolin video supports various cameras and file formats through
+// different 3rd party libraries.
+//
+// Video URI's take the following form:
+// scheme:[param1=value1,param2=value2,...]//device
+//
+// scheme = file | files | pango | shmem | dc1394 | uvc | v4l | openni2 |
+// openni | depthsense | pleora | teli | mjpeg | test |
+// thread | convert | debayer | split | join | shift | mirror | unpack
+//
+// file/files - read one or more streams from image file(s) / video
+// e.g. "files://~/data/dataset/img_*.jpg"
+// e.g. "files://~/data/dataset/img_[left,right]_*.pgm"
+// e.g. "files:///home/user/sequence/foo%03d.jpeg"
+//
+// e.g. "file:[fmt=GRAY8,size=640x480]///home/user/raw_image.bin"
+// e.g. "file:[realtime=1]///home/user/video/movie.pango"
+// e.g. "file:[stream=1]///home/user/video/movie.avi"
+//
+// dc1394 - capture video through a firewire camera
+// e.g. "dc1394:[fmt=RGB24,size=640x480,fps=30,iso=400,dma=10]//0"
+// e.g. "dc1394:[fmt=FORMAT7_1,size=640x480,pos=2+2,iso=400,dma=10]//0"
+// e.g. "dc1394:[fmt=FORMAT7_3,deinterlace=1]//0"
+//
+// v4l - capture video from a Video4Linux (USB) camera (normally YUVY422 format)
+// method=mmap|read|userptr
+// e.g. "v4l:///dev/video0"
+// e.g. "v4l[method=mmap]:///dev/video0"
+//
+// openni2 - capture video / depth from OpenNI2 SDK (Kinect / Xtrion etc)
+// imgN=grey|rgb|ir|ir8|ir24|depth|reg_depth
+// e.g. "openni2://'
+// e.g. "openni2:[img1=rgb,img2=depth,coloursync=true]//"
+// e.g. "openni2:[img1=depth,close=closerange,holefilter=true]//"
+// e.g. "openni2:[size=320x240,fps=60,img1=ir]//"
+//
+// openni - capture video / depth from OpenNI 1.0 SDK (Kinect / Xtrion etc)
+// Sensor modes containing '8' will truncate to 8-bits.
+// Sensor modes containing '+' explicitly enable IR illuminator
+// imgN=rgb|ir|ir8|ir+|ir8+|depth|reg_depth
+// autoexposure=true|false
+// e.g. "openni://'
+// e.g. "openni:[img1=rgb,img2=depth]//"
+// e.g. "openni:[size=320x240,fps=60,img1=ir]//"
+//
+// depthsense - capture video / depth from DepthSense SDK.
+// DepthSenseViewer can be used to alter capture settings.
+// imgN=depth|rgb
+// sizeN=QVGA|320x240|...
+// fpsN=25|30|60|...
+// e.g. "depthsense://"
+// e.g. "depthsense:[img1=depth,img2=rgb]//"
+//
+// pleora - USB 3 vision cameras accepts any option in the same format reported by eBUSPlayer
+// e.g. for lightwise cameras: "pleora:[size=512x256,pos=712x512,sn=00000274,ExposureTime=10000,PixelFormat=Mono12p,AcquisitionMode=SingleFrame,TriggerSource=Line0,TriggerMode=On]//"
+// e.g. for toshiba cameras: "pleora:[size=512x256,pos=712x512,sn=0300056,PixelSize=Bpp12,ExposureTime=10000,ImageFormatSelector=Format1,BinningHorizontal=2,BinningVertical=2]//"
+// e.g. toshiba alternated "pleora:[UserSetSelector=UserSet1,ExposureTime=10000,PixelSize=Bpp12,Width=1400,OffsetX=0,Height=1800,OffsetY=124,LineSelector=Line1,LineSource=ExposureActive,LineSelector=Line2,LineSource=Off,LineModeAll=6,LineInverterAll=6,UserSetSave=Execute,
+// UserSetSelector=UserSet2,PixelSize=Bpp12,Width=1400,OffsetX=1048,Height=1800,OffsetY=124,ExposureTime=10000,LineSelector=Line1,LineSource=Off,LineSelector=Line2,LineSource=ExposureActive,LineModeAll=6,LineInverterAll=6,UserSetSave=Execute,
+// SequentialShutterIndex=1,SequentialShutterEntry=1,SequentialShutterIndex=2,SequentialShutterEntry=2,SequentialShutterTerminateAt=2,SequentialShutterEnable=On,,AcquisitionFrameRateControl=Manual,AcquisitionFrameRate=70]//"
+//
+// thread - thread that continuously pulls from the child streams so that data in, unpacking, debayering etc can be decoupled from the main application thread
+// e.g. thread://pleora://
+// e.g. thread://unpack://pleora:[PixelFormat=Mono12p]//
+//
+// convert - use FFMPEG to convert between video pixel formats
+// e.g. "convert:[fmt=RGB24]//v4l:///dev/video0"
+// e.g. "convert:[fmt=GRAY8]//v4l:///dev/video0"
+//
+// mjpeg - capture from (possibly networked) motion jpeg stream using FFMPEG
+// e.g. "mjpeg://http://127.0.0.1/?action=stream"
+//
+// debayer - debayer an input video stream
+// e.g. "debayer:[tile="BGGR",method="downsample"]//v4l:///dev/video0
+//
+// split - split an input video into a one or more streams based on Region of Interest / memory specification
+// roiN=X+Y+WxH
+// memN=Offset:WxH:PitchBytes:Format
+// e.g. "split:[roi1=0+0+640x480,roi2=640+0+640x480]//files:///home/user/sequence/foo%03d.jpeg"
+// e.g. "split:[mem1=307200:640x480:1280:GRAY8,roi2=640+0+640x480]//files:///home/user/sequence/foo%03d.jpeg"
+// e.g. "split:[stream1=2,stream2=1]//pango://video.pango"
+//
+// truncate - select a subregion of a video based on start and end (last index+1) index
+// e.g. Generate 30 random frames: "truncate:[end=30]//test://"
+// e.g. "truncate:[begin=100,end=120]"
+//
+// join - join streams
+// e.g. "join:[sync_tolerance_us=100, sync_continuously=true]//{pleora:[sn=00000274]//}{pleora:[sn=00000275]//}"
+//
+// test - output test video sequence
+// e.g. "test://"
+// e.g. "test:[size=640x480,fmt=RGB24]//"
+
+#include <pangolin/utils/uri.h>
+#include <pangolin/video/video_exception.h>
+#include <pangolin/video/video_interface.h>
+#include <pangolin/video/video_output_interface.h>
+
+namespace pangolin
+{
+
+//! Open Video Interface from string specification (as described in this files header)
+PANGOLIN_EXPORT
+std::unique_ptr<VideoInterface> OpenVideo(const std::string& uri);
+
+//! Open Video Interface from Uri specification
+PANGOLIN_EXPORT
+std::unique_ptr<VideoInterface> OpenVideo(const Uri& uri);
+
+//! Open VideoOutput Interface from string specification (as described in this files header)
+PANGOLIN_EXPORT
+std::unique_ptr<VideoOutputInterface> OpenVideoOutput(const std::string& str_uri);
+
+//! Open VideoOutput Interface from Uri specification
+PANGOLIN_EXPORT
+std::unique_ptr<VideoOutputInterface> OpenVideoOutput(const Uri& uri);
+
+//! Create vector of matching interfaces either through direct cast or filter interface.
+template<typename T>
+std::vector<T*> FindMatchingVideoInterfaces( VideoInterface& video )
+{
+ std::vector<T*> matches;
+
+ T* vid = dynamic_cast<T*>(&video);
+ if(vid) {
+ matches.push_back(vid);
+ }
+
+ VideoFilterInterface* vidf = dynamic_cast<VideoFilterInterface*>(&video);
+ if(vidf) {
+ std::vector<T*> fmatches = vidf->FindMatchingStreams<T>();
+ matches.insert(matches.begin(), fmatches.begin(), fmatches.end());
+ }
+
+ return matches;
+}
+
+template<typename T>
+T* FindFirstMatchingVideoInterface( VideoInterface& video )
+{
+ T* vid = dynamic_cast<T*>(&video);
+ if(vid) {
+ return vid;
+ }
+
+ VideoFilterInterface* vidf = dynamic_cast<VideoFilterInterface*>(&video);
+ if(vidf) {
+ std::vector<T*> fmatches = vidf->FindMatchingStreams<T>();
+ if(fmatches.size()) {
+ return fmatches[0];
+ }
+ }
+
+ return 0;
+}
+
+inline
+picojson::value GetVideoFrameProperties(VideoInterface* video)
+{
+ VideoPropertiesInterface* pi = dynamic_cast<VideoPropertiesInterface*>(video);
+ VideoFilterInterface* fi = dynamic_cast<VideoFilterInterface*>(video);
+
+ if(pi) {
+ return pi->FrameProperties();
+ }else if(fi){
+ if(fi->InputStreams().size() == 1) {
+ return GetVideoFrameProperties(fi->InputStreams()[0]);
+ }else if(fi->InputStreams().size() > 0){
+ picojson::value streams;
+
+ for(size_t i=0; i< fi->InputStreams().size(); ++i) {
+ const picojson::value dev_props = GetVideoFrameProperties(fi->InputStreams()[i]);
+ if(dev_props.contains("streams")) {
+ const picojson::value& dev_streams = dev_props["streams"];
+ for(size_t j=0; j < dev_streams.size(); ++j) {
+ streams.push_back(dev_streams[j]);
+ }
+ }else{
+ streams.push_back(dev_props);
+ }
+ }
+
+ if(streams.size() > 1) {
+ picojson::value json = streams[0];
+ json["streams"] = streams;
+ return json;
+ }else{
+ return streams[0];
+ }
+ }
+ }
+ return picojson::value();
+}
+
+inline
+picojson::value GetVideoDeviceProperties(VideoInterface* video)
+{
+ VideoPropertiesInterface* pi = dynamic_cast<VideoPropertiesInterface*>(video);
+ VideoFilterInterface* fi = dynamic_cast<VideoFilterInterface*>(video);
+
+ if(pi) {
+ return pi->DeviceProperties();
+ }else if(fi){
+ if(fi->InputStreams().size() == 1) {
+ return GetVideoDeviceProperties(fi->InputStreams()[0]);
+ }else if(fi->InputStreams().size() > 0){
+ picojson::value streams;
+
+ for(size_t i=0; i< fi->InputStreams().size(); ++i) {
+ const picojson::value dev_props = GetVideoDeviceProperties(fi->InputStreams()[i]);
+ if(dev_props.contains("streams")) {
+ const picojson::value& dev_streams = dev_props["streams"];
+ for(size_t j=0; j < dev_streams.size(); ++j) {
+ streams.push_back(dev_streams[j]);
+ }
+ }else{
+ streams.push_back(dev_props);
+ }
+ }
+
+ if(streams.size() > 1) {
+ picojson::value json = streams[0];
+ json["streams"] = streams;
+ return json;
+ }else{
+ return streams[0];
+ }
+ }
+ }
+ return picojson::value();
+}
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video_exception.h b/externals/Pangolin/include/pangolin/video/video_exception.h
new file mode 100644
index 0000000000000000000000000000000000000000..60208c282d7a1f8bc90ae2b16efdab1efb0b8dc9
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_exception.h
@@ -0,0 +1,29 @@
+#pragma once
+
+#include <exception>
+#include <pangolin/platform.h>
+#include <string>
+
+namespace pangolin {
+
+struct PANGOLIN_EXPORT VideoException : std::exception
+{
+ VideoException(std::string str) : desc(str) {}
+ VideoException(std::string str, std::string detail) {
+ desc = str + "\n\t" + detail;
+ }
+ ~VideoException() throw() {}
+ const char* what() const throw() { return desc.c_str(); }
+ std::string desc;
+};
+
+struct PANGOLIN_EXPORT VideoExceptionNoKnownHandler : public VideoException
+{
+ VideoExceptionNoKnownHandler(const std::string& scheme)
+ : VideoException("No known video handler for URI '" + scheme + "'")
+ {
+ }
+};
+
+}
+
diff --git a/externals/Pangolin/include/pangolin/video/video_input.h b/externals/Pangolin/include/pangolin/video/video_input.h
new file mode 100644
index 0000000000000000000000000000000000000000..b496241a3bb8377516ef8071a0d4000294af3f35
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_input.h
@@ -0,0 +1,139 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/video/video.h>
+#include <pangolin/video/video_output.h>
+
+namespace pangolin
+{
+
+struct PANGOLIN_EXPORT VideoInput
+ : public VideoInterface,
+ public VideoFilterInterface
+{
+ /////////////////////////////////////////////////////////////
+ // VideoInterface Methods
+ /////////////////////////////////////////////////////////////
+
+ size_t SizeBytes() const override;
+ const std::vector<StreamInfo>& Streams() const override;
+ void Start() override;
+ void Stop() override;
+ bool GrabNext( unsigned char* image, bool wait = true ) override;
+ bool GrabNewest( unsigned char* image, bool wait = true ) override;
+
+ /////////////////////////////////////////////////////////////
+ // VideoFilterInterface Methods
+ /////////////////////////////////////////////////////////////
+
+ std::vector<VideoInterface*>& InputStreams() override
+ {
+ return videos;
+ }
+
+ /////////////////////////////////////////////////////////////
+ // VideoInput Methods
+ /////////////////////////////////////////////////////////////
+
+ VideoInput();
+ VideoInput(const std::string &input_uri, const std::string &output_uri = "pango:[buffer_size_mb=100]//video_log.pango");
+ ~VideoInput();
+
+ void Open(const std::string &input_uri, const std::string &output_uri = "pango:[buffer_size_mb=100]//video_log.pango");
+ void Close();
+
+ // experimental - not stable
+ bool Grab( unsigned char* buffer, std::vector<Image<unsigned char> >& images, bool wait = true, bool newest = false);
+
+ // Return details of first stream
+ unsigned int Width() const {
+ return (unsigned int)Streams()[0].Width();
+ }
+ unsigned int Height() const {
+ return (unsigned int)Streams()[0].Height();
+ }
+ PixelFormat PixFormat() const {
+ return Streams()[0].PixFormat();
+ }
+ const Uri& VideoUri() const {
+ return uri_input;
+ }
+
+ void Reset() {
+ Close();
+ Open(uri_input.full_uri, uri_output.full_uri);
+ }
+
+ // Return pointer to inner video class as VideoType
+ template<typename VideoType>
+ VideoType* Cast() {
+ return dynamic_cast<VideoType*>(video_src.get());
+ }
+
+ const std::string& LogFilename() const;
+ std::string& LogFilename();
+
+ // Switch to live video and record output to file
+ void Record();
+
+ // Switch to live video and record a single frame
+ void RecordOneFrame();
+
+ // Specify that one in n frames are logged to file. Default is 1.
+ void SetTimelapse(size_t one_in_n_frames);
+
+ // True iff grabbed live frames are being logged to file
+ bool IsRecording() const;
+
+protected:
+ void InitialiseRecorder();
+
+ Uri uri_input;
+ Uri uri_output;
+
+ std::unique_ptr<VideoInterface> video_src;
+ std::unique_ptr<VideoOutputInterface> video_recorder;
+
+ // Use to store either video_src or video_file for VideoFilterInterface,
+ // depending on which is active
+ std::vector<VideoInterface*> videos;
+
+ int buffer_size_bytes;
+
+ int frame_num;
+ size_t record_frame_skip;
+
+ bool record_once;
+ bool record_continuous;
+};
+
+// VideoInput subsumes the previous VideoRecordRepeat class.
+typedef VideoInput VideoRecordRepeat;
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video_interface.h b/externals/Pangolin/include/pangolin/video/video_interface.h
new file mode 100755
index 0000000000000000000000000000000000000000..f37302d7574afad4104cdadcc6d2ea4283a5668a
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_interface.h
@@ -0,0 +1,183 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <pangolin/utils/picojson.h>
+#include <pangolin/video/stream_info.h>
+
+#include <memory>
+#include <vector>
+
+#define PANGO_HAS_TIMING_DATA "has_timing_data"
+#define PANGO_HOST_RECEPTION_TIME_US "host_reception_time_us"
+#define PANGO_CAPTURE_TIME_US "capture_time_us"
+#define PANGO_EXPOSURE_US "exposure_us"
+#define PANGO_GAMMA "gamma"
+// analog gain is in linear scale and not dB
+#define PANGO_ANALOG_GAIN "analog_gain"
+#define PANGO_ANALOG_BLACK_LEVEL "analog_black_level"
+#define PANGO_SENSOR_TEMPERATURE_C "sensor_temperature_C"
+#define PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US "estimated_center_capture_time_us"
+#define PANGO_JOIN_OFFSET_US "join_offset_us"
+#define PANGO_FRAME_COUNTER "frame_counter"
+
+namespace pangolin {
+
+//! Interface to video capture sources
+struct PANGOLIN_EXPORT VideoInterface
+{
+ virtual ~VideoInterface() {}
+
+ //! Required buffer size to store all frames
+ virtual size_t SizeBytes() const = 0;
+
+ //! Get format and dimensions of all video streams
+ virtual const std::vector<StreamInfo>& Streams() const = 0;
+
+ //! Start Video device
+ virtual void Start() = 0;
+
+ //! Stop Video device
+ virtual void Stop() = 0;
+
+ //! Copy the next frame from the camera to image.
+ //! Optionally wait for a frame if one isn't ready
+ //! Returns true iff image was copied
+ virtual bool GrabNext( unsigned char* image, bool wait = true ) = 0;
+
+ //! Copy the newest frame from the camera to image
+ //! discarding all older frames.
+ //! Optionally wait for a frame if one isn't ready
+ //! Returns true iff image was copied
+ virtual bool GrabNewest( unsigned char* image, bool wait = true ) = 0;
+};
+
+//! Interface to GENICAM video capture sources
+struct PANGOLIN_EXPORT GenicamVideoInterface
+{
+ virtual ~GenicamVideoInterface() {}
+
+ virtual bool GetParameter(const std::string& name, std::string& result) = 0;
+
+ virtual bool SetParameter(const std::string& name, const std::string& value) = 0;
+
+ virtual size_t CameraCount() const
+ {
+ return 1;
+ }
+};
+
+struct PANGOLIN_EXPORT BufferAwareVideoInterface
+{
+ virtual ~BufferAwareVideoInterface() {}
+
+ //! Returns number of available frames
+ virtual uint32_t AvailableFrames() const = 0;
+
+ //! Drops N frames in the queue starting from the oldest
+ //! returns false if less than n frames arae available
+ virtual bool DropNFrames(uint32_t n) = 0;
+};
+
+struct PANGOLIN_EXPORT VideoPropertiesInterface
+{
+ virtual ~VideoPropertiesInterface() {}
+
+ //! Access JSON properties of device
+ virtual const picojson::value& DeviceProperties() const = 0;
+
+ //! Access JSON properties of most recently captured frame
+ virtual const picojson::value& FrameProperties() const = 0;
+};
+
+enum UvcRequestCode {
+ UVC_RC_UNDEFINED = 0x00,
+ UVC_SET_CUR = 0x01,
+ UVC_GET_CUR = 0x81,
+ UVC_GET_MIN = 0x82,
+ UVC_GET_MAX = 0x83,
+ UVC_GET_RES = 0x84,
+ UVC_GET_LEN = 0x85,
+ UVC_GET_INFO = 0x86,
+ UVC_GET_DEF = 0x87
+};
+
+struct PANGOLIN_EXPORT VideoFilterInterface
+{
+ virtual ~VideoFilterInterface() {}
+
+ template<typename T>
+ std::vector<T*> FindMatchingStreams()
+ {
+ std::vector<T*> matches;
+ std::vector<VideoInterface*> children = InputStreams();
+ for(size_t c=0; c < children.size(); ++c) {
+ T* concrete_video = dynamic_cast<T*>(children[c]);
+ if(concrete_video) {
+ matches.push_back(concrete_video);
+ }else{
+ VideoFilterInterface* filter_video = dynamic_cast<VideoFilterInterface*>(children[c]);
+ if(filter_video) {
+ std::vector<T*> child_matches = filter_video->FindMatchingStreams<T>();
+ matches.insert(matches.end(), child_matches.begin(), child_matches.end());
+ }
+ }
+ }
+ return matches;
+ }
+
+ virtual std::vector<VideoInterface*>& InputStreams() = 0;
+};
+
+struct PANGOLIN_EXPORT VideoUvcInterface
+{
+ virtual ~VideoUvcInterface() {}
+ virtual int IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code) = 0;
+ virtual bool GetExposure(int& exp_us) = 0;
+ virtual bool SetExposure(int exp_us) = 0;
+ virtual bool GetGain(float& gain) = 0;
+ virtual bool SetGain(float gain) = 0;
+};
+
+struct PANGOLIN_EXPORT VideoPlaybackInterface
+{
+ virtual ~VideoPlaybackInterface() {}
+
+ /// Return monotonic id of current frame
+ /// The 'current frame' is the frame returned from the last successful call to Grab
+ virtual size_t GetCurrentFrameId() const = 0;
+
+ /// Return total number of frames to be captured from device,
+ /// or 0 if unknown.
+ virtual size_t GetTotalFrames() const = 0;
+
+ /// Return frameid on success, or next frame on failure
+ virtual size_t Seek(size_t frameid) = 0;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video_output.h b/externals/Pangolin/include/pangolin/video/video_output.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b9105a0ce8b0b7a03fc3eb584144a9629be6131
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_output.h
@@ -0,0 +1,92 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+// Pangolin video output supports various formats using
+// different 3rd party libraries. (Only one right now)
+//
+// VideoOutput URI's take the following form:
+// scheme:[param1=value1,param2=value2,...]//device
+//
+// scheme = ffmpeg
+//
+// ffmpeg - encode to compressed file using ffmpeg
+// fps : fps to embed in encoded file.
+// bps : bits per second
+// unique_filename : append unique suffix if file already exists
+//
+// e.g. ffmpeg://output_file.avi
+// e.g. ffmpeg:[fps=30,bps=1000000,unique_filename]//output_file.avi
+
+#include <pangolin/video/video_output_interface.h>
+#include <pangolin/utils/uri.h>
+#include <memory>
+
+namespace pangolin
+{
+
+//! VideoOutput wrap to generically construct instances of VideoOutputInterface.
+class PANGOLIN_EXPORT VideoOutput : public VideoOutputInterface
+{
+public:
+ VideoOutput();
+ VideoOutput(const std::string& uri);
+ ~VideoOutput();
+
+ bool IsOpen() const;
+ void Open(const std::string& uri);
+ void Close();
+
+ const std::vector<StreamInfo>& Streams() const override;
+
+ void SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri = "", const picojson::value& properties = picojson::value() ) override;
+
+ int WriteStreams(const unsigned char* data, const picojson::value& frame_properties = picojson::value() ) override;
+
+ bool IsPipe() const override;
+
+ void AddStream(const PixelFormat& pf, size_t w,size_t h,size_t pitch);
+
+ void AddStream(const PixelFormat& pf, size_t w,size_t h);
+
+ void SetStreams(const std::string& uri = "", const picojson::value& properties = picojson::value() );
+
+ size_t SizeBytes(void) const ;
+
+ std::vector<Image<unsigned char>> GetOutputImages(unsigned char* buffer) const ;
+
+ std::vector<Image<unsigned char>> GetOutputImages(std::vector<unsigned char>& buffer) const ;
+
+
+protected:
+ std::vector<StreamInfo> streams;
+ Uri uri;
+ std::unique_ptr<VideoOutputInterface> recorder;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video_output_interface.h b/externals/Pangolin/include/pangolin/video/video_output_interface.h
new file mode 100644
index 0000000000000000000000000000000000000000..efbec4d1366940ff7a70d32cd563e6e54a1acc86
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_output_interface.h
@@ -0,0 +1,52 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+#include <vector>
+#include <pangolin/platform.h>
+#include <pangolin/video/stream_info.h>
+#include <pangolin/utils/picojson.h>
+
+namespace pangolin {
+
+//! Interface to video recording destinations
+struct PANGOLIN_EXPORT VideoOutputInterface
+{
+ virtual ~VideoOutputInterface() {}
+
+ //! Get format and dimensions of all video streams
+ virtual const std::vector<StreamInfo>& Streams() const = 0;
+
+ virtual void SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri ="", const picojson::value& properties = picojson::value() ) = 0;
+
+ virtual int WriteStreams(const unsigned char* data, const picojson::value& frame_properties = picojson::value() ) = 0;
+
+ virtual bool IsPipe() const = 0;
+};
+
+}
diff --git a/externals/Pangolin/include/pangolin/video/video_record_repeat.h b/externals/Pangolin/include/pangolin/video/video_record_repeat.h
new file mode 100644
index 0000000000000000000000000000000000000000..79e8571c5b055a1409fd1f26828596c7a16560c3
--- /dev/null
+++ b/externals/Pangolin/include/pangolin/video/video_record_repeat.h
@@ -0,0 +1,31 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#pragma once
+
+// VideoInput subsumes the previous VideoRecordRepeat class.
+#include <pangolin/video/video_input.h>
diff --git a/externals/Pangolin/include/tinyobj/tiny_obj_loader.h b/externals/Pangolin/include/tinyobj/tiny_obj_loader.h
new file mode 100644
index 0000000000000000000000000000000000000000..d19040c0dd91ceaba1158ae6696164d340e4e421
--- /dev/null
+++ b/externals/Pangolin/include/tinyobj/tiny_obj_loader.h
@@ -0,0 +1,2547 @@
+/*
+The MIT License (MIT)
+
+Copyright (c) 2012-2018 Syoyo Fujita and many contributors.
+
+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.
+*/
+
+//
+// version 1.3.1 : Make ParseTextureNameAndOption API public
+// version 1.3.0 : Separate warning and error message(breaking API of LoadObj)
+// version 1.2.3 : Added color space extension('-colorspace') to tex opts.
+// version 1.2.2 : Parse multiple group names.
+// version 1.2.1 : Added initial support for line('l') primitive(PR #178)
+// version 1.2.0 : Hardened implementation(#175)
+// version 1.1.1 : Support smoothing groups(#162)
+// version 1.1.0 : Support parsing vertex color(#144)
+// version 1.0.8 : Fix parsing `g` tag just after `usemtl`(#138)
+// version 1.0.7 : Support multiple tex options(#126)
+// version 1.0.6 : Add TINYOBJLOADER_USE_DOUBLE option(#124)
+// version 1.0.5 : Ignore `Tr` when `d` exists in MTL(#43)
+// version 1.0.4 : Support multiple filenames for 'mtllib'(#112)
+// version 1.0.3 : Support parsing texture options(#85)
+// version 1.0.2 : Improve parsing speed by about a factor of 2 for large
+// files(#105)
+// version 1.0.1 : Fixes a shape is lost if obj ends with a 'usemtl'(#104)
+// version 1.0.0 : Change data structure. Change license from BSD to MIT.
+//
+
+//
+// Use this in *one* .cc
+// #define TINYOBJLOADER_IMPLEMENTATION
+// #include "tiny_obj_loader.h"
+//
+
+#ifndef TINY_OBJ_LOADER_H_
+#define TINY_OBJ_LOADER_H_
+
+#include <map>
+#include <string>
+#include <vector>
+
+namespace tinyobj {
+
+#ifdef __clang__
+#pragma clang diagnostic push
+#if __has_warning("-Wzero-as-null-pointer-constant")
+#pragma clang diagnostic ignored "-Wzero-as-null-pointer-constant"
+#endif
+
+#pragma clang diagnostic ignored "-Wpadded"
+
+#endif
+
+// https://en.wikipedia.org/wiki/Wavefront_.obj_file says ...
+//
+// -blendu on | off # set horizontal texture blending
+// (default on)
+// -blendv on | off # set vertical texture blending
+// (default on)
+// -boost real_value # boost mip-map sharpness
+// -mm base_value gain_value # modify texture map values (default
+// 0 1)
+// # base_value = brightness,
+// gain_value = contrast
+// -o u [v [w]] # Origin offset (default
+// 0 0 0)
+// -s u [v [w]] # Scale (default
+// 1 1 1)
+// -t u [v [w]] # Turbulence (default
+// 0 0 0)
+// -texres resolution # texture resolution to create
+// -clamp on | off # only render texels in the clamped
+// 0-1 range (default off)
+// # When unclamped, textures are
+// repeated across a surface,
+// # when clamped, only texels which
+// fall within the 0-1
+// # range are rendered.
+// -bm mult_value # bump multiplier (for bump maps
+// only)
+//
+// -imfchan r | g | b | m | l | z # specifies which channel of the file
+// is used to
+// # create a scalar or bump texture.
+// r:red, g:green,
+// # b:blue, m:matte, l:luminance,
+// z:z-depth..
+// # (the default for bump is 'l' and
+// for decal is 'm')
+// bump -imfchan r bumpmap.tga # says to use the red channel of
+// bumpmap.tga as the bumpmap
+//
+// For reflection maps...
+//
+// -type sphere # specifies a sphere for a "refl"
+// reflection map
+// -type cube_top | cube_bottom | # when using a cube map, the texture
+// file for each
+// cube_front | cube_back | # side of the cube is specified
+// separately
+// cube_left | cube_right
+//
+// TinyObjLoader extension.
+//
+// -colorspace SPACE # Color space of the texture. e.g.
+// 'sRGB` or 'linear'
+//
+
+#ifdef TINYOBJLOADER_USE_DOUBLE
+//#pragma message "using double"
+typedef double real_t;
+#else
+//#pragma message "using float"
+typedef float real_t;
+#endif
+
+typedef enum {
+ TEXTURE_TYPE_NONE, // default
+ TEXTURE_TYPE_SPHERE,
+ TEXTURE_TYPE_CUBE_TOP,
+ TEXTURE_TYPE_CUBE_BOTTOM,
+ TEXTURE_TYPE_CUBE_FRONT,
+ TEXTURE_TYPE_CUBE_BACK,
+ TEXTURE_TYPE_CUBE_LEFT,
+ TEXTURE_TYPE_CUBE_RIGHT
+} texture_type_t;
+
+typedef struct {
+ texture_type_t type; // -type (default TEXTURE_TYPE_NONE)
+ real_t sharpness; // -boost (default 1.0?)
+ real_t brightness; // base_value in -mm option (default 0)
+ real_t contrast; // gain_value in -mm option (default 1)
+ real_t origin_offset[3]; // -o u [v [w]] (default 0 0 0)
+ real_t scale[3]; // -s u [v [w]] (default 1 1 1)
+ real_t turbulence[3]; // -t u [v [w]] (default 0 0 0)
+ // int texture_resolution; // -texres resolution (default = ?) TODO
+ bool clamp; // -clamp (default false)
+ char imfchan; // -imfchan (the default for bump is 'l' and for decal is 'm')
+ bool blendu; // -blendu (default on)
+ bool blendv; // -blendv (default on)
+ real_t bump_multiplier; // -bm (for bump maps only, default 1.0)
+
+ // extension
+ std::string colorspace; // Explicitly specify color space of stored value.
+ // Usually `sRGB` or `linear` (default empty).
+} texture_option_t;
+
+typedef struct {
+ std::string name;
+
+ real_t ambient[3];
+ real_t diffuse[3];
+ real_t specular[3];
+ real_t transmittance[3];
+ real_t emission[3];
+ real_t shininess;
+ real_t ior; // index of refraction
+ real_t dissolve; // 1 == opaque; 0 == fully transparent
+ // illumination model (see http://www.fileformat.info/format/material/)
+ int illum;
+
+ int dummy; // Suppress padding warning.
+
+ std::string ambient_texname; // map_Ka
+ std::string diffuse_texname; // map_Kd
+ std::string specular_texname; // map_Ks
+ std::string specular_highlight_texname; // map_Ns
+ std::string bump_texname; // map_bump, map_Bump, bump
+ std::string displacement_texname; // disp
+ std::string alpha_texname; // map_d
+ std::string reflection_texname; // refl
+
+ texture_option_t ambient_texopt;
+ texture_option_t diffuse_texopt;
+ texture_option_t specular_texopt;
+ texture_option_t specular_highlight_texopt;
+ texture_option_t bump_texopt;
+ texture_option_t displacement_texopt;
+ texture_option_t alpha_texopt;
+ texture_option_t reflection_texopt;
+
+ // PBR extension
+ // http://exocortex.com/blog/extending_wavefront_mtl_to_support_pbr
+ real_t roughness; // [0, 1] default 0
+ real_t metallic; // [0, 1] default 0
+ real_t sheen; // [0, 1] default 0
+ real_t clearcoat_thickness; // [0, 1] default 0
+ real_t clearcoat_roughness; // [0, 1] default 0
+ real_t anisotropy; // aniso. [0, 1] default 0
+ real_t anisotropy_rotation; // anisor. [0, 1] default 0
+ real_t pad0;
+ std::string roughness_texname; // map_Pr
+ std::string metallic_texname; // map_Pm
+ std::string sheen_texname; // map_Ps
+ std::string emissive_texname; // map_Ke
+ std::string normal_texname; // norm. For normal mapping.
+
+ texture_option_t roughness_texopt;
+ texture_option_t metallic_texopt;
+ texture_option_t sheen_texopt;
+ texture_option_t emissive_texopt;
+ texture_option_t normal_texopt;
+
+ int pad2;
+
+ std::map<std::string, std::string> unknown_parameter;
+} material_t;
+
+typedef struct {
+ std::string name;
+
+ std::vector<int> intValues;
+ std::vector<real_t> floatValues;
+ std::vector<std::string> stringValues;
+} tag_t;
+
+// Index struct to support different indices for vtx/normal/texcoord.
+// -1 means not used.
+typedef struct {
+ int vertex_index;
+ int normal_index;
+ int texcoord_index;
+} index_t;
+
+typedef struct {
+ std::vector<index_t> indices;
+ std::vector<unsigned char> num_face_vertices; // The number of vertices per
+ // face. 3 = polygon, 4 = quad,
+ // ... Up to 255.
+ std::vector<int> material_ids; // per-face material ID
+ std::vector<unsigned int> smoothing_group_ids; // per-face smoothing group
+ // ID(0 = off. positive value
+ // = group id)
+ std::vector<tag_t> tags; // SubD tag
+} mesh_t;
+
+typedef struct {
+ std::vector<int> indices; // pairs of indices for lines
+} path_t;
+
+typedef struct {
+ std::string name;
+ mesh_t mesh;
+ path_t path;
+} shape_t;
+
+// Vertex attributes
+typedef struct {
+ std::vector<real_t> vertices; // 'v'
+ std::vector<real_t> normals; // 'vn'
+ std::vector<real_t> texcoords; // 'vt'
+ std::vector<real_t> colors; // extension: vertex colors
+} attrib_t;
+
+typedef struct callback_t_ {
+ // W is optional and set to 1 if there is no `w` item in `v` line
+ void (*vertex_cb)(void *user_data, real_t x, real_t y, real_t z, real_t w);
+ void (*normal_cb)(void *user_data, real_t x, real_t y, real_t z);
+
+ // y and z are optional and set to 0 if there is no `y` and/or `z` item(s) in
+ // `vt` line.
+ void (*texcoord_cb)(void *user_data, real_t x, real_t y, real_t z);
+
+ // called per 'f' line. num_indices is the number of face indices(e.g. 3 for
+ // triangle, 4 for quad)
+ // 0 will be passed for undefined index in index_t members.
+ void (*index_cb)(void *user_data, index_t *indices, int num_indices);
+ // `name` material name, `material_id` = the array index of material_t[]. -1
+ // if
+ // a material not found in .mtl
+ void (*usemtl_cb)(void *user_data, const char *name, int material_id);
+ // `materials` = parsed material data.
+ void (*mtllib_cb)(void *user_data, const material_t *materials,
+ int num_materials);
+ // There may be multiple group names
+ void (*group_cb)(void *user_data, const char **names, int num_names);
+ void (*object_cb)(void *user_data, const char *name);
+
+ callback_t_()
+ : vertex_cb(NULL),
+ normal_cb(NULL),
+ texcoord_cb(NULL),
+ index_cb(NULL),
+ usemtl_cb(NULL),
+ mtllib_cb(NULL),
+ group_cb(NULL),
+ object_cb(NULL) {}
+} callback_t;
+
+class MaterialReader {
+ public:
+ MaterialReader() {}
+ virtual ~MaterialReader();
+
+ virtual bool operator()(const std::string &matId,
+ std::vector<material_t> *materials,
+ std::map<std::string, int> *matMap, std::string *warn,
+ std::string *err) = 0;
+};
+
+class MaterialFileReader : public MaterialReader {
+ public:
+ explicit MaterialFileReader(const std::string &mtl_basedir)
+ : m_mtlBaseDir(mtl_basedir) {}
+ virtual ~MaterialFileReader() {}
+ virtual bool operator()(const std::string &matId,
+ std::vector<material_t> *materials,
+ std::map<std::string, int> *matMap, std::string *warn,
+ std::string *err);
+
+ private:
+ std::string m_mtlBaseDir;
+};
+
+class MaterialStreamReader : public MaterialReader {
+ public:
+ explicit MaterialStreamReader(std::istream &inStream)
+ : m_inStream(inStream) {}
+ virtual ~MaterialStreamReader() {}
+ virtual bool operator()(const std::string &matId,
+ std::vector<material_t> *materials,
+ std::map<std::string, int> *matMap, std::string *warn,
+ std::string *err);
+
+ private:
+ std::istream &m_inStream;
+};
+
+/// Loads .obj from a file.
+/// 'attrib', 'shapes' and 'materials' will be filled with parsed shape data
+/// 'shapes' will be filled with parsed shape data
+/// Returns true when loading .obj become success.
+/// Returns warning message into `warn`, and error message into `err`
+/// 'mtl_basedir' is optional, and used for base directory for .mtl file.
+/// In default(`NULL'), .mtl file is searched from an application's working
+/// directory.
+/// 'triangulate' is optional, and used whether triangulate polygon face in .obj
+/// or not.
+/// Option 'default_vcols_fallback' specifies whether vertex colors should
+/// always be defined, even if no colors are given (fallback to white).
+bool LoadObj(attrib_t *attrib, std::vector<shape_t> *shapes,
+ std::vector<material_t> *materials, std::string *warn,
+ std::string *err, const char *filename,
+ const char *mtl_basedir = NULL, bool triangulate = true,
+ bool default_vcols_fallback = true);
+
+/// Loads .obj from a file with custom user callback.
+/// .mtl is loaded as usual and parsed material_t data will be passed to
+/// `callback.mtllib_cb`.
+/// Returns true when loading .obj/.mtl become success.
+/// Returns warning message into `warn`, and error message into `err`
+/// See `examples/callback_api/` for how to use this function.
+bool LoadObjWithCallback(std::istream &inStream, const callback_t &callback,
+ void *user_data = NULL,
+ MaterialReader *readMatFn = NULL,
+ std::string *warn = NULL, std::string *err = NULL);
+
+/// Loads object from a std::istream, uses GetMtlIStreamFn to retrieve
+/// std::istream for materials.
+/// Returns true when loading .obj become success.
+/// Returns warning and error message into `err`
+bool LoadObj(attrib_t *attrib, std::vector<shape_t> *shapes,
+ std::vector<material_t> *materials, std::string *warn,
+ std::string *err, std::istream *inStream,
+ MaterialReader *readMatFn = NULL, bool triangulate = true,
+ bool default_vcols_fallback = true);
+
+/// Loads materials into std::map
+void LoadMtl(std::map<std::string, int> *material_map,
+ std::vector<material_t> *materials, std::istream *inStream,
+ std::string *warning, std::string *err);
+
+///
+/// Parse texture name and texture option for custom texture parameter through material::unknown_parameter
+///
+/// @param[out] texname Parsed texture name
+/// @param[out] texopt Parsed texopt
+/// @param[in] linebuf Input string
+/// @param[in] is_bump Is this texture bump/normal?
+///
+bool ParseTextureNameAndOption(std::string *texname,
+ texture_option_t *texopt,
+ const char *linebuf,
+ const bool is_bump);
+} // namespace tinyobj
+
+#endif // TINY_OBJ_LOADER_H_
+
+#ifdef TINYOBJLOADER_IMPLEMENTATION
+#include <cassert>
+#include <cctype>
+#include <cmath>
+#include <cstddef>
+#include <cstdlib>
+#include <cstring>
+#include <limits>
+#include <utility>
+
+#include <fstream>
+#include <sstream>
+
+namespace tinyobj {
+
+MaterialReader::~MaterialReader() {}
+
+struct vertex_index_t {
+ int v_idx, vt_idx, vn_idx;
+ vertex_index_t() : v_idx(-1), vt_idx(-1), vn_idx(-1) {}
+ explicit vertex_index_t(int idx) : v_idx(idx), vt_idx(idx), vn_idx(idx) {}
+ vertex_index_t(int vidx, int vtidx, int vnidx)
+ : v_idx(vidx), vt_idx(vtidx), vn_idx(vnidx) {}
+};
+
+// Internal data structure for face representation
+// index + smoothing group.
+struct face_t {
+ unsigned int
+ smoothing_group_id; // smoothing group id. 0 = smoothing groupd is off.
+ int pad_;
+ std::vector<vertex_index_t> vertex_indices; // face vertex indices.
+
+ face_t() : smoothing_group_id(0) {}
+};
+
+struct line_t {
+ int idx0;
+ int idx1;
+};
+
+struct tag_sizes {
+ tag_sizes() : num_ints(0), num_reals(0), num_strings(0) {}
+ int num_ints;
+ int num_reals;
+ int num_strings;
+};
+
+struct obj_shape {
+ std::vector<real_t> v;
+ std::vector<real_t> vn;
+ std::vector<real_t> vt;
+};
+
+// See
+// http://stackoverflow.com/questions/6089231/getting-std-ifstream-to-handle-lf-cr-and-crlf
+static std::istream &safeGetline(std::istream &is, std::string &t) {
+ t.clear();
+
+ // The characters in the stream are read one-by-one using a std::streambuf.
+ // That is faster than reading them one-by-one using the std::istream.
+ // Code that uses streambuf this way must be guarded by a sentry object.
+ // The sentry object performs various tasks,
+ // such as thread synchronization and updating the stream state.
+
+ std::istream::sentry se(is, true);
+ std::streambuf *sb = is.rdbuf();
+
+ if (se) {
+ for (;;) {
+ int c = sb->sbumpc();
+ switch (c) {
+ case '\n':
+ return is;
+ case '\r':
+ if (sb->sgetc() == '\n') sb->sbumpc();
+ return is;
+ case EOF:
+ // Also handle the case when the last line has no line ending
+ if (t.empty()) is.setstate(std::ios::eofbit);
+ return is;
+ default:
+ t += static_cast<char>(c);
+ }
+ }
+ }
+
+ return is;
+}
+
+#define IS_SPACE(x) (((x) == ' ') || ((x) == '\t'))
+#define IS_DIGIT(x) \
+ (static_cast<unsigned int>((x) - '0') < static_cast<unsigned int>(10))
+#define IS_NEW_LINE(x) (((x) == '\r') || ((x) == '\n') || ((x) == '\0'))
+
+// Make index zero-base, and also support relative index.
+static inline bool fixIndex(int idx, int n, int *ret) {
+ if (!ret) {
+ return false;
+ }
+
+ if (idx > 0) {
+ (*ret) = idx - 1;
+ return true;
+ }
+
+ if (idx == 0) {
+ // zero is not allowed according to the spec.
+ return false;
+ }
+
+ if (idx < 0) {
+ (*ret) = n + idx; // negative value = relative
+ return true;
+ }
+
+ return false; // never reach here.
+}
+
+static inline std::string parseString(const char **token) {
+ std::string s;
+ (*token) += strspn((*token), " \t");
+ size_t e = strcspn((*token), " \t\r");
+ s = std::string((*token), &(*token)[e]);
+ (*token) += e;
+ return s;
+}
+
+static inline int parseInt(const char **token) {
+ (*token) += strspn((*token), " \t");
+ int i = atoi((*token));
+ (*token) += strcspn((*token), " \t\r");
+ return i;
+}
+
+// Tries to parse a floating point number located at s.
+//
+// s_end should be a location in the string where reading should absolutely
+// stop. For example at the end of the string, to prevent buffer overflows.
+//
+// Parses the following EBNF grammar:
+// sign = "+" | "-" ;
+// END = ? anything not in digit ?
+// digit = "0" | "1" | "2" | "3" | "4" | "5" | "6" | "7" | "8" | "9" ;
+// integer = [sign] , digit , {digit} ;
+// decimal = integer , ["." , integer] ;
+// float = ( decimal , END ) | ( decimal , ("E" | "e") , integer , END ) ;
+//
+// Valid strings are for example:
+// -0 +3.1417e+2 -0.0E-3 1.0324 -1.41 11e2
+//
+// If the parsing is a success, result is set to the parsed value and true
+// is returned.
+//
+// The function is greedy and will parse until any of the following happens:
+// - a non-conforming character is encountered.
+// - s_end is reached.
+//
+// The following situations triggers a failure:
+// - s >= s_end.
+// - parse failure.
+//
+static bool tryParseDouble(const char *s, const char *s_end, double *result) {
+ if (s >= s_end) {
+ return false;
+ }
+
+ double mantissa = 0.0;
+ // This exponent is base 2 rather than 10.
+ // However the exponent we parse is supposed to be one of ten,
+ // thus we must take care to convert the exponent/and or the
+ // mantissa to a * 2^E, where a is the mantissa and E is the
+ // exponent.
+ // To get the final double we will use ldexp, it requires the
+ // exponent to be in base 2.
+ int exponent = 0;
+
+ // NOTE: THESE MUST BE DECLARED HERE SINCE WE ARE NOT ALLOWED
+ // TO JUMP OVER DEFINITIONS.
+ char sign = '+';
+ char exp_sign = '+';
+ char const *curr = s;
+
+ // How many characters were read in a loop.
+ int read = 0;
+ // Tells whether a loop terminated due to reaching s_end.
+ bool end_not_reached = false;
+
+ /*
+ BEGIN PARSING.
+ */
+
+ // Find out what sign we've got.
+ if (*curr == '+' || *curr == '-') {
+ sign = *curr;
+ curr++;
+ } else if (IS_DIGIT(*curr)) { /* Pass through. */
+ } else {
+ goto fail;
+ }
+
+ // Read the integer part.
+ end_not_reached = (curr != s_end);
+ while (end_not_reached && IS_DIGIT(*curr)) {
+ mantissa *= 10;
+ mantissa += static_cast<int>(*curr - 0x30);
+ curr++;
+ read++;
+ end_not_reached = (curr != s_end);
+ }
+
+ // We must make sure we actually got something.
+ if (read == 0) goto fail;
+ // We allow numbers of form "#", "###" etc.
+ if (!end_not_reached) goto assemble;
+
+ // Read the decimal part.
+ if (*curr == '.') {
+ curr++;
+ read = 1;
+ end_not_reached = (curr != s_end);
+ while (end_not_reached && IS_DIGIT(*curr)) {
+ static const double pow_lut[] = {
+ 1.0, 0.1, 0.01, 0.001, 0.0001, 0.00001, 0.000001, 0.0000001,
+ };
+ const int lut_entries = sizeof pow_lut / sizeof pow_lut[0];
+
+ // NOTE: Don't use powf here, it will absolutely murder precision.
+ mantissa += static_cast<int>(*curr - 0x30) *
+ (read < lut_entries ? pow_lut[read] : std::pow(10.0, -read));
+ read++;
+ curr++;
+ end_not_reached = (curr != s_end);
+ }
+ } else if (*curr == 'e' || *curr == 'E') {
+ } else {
+ goto assemble;
+ }
+
+ if (!end_not_reached) goto assemble;
+
+ // Read the exponent part.
+ if (*curr == 'e' || *curr == 'E') {
+ curr++;
+ // Figure out if a sign is present and if it is.
+ end_not_reached = (curr != s_end);
+ if (end_not_reached && (*curr == '+' || *curr == '-')) {
+ exp_sign = *curr;
+ curr++;
+ } else if (IS_DIGIT(*curr)) { /* Pass through. */
+ } else {
+ // Empty E is not allowed.
+ goto fail;
+ }
+
+ read = 0;
+ end_not_reached = (curr != s_end);
+ while (end_not_reached && IS_DIGIT(*curr)) {
+ exponent *= 10;
+ exponent += static_cast<int>(*curr - 0x30);
+ curr++;
+ read++;
+ end_not_reached = (curr != s_end);
+ }
+ exponent *= (exp_sign == '+' ? 1 : -1);
+ if (read == 0) goto fail;
+ }
+
+assemble:
+ *result = (sign == '+' ? 1 : -1) *
+ (exponent ? std::ldexp(mantissa * std::pow(5.0, exponent), exponent)
+ : mantissa);
+ return true;
+fail:
+ return false;
+}
+
+static inline real_t parseReal(const char **token, double default_value = 0.0) {
+ (*token) += strspn((*token), " \t");
+ const char *end = (*token) + strcspn((*token), " \t\r");
+ double val = default_value;
+ tryParseDouble((*token), end, &val);
+ real_t f = static_cast<real_t>(val);
+ (*token) = end;
+ return f;
+}
+
+static inline bool parseReal(const char **token, real_t *out) {
+ (*token) += strspn((*token), " \t");
+ const char *end = (*token) + strcspn((*token), " \t\r");
+ double val;
+ bool ret = tryParseDouble((*token), end, &val);
+ if (ret) {
+ real_t f = static_cast<real_t>(val);
+ (*out) = f;
+ }
+ (*token) = end;
+ return ret;
+}
+
+static inline void parseReal2(real_t *x, real_t *y, const char **token,
+ const double default_x = 0.0,
+ const double default_y = 0.0) {
+ (*x) = parseReal(token, default_x);
+ (*y) = parseReal(token, default_y);
+}
+
+static inline void parseReal3(real_t *x, real_t *y, real_t *z,
+ const char **token, const double default_x = 0.0,
+ const double default_y = 0.0,
+ const double default_z = 0.0) {
+ (*x) = parseReal(token, default_x);
+ (*y) = parseReal(token, default_y);
+ (*z) = parseReal(token, default_z);
+}
+
+static inline void parseV(real_t *x, real_t *y, real_t *z, real_t *w,
+ const char **token, const double default_x = 0.0,
+ const double default_y = 0.0,
+ const double default_z = 0.0,
+ const double default_w = 1.0) {
+ (*x) = parseReal(token, default_x);
+ (*y) = parseReal(token, default_y);
+ (*z) = parseReal(token, default_z);
+ (*w) = parseReal(token, default_w);
+}
+
+// Extension: parse vertex with colors(6 items)
+static inline bool parseVertexWithColor(real_t *x, real_t *y, real_t *z,
+ real_t *r, real_t *g, real_t *b,
+ const char **token,
+ const double default_x = 0.0,
+ const double default_y = 0.0,
+ const double default_z = 0.0) {
+ (*x) = parseReal(token, default_x);
+ (*y) = parseReal(token, default_y);
+ (*z) = parseReal(token, default_z);
+
+ const bool found_color =
+ parseReal(token, r) && parseReal(token, g) && parseReal(token, b);
+
+ if (!found_color) {
+ (*r) = (*g) = (*b) = 1.0;
+ }
+
+ return found_color;
+}
+
+static inline bool parseOnOff(const char **token, bool default_value = true) {
+ (*token) += strspn((*token), " \t");
+ const char *end = (*token) + strcspn((*token), " \t\r");
+
+ bool ret = default_value;
+ if ((0 == strncmp((*token), "on", 2))) {
+ ret = true;
+ } else if ((0 == strncmp((*token), "off", 3))) {
+ ret = false;
+ }
+
+ (*token) = end;
+ return ret;
+}
+
+static inline texture_type_t parseTextureType(
+ const char **token, texture_type_t default_value = TEXTURE_TYPE_NONE) {
+ (*token) += strspn((*token), " \t");
+ const char *end = (*token) + strcspn((*token), " \t\r");
+ texture_type_t ty = default_value;
+
+ if ((0 == strncmp((*token), "cube_top", strlen("cube_top")))) {
+ ty = TEXTURE_TYPE_CUBE_TOP;
+ } else if ((0 == strncmp((*token), "cube_bottom", strlen("cube_bottom")))) {
+ ty = TEXTURE_TYPE_CUBE_BOTTOM;
+ } else if ((0 == strncmp((*token), "cube_left", strlen("cube_left")))) {
+ ty = TEXTURE_TYPE_CUBE_LEFT;
+ } else if ((0 == strncmp((*token), "cube_right", strlen("cube_right")))) {
+ ty = TEXTURE_TYPE_CUBE_RIGHT;
+ } else if ((0 == strncmp((*token), "cube_front", strlen("cube_front")))) {
+ ty = TEXTURE_TYPE_CUBE_FRONT;
+ } else if ((0 == strncmp((*token), "cube_back", strlen("cube_back")))) {
+ ty = TEXTURE_TYPE_CUBE_BACK;
+ } else if ((0 == strncmp((*token), "sphere", strlen("sphere")))) {
+ ty = TEXTURE_TYPE_SPHERE;
+ }
+
+ (*token) = end;
+ return ty;
+}
+
+static tag_sizes parseTagTriple(const char **token) {
+ tag_sizes ts;
+
+ (*token) += strspn((*token), " \t");
+ ts.num_ints = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ return ts;
+ }
+
+ (*token)++; // Skip '/'
+
+ (*token) += strspn((*token), " \t");
+ ts.num_reals = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ return ts;
+ }
+ (*token)++; // Skip '/'
+
+ ts.num_strings = parseInt(token);
+
+ return ts;
+}
+
+// Parse triples with index offsets: i, i/j/k, i//k, i/j
+static bool parseTriple(const char **token, int vsize, int vnsize, int vtsize,
+ vertex_index_t *ret) {
+ if (!ret) {
+ return false;
+ }
+
+ vertex_index_t vi(-1);
+
+ if (!fixIndex(atoi((*token)), vsize, &(vi.v_idx))) {
+ return false;
+ }
+
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ (*ret) = vi;
+ return true;
+ }
+ (*token)++;
+
+ // i//k
+ if ((*token)[0] == '/') {
+ (*token)++;
+ if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) {
+ return false;
+ }
+ (*token) += strcspn((*token), "/ \t\r");
+ (*ret) = vi;
+ return true;
+ }
+
+ // i/j/k or i/j
+ if (!fixIndex(atoi((*token)), vtsize, &(vi.vt_idx))) {
+ return false;
+ }
+
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ (*ret) = vi;
+ return true;
+ }
+
+ // i/j/k
+ (*token)++; // skip '/'
+ if (!fixIndex(atoi((*token)), vnsize, &(vi.vn_idx))) {
+ return false;
+ }
+ (*token) += strcspn((*token), "/ \t\r");
+
+ (*ret) = vi;
+
+ return true;
+}
+
+// Parse raw triples: i, i/j/k, i//k, i/j
+static vertex_index_t parseRawTriple(const char **token) {
+ vertex_index_t vi(static_cast<int>(0)); // 0 is an invalid index in OBJ
+
+ vi.v_idx = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ return vi;
+ }
+ (*token)++;
+
+ // i//k
+ if ((*token)[0] == '/') {
+ (*token)++;
+ vi.vn_idx = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ return vi;
+ }
+
+ // i/j/k or i/j
+ vi.vt_idx = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ if ((*token)[0] != '/') {
+ return vi;
+ }
+
+ // i/j/k
+ (*token)++; // skip '/'
+ vi.vn_idx = atoi((*token));
+ (*token) += strcspn((*token), "/ \t\r");
+ return vi;
+}
+
+bool ParseTextureNameAndOption(std::string *texname,
+ texture_option_t *texopt,
+ const char *linebuf, const bool is_bump) {
+ // @todo { write more robust lexer and parser. }
+ bool found_texname = false;
+ std::string texture_name;
+
+ // Fill with default value for texopt.
+ if (is_bump) {
+ texopt->imfchan = 'l';
+ } else {
+ texopt->imfchan = 'm';
+ }
+ texopt->bump_multiplier = static_cast<real_t>(1.0);
+ texopt->clamp = false;
+ texopt->blendu = true;
+ texopt->blendv = true;
+ texopt->sharpness = static_cast<real_t>(1.0);
+ texopt->brightness = static_cast<real_t>(0.0);
+ texopt->contrast = static_cast<real_t>(1.0);
+ texopt->origin_offset[0] = static_cast<real_t>(0.0);
+ texopt->origin_offset[1] = static_cast<real_t>(0.0);
+ texopt->origin_offset[2] = static_cast<real_t>(0.0);
+ texopt->scale[0] = static_cast<real_t>(1.0);
+ texopt->scale[1] = static_cast<real_t>(1.0);
+ texopt->scale[2] = static_cast<real_t>(1.0);
+ texopt->turbulence[0] = static_cast<real_t>(0.0);
+ texopt->turbulence[1] = static_cast<real_t>(0.0);
+ texopt->turbulence[2] = static_cast<real_t>(0.0);
+ texopt->type = TEXTURE_TYPE_NONE;
+
+ const char *token = linebuf; // Assume line ends with NULL
+
+ while (!IS_NEW_LINE((*token))) {
+ token += strspn(token, " \t"); // skip space
+ if ((0 == strncmp(token, "-blendu", 7)) && IS_SPACE((token[7]))) {
+ token += 8;
+ texopt->blendu = parseOnOff(&token, /* default */ true);
+ } else if ((0 == strncmp(token, "-blendv", 7)) && IS_SPACE((token[7]))) {
+ token += 8;
+ texopt->blendv = parseOnOff(&token, /* default */ true);
+ } else if ((0 == strncmp(token, "-clamp", 6)) && IS_SPACE((token[6]))) {
+ token += 7;
+ texopt->clamp = parseOnOff(&token, /* default */ true);
+ } else if ((0 == strncmp(token, "-boost", 6)) && IS_SPACE((token[6]))) {
+ token += 7;
+ texopt->sharpness = parseReal(&token, 1.0);
+ } else if ((0 == strncmp(token, "-bm", 3)) && IS_SPACE((token[3]))) {
+ token += 4;
+ texopt->bump_multiplier = parseReal(&token, 1.0);
+ } else if ((0 == strncmp(token, "-o", 2)) && IS_SPACE((token[2]))) {
+ token += 3;
+ parseReal3(&(texopt->origin_offset[0]), &(texopt->origin_offset[1]),
+ &(texopt->origin_offset[2]), &token);
+ } else if ((0 == strncmp(token, "-s", 2)) && IS_SPACE((token[2]))) {
+ token += 3;
+ parseReal3(&(texopt->scale[0]), &(texopt->scale[1]), &(texopt->scale[2]),
+ &token, 1.0, 1.0, 1.0);
+ } else if ((0 == strncmp(token, "-t", 2)) && IS_SPACE((token[2]))) {
+ token += 3;
+ parseReal3(&(texopt->turbulence[0]), &(texopt->turbulence[1]),
+ &(texopt->turbulence[2]), &token);
+ } else if ((0 == strncmp(token, "-type", 5)) && IS_SPACE((token[5]))) {
+ token += 5;
+ texopt->type = parseTextureType((&token), TEXTURE_TYPE_NONE);
+ } else if ((0 == strncmp(token, "-imfchan", 8)) && IS_SPACE((token[8]))) {
+ token += 9;
+ token += strspn(token, " \t");
+ const char *end = token + strcspn(token, " \t\r");
+ if ((end - token) == 1) { // Assume one char for -imfchan
+ texopt->imfchan = (*token);
+ }
+ token = end;
+ } else if ((0 == strncmp(token, "-mm", 3)) && IS_SPACE((token[3]))) {
+ token += 4;
+ parseReal2(&(texopt->brightness), &(texopt->contrast), &token, 0.0, 1.0);
+ } else if ((0 == strncmp(token, "-colorspace", 11)) &&
+ IS_SPACE((token[11]))) {
+ token += 12;
+ texopt->colorspace = parseString(&token);
+ } else {
+// Assume texture filename
+#if 0
+ size_t len = strcspn(token, " \t\r"); // untile next space
+ texture_name = std::string(token, token + len);
+ token += len;
+
+ token += strspn(token, " \t"); // skip space
+#else
+ // Read filename until line end to parse filename containing whitespace
+ // TODO(syoyo): Support parsing texture option flag after the filename.
+ texture_name = std::string(token);
+ token += texture_name.length();
+#endif
+
+ found_texname = true;
+ }
+ }
+
+ if (found_texname) {
+ (*texname) = texture_name;
+ return true;
+ } else {
+ return false;
+ }
+}
+
+static void InitMaterial(material_t *material) {
+ material->name = "";
+ material->ambient_texname = "";
+ material->diffuse_texname = "";
+ material->specular_texname = "";
+ material->specular_highlight_texname = "";
+ material->bump_texname = "";
+ material->displacement_texname = "";
+ material->reflection_texname = "";
+ material->alpha_texname = "";
+ for (int i = 0; i < 3; i++) {
+ material->ambient[i] = static_cast<real_t>(0.0);
+ material->diffuse[i] = static_cast<real_t>(0.0);
+ material->specular[i] = static_cast<real_t>(0.0);
+ material->transmittance[i] = static_cast<real_t>(0.0);
+ material->emission[i] = static_cast<real_t>(0.0);
+ }
+ material->illum = 0;
+ material->dissolve = static_cast<real_t>(1.0);
+ material->shininess = static_cast<real_t>(1.0);
+ material->ior = static_cast<real_t>(1.0);
+
+ material->roughness = static_cast<real_t>(0.0);
+ material->metallic = static_cast<real_t>(0.0);
+ material->sheen = static_cast<real_t>(0.0);
+ material->clearcoat_thickness = static_cast<real_t>(0.0);
+ material->clearcoat_roughness = static_cast<real_t>(0.0);
+ material->anisotropy_rotation = static_cast<real_t>(0.0);
+ material->anisotropy = static_cast<real_t>(0.0);
+ material->roughness_texname = "";
+ material->metallic_texname = "";
+ material->sheen_texname = "";
+ material->emissive_texname = "";
+ material->normal_texname = "";
+
+ material->unknown_parameter.clear();
+}
+
+// code from https://wrf.ecse.rpi.edu//Research/Short_Notes/pnpoly.html
+template <typename T>
+static int pnpoly(int nvert, T *vertx, T *verty, T testx, T testy) {
+ int i, j, c = 0;
+ for (i = 0, j = nvert - 1; i < nvert; j = i++) {
+ if (((verty[i] > testy) != (verty[j] > testy)) &&
+ (testx <
+ (vertx[j] - vertx[i]) * (testy - verty[i]) / (verty[j] - verty[i]) +
+ vertx[i]))
+ c = !c;
+ }
+ return c;
+}
+
+// TODO(syoyo): refactor function.
+static bool exportGroupsToShape(shape_t *shape,
+ const std::vector<face_t> &faceGroup,
+ std::vector<int> &lineGroup,
+ const std::vector<tag_t> &tags,
+ const int material_id, const std::string &name,
+ bool triangulate,
+ const std::vector<real_t> &v) {
+ if (faceGroup.empty() && lineGroup.empty()) {
+ return false;
+ }
+
+ if (!faceGroup.empty()) {
+ // Flatten vertices and indices
+ for (size_t i = 0; i < faceGroup.size(); i++) {
+ const face_t &face = faceGroup[i];
+
+ size_t npolys = face.vertex_indices.size();
+
+ if (npolys < 3) {
+ // Face must have 3+ vertices.
+ continue;
+ }
+
+ vertex_index_t i0 = face.vertex_indices[0];
+ vertex_index_t i1(-1);
+ vertex_index_t i2 = face.vertex_indices[1];
+
+ if (triangulate) {
+ // find the two axes to work in
+ size_t axes[2] = {1, 2};
+ for (size_t k = 0; k < npolys; ++k) {
+ i0 = face.vertex_indices[(k + 0) % npolys];
+ i1 = face.vertex_indices[(k + 1) % npolys];
+ i2 = face.vertex_indices[(k + 2) % npolys];
+ size_t vi0 = size_t(i0.v_idx);
+ size_t vi1 = size_t(i1.v_idx);
+ size_t vi2 = size_t(i2.v_idx);
+
+ if (((3 * vi0 + 2) >= v.size()) || ((3 * vi1 + 2) >= v.size()) ||
+ ((3 * vi2 + 2) >= v.size())) {
+ // Invalid triangle.
+ // FIXME(syoyo): Is it ok to simply skip this invalid triangle?
+ continue;
+ }
+ real_t v0x = v[vi0 * 3 + 0];
+ real_t v0y = v[vi0 * 3 + 1];
+ real_t v0z = v[vi0 * 3 + 2];
+ real_t v1x = v[vi1 * 3 + 0];
+ real_t v1y = v[vi1 * 3 + 1];
+ real_t v1z = v[vi1 * 3 + 2];
+ real_t v2x = v[vi2 * 3 + 0];
+ real_t v2y = v[vi2 * 3 + 1];
+ real_t v2z = v[vi2 * 3 + 2];
+ real_t e0x = v1x - v0x;
+ real_t e0y = v1y - v0y;
+ real_t e0z = v1z - v0z;
+ real_t e1x = v2x - v1x;
+ real_t e1y = v2y - v1y;
+ real_t e1z = v2z - v1z;
+ real_t cx = std::fabs(e0y * e1z - e0z * e1y);
+ real_t cy = std::fabs(e0z * e1x - e0x * e1z);
+ real_t cz = std::fabs(e0x * e1y - e0y * e1x);
+ const real_t epsilon = std::numeric_limits<real_t>::epsilon();
+ if (cx > epsilon || cy > epsilon || cz > epsilon) {
+ // found a corner
+ if (cx > cy && cx > cz) {
+ } else {
+ axes[0] = 0;
+ if (cz > cx && cz > cy) axes[1] = 1;
+ }
+ break;
+ }
+ }
+
+ real_t area = 0;
+ for (size_t k = 0; k < npolys; ++k) {
+ i0 = face.vertex_indices[(k + 0) % npolys];
+ i1 = face.vertex_indices[(k + 1) % npolys];
+ size_t vi0 = size_t(i0.v_idx);
+ size_t vi1 = size_t(i1.v_idx);
+ if (((vi0 * 3 + axes[0]) >= v.size()) ||
+ ((vi0 * 3 + axes[1]) >= v.size()) ||
+ ((vi1 * 3 + axes[0]) >= v.size()) ||
+ ((vi1 * 3 + axes[1]) >= v.size())) {
+ // Invalid index.
+ continue;
+ }
+ real_t v0x = v[vi0 * 3 + axes[0]];
+ real_t v0y = v[vi0 * 3 + axes[1]];
+ real_t v1x = v[vi1 * 3 + axes[0]];
+ real_t v1y = v[vi1 * 3 + axes[1]];
+ area += (v0x * v1y - v0y * v1x) * static_cast<real_t>(0.5);
+ }
+
+ int maxRounds = 10; // arbitrary max loop count to protect against
+ // unexpected errors
+
+ face_t remainingFace = face; // copy
+ size_t guess_vert = 0;
+ vertex_index_t ind[3];
+ real_t vx[3];
+ real_t vy[3];
+ while (remainingFace.vertex_indices.size() > 3 && maxRounds > 0) {
+ npolys = remainingFace.vertex_indices.size();
+ if (guess_vert >= npolys) {
+ maxRounds -= 1;
+ guess_vert -= npolys;
+ }
+ for (size_t k = 0; k < 3; k++) {
+ ind[k] = remainingFace.vertex_indices[(guess_vert + k) % npolys];
+ size_t vi = size_t(ind[k].v_idx);
+ if (((vi * 3 + axes[0]) >= v.size()) ||
+ ((vi * 3 + axes[1]) >= v.size())) {
+ // ???
+ vx[k] = static_cast<real_t>(0.0);
+ vy[k] = static_cast<real_t>(0.0);
+ } else {
+ vx[k] = v[vi * 3 + axes[0]];
+ vy[k] = v[vi * 3 + axes[1]];
+ }
+ }
+ real_t e0x = vx[1] - vx[0];
+ real_t e0y = vy[1] - vy[0];
+ real_t e1x = vx[2] - vx[1];
+ real_t e1y = vy[2] - vy[1];
+ real_t cross = e0x * e1y - e0y * e1x;
+ // if an internal angle
+ if (cross * area < static_cast<real_t>(0.0)) {
+ guess_vert += 1;
+ continue;
+ }
+
+ // check all other verts in case they are inside this triangle
+ bool overlap = false;
+ for (size_t otherVert = 3; otherVert < npolys; ++otherVert) {
+ size_t idx = (guess_vert + otherVert) % npolys;
+
+ if (idx >= remainingFace.vertex_indices.size()) {
+ // ???
+ continue;
+ }
+
+ size_t ovi = size_t(remainingFace.vertex_indices[idx].v_idx);
+
+ if (((ovi * 3 + axes[0]) >= v.size()) ||
+ ((ovi * 3 + axes[1]) >= v.size())) {
+ // ???
+ continue;
+ }
+ real_t tx = v[ovi * 3 + axes[0]];
+ real_t ty = v[ovi * 3 + axes[1]];
+ if (pnpoly(3, vx, vy, tx, ty)) {
+ overlap = true;
+ break;
+ }
+ }
+
+ if (overlap) {
+ guess_vert += 1;
+ continue;
+ }
+
+ // this triangle is an ear
+ {
+ index_t idx0, idx1, idx2;
+ idx0.vertex_index = ind[0].v_idx;
+ idx0.normal_index = ind[0].vn_idx;
+ idx0.texcoord_index = ind[0].vt_idx;
+ idx1.vertex_index = ind[1].v_idx;
+ idx1.normal_index = ind[1].vn_idx;
+ idx1.texcoord_index = ind[1].vt_idx;
+ idx2.vertex_index = ind[2].v_idx;
+ idx2.normal_index = ind[2].vn_idx;
+ idx2.texcoord_index = ind[2].vt_idx;
+
+ shape->mesh.indices.push_back(idx0);
+ shape->mesh.indices.push_back(idx1);
+ shape->mesh.indices.push_back(idx2);
+
+ shape->mesh.num_face_vertices.push_back(3);
+ shape->mesh.material_ids.push_back(material_id);
+ shape->mesh.smoothing_group_ids.push_back(face.smoothing_group_id);
+ }
+
+ // remove v1 from the list
+ size_t removed_vert_index = (guess_vert + 1) % npolys;
+ while (removed_vert_index + 1 < npolys) {
+ remainingFace.vertex_indices[removed_vert_index] =
+ remainingFace.vertex_indices[removed_vert_index + 1];
+ removed_vert_index += 1;
+ }
+ remainingFace.vertex_indices.pop_back();
+ }
+
+ if (remainingFace.vertex_indices.size() == 3) {
+ i0 = remainingFace.vertex_indices[0];
+ i1 = remainingFace.vertex_indices[1];
+ i2 = remainingFace.vertex_indices[2];
+ {
+ index_t idx0, idx1, idx2;
+ idx0.vertex_index = i0.v_idx;
+ idx0.normal_index = i0.vn_idx;
+ idx0.texcoord_index = i0.vt_idx;
+ idx1.vertex_index = i1.v_idx;
+ idx1.normal_index = i1.vn_idx;
+ idx1.texcoord_index = i1.vt_idx;
+ idx2.vertex_index = i2.v_idx;
+ idx2.normal_index = i2.vn_idx;
+ idx2.texcoord_index = i2.vt_idx;
+
+ shape->mesh.indices.push_back(idx0);
+ shape->mesh.indices.push_back(idx1);
+ shape->mesh.indices.push_back(idx2);
+
+ shape->mesh.num_face_vertices.push_back(3);
+ shape->mesh.material_ids.push_back(material_id);
+ shape->mesh.smoothing_group_ids.push_back(face.smoothing_group_id);
+ }
+ }
+ } else {
+ for (size_t k = 0; k < npolys; k++) {
+ index_t idx;
+ idx.vertex_index = face.vertex_indices[k].v_idx;
+ idx.normal_index = face.vertex_indices[k].vn_idx;
+ idx.texcoord_index = face.vertex_indices[k].vt_idx;
+ shape->mesh.indices.push_back(idx);
+ }
+
+ shape->mesh.num_face_vertices.push_back(
+ static_cast<unsigned char>(npolys));
+ shape->mesh.material_ids.push_back(material_id); // per face
+ shape->mesh.smoothing_group_ids.push_back(
+ face.smoothing_group_id); // per face
+ }
+ }
+
+ shape->name = name;
+ shape->mesh.tags = tags;
+ }
+
+ if (!lineGroup.empty()) {
+ shape->path.indices.swap(lineGroup);
+ }
+
+ return true;
+}
+
+// Split a string with specified delimiter character.
+// http://stackoverflow.com/questions/236129/split-a-string-in-c
+static void SplitString(const std::string &s, char delim,
+ std::vector<std::string> &elems) {
+ std::stringstream ss;
+ ss.str(s);
+ std::string item;
+ while (std::getline(ss, item, delim)) {
+ elems.push_back(item);
+ }
+}
+
+void LoadMtl(std::map<std::string, int> *material_map,
+ std::vector<material_t> *materials, std::istream *inStream,
+ std::string *warning, std::string *err) {
+ (void)err;
+
+ // Create a default material anyway.
+ material_t material;
+ InitMaterial(&material);
+
+ // Issue 43. `d` wins against `Tr` since `Tr` is not in the MTL specification.
+ bool has_d = false;
+ bool has_tr = false;
+
+ std::stringstream warn_ss;
+
+ size_t line_no = 0;
+ std::string linebuf;
+ while (inStream->peek() != -1) {
+ safeGetline(*inStream, linebuf);
+ line_no++;
+
+ // Trim trailing whitespace.
+ if (linebuf.size() > 0) {
+ linebuf = linebuf.substr(0, linebuf.find_last_not_of(" \t") + 1);
+ }
+
+ // Trim newline '\r\n' or '\n'
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\n')
+ linebuf.erase(linebuf.size() - 1);
+ }
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\r')
+ linebuf.erase(linebuf.size() - 1);
+ }
+
+ // Skip if empty line.
+ if (linebuf.empty()) {
+ continue;
+ }
+
+ // Skip leading space.
+ const char *token = linebuf.c_str();
+ token += strspn(token, " \t");
+
+ assert(token);
+ if (token[0] == '\0') continue; // empty line
+
+ if (token[0] == '#') continue; // comment line
+
+ // new mtl
+ if ((0 == strncmp(token, "newmtl", 6)) && IS_SPACE((token[6]))) {
+ // flush previous material.
+ if (!material.name.empty()) {
+ material_map->insert(std::pair<std::string, int>(
+ material.name, static_cast<int>(materials->size())));
+ materials->push_back(material);
+ }
+
+ // initial temporary material
+ InitMaterial(&material);
+
+ has_d = false;
+ has_tr = false;
+
+ // set new mtl name
+ token += 7;
+ {
+ std::stringstream sstr;
+ sstr << token;
+ material.name = sstr.str();
+ }
+ continue;
+ }
+
+ // ambient
+ if (token[0] == 'K' && token[1] == 'a' && IS_SPACE((token[2]))) {
+ token += 2;
+ real_t r, g, b;
+ parseReal3(&r, &g, &b, &token);
+ material.ambient[0] = r;
+ material.ambient[1] = g;
+ material.ambient[2] = b;
+ continue;
+ }
+
+ // diffuse
+ if (token[0] == 'K' && token[1] == 'd' && IS_SPACE((token[2]))) {
+ token += 2;
+ real_t r, g, b;
+ parseReal3(&r, &g, &b, &token);
+ material.diffuse[0] = r;
+ material.diffuse[1] = g;
+ material.diffuse[2] = b;
+ continue;
+ }
+
+ // specular
+ if (token[0] == 'K' && token[1] == 's' && IS_SPACE((token[2]))) {
+ token += 2;
+ real_t r, g, b;
+ parseReal3(&r, &g, &b, &token);
+ material.specular[0] = r;
+ material.specular[1] = g;
+ material.specular[2] = b;
+ continue;
+ }
+
+ // transmittance
+ if ((token[0] == 'K' && token[1] == 't' && IS_SPACE((token[2]))) ||
+ (token[0] == 'T' && token[1] == 'f' && IS_SPACE((token[2])))) {
+ token += 2;
+ real_t r, g, b;
+ parseReal3(&r, &g, &b, &token);
+ material.transmittance[0] = r;
+ material.transmittance[1] = g;
+ material.transmittance[2] = b;
+ continue;
+ }
+
+ // ior(index of refraction)
+ if (token[0] == 'N' && token[1] == 'i' && IS_SPACE((token[2]))) {
+ token += 2;
+ material.ior = parseReal(&token);
+ continue;
+ }
+
+ // emission
+ if (token[0] == 'K' && token[1] == 'e' && IS_SPACE(token[2])) {
+ token += 2;
+ real_t r, g, b;
+ parseReal3(&r, &g, &b, &token);
+ material.emission[0] = r;
+ material.emission[1] = g;
+ material.emission[2] = b;
+ continue;
+ }
+
+ // shininess
+ if (token[0] == 'N' && token[1] == 's' && IS_SPACE(token[2])) {
+ token += 2;
+ material.shininess = parseReal(&token);
+ continue;
+ }
+
+ // illum model
+ if (0 == strncmp(token, "illum", 5) && IS_SPACE(token[5])) {
+ token += 6;
+ material.illum = parseInt(&token);
+ continue;
+ }
+
+ // dissolve
+ if ((token[0] == 'd' && IS_SPACE(token[1]))) {
+ token += 1;
+ material.dissolve = parseReal(&token);
+
+ if (has_tr) {
+ warn_ss << "Both `d` and `Tr` parameters defined for \""
+ << material.name << "\". Use the value of `d` for dissolve (line "
+ << line_no << " in .mtl.)"
+ << std::endl;
+ }
+ has_d = true;
+ continue;
+ }
+ if (token[0] == 'T' && token[1] == 'r' && IS_SPACE(token[2])) {
+ token += 2;
+ if (has_d) {
+ // `d` wins. Ignore `Tr` value.
+ warn_ss << "Both `d` and `Tr` parameters defined for \""
+ << material.name << "\". Use the value of `d` for dissolve (line "
+ << line_no << " in .mtl.)"
+ << std::endl;
+ } else {
+ // We invert value of Tr(assume Tr is in range [0, 1])
+ // NOTE: Interpretation of Tr is application(exporter) dependent. For
+ // some application(e.g. 3ds max obj exporter), Tr = d(Issue 43)
+ material.dissolve = static_cast<real_t>(1.0) - parseReal(&token);
+ }
+ has_tr = true;
+ continue;
+ }
+
+ // PBR: roughness
+ if (token[0] == 'P' && token[1] == 'r' && IS_SPACE(token[2])) {
+ token += 2;
+ material.roughness = parseReal(&token);
+ continue;
+ }
+
+ // PBR: metallic
+ if (token[0] == 'P' && token[1] == 'm' && IS_SPACE(token[2])) {
+ token += 2;
+ material.metallic = parseReal(&token);
+ continue;
+ }
+
+ // PBR: sheen
+ if (token[0] == 'P' && token[1] == 's' && IS_SPACE(token[2])) {
+ token += 2;
+ material.sheen = parseReal(&token);
+ continue;
+ }
+
+ // PBR: clearcoat thickness
+ if (token[0] == 'P' && token[1] == 'c' && IS_SPACE(token[2])) {
+ token += 2;
+ material.clearcoat_thickness = parseReal(&token);
+ continue;
+ }
+
+ // PBR: clearcoat roughness
+ if ((0 == strncmp(token, "Pcr", 3)) && IS_SPACE(token[3])) {
+ token += 4;
+ material.clearcoat_roughness = parseReal(&token);
+ continue;
+ }
+
+ // PBR: anisotropy
+ if ((0 == strncmp(token, "aniso", 5)) && IS_SPACE(token[5])) {
+ token += 6;
+ material.anisotropy = parseReal(&token);
+ continue;
+ }
+
+ // PBR: anisotropy rotation
+ if ((0 == strncmp(token, "anisor", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ material.anisotropy_rotation = parseReal(&token);
+ continue;
+ }
+
+ // ambient texture
+ if ((0 == strncmp(token, "map_Ka", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.ambient_texname),
+ &(material.ambient_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // diffuse texture
+ if ((0 == strncmp(token, "map_Kd", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.diffuse_texname),
+ &(material.diffuse_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // specular texture
+ if ((0 == strncmp(token, "map_Ks", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.specular_texname),
+ &(material.specular_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // specular highlight texture
+ if ((0 == strncmp(token, "map_Ns", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.specular_highlight_texname),
+ &(material.specular_highlight_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // bump texture
+ if ((0 == strncmp(token, "map_bump", 8)) && IS_SPACE(token[8])) {
+ token += 9;
+ ParseTextureNameAndOption(&(material.bump_texname),
+ &(material.bump_texopt), token,
+ /* is_bump */ true);
+ continue;
+ }
+
+ // bump texture
+ if ((0 == strncmp(token, "map_Bump", 8)) && IS_SPACE(token[8])) {
+ token += 9;
+ ParseTextureNameAndOption(&(material.bump_texname),
+ &(material.bump_texopt), token,
+ /* is_bump */ true);
+ continue;
+ }
+
+ // bump texture
+ if ((0 == strncmp(token, "bump", 4)) && IS_SPACE(token[4])) {
+ token += 5;
+ ParseTextureNameAndOption(&(material.bump_texname),
+ &(material.bump_texopt), token,
+ /* is_bump */ true);
+ continue;
+ }
+
+ // alpha texture
+ if ((0 == strncmp(token, "map_d", 5)) && IS_SPACE(token[5])) {
+ token += 6;
+ material.alpha_texname = token;
+ ParseTextureNameAndOption(&(material.alpha_texname),
+ &(material.alpha_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // displacement texture
+ if ((0 == strncmp(token, "disp", 4)) && IS_SPACE(token[4])) {
+ token += 5;
+ ParseTextureNameAndOption(&(material.displacement_texname),
+ &(material.displacement_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // reflection map
+ if ((0 == strncmp(token, "refl", 4)) && IS_SPACE(token[4])) {
+ token += 5;
+ ParseTextureNameAndOption(&(material.reflection_texname),
+ &(material.reflection_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // PBR: roughness texture
+ if ((0 == strncmp(token, "map_Pr", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.roughness_texname),
+ &(material.roughness_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // PBR: metallic texture
+ if ((0 == strncmp(token, "map_Pm", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.metallic_texname),
+ &(material.metallic_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // PBR: sheen texture
+ if ((0 == strncmp(token, "map_Ps", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.sheen_texname),
+ &(material.sheen_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // PBR: emissive texture
+ if ((0 == strncmp(token, "map_Ke", 6)) && IS_SPACE(token[6])) {
+ token += 7;
+ ParseTextureNameAndOption(&(material.emissive_texname),
+ &(material.emissive_texopt), token,
+ /* is_bump */ false);
+ continue;
+ }
+
+ // PBR: normal map texture
+ if ((0 == strncmp(token, "norm", 4)) && IS_SPACE(token[4])) {
+ token += 5;
+ ParseTextureNameAndOption(
+ &(material.normal_texname), &(material.normal_texopt), token,
+ /* is_bump */ false); // @fixme { is_bump will be true? }
+ continue;
+ }
+
+ // unknown parameter
+ const char *_space = strchr(token, ' ');
+ if (!_space) {
+ _space = strchr(token, '\t');
+ }
+ if (_space) {
+ std::ptrdiff_t len = _space - token;
+ std::string key(token, static_cast<size_t>(len));
+ std::string value = _space + 1;
+ material.unknown_parameter.insert(
+ std::pair<std::string, std::string>(key, value));
+ }
+ }
+ // flush last material.
+ material_map->insert(std::pair<std::string, int>(
+ material.name, static_cast<int>(materials->size())));
+ materials->push_back(material);
+
+ if (warning) {
+ (*warning) = warn_ss.str();
+ }
+}
+
+bool MaterialFileReader::operator()(const std::string &matId,
+ std::vector<material_t> *materials,
+ std::map<std::string, int> *matMap,
+ std::string *warn, std::string *err) {
+ std::string filepath;
+
+ if (!m_mtlBaseDir.empty()) {
+ filepath = std::string(m_mtlBaseDir) + matId;
+ } else {
+ filepath = matId;
+ }
+
+ std::ifstream matIStream(filepath.c_str());
+ if (!matIStream) {
+ std::stringstream ss;
+ ss << "Material file [ " << filepath << " ] not found." << std::endl;
+ if (warn) {
+ (*warn) += ss.str();
+ }
+ return false;
+ }
+
+ LoadMtl(matMap, materials, &matIStream, warn, err);
+
+ return true;
+}
+
+bool MaterialStreamReader::operator()(const std::string &matId,
+ std::vector<material_t> *materials,
+ std::map<std::string, int> *matMap,
+ std::string *warn, std::string *err) {
+ (void)err;
+ (void)matId;
+ if (!m_inStream) {
+ std::stringstream ss;
+ ss << "Material stream in error state. " << std::endl;
+ if (warn) {
+ (*warn) += ss.str();
+ }
+ return false;
+ }
+
+ LoadMtl(matMap, materials, &m_inStream, warn, err);
+
+ return true;
+}
+
+bool LoadObj(attrib_t *attrib, std::vector<shape_t> *shapes,
+ std::vector<material_t> *materials, std::string *warn,
+ std::string *err, const char *filename, const char *mtl_basedir,
+ bool trianglulate, bool default_vcols_fallback) {
+ attrib->vertices.clear();
+ attrib->normals.clear();
+ attrib->texcoords.clear();
+ attrib->colors.clear();
+ shapes->clear();
+
+ std::stringstream errss;
+
+ std::ifstream ifs(filename);
+ if (!ifs) {
+ errss << "Cannot open file [" << filename << "]" << std::endl;
+ if (err) {
+ (*err) = errss.str();
+ }
+ return false;
+ }
+
+ std::string baseDir = mtl_basedir ? mtl_basedir : "";
+ if (!baseDir.empty()) {
+#ifndef _WIN32
+ const char dirsep = '/';
+#else
+ const char dirsep = '\\';
+#endif
+ if (baseDir[baseDir.length() - 1] != dirsep) baseDir += dirsep;
+ }
+ MaterialFileReader matFileReader(baseDir);
+
+ return LoadObj(attrib, shapes, materials, warn, err, &ifs, &matFileReader,
+ trianglulate, default_vcols_fallback);
+}
+
+bool LoadObj(attrib_t *attrib, std::vector<shape_t> *shapes,
+ std::vector<material_t> *materials, std::string *warn,
+ std::string *err, std::istream *inStream,
+ MaterialReader *readMatFn /*= NULL*/, bool triangulate,
+ bool default_vcols_fallback) {
+ std::stringstream errss;
+
+ std::vector<real_t> v;
+ std::vector<real_t> vn;
+ std::vector<real_t> vt;
+ std::vector<real_t> vc;
+ std::vector<tag_t> tags;
+ std::vector<face_t> faceGroup;
+ std::vector<int> lineGroup;
+ std::string name;
+
+ // material
+ std::map<std::string, int> material_map;
+ int material = -1;
+
+ // smoothing group id
+ unsigned int current_smoothing_id =
+ 0; // Initial value. 0 means no smoothing.
+
+ int greatest_v_idx = -1;
+ int greatest_vn_idx = -1;
+ int greatest_vt_idx = -1;
+
+ shape_t shape;
+
+ bool found_all_colors = true;
+
+ size_t line_num = 0;
+ std::string linebuf;
+ while (inStream->peek() != -1) {
+ safeGetline(*inStream, linebuf);
+
+ line_num++;
+
+ // Trim newline '\r\n' or '\n'
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\n')
+ linebuf.erase(linebuf.size() - 1);
+ }
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\r')
+ linebuf.erase(linebuf.size() - 1);
+ }
+
+ // Skip if empty line.
+ if (linebuf.empty()) {
+ continue;
+ }
+
+ // Skip leading space.
+ const char *token = linebuf.c_str();
+ token += strspn(token, " \t");
+
+ assert(token);
+ if (token[0] == '\0') continue; // empty line
+
+ if (token[0] == '#') continue; // comment line
+
+ // vertex
+ if (token[0] == 'v' && IS_SPACE((token[1]))) {
+ token += 2;
+ real_t x, y, z;
+ real_t r, g, b;
+
+ found_all_colors &= parseVertexWithColor(&x, &y, &z, &r, &g, &b, &token);
+
+ v.push_back(x);
+ v.push_back(y);
+ v.push_back(z);
+
+ if (found_all_colors || default_vcols_fallback) {
+ vc.push_back(r);
+ vc.push_back(g);
+ vc.push_back(b);
+ }
+
+ continue;
+ }
+
+ // normal
+ if (token[0] == 'v' && token[1] == 'n' && IS_SPACE((token[2]))) {
+ token += 3;
+ real_t x, y, z;
+ parseReal3(&x, &y, &z, &token);
+ vn.push_back(x);
+ vn.push_back(y);
+ vn.push_back(z);
+ continue;
+ }
+
+ // texcoord
+ if (token[0] == 'v' && token[1] == 't' && IS_SPACE((token[2]))) {
+ token += 3;
+ real_t x, y;
+ parseReal2(&x, &y, &token);
+ vt.push_back(x);
+ vt.push_back(y);
+ continue;
+ }
+
+ // line
+ if (token[0] == 'l' && IS_SPACE((token[1]))) {
+ token += 2;
+
+ line_t line_cache;
+ bool end_line_bit = 0;
+ while (!IS_NEW_LINE(token[0])) {
+ // get index from string
+ int idx = 0;
+ fixIndex(parseInt(&token), 0, &idx);
+
+ size_t n = strspn(token, " \t\r");
+ token += n;
+
+ if (!end_line_bit) {
+ line_cache.idx0 = idx;
+ } else {
+ line_cache.idx1 = idx;
+ lineGroup.push_back(line_cache.idx0);
+ lineGroup.push_back(line_cache.idx1);
+ line_cache = line_t();
+ }
+ end_line_bit = !end_line_bit;
+ }
+
+ continue;
+ }
+ // face
+ if (token[0] == 'f' && IS_SPACE((token[1]))) {
+ token += 2;
+ token += strspn(token, " \t");
+
+ face_t face;
+
+ face.smoothing_group_id = current_smoothing_id;
+ face.vertex_indices.reserve(3);
+
+ while (!IS_NEW_LINE(token[0])) {
+ vertex_index_t vi;
+ if (!parseTriple(&token, static_cast<int>(v.size() / 3),
+ static_cast<int>(vn.size() / 3),
+ static_cast<int>(vt.size() / 2), &vi)) {
+ if (err) {
+ std::stringstream ss;
+ ss << "Failed parse `f' line(e.g. zero value for face index. line " << line_num << ".)\n";
+ (*err) += ss.str();
+ }
+ return false;
+ }
+
+ greatest_v_idx = greatest_v_idx > vi.v_idx ? greatest_v_idx : vi.v_idx;
+ greatest_vn_idx =
+ greatest_vn_idx > vi.vn_idx ? greatest_vn_idx : vi.vn_idx;
+ greatest_vt_idx =
+ greatest_vt_idx > vi.vt_idx ? greatest_vt_idx : vi.vt_idx;
+
+ face.vertex_indices.push_back(vi);
+ size_t n = strspn(token, " \t\r");
+ token += n;
+ }
+
+ // replace with emplace_back + std::move on C++11
+ faceGroup.push_back(face);
+
+ continue;
+ }
+
+ // use mtl
+ if ((0 == strncmp(token, "usemtl", 6)) && IS_SPACE((token[6]))) {
+ token += 7;
+ std::stringstream ss;
+ ss << token;
+ std::string namebuf = ss.str();
+
+ int newMaterialId = -1;
+ if (material_map.find(namebuf) != material_map.end()) {
+ newMaterialId = material_map[namebuf];
+ } else {
+ // { error!! material not found }
+ }
+
+ if (newMaterialId != material) {
+ // Create per-face material. Thus we don't add `shape` to `shapes` at
+ // this time.
+ // just clear `faceGroup` after `exportGroupsToShape()` call.
+ exportGroupsToShape(&shape, faceGroup, lineGroup, tags, material, name,
+ triangulate, v);
+ faceGroup.clear();
+ material = newMaterialId;
+ }
+
+ continue;
+ }
+
+ // load mtl
+ if ((0 == strncmp(token, "mtllib", 6)) && IS_SPACE((token[6]))) {
+ if (readMatFn) {
+ token += 7;
+
+ std::vector<std::string> filenames;
+ SplitString(std::string(token), ' ', filenames);
+
+ if (filenames.empty()) {
+ if (warn) {
+ std::stringstream ss;
+ ss << "Looks like empty filename for mtllib. Use default "
+ "material (line " << line_num << ".)\n";
+
+ (*warn) += ss.str();
+ }
+ } else {
+ bool found = false;
+ for (size_t s = 0; s < filenames.size(); s++) {
+ std::string warn_mtl;
+ std::string err_mtl;
+ bool ok = (*readMatFn)(filenames[s].c_str(), materials,
+ &material_map, &warn_mtl, &err_mtl);
+ if (warn && (!warn_mtl.empty())) {
+ (*warn) += warn_mtl;
+ }
+
+ if (err && (!err_mtl.empty())) {
+ (*err) += err_mtl;
+ }
+
+ if (ok) {
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) {
+ if (warn) {
+ (*warn) +=
+ "Failed to load material file(s). Use default "
+ "material.\n";
+ }
+ }
+ }
+ }
+
+ continue;
+ }
+
+ // group name
+ if (token[0] == 'g' && IS_SPACE((token[1]))) {
+ // flush previous face group.
+ bool ret = exportGroupsToShape(&shape, faceGroup, lineGroup, tags,
+ material, name, triangulate, v);
+ (void)ret; // return value not used.
+
+ if (shape.mesh.indices.size() > 0) {
+ shapes->push_back(shape);
+ }
+
+ shape = shape_t();
+
+ // material = -1;
+ faceGroup.clear();
+
+ std::vector<std::string> names;
+
+ while (!IS_NEW_LINE(token[0])) {
+ std::string str = parseString(&token);
+ names.push_back(str);
+ token += strspn(token, " \t\r"); // skip tag
+ }
+
+ // names[0] must be 'g'
+
+ if (names.size() < 2) {
+ // 'g' with empty names
+ if (warn) {
+ std::stringstream ss;
+ ss << "Empty group name. line: " << line_num << "\n";
+ (*warn) += ss.str();
+ name = "";
+ }
+ } else {
+ std::stringstream ss;
+ ss << names[1];
+
+ // tinyobjloader does not support multiple groups for a primitive.
+ // Currently we concatinate multiple group names with a space to get
+ // single group name.
+
+ for (size_t i = 2; i < names.size(); i++) {
+ ss << " " << names[i];
+ }
+
+ name = ss.str();
+ }
+
+ continue;
+ }
+
+ // object name
+ if (token[0] == 'o' && IS_SPACE((token[1]))) {
+ // flush previous face group.
+ bool ret = exportGroupsToShape(&shape, faceGroup, lineGroup, tags,
+ material, name, triangulate, v);
+ if (ret) {
+ shapes->push_back(shape);
+ }
+
+ // material = -1;
+ faceGroup.clear();
+ shape = shape_t();
+
+ // @todo { multiple object name? }
+ token += 2;
+ std::stringstream ss;
+ ss << token;
+ name = ss.str();
+
+ continue;
+ }
+
+ if (token[0] == 't' && IS_SPACE(token[1])) {
+ const int max_tag_nums = 8192; // FIXME(syoyo): Parameterize.
+ tag_t tag;
+
+ token += 2;
+
+ tag.name = parseString(&token);
+
+ tag_sizes ts = parseTagTriple(&token);
+
+ if (ts.num_ints < 0) {
+ ts.num_ints = 0;
+ }
+ if (ts.num_ints > max_tag_nums) {
+ ts.num_ints = max_tag_nums;
+ }
+
+ if (ts.num_reals < 0) {
+ ts.num_reals = 0;
+ }
+ if (ts.num_reals > max_tag_nums) {
+ ts.num_reals = max_tag_nums;
+ }
+
+ if (ts.num_strings < 0) {
+ ts.num_strings = 0;
+ }
+ if (ts.num_strings > max_tag_nums) {
+ ts.num_strings = max_tag_nums;
+ }
+
+ tag.intValues.resize(static_cast<size_t>(ts.num_ints));
+
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_ints); ++i) {
+ tag.intValues[i] = parseInt(&token);
+ }
+
+ tag.floatValues.resize(static_cast<size_t>(ts.num_reals));
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_reals); ++i) {
+ tag.floatValues[i] = parseReal(&token);
+ }
+
+ tag.stringValues.resize(static_cast<size_t>(ts.num_strings));
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_strings); ++i) {
+ tag.stringValues[i] = parseString(&token);
+ }
+
+ tags.push_back(tag);
+
+ continue;
+ }
+
+ if (token[0] == 's' && IS_SPACE(token[1])) {
+ // smoothing group id
+ token += 2;
+
+ // skip space.
+ token += strspn(token, " \t"); // skip space
+
+ if (token[0] == '\0') {
+ continue;
+ }
+
+ if (token[0] == '\r' || token[1] == '\n') {
+ continue;
+ }
+
+ if (strlen(token) >= 3) {
+ if (token[0] == 'o' && token[1] == 'f' && token[2] == 'f') {
+ current_smoothing_id = 0;
+ }
+ } else {
+ // assume number
+ int smGroupId = parseInt(&token);
+ if (smGroupId < 0) {
+ // parse error. force set to 0.
+ // FIXME(syoyo): Report warning.
+ current_smoothing_id = 0;
+ } else {
+ current_smoothing_id = static_cast<unsigned int>(smGroupId);
+ }
+ }
+
+ continue;
+ } // smoothing group id
+
+ // Ignore unknown command.
+ }
+
+ // not all vertices have colors, no default colors desired? -> clear colors
+ if (!found_all_colors && !default_vcols_fallback) {
+ vc.clear();
+ }
+
+ if (greatest_v_idx >= static_cast<int>(v.size() / 3)) {
+ if (warn) {
+ std::stringstream ss;
+ ss << "Vertex indices out of bounds (line " << line_num << ".)\n" << std::endl;
+ (*warn) += ss.str();
+ }
+ }
+ if (greatest_vn_idx >= static_cast<int>(vn.size() / 3)) {
+ if (warn) {
+ std::stringstream ss;
+ ss << "Vertex normal indices out of bounds (line " << line_num << ".)\n" << std::endl;
+ (*warn) += ss.str();
+ }
+ }
+ if (greatest_vt_idx >= static_cast<int>(vt.size() / 2)) {
+ if (warn) {
+ std::stringstream ss;
+ ss << "Vertex texcoord indices out of bounds (line " << line_num << ".)\n" << std::endl;
+ (*warn) += ss.str();
+ }
+ }
+
+ bool ret = exportGroupsToShape(&shape, faceGroup, lineGroup, tags, material,
+ name, triangulate, v);
+ // exportGroupsToShape return false when `usemtl` is called in the last
+ // line.
+ // we also add `shape` to `shapes` when `shape.mesh` has already some
+ // faces(indices)
+ if (ret || shape.mesh.indices.size()) {
+ shapes->push_back(shape);
+ }
+ faceGroup.clear(); // for safety
+
+ if (err) {
+ (*err) += errss.str();
+ }
+
+ attrib->vertices.swap(v);
+ attrib->normals.swap(vn);
+ attrib->texcoords.swap(vt);
+ attrib->colors.swap(vc);
+
+ return true;
+}
+
+bool LoadObjWithCallback(std::istream &inStream, const callback_t &callback,
+ void *user_data /*= NULL*/,
+ MaterialReader *readMatFn /*= NULL*/,
+ std::string *warn, /* = NULL*/
+ std::string *err /*= NULL*/) {
+ std::stringstream errss;
+
+ // material
+ std::map<std::string, int> material_map;
+ int material_id = -1; // -1 = invalid
+
+ std::vector<index_t> indices;
+ std::vector<material_t> materials;
+ std::vector<std::string> names;
+ names.reserve(2);
+ std::vector<const char *> names_out;
+
+ std::string linebuf;
+ while (inStream.peek() != -1) {
+ safeGetline(inStream, linebuf);
+
+ // Trim newline '\r\n' or '\n'
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\n')
+ linebuf.erase(linebuf.size() - 1);
+ }
+ if (linebuf.size() > 0) {
+ if (linebuf[linebuf.size() - 1] == '\r')
+ linebuf.erase(linebuf.size() - 1);
+ }
+
+ // Skip if empty line.
+ if (linebuf.empty()) {
+ continue;
+ }
+
+ // Skip leading space.
+ const char *token = linebuf.c_str();
+ token += strspn(token, " \t");
+
+ assert(token);
+ if (token[0] == '\0') continue; // empty line
+
+ if (token[0] == '#') continue; // comment line
+
+ // vertex
+ if (token[0] == 'v' && IS_SPACE((token[1]))) {
+ token += 2;
+ // TODO(syoyo): Support parsing vertex color extension.
+ real_t x, y, z, w; // w is optional. default = 1.0
+ parseV(&x, &y, &z, &w, &token);
+ if (callback.vertex_cb) {
+ callback.vertex_cb(user_data, x, y, z, w);
+ }
+ continue;
+ }
+
+ // normal
+ if (token[0] == 'v' && token[1] == 'n' && IS_SPACE((token[2]))) {
+ token += 3;
+ real_t x, y, z;
+ parseReal3(&x, &y, &z, &token);
+ if (callback.normal_cb) {
+ callback.normal_cb(user_data, x, y, z);
+ }
+ continue;
+ }
+
+ // texcoord
+ if (token[0] == 'v' && token[1] == 't' && IS_SPACE((token[2]))) {
+ token += 3;
+ real_t x, y, z; // y and z are optional. default = 0.0
+ parseReal3(&x, &y, &z, &token);
+ if (callback.texcoord_cb) {
+ callback.texcoord_cb(user_data, x, y, z);
+ }
+ continue;
+ }
+
+ // face
+ if (token[0] == 'f' && IS_SPACE((token[1]))) {
+ token += 2;
+ token += strspn(token, " \t");
+
+ indices.clear();
+ while (!IS_NEW_LINE(token[0])) {
+ vertex_index_t vi = parseRawTriple(&token);
+
+ index_t idx;
+ idx.vertex_index = vi.v_idx;
+ idx.normal_index = vi.vn_idx;
+ idx.texcoord_index = vi.vt_idx;
+
+ indices.push_back(idx);
+ size_t n = strspn(token, " \t\r");
+ token += n;
+ }
+
+ if (callback.index_cb && indices.size() > 0) {
+ callback.index_cb(user_data, &indices.at(0),
+ static_cast<int>(indices.size()));
+ }
+
+ continue;
+ }
+
+ // use mtl
+ if ((0 == strncmp(token, "usemtl", 6)) && IS_SPACE((token[6]))) {
+ token += 7;
+ std::stringstream ss;
+ ss << token;
+ std::string namebuf = ss.str();
+
+ int newMaterialId = -1;
+ if (material_map.find(namebuf) != material_map.end()) {
+ newMaterialId = material_map[namebuf];
+ } else {
+ // { error!! material not found }
+ }
+
+ if (newMaterialId != material_id) {
+ material_id = newMaterialId;
+ }
+
+ if (callback.usemtl_cb) {
+ callback.usemtl_cb(user_data, namebuf.c_str(), material_id);
+ }
+
+ continue;
+ }
+
+ // load mtl
+ if ((0 == strncmp(token, "mtllib", 6)) && IS_SPACE((token[6]))) {
+ if (readMatFn) {
+ token += 7;
+
+ std::vector<std::string> filenames;
+ SplitString(std::string(token), ' ', filenames);
+
+ if (filenames.empty()) {
+ if (warn) {
+ (*warn) +=
+ "Looks like empty filename for mtllib. Use default "
+ "material. \n";
+ }
+ } else {
+ bool found = false;
+ for (size_t s = 0; s < filenames.size(); s++) {
+ std::string warn_mtl;
+ std::string err_mtl;
+ bool ok = (*readMatFn)(filenames[s].c_str(), &materials,
+ &material_map, &warn_mtl, &err_mtl);
+
+ if (warn && (!warn_mtl.empty())) {
+ (*warn) += warn_mtl; // This should be warn message.
+ }
+
+ if (err && (!err_mtl.empty())) {
+ (*err) += err_mtl;
+ }
+
+ if (ok) {
+ found = true;
+ break;
+ }
+ }
+
+ if (!found) {
+ if (warn) {
+ (*warn) +=
+ "Failed to load material file(s). Use default "
+ "material.\n";
+ }
+ } else {
+ if (callback.mtllib_cb) {
+ callback.mtllib_cb(user_data, &materials.at(0),
+ static_cast<int>(materials.size()));
+ }
+ }
+ }
+ }
+
+ continue;
+ }
+
+ // group name
+ if (token[0] == 'g' && IS_SPACE((token[1]))) {
+ names.clear();
+
+ while (!IS_NEW_LINE(token[0])) {
+ std::string str = parseString(&token);
+ names.push_back(str);
+ token += strspn(token, " \t\r"); // skip tag
+ }
+
+ assert(names.size() > 0);
+
+ if (callback.group_cb) {
+ if (names.size() > 1) {
+ // create const char* array.
+ names_out.resize(names.size() - 1);
+ for (size_t j = 0; j < names_out.size(); j++) {
+ names_out[j] = names[j + 1].c_str();
+ }
+ callback.group_cb(user_data, &names_out.at(0),
+ static_cast<int>(names_out.size()));
+
+ } else {
+ callback.group_cb(user_data, NULL, 0);
+ }
+ }
+
+ continue;
+ }
+
+ // object name
+ if (token[0] == 'o' && IS_SPACE((token[1]))) {
+ // @todo { multiple object name? }
+ token += 2;
+
+ std::stringstream ss;
+ ss << token;
+ std::string object_name = ss.str();
+
+ if (callback.object_cb) {
+ callback.object_cb(user_data, object_name.c_str());
+ }
+
+ continue;
+ }
+
+#if 0 // @todo
+ if (token[0] == 't' && IS_SPACE(token[1])) {
+ tag_t tag;
+
+ token += 2;
+ std::stringstream ss;
+ ss << token;
+ tag.name = ss.str();
+
+ token += tag.name.size() + 1;
+
+ tag_sizes ts = parseTagTriple(&token);
+
+ tag.intValues.resize(static_cast<size_t>(ts.num_ints));
+
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_ints); ++i) {
+ tag.intValues[i] = atoi(token);
+ token += strcspn(token, "/ \t\r") + 1;
+ }
+
+ tag.floatValues.resize(static_cast<size_t>(ts.num_reals));
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_reals); ++i) {
+ tag.floatValues[i] = parseReal(&token);
+ token += strcspn(token, "/ \t\r") + 1;
+ }
+
+ tag.stringValues.resize(static_cast<size_t>(ts.num_strings));
+ for (size_t i = 0; i < static_cast<size_t>(ts.num_strings); ++i) {
+ std::stringstream ss;
+ ss << token;
+ tag.stringValues[i] = ss.str();
+ token += tag.stringValues[i].size() + 1;
+ }
+
+ tags.push_back(tag);
+ }
+#endif
+
+ // Ignore unknown command.
+ }
+
+ if (err) {
+ (*err) += errss.str();
+ }
+
+ return true;
+}
+
+#ifdef __clang__
+#pragma clang diagnostic pop
+#endif
+} // namespace tinyobj
+
+#endif
diff --git a/externals/Pangolin/package.xml b/externals/Pangolin/package.xml
new file mode 100644
index 0000000000000000000000000000000000000000..8e936d174c91489ba69d73b90f491ea34a3203c8
--- /dev/null
+++ b/externals/Pangolin/package.xml
@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+<package format="2">
+ <name>pangolin</name>
+ <version>0.5.0</version>
+ <description>pangolin</description>
+ <maintainer email="stevenlovegrove@gmail.com">Steven Lovegrove</maintainer>
+ <author email="stevenlovegrove@gmail.com">Steven Lovegrove</author>
+ <license>MIT</license>
+ <buildtool_depend>cmake</buildtool_depend>
+ <depend>libglew-dev</depend>
+ <depend>cmake</depend>
+ <depend>python</depend>
+ <export>
+ <build_type>cmake</build_type>
+ </export>
+</package>
diff --git a/externals/Pangolin/src/Doxyfile.in b/externals/Pangolin/src/Doxyfile.in
new file mode 100644
index 0000000000000000000000000000000000000000..8146355eaed1ebd58922ebce515d82a4d2d49ad5
--- /dev/null
+++ b/externals/Pangolin/src/Doxyfile.in
@@ -0,0 +1,2281 @@
+# Doxyfile 1.8.5
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the config file
+# that follow. The default is UTF-8 which is also the encoding used for all text
+# before the first occurrence of this tag. Doxygen uses libiconv (or the iconv
+# built into libc) for the transcoding. See http://www.gnu.org/software/libiconv
+# for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME = "Pangolin"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER =
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF = "Pangolin is a lightweight rapid development library for managing OpenGL display / interaction and video input"
+
+# With the PROJECT_LOGO tag one can specify an logo or icon that is included in
+# the documentation. The maximum height of the logo should not exceed 55 pixels
+# and the maximum width should not exceed 200 pixels. Doxygen will copy the logo
+# to the output directory.
+
+PROJECT_LOGO =
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY =
+
+# If the CREATE_SUBDIRS tag is set to YES, then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Brazilian, Catalan, Chinese, Chinese-
+# Traditional, Croatian, Czech, Danish, Dutch, English, Esperanto, Farsi,
+# Finnish, French, German, Greek, Hungarian, Italian, Japanese, Japanese-en,
+# Korean, Korean-en, Latvian, Norwegian, Macedonian, Persian, Polish,
+# Portuguese, Romanian, Russian, Serbian, Slovak, Slovene, Spanish, Swedish,
+# Turkish, Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE = English
+
+# If the BRIEF_MEMBER_DESC tag is set to YES doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC = YES
+
+# If the REPEAT_BRIEF tag is set to YES doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF =
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB = NO
+
+# If the FULL_PATH_NAMES tag is set to YES doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF = YES
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES, then doxygen will produce a
+# new page for each member. If set to NO, the documentation of a member will be
+# part of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines.
+
+ALIASES =
+
+# This tag can be used to specify a number of word-keyword mappings (TCL only).
+# A mapping has the form "name=value". For example adding "class=itcl::class"
+# will allow you to use the command class in the itcl::class meaning.
+
+TCL_SUBST =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, Javascript,
+# C#, C, C++, D, PHP, Objective-C, Python, Fortran, VHDL. For instance to make
+# doxygen treat .inc files as Fortran files (default is PHP), and .f files as C
+# (default is Fortran), use: inc=Fortran f=C.
+#
+# Note For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen.
+
+EXTENSION_MAPPING =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See http://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT = YES
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by by putting a % sign in front of the word
+# or globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# http://www.riverbankcomputing.co.uk/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES, then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE = 0
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE = NO
+
+# If the EXTRACT_STATIC tag is set to YES all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES = YES
+
+# This flag is only useful for Objective-C code. When set to YES local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES = NO
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO these classes will be included in the various overviews. This option has
+# no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# (class|struct|union) declarations. If set to NO these declarations will be
+# included in the documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS = NO
+
+# If the CASE_SENSE_NAMES tag is set to NO then doxygen will only generate file
+# names in lower-case letters. If set to YES upper-case letters are also
+# allowed. This is useful if you have classes or files whose names only differ
+# in case and if your file system supports case sensitive file names. Windows
+# and Mac users are advised to set this option to NO.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES = NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES = YES
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO the members will appear in declaration order.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING = NO
+
+# The GENERATE_TODOLIST tag can be used to enable ( YES) or disable ( NO) the
+# todo list. This list is created by putting \todo commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST = YES
+
+# The GENERATE_TESTLIST tag can be used to enable ( YES) or disable ( NO) the
+# test list. This list is created by putting \test commands in the
+# documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST = YES
+
+# The GENERATE_BUGLIST tag can be used to enable ( YES) or disable ( NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable ( YES) or disable ( NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES the list
+# will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also http://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. Do not use file names with spaces, bibtex cannot handle them. See
+# also \cite for info how to create references.
+
+CITE_BIB_FILES =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error ( stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES, then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO doxygen will only warn about wrong or incomplete parameter
+# documentation, but not about the absence of documentation.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces.
+# Note: If this tag is empty the current directory is searched.
+
+INPUT = @CMAKE_CURRENT_SOURCE_DIR@/../include
+INPUT += @CMAKE_CURRENT_SOURCE_DIR@/../README.md
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see: http://www.gnu.org/software/libiconv) for the list of
+# possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank the
+# following patterns are tested:*.c, *.cc, *.cxx, *.cpp, *.c++, *.java, *.ii,
+# *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp,
+# *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown,
+# *.md, *.mm, *.dox, *.py, *.f90, *.f, *.for, *.tcl, *.vhd, *.vhdl, *.ucf,
+# *.qsf, *.as and *.js.
+
+FILE_PATTERNS = *.h *.md
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS = *internal*
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS = android_app android_poll_source PangolinAppDelegate
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH =
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS =
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+
+INPUT_FILTER =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+
+FILTER_PATTERNS =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER ) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER = NO
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS = NO
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# function all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES, then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see http://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the config file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX = YES
+
+# The COLS_IN_ALPHA_INDEX tag can be used to specify the number of columns in
+# which the alphabetical index list will be split.
+# Minimum value: 1, maximum value: 20, default value: 5.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+COLS_IN_ALPHA_INDEX = 5
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify an additional user-
+# defined cascading style sheet that is included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefor more robust against future updates.
+# Doxygen will copy the style sheet file to the output directory. For an example
+# see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the stylesheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# http://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see: http://developer.apple.com/tools/xcode/), introduced with
+# OSX 10.5 (Leopard). To create a documentation set, doxygen will generate a
+# Makefile in the HTML output directory. Running make will produce the docset in
+# that directory and running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See http://developer.apple.com/tools/creatingdocsetswithdoxygen.html
+# for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see: http://www.microsoft.com/en-us/download/details.aspx?id=21138) on
+# Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler ( hhc.exe). If non-empty
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated (
+# YES) or that it should be included in the master .chm file ( NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index ( hhk), content ( hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated (
+# YES) or a normal table of contents ( NO) in the .chm file.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#virtual-
+# folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see: http://qt-project.org/doc/qt-4.8/qthelpproject.html#custom-
+# filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# http://qt-project.org/doc/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS =
+
+# The QHG_LOCATION tag can be used to specify the location of Qt's
+# qhelpgenerator. If non-empty doxygen will try to run qhelpgenerator on the
+# generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom stylesheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH = 250
+
+# When the EXT_LINKS_IN_WINDOW option is set to YES doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW = NO
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE = 10
+
+# Use the FORMULA_TRANPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT = YES
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# http://www.mathjax.org) which uses client side Javascript for the rendering
+# instead of using prerendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX = NO
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from http://www.mathjax.org before deployment.
+# The default value is: http://cdn.mathjax.org/mathjax/latest.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH = http://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS =
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see: http://docs.mathjax.org/en/latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using Javascript. There
+# are two flavours of web server based searching depending on the
+# EXTERNAL_SEARCH setting. When disabled, doxygen will generate a PHP script for
+# searching and an index file used by the script. When EXTERNAL_SEARCH is
+# enabled the indexing and searching needs to be provided by external tools. See
+# the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer ( doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see: http://xapian.org/). See the section "External Indexing and
+# Searching" for details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX = YES
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when enabling USE_PDFLATEX this option is only used for generating
+# bitmaps for formulas in the HTML output, but not in the Makefile that is
+# written to the output directory.
+# The default file is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME = makeindex
+
+# If the COMPACT_LATEX tag is set to YES doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. To get the times font for
+# instance you can specify
+# EXTRA_PACKAGES=times
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber. Doxygen will
+# replace them by respectively the title of the page, the current date and time,
+# only the current date, the version number of doxygen, the project name (see
+# PROJECT_NAME), or the project number (see PROJECT_NUMBER).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS = YES
+
+# If the LATEX_PDFLATEX tag is set to YES, doxygen will use pdflatex to generate
+# the PDF file directly from the LaTeX files. Set this option to YES to get a
+# higher quality PDF documentation.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# http://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE = plain
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT = rtf
+
+# If the COMPACT_RTF tag is set to YES doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's config
+# file, i.e. a series of assignments. You only have to provide replacements,
+# missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's config file. A template extensions file can be generated
+# using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION = .3
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT = xml
+
+# The XML_SCHEMA tag can be used to specify a XML schema, which can be used by a
+# validating XML parser to check the syntax of the XML files.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_SCHEMA =
+
+# The XML_DTD tag can be used to specify a XML DTD, which can be used by a
+# validating XML parser to check the syntax of the XML files.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_DTD =
+
+# If the XML_PROGRAMLISTING tag is set to YES doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT = docbook
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES doxygen will generate an AutoGen
+# Definitions (see http://autogen.sf.net) file that captures the structure of
+# the code including all documentation. Note that this feature is still
+# experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD = NO
+
+# If the PERLMOD_LATEX tag is set to YES doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX = NO
+
+# If the PERLMOD_PRETTY tag is set to YES the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING = YES
+
+# If the MACRO_EXPANSION tag is set to YES doxygen will expand all macro names
+# in the source code. If set to NO only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION = YES
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF = YES
+
+# If the SEARCH_INCLUDES tag is set to YES the includes files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED = PANGOLIN_EXPORT = ""
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all refrences to function-like macros that are alone on a line, have an
+# all uppercase name, and do not end with a semicolon. Such function macros are
+# typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have an unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE =
+
+# If the ALLEXTERNALS tag is set to YES all external class will be listed in the
+# class index. If set to NO only the inherited external classes will be listed.
+# The default value is: NO.
+
+ALLEXTERNALS = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES all external groups will be listed in
+# the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS = YES
+
+# If the EXTERNAL_PAGES tag is set to YES all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES = YES
+
+# The PERL_PATH should be the absolute path and name of the perl script
+# interpreter (i.e. the result of 'which perl').
+# The default file (with absolute path) is: /usr/bin/perl.
+
+PERL_PATH = /usr/bin/perl
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS = YES
+
+# You can define message sequence charts within doxygen comments using the \msc
+# command. Doxygen will then run the mscgen tool (see:
+# http://www.mcternan.me.uk/mscgen/)) to produce the chart and insert it in the
+# documentation. The MSCGEN_PATH tag allows you to specify the directory where
+# the mscgen tool resides. If left empty the tool is assumed to be found in the
+# default search path.
+
+MSCGEN_PATH =
+
+# If set to YES, the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: NO.
+
+HAVE_DOT = NO
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS = 0
+
+# When you want a differently looking font n the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS = YES
+
+# If the UML_LOOK tag is set to YES doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LIMIT_NUM_FIELDS = 10
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot.
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, jpg, gif and svg.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND = YES
+
+# If the DOT_CLEANUP tag is set to YES doxygen will remove the intermediate dot
+# files that are used to generate the various graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_CLEANUP = YES
diff --git a/externals/Pangolin/src/PangolinConfig.cmake.in b/externals/Pangolin/src/PangolinConfig.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..c81cfd9b8f1f519c9862bf03cd24e5167ae8b05b
--- /dev/null
+++ b/externals/Pangolin/src/PangolinConfig.cmake.in
@@ -0,0 +1,14 @@
+# Compute paths
+get_filename_component( PROJECT_CMAKE_DIR "${CMAKE_CURRENT_LIST_FILE}" PATH )
+SET( Pangolin_INCLUDE_DIRS "@EXPORT_LIB_INC_DIR@;@USER_INC@" )
+SET( Pangolin_INCLUDE_DIR "@EXPORT_LIB_INC_DIR@;@USER_INC@" )
+
+# Library dependencies (contains definitions for IMPORTED targets)
+if( NOT TARGET @LIBRARY_NAME@ AND NOT @PROJECT_NAME@_BINARY_DIR )
+ include( "${PROJECT_CMAKE_DIR}/@PROJECT_NAME@Targets.cmake" )
+ @ExternConfig@
+endif()
+
+SET( Pangolin_LIBRARIES @LIBRARY_NAME@ )
+SET( Pangolin_LIBRARY @LIBRARY_NAME@ )
+SET( Pangolin_CMAKEMODULES @CMAKE_CURRENT_SOURCE_DIR@/../CMakeModules )
diff --git a/externals/Pangolin/src/PangolinConfigVersion.cmake.in b/externals/Pangolin/src/PangolinConfigVersion.cmake.in
new file mode 100644
index 0000000000000000000000000000000000000000..3b3daec5671e416400eeb33ce38eeba0330e5821
--- /dev/null
+++ b/externals/Pangolin/src/PangolinConfigVersion.cmake.in
@@ -0,0 +1,17 @@
+set(PACKAGE_VERSION "@PANGOLIN_VERSION@")
+
+# Check build type is valid
+if( "System:${CMAKE_SYSTEM_NAME},Win64:${CMAKE_CL_64},Android:${ANDROID},iOS:${IOS}" STREQUAL
+ "System:@CMAKE_SYSTEM_NAME@,Win64:@CMAKE_CL_64@,Android:@ANDROID@,iOS:@IOS@" )
+ # Check whether the requested PACKAGE_FIND_VERSION is compatible
+ if("${PACKAGE_VERSION}" VERSION_LESS "${PACKAGE_FIND_VERSION}")
+ set(PACKAGE_VERSION_COMPATIBLE FALSE)
+ else()
+ set(PACKAGE_VERSION_COMPATIBLE TRUE)
+ if ("${PACKAGE_VERSION}" VERSION_EQUAL "${PACKAGE_FIND_VERSION}")
+ set(PACKAGE_VERSION_EXACT TRUE)
+ endif()
+ endif()
+else()
+ set(PACKAGE_VERSION_COMPATIBLE FALSE)
+endif()
diff --git a/externals/Pangolin/src/config.h.in b/externals/Pangolin/src/config.h.in
new file mode 100644
index 0000000000000000000000000000000000000000..0c6937ba248631cd26a031ad3abff22f8f579b71
--- /dev/null
+++ b/externals/Pangolin/src/config.h.in
@@ -0,0 +1,72 @@
+#ifndef PANGOLIN_CONFIG_H
+#define PANGOLIN_CONFIG_H
+
+/*
+ * Configuration Header for Pangolin
+ */
+
+/// Version
+#define PANGOLIN_VERSION_MAJOR @PANGOLIN_VERSION_MAJOR@
+#define PANGOLIN_VERSION_MINOR @PANGOLIN_VERSION_MINOR@
+#define PANGOLIN_VERSION_STRING "@PANGOLIN_VERSION@"
+
+/// Pangolin options
+#cmakedefine BUILD_PANGOLIN_GUI
+#cmakedefine BUILD_PANGOLIN_VARS
+#cmakedefine BUILD_PANGOLIN_VIDEO
+
+/// Configured libraries
+#cmakedefine HAVE_CUDA
+#cmakedefine HAVE_PYTHON
+
+#cmakedefine HAVE_EIGEN
+#cmakedefine HAVE_TOON
+
+#cmakedefine HAVE_DC1394
+#cmakedefine HAVE_V4L
+#cmakedefine HAVE_OPENNI
+#cmakedefine HAVE_LIBREALSENSE
+#cmakedefine HAVE_OPENNI2
+#cmakedefine HAVE_UVC
+#cmakedefine HAVE_DEPTHSENSE
+#cmakedefine HAVE_TELICAM
+#cmakedefine HAVE_PLEORA
+
+#cmakedefine HAVE_FFMPEG
+#cmakedefine HAVE_FFMPEG_MAX_ANALYZE_DURATION2
+#cmakedefine HAVE_FFMPEG_AVFORMAT_ALLOC_OUTPUT_CONTEXT2
+#cmakedefine HAVE_FFMPEG_AVPIXELFORMAT
+
+#cmakedefine HAVE_GLEW
+#cmakedefine GLEW_STATIC
+
+#cmakedefine HAVE_APPLE_OPENGL_FRAMEWORK
+#cmakedefine HAVE_GLES
+#cmakedefine HAVE_GLES_2
+#cmakedefine HAVE_OCULUS
+
+#cmakedefine HAVE_PNG
+#cmakedefine HAVE_JPEG
+#cmakedefine HAVE_TIFF
+#cmakedefine HAVE_OPENEXR
+#cmakedefine HAVE_ZSTD
+#cmakedefine HAVE_LZ4
+
+/// Platform
+#cmakedefine _UNIX_
+#cmakedefine _WIN_
+#cmakedefine _OSX_
+#cmakedefine _LINUX_
+#cmakedefine _ANDROID_
+#cmakedefine _IOS_
+
+/// Compiler
+#cmakedefine _GCC_
+#cmakedefine _CLANG_
+#cmakedefine _MSVC_
+
+#if (__cplusplus > 199711L) || (_MSC_VER >= 1800)
+#define CALLEE_HAS_VARIADIC_TEMPLATES
+#endif
+
+#endif //PANGOLIN_CONFIG_H
diff --git a/externals/Pangolin/src/console/ConsoleView.cpp b/externals/Pangolin/src/console/ConsoleView.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1fdd72b68b2fb8ec7f50ebf663cf10b72022f632
--- /dev/null
+++ b/externals/Pangolin/src/console/ConsoleView.cpp
@@ -0,0 +1,327 @@
+#include <iterator>
+#include <pangolin/console/ConsoleView.h>
+#include <pangolin/utils/picojson.h>
+#include <pangolin/gl/gldraw.h>
+
+namespace pangolin
+{
+
+inline Colour ParseJson(const picojson::value& val)
+{
+ return Colour(
+ val.contains("r") ? val["r"].get<double>() : 0.0,
+ val.contains("g") ? val["g"].get<double>() : 0.0,
+ val.contains("b") ? val["b"].get<double>() : 0.0,
+ val.contains("a") ? val["a"].get<double>() : 1.0
+ );
+}
+
+inline picojson::value toJson(const Colour& colour)
+{
+ picojson::value ret(picojson::object_type,true);
+ ret["r"] = colour.r;
+ ret["g"] = colour.g;
+ ret["b"] = colour.b;
+ ret["a"] = colour.a;
+ return ret;
+}
+
+inline std::ostream& operator<<(std::ostream& os, const Colour& colour)
+{
+ os << toJson(colour).serialize();
+ return os;
+}
+
+inline std::istream& operator>>(std::istream& is, Colour& colour)
+{
+ picojson::value val;
+ picojson::parse(val,is);
+ colour = ParseJson(val);
+ return is;
+}
+
+inline void glColour(const Colour& c)
+{
+ glColor4f(c.r,c.g,c.b,c.a);
+}
+
+ConsoleView::ConsoleView(ConsoleInterpreter* interpreter)
+ : interpreter(interpreter),
+ font(GlFont::I()),
+ carat(0),
+ hiding(false),
+ bottom(1.0f),
+ background_colour(0.2f, 0.0f, 0.0f, 0.6f),
+ animation_speed(0.2)
+{
+ SetHandler(this);
+
+ line_colours[ConsoleLineTypeCmd] = Colour(1.0f,1.0f,1.0f,1.0f);
+ line_colours[ConsoleLineTypeCmdOptions] = Colour(0.9f,0.9f,0.9f,1.0f);
+ line_colours[ConsoleLineTypeOutput] = Colour(0.0f,1.0f,1.0f,1.0f);
+ line_colours[ConsoleLineTypeHelp] = Colour(1.0f,0.8f,1.0f,1.0f);
+ line_colours[ConsoleLineTypeStdout] = Colour(0.0f,0.0f,1.0f,1.0f);
+ line_colours[ConsoleLineTypeStderr] = Colour(1.0f,0.8f,0.8f,1.0f);
+
+ Var<Colour>::Attach("pangolin.console.colours.Background", background_colour);
+ Var<Colour>::Attach("pangolin.console.colours.Cmd", line_colours[ConsoleLineTypeCmd]);
+ Var<Colour>::Attach("pangolin.console.colours.CmdOptions", line_colours[ConsoleLineTypeCmdOptions]);
+ Var<Colour>::Attach("pangolin.console.colours.Stdout", line_colours[ConsoleLineTypeStdout]);
+ Var<Colour>::Attach("pangolin.console.colours.Stderr", line_colours[ConsoleLineTypeStderr]);
+ Var<Colour>::Attach("pangolin.console.colours.Output", line_colours[ConsoleLineTypeOutput]);
+ Var<Colour>::Attach("pangolin.console.colours.Help", line_colours[ConsoleLineTypeHelp]);
+
+ Var<float>::Attach("pango.console.animation_speed", animation_speed);
+
+ AddLine("Pangolin Python Command Prompt:", ConsoleLineTypeHelp);
+ AddLine("===============================", ConsoleLineTypeHelp);
+}
+
+ConsoleView::~ConsoleView() {
+ delete interpreter;
+}
+
+void ConsoleView::ProcessOutputLines()
+{
+ // empty output queue
+ ConsoleLine line_in;
+ while(interpreter->PullLine(line_in))
+ {
+ AddLine(line_in.text, line_in.linetype);
+ }
+}
+
+View& ConsoleView::ShowWithoutAnimation(bool should_show){
+ Show(should_show);
+ bottom = show ? 1.0 : 0.0;
+ return *this;
+}
+
+View& ConsoleView::Show(bool should_show)
+{
+ if(should_show) {
+ hiding = false;
+ show = true;
+ }else{
+ hiding = true;
+ }
+ return *this;
+}
+
+void ConsoleView::ToggleShow()
+{
+ Show(!IsShown());
+}
+
+bool ConsoleView::IsShown() const
+{
+ return show && !hiding;
+}
+
+void ConsoleView::DrawLine(const ConsoleView::Line& l, int carat=-1)
+{
+ glColour(line_colours[l.linetype]);
+ l.text.Draw();
+ if(carat >= 0) {
+ const double w = font.Text(l.text.str.substr(0,carat)).Width();
+ glDrawLine(w,-2,w,font.Height()-4);
+ }
+}
+
+void ConsoleView::Render()
+{
+ if(hiding) {
+ bottom += (1.0f - bottom) * animation_speed;
+ if(1.0 - bottom < 0.01) {
+ bottom = 1.0;
+ show = false;
+ hiding = false;
+ return;
+ }
+ }else{
+ if(bottom > 0.01f) {
+ bottom -= bottom*animation_speed;
+ }else{
+ bottom = 0.0f;
+ }
+ }
+
+ ProcessOutputLines();
+
+#ifndef HAVE_GLES
+ glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_DEPTH_BUFFER_BIT | GL_SCISSOR_BIT | GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_TRANSFORM_BIT);
+#endif
+
+ this->ActivatePixelOrthographic();
+ glDisable(GL_DEPTH_TEST );
+ glDisable(GL_LIGHTING);
+ glDisable(GL_SCISSOR_TEST);
+ glDisable(GL_LINE_SMOOTH);
+ glDisable( GL_COLOR_MATERIAL );
+ glLineWidth(1.0);
+
+ glEnable(GL_BLEND);
+ glBlendFunc( GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA );
+
+ glColour(background_colour);
+
+ GLfloat verts[] = { 0.0f, (GLfloat)v.h,
+ (GLfloat)v.w, (GLfloat)v.h,
+ (GLfloat)v.w, bottom*v.h,
+ 0.0f, bottom*v.h };
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, verts);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisableClientState(GL_VERTEX_ARRAY);
+
+
+ const double line_space = font.Height();
+ glTranslated(10.0, 10.0 + bottom*v.h, 0.0 );
+ DrawLine(current_line, carat);
+ glTranslated(0.0, line_space, 0.0);
+ for(size_t l=0; l < line_buffer.size(); ++l) {
+ DrawLine(line_buffer[l]);
+ glTranslated(0.0, line_space, 0.0);
+ }
+
+#ifndef HAVE_GLES
+ glPopAttrib();
+#endif
+}
+
+inline std::string CommonPrefix(const std::vector<std::string>& vec)
+{
+ if(!vec.size()) return "";
+
+ size_t cmn = vec[0].size();
+ for(size_t i=1; i<vec.size(); ++i) {
+ cmn = std::min(vec[i].size(), cmn);
+ for(size_t p=0; p < cmn; ++p) {
+ if(vec[i][p] != vec[0][p]) {
+ cmn = p;
+ break;
+ }
+ }
+ }
+
+ return vec[0].substr(0,cmn);
+}
+
+void ConsoleView::Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed)
+{
+ static int hist_id = -1;
+ static std::string prefix;
+ static bool edited = true;
+
+ if(pressed) {
+ if(key=='\r') key = '\n';
+
+ GlText& txt = current_line.text;
+ const std::string cmd = txt.Text();
+
+ if(key=='`') {
+ ToggleShow();
+ } else if(key=='\n') {
+ interpreter->PushCommand(cmd);
+ line_buffer.push_front(current_line);
+ hist_id = -1;
+ prefix = "";
+ edited = true;
+ carat = 0;
+ txt.Clear();
+ }else if(key=='\t') {
+ std::vector<std::string> options = interpreter->Complete(cmd,100);
+ if(options.size()) {
+ const std::string common = CommonPrefix(options);
+ if(common.size() > cmd.size()) {
+ current_line = font.Text("%s", common.c_str());
+ carat = common.size();
+ }else{
+ std::stringstream s;
+ std::copy(options.begin(), options.end(), std::ostream_iterator<std::string>(s,", "));
+ AddLine(s.str(), ConsoleLineTypeCmdOptions);
+ }
+ }
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_UP) {
+ if(edited) {
+ prefix = cmd;
+ edited = false;
+ }
+ Line* hist_line = GetLine(hist_id+1, ConsoleLineTypeCmd, prefix);
+ if(hist_line) {
+ current_line = *hist_line;
+ hist_id++;
+ }
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_DOWN) {
+ if(edited) {
+ prefix = cmd;
+ edited = false;
+ }
+ Line* hist_line = GetLine(hist_id-1, ConsoleLineTypeCmd, prefix);
+ if(hist_line) {
+ current_line = *hist_line;
+ hist_id--;
+ }
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_LEFT) {
+ if(carat > 0) carat--;
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_RIGHT) {
+ if(carat < (int)txt.str.size()) carat++;
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_HOME) {
+ carat = 0;
+ }else if(key==PANGO_SPECIAL + PANGO_KEY_END) {
+ carat = txt.Text().size();
+ }else if(key=='\b') {
+ if(carat > 0) {
+ std::string newstr = txt.Text();
+ newstr.erase(newstr.begin()+carat-1);
+ txt = font.Text("%s", newstr.c_str() );
+ carat--;
+ edited = true;
+ }
+ }else if(key==127) { // delete
+ if(carat < (int)txt.Text().size() ) {
+ std::string newstr = txt.Text();
+ newstr.erase(newstr.begin()+carat);
+ txt = font.Text("%s", newstr.c_str() );
+ edited = true;
+ }
+ }else if(key==PANGO_CTRL + 'c') {
+ txt = font.Text("");
+ carat = 0;
+ edited = true;
+ }else if(key < PANGO_SPECIAL){
+ std::string newstr = txt.Text();
+ newstr.insert(carat, 1, key);
+ txt = font.Text("%s", newstr.c_str());
+ ++carat;
+ edited = true;
+ }
+ }
+}
+
+void ConsoleView::AddLine(const std::string& text, ConsoleLineType linetype )
+{
+ line_buffer.push_front( Line( font.Text("%s",text.c_str()), linetype) );
+}
+
+ConsoleView::Line* ConsoleView::GetLine(int id, ConsoleLineType line_type, const std::string& prefix )
+{
+ int match = 0;
+ for(Line& l : line_buffer)
+ {
+ if(l.linetype == line_type) {
+ const std::string substr = l.text.Text().substr(0,prefix.size());
+ if(substr == prefix ) {
+ if(id == match) {
+ return &l;
+ }else{
+ ++match;
+ }
+ }
+ }
+ }
+
+ return nullptr;
+}
+
+}
diff --git a/externals/Pangolin/src/display/device/PangolinNSApplication.mm b/externals/Pangolin/src/display/device/PangolinNSApplication.mm
new file mode 100644
index 0000000000000000000000000000000000000000..96be78ba3a1c65105f3b5158f287bb1240dd8ea4
--- /dev/null
+++ b/externals/Pangolin/src/display/device/PangolinNSApplication.mm
@@ -0,0 +1,95 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/platform.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/device/PangolinNSApplication.h>
+
+#if MAC_OS_X_VERSION_MAX_ALLOWED >= MAC_OS_X_VERSION_10_12
+# define NSAnyEventMask NSEventMaskAny
+#endif
+
+////////////////////////////////////////////////////////////////////
+// PangolinNSApplication
+////////////////////////////////////////////////////////////////////
+
+@implementation PangolinNSApplication
+
++ (void)run_pre
+{
+ [[NSNotificationCenter defaultCenter]
+ postNotificationName:NSApplicationWillFinishLaunchingNotification
+ object:NSApp];
+ [[NSNotificationCenter defaultCenter]
+ postNotificationName:NSApplicationDidFinishLaunchingNotification
+ object:NSApp];
+}
+
++ (void)run_step
+{
+ NSEvent *event;
+ do{
+ event = [NSApp
+ nextEventMatchingMask:NSAnyEventMask
+ untilDate:nil
+// untilDate: [NSDate distantFuture]
+ inMode:NSDefaultRunLoopMode
+ dequeue:YES];
+ [NSApp sendEvent:event];
+ [NSApp updateWindows];
+ }while(event != nil);
+}
+
+@end
+
+////////////////////////////////////////////////////////////////////
+// PangolinWindowDelegate
+////////////////////////////////////////////////////////////////////
+
+@implementation PangolinWindowDelegate
+
+- (BOOL)windowShouldClose:(id)sender {
+ PANGOLIN_UNUSED(sender);
+
+ pangolin::Quit();
+ return YES;
+}
+
+@end
+
+////////////////////////////////////////////////////////////////////
+// PangolinAppDelegate
+////////////////////////////////////////////////////////////////////
+
+@implementation PangolinAppDelegate
+
+- (void)dealloc
+{
+ [super dealloc];
+}
+
+@end
diff --git a/externals/Pangolin/src/display/device/PangolinNSGLView.mm b/externals/Pangolin/src/display/device/PangolinNSGLView.mm
new file mode 100644
index 0000000000000000000000000000000000000000..010050d2d74d5f34defa30ee5189d611a658bc30
--- /dev/null
+++ b/externals/Pangolin/src/display/device/PangolinNSGLView.mm
@@ -0,0 +1,325 @@
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/device/PangolinNSGLView.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/handler/handler_enums.h>
+
+#if MAC_OS_X_VERSION_MAX_ALLOWED >= MAC_OS_X_VERSION_10_12
+# define NSDeviceIndependentModifierFlagsMask NSEventModifierFlagDeviceIndependentFlagsMask
+# define NSShiftKeyMask NSEventModifierFlagShift
+# define NSControlKeyMask NSEventModifierFlagControl
+# define NSAlternateKeyMask NSEventModifierFlagOption
+# define NSCommandKeyMask NSEventModifierFlagCommand
+# define NSFunctionKeyMask NSEventModifierFlagFunction
+#endif
+
+namespace pangolin
+{
+extern __thread PangolinGl* context;
+}
+
+////////////////////////////////////////////////////////////////////
+// Input maps
+////////////////////////////////////////////////////////////////////
+
+inline
+int mapMouseButton(int osx_button )
+{
+ const int map[] = {0, 2, 1, 3, 4, 5, 6, 7, 8, 9, 10};
+ return map[osx_button];
+}
+
+inline
+int mapKeymap(int osx_key)
+{
+ if(osx_key == NSUpArrowFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_UP;
+ else if(osx_key == NSDownArrowFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_DOWN;
+ else if(osx_key == NSLeftArrowFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_LEFT;
+ else if(osx_key == NSRightArrowFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_RIGHT;
+ else if(osx_key == NSPageUpFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_PAGE_UP;
+ else if(osx_key == NSPageDownFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_PAGE_DOWN;
+ else if(osx_key == NSHomeFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_HOME;
+ else if(osx_key == NSEndFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_END;
+ else if(osx_key == NSInsertFunctionKey)
+ return pangolin::PANGO_SPECIAL + pangolin::PANGO_KEY_INSERT;
+ else if(osx_key == NSDeleteCharacter )
+ return NSBackspaceCharacter;
+ else if(osx_key == NSDeleteFunctionKey)
+ return NSDeleteCharacter;
+ else {
+ return osx_key;
+ }
+}
+
+////////////////////////////////////////////////////////////////////
+// PangolinNSGLView
+////////////////////////////////////////////////////////////////////
+
+@implementation PangolinNSGLView
+
+-(id)initWithFrame:(NSRect)frameRect pixelFormat:(NSOpenGLPixelFormat *)format
+{
+ self = [super initWithFrame:frameRect pixelFormat:format];
+ context = pangolin::context;
+ return(self);
+}
+
+- (void)prepareOpenGL
+{
+ [super prepareOpenGL];
+}
+
+-(void)reshape
+{
+#if MAC_OS_X_VERSION_MAX_ALLOWED >= MAC_OS_X_VERSION_10_7
+ if ( [self wantsBestResolutionOpenGLSurface] && [ _window respondsToSelector:@selector(backingScaleFactor) ] )
+ backing_scale = [_window backingScaleFactor];
+ else
+#endif
+ backing_scale = 1.0;
+
+ pangolin::process::Resize(self.bounds.size.width * backing_scale, self.bounds.size.height * backing_scale);
+
+ [[self openGLContext] update];
+}
+
+-(BOOL)acceptsFirstResponder
+{
+ return(YES);
+}
+
+-(BOOL)becomeFirstResponder
+{
+ return(YES);
+}
+
+-(BOOL)resignFirstResponder
+{
+ return(YES);
+}
+
+-(BOOL)isFlipped
+{
+ return(YES);
+}
+
+-(NSPoint)_Location:(NSEvent *)theEvent
+{
+ NSPoint location = [self convertPoint: [theEvent locationInWindow] fromView: nil];
+ location.x *= backing_scale;
+ location.y *= backing_scale;
+ return location;
+}
+
+////////////////////////////////////////////////////////////////////
+// Keyboard
+////////////////////////////////////////////////////////////////////
+
+-(void)keyDown:(NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+ NSString *str = [theEvent characters];
+ int len = (int)[str length];
+ for(int i = 0; i < len; i++)
+ {
+ const int osx_key = [str characterAtIndex:i];
+ pangolin::process::Keyboard(mapKeymap(osx_key), location.x, location.y);
+ }
+}
+
+-(void)keyUp:(NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+ NSString *str = [theEvent characters];
+ int len = (int)[str length];
+ for(int i = 0; i < len; i++)
+ {
+ const int osx_key = [str characterAtIndex:i];
+ pangolin::process::KeyboardUp(mapKeymap(osx_key), location.x, location.y);
+ }
+}
+
+- (void)flagsChanged:(NSEvent *)event
+{
+ unsigned int flags = [event modifierFlags] & NSDeviceIndependentModifierFlagsMask;
+
+ if(flags&NSShiftKeyMask) {
+ context->mouse_state |= pangolin::KeyModifierShift;
+ }else{
+ context->mouse_state &= ~pangolin::KeyModifierShift;
+ }
+
+ if(flags&NSControlKeyMask) {
+ context->mouse_state |= pangolin::KeyModifierCtrl;
+ }else{
+ context->mouse_state &= ~pangolin::KeyModifierCtrl;
+ }
+
+ if(flags&NSAlternateKeyMask) {
+ context->mouse_state |= pangolin::KeyModifierAlt;
+ }else{
+ context->mouse_state &= ~pangolin::KeyModifierAlt;
+ }
+
+ if(flags&NSCommandKeyMask) {
+ context->mouse_state |= pangolin::KeyModifierCmd;
+ }else{
+ context->mouse_state &= ~pangolin::KeyModifierCmd;
+ }
+
+ if(flags&NSFunctionKeyMask) {
+ context->mouse_state |= pangolin::KeyModifierFnc;
+ }else{
+ context->mouse_state &= ~pangolin::KeyModifierFnc;
+ }
+}
+
+////////////////////////////////////////////////////////////////////
+// Mouse Input
+////////////////////////////////////////////////////////////////////
+
+-(void)mouseDownCommon:(NSEvent *)theEvent
+{
+ const int button = (int)[theEvent buttonNumber];
+ const NSPoint location = [self _Location: theEvent];
+ pangolin::process::Mouse(mapMouseButton(button), 0, location.x, location.y);
+}
+
+-(void)mouseUpCommon:(NSEvent *)theEvent
+{
+ const int button = (int)[theEvent buttonNumber];
+ const NSPoint location = [self _Location: theEvent];
+ pangolin::process::Mouse(mapMouseButton(button), 1, location.x, location.y);
+}
+
+- (void)mouseDraggedCommon: (NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+ pangolin::process::MouseMotion(location.x, location.y);
+// pangolin::process::SubpixMotion(location.x, location.y, 1.0, 0.0, 0.0, 0.0);
+}
+
+-(void)mouseDown:(NSEvent *)theEvent
+{
+ [self mouseDownCommon:theEvent];
+}
+
+-(void)mouseUp:(NSEvent *)theEvent
+{
+ [self mouseUpCommon:theEvent];
+}
+
+- (void)mouseDragged: (NSEvent *)theEvent
+{
+ [self mouseDraggedCommon:theEvent];
+}
+
+-(void)rightMouseDown:(NSEvent *)theEvent
+{
+ [self mouseDownCommon:theEvent];
+}
+
+-(void)rightMouseUp:(NSEvent *)theEvent
+{
+ [self mouseUpCommon:theEvent];
+}
+
+- (void)rightMouseDragged: (NSEvent *)theEvent
+{
+ [self mouseDraggedCommon:theEvent];
+}
+
+-(void)otherMouseDown:(NSEvent *)theEvent
+{
+ [self mouseDownCommon:theEvent];
+}
+
+-(void)otherMouseUp:(NSEvent *)theEvent
+{
+ [self mouseUpCommon:theEvent];
+}
+
+- (void)otherMouseDragged: (NSEvent *)theEvent
+{
+ [self mouseDraggedCommon:theEvent];
+}
+
+- (void)mouseMoved: (NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+// pangolin::process::PassiveMouseMotion(location.x, location.y);
+ pangolin::process::SubpixMotion(location.x, location.y, 0.0, 0.0, 0.0, 0.0);
+}
+
+- (void)scrollWheel:(NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+
+ float dx, dy;
+ if([theEvent respondsToSelector:@selector(scrollingDeltaX)]) {
+ dx = theEvent.scrollingDeltaX; dy = theEvent.scrollingDeltaY;
+ } else {
+ dx = theEvent.deltaX; dy = theEvent.deltaY;
+ }
+
+ if(dx != 0.0f || dy != 0.0f) {
+ pangolin::process::SpecialInput(
+ pangolin::InputSpecialScroll,
+ location.x, context->base.v.h - location.y,
+ dx, dy,
+ 0, 0
+ );
+ }
+}
+
+- (void)magnifyWithEvent: (NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+ const float dm = theEvent.magnification;
+ if(dm != 0.0f) {
+ pangolin::process::SpecialInput(
+ pangolin::InputSpecialZoom,
+ location.x, context->base.v.h - location.y,
+ dm, 0.0f, 0.0f, 0.0f
+ );
+ }
+}
+
+- (void)rotateWithEvent: (NSEvent *)theEvent
+{
+ const NSPoint location = [self _Location: theEvent];
+ const float dr = theEvent.rotation;
+ if(dr != 0.0f) {
+ pangolin::process::SpecialInput(
+ pangolin::InputSpecialRotate,
+ location.x, context->base.v.h - location.y,
+ dr, 0.0f, 0.0f, 0.0f
+ );
+ }
+}
+
+- (void)mouseEntered: (NSEvent *)theEvent
+{
+ PANGOLIN_UNUSED(theEvent);
+}
+
+- (void)mouseExited: (NSEvent *)theEvent
+{
+ PANGOLIN_UNUSED(theEvent);
+}
+
+-(void)dealloc
+{
+ [super dealloc];
+}
+
+@end
diff --git a/externals/Pangolin/src/display/device/display_android.cpp b/externals/Pangolin/src/display/device/display_android.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..80139870137238c780c8c17aba89b3d60e43cb82
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_android.cpp
@@ -0,0 +1,1026 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * Based largely on android_native_app_glue.c from Android NDK
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/display/device/display_android.h>
+
+#include <poll.h>
+#include <pthread.h>
+#include <sched.h>
+#include <errno.h>
+#include <dlfcn.h>
+
+#include <android/configuration.h>
+#include <android/looper.h>
+#include <android/native_activity.h>
+#include <android/sensor.h>
+#include <android/log.h>
+#include <android/window.h>
+
+#include <iostream>
+#include <string>
+#include <sstream>
+#include <algorithm>
+#include <iterator>
+#include <stdexcept>
+
+#include <jni.h>
+
+#include <errno.h>
+#include <string.h>
+#include <unistd.h>
+#include <sys/resource.h>
+
+namespace pangolin
+{
+
+extern __thread PangolinGl* context;
+
+/**
+ * Shared state for our app.
+ */
+struct engine {
+ android_app* app;
+ ANativeActivity* activity;
+
+ int has_focus;
+ EGLDisplay display;
+ EGLSurface surface;
+ EGLContext context;
+ int32_t width;
+ int32_t height;
+// struct saved_state state;
+};
+
+/**
+ * Initialize an EGL context for the current display.
+ */
+static int engine_init_display(struct engine* engine) {
+ // initialize OpenGL ES and EGL
+
+ /*
+ * Here specify the attributes of the desired configuration.
+ * Below, we select an EGLConfig with at least 8 bits per color
+ * component compatible with on-screen windows
+ */
+ const EGLint attribs[] = {
+ EGL_SURFACE_TYPE, EGL_WINDOW_BIT,
+ EGL_BLUE_SIZE, 8,
+ EGL_GREEN_SIZE, 8,
+ EGL_RED_SIZE, 8,
+ EGL_NONE
+ };
+ EGLint w, h, format;
+ EGLint numConfigs;
+ EGLConfig config;
+ EGLSurface surface;
+ EGLContext context;
+
+ EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+
+ eglInitialize(display, 0, 0);
+
+ /* Here, the application chooses the configuration it desires. In this
+ * sample, we have a very simplified selection process, where we pick
+ * the first EGLConfig that matches our criteria */
+ eglChooseConfig(display, attribs, &config, 1, &numConfigs);
+
+ /* EGL_NATIVE_VISUAL_ID is an attribute of the EGLConfig that is
+ * guaranteed to be accepted by ANativeWindow_setBuffersGeometry().
+ * As soon as we picked a EGLConfig, we can safely reconfigure the
+ * ANativeWindow buffers to match, using EGL_NATIVE_VISUAL_ID. */
+ eglGetConfigAttrib(display, config, EGL_NATIVE_VISUAL_ID, &format);
+
+ ANativeWindow_setBuffersGeometry(engine->app->window, 0, 0, format);
+// ANativeActivity_setWindowFlags(engine->app->activity, AWINDOW_FLAG_FULLSCREEN, 0 );
+
+ EGLint const attrib_list[] = {
+#ifdef HAVE_GLES_2
+ EGL_CONTEXT_CLIENT_VERSION, 2,
+#endif
+ EGL_NONE
+ };
+
+ surface = eglCreateWindowSurface(display, config, engine->app->window, NULL);
+ context = eglCreateContext(display, config, NULL, attrib_list);
+
+ if (eglMakeCurrent(display, surface, surface, context) == EGL_FALSE) {
+ LOGW("Unable to eglMakeCurrent");
+ return -1;
+ }
+
+ eglQuerySurface(display, surface, EGL_WIDTH, &w);
+ eglQuerySurface(display, surface, EGL_HEIGHT, &h);
+
+ engine->display = display;
+ engine->context = context;
+ engine->surface = surface;
+ engine->width = w;
+ engine->height = h;
+
+ pangolin::process::Resize(engine->width,engine->height);
+
+ // Initialize GL state.
+ glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST);
+ glEnable(GL_CULL_FACE);
+ glShadeModel(GL_SMOOTH);
+ glDisable(GL_DEPTH_TEST);
+
+ return 0;
+}
+
+/**
+ * Just the current frame in the display.
+ */
+static void engine_draw_frame(struct engine* engine) {
+ if (engine->display != NULL) {
+ }
+}
+
+/**
+ * Tear down the EGL context currently associated with the display.
+ */
+static void engine_term_display(struct engine* engine) {
+ if (engine->display != EGL_NO_DISPLAY) {
+ eglMakeCurrent(engine->display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT);
+ if (engine->context != EGL_NO_CONTEXT) {
+ eglDestroyContext(engine->display, engine->context);
+ }
+ if (engine->surface != EGL_NO_SURFACE) {
+ eglDestroySurface(engine->display, engine->surface);
+ }
+ eglTerminate(engine->display);
+ }
+ engine->has_focus = 0;
+ engine->display = EGL_NO_DISPLAY;
+ engine->context = EGL_NO_CONTEXT;
+ engine->surface = EGL_NO_SURFACE;
+}
+
+void UnpressAll(float last_x, float last_y)
+{
+ if(context->mouse_state & pangolin::MouseButtonLeft) {
+ pangolin::process::Mouse(0, 1, last_x, last_y);
+ }
+ if(context->mouse_state & pangolin::MouseButtonMiddle) {
+ pangolin::process::Mouse(1, 1, last_x, last_y);
+ }
+ if(context->mouse_state & pangolin::MouseButtonRight) {
+ pangolin::process::Mouse(2, 1, last_x, last_y);
+ }
+ if(context->mouse_state & pangolin::MouseWheelUp) {
+ pangolin::process::Mouse(3, 1, last_x, last_y);
+ }
+ if(context->mouse_state & pangolin::MouseWheelDown) {
+ pangolin::process::Mouse(4, 1, last_x, last_y);
+ }
+ context->mouse_state = 0;
+}
+
+int PangolinKeyFromAndroidKeycode(int32_t keycode, bool shift)
+{
+ if( AKEYCODE_0 <= keycode && keycode <= AKEYCODE_9) {
+ return '0' + (keycode - AKEYCODE_0);
+ }
+
+ if( AKEYCODE_A <= keycode && keycode <= AKEYCODE_Z) {
+ return (shift ? 'A' : 'a') + (keycode - AKEYCODE_A);
+ }
+
+ if(shift) {
+ switch (keycode) {
+ case AKEYCODE_GRAVE: return '~';
+ default:
+ LOGI("Unknown keycode (with shift): %d", keycode);
+ return '?';
+ }
+ }else{
+ switch (keycode) {
+ case AKEYCODE_COMMA: return ',';
+ case AKEYCODE_PERIOD: return '.';
+ case AKEYCODE_SPACE: return ' ';
+ case AKEYCODE_ENTER: return '\r';
+ case AKEYCODE_TAB: return '\t';
+ case AKEYCODE_DEL: return '\b';
+ case AKEYCODE_SLASH: return '/';
+ case AKEYCODE_BACKSLASH: return '\\';
+ case AKEYCODE_SEMICOLON: return ';';
+ case AKEYCODE_APOSTROPHE:return '\'';
+ case AKEYCODE_MINUS: return '-';
+ case AKEYCODE_EQUALS: return '=';
+ case AKEYCODE_PLUS: return '+';
+ case AKEYCODE_AT: return '@';
+ case AKEYCODE_GRAVE: return '`';
+ default:
+ LOGI("Unknown keycode: %d", keycode);
+ return '?';
+ }
+ }
+}
+
+/**
+ * Process the next input event.
+ */
+static int32_t engine_handle_input(struct android_app* app, AInputEvent* event) {
+ struct engine* engine = (struct engine*)app->userData;
+
+ static float last_x = 0;
+ static float last_y = 0;
+
+ const int32_t input_type = AInputEvent_getType(event);
+
+ if (input_type == AINPUT_EVENT_TYPE_MOTION) {
+ engine->has_focus = 1;
+
+
+ const float x = AMotionEvent_getX(event, 0);
+ const float y = AMotionEvent_getY(event, 0);
+ const int32_t actionAndPtr = AMotionEvent_getAction(event);
+ const int32_t action = AMOTION_EVENT_ACTION_MASK & actionAndPtr;
+// const int32_t ptrindex = (AMOTION_EVENT_ACTION_POINTER_INDEX_MASK & actionAndPtr) >> AMOTION_EVENT_ACTION_POINTER_INDEX_SHIFT;
+
+ const size_t num_ptrs = AMotionEvent_getPointerCount(event);
+
+ switch(action)
+ {
+ case AMOTION_EVENT_ACTION_UP:
+ case AMOTION_EVENT_ACTION_POINTER_UP:
+ UnpressAll(last_x, last_y);
+ break;
+ case AMOTION_EVENT_ACTION_DOWN:
+ case AMOTION_EVENT_ACTION_POINTER_DOWN:
+ UnpressAll(last_x, last_y);
+ if(num_ptrs <=2) {
+ const int button = (num_ptrs==1) ? 0 : 2;
+ pangolin::process::Mouse(button, 0, x, y);
+ }
+ break;
+ case AMOTION_EVENT_ACTION_MOVE:
+ if(num_ptrs == 3) {
+ const double dx = x - last_x;
+ const double dy = y - last_y;
+ pangolin::process::Scroll(dx,dy);
+ }else{
+ pangolin::process::MouseMotion(x,y);
+ }
+ break;
+ default:
+ break;
+ }
+
+ last_x = x;
+ last_y = y;
+
+ return 1;
+ }else if(AINPUT_EVENT_TYPE_KEY) {
+ static bool shift = false;
+
+ const int32_t action = AKeyEvent_getAction(event);
+ const int32_t keycode = AKeyEvent_getKeyCode(event);
+
+ if(keycode == AKEYCODE_SHIFT_LEFT) {
+ shift = (action == AKEY_EVENT_ACTION_DOWN);
+ return 1;
+ }
+
+ unsigned char key = PangolinKeyFromAndroidKeycode(keycode, shift);
+
+ if(action == AKEY_EVENT_ACTION_DOWN) {
+ pangolin::process::Keyboard(key, last_x, last_y);
+ }else{
+ pangolin::process::KeyboardUp(key, last_x, last_y);
+ }
+ }
+ return 1;
+}
+
+/**
+ * Process the next main command.
+ */
+static void engine_handle_cmd(struct android_app* app, int32_t cmd) {
+ struct engine* engine = (struct engine*)app->userData;
+ switch (cmd) {
+ case APP_CMD_SAVE_STATE:
+// // The system has asked us to save our current state. Do so.
+// engine->app->savedState = malloc(sizeof(struct saved_state));
+// *((struct saved_state*)engine->app->savedState) = engine->state;
+// engine->app->savedStateSize = sizeof(struct saved_state);
+ break;
+ case APP_CMD_INIT_WINDOW:
+ // The window is being shown, get it ready.
+ if (engine->app->window != NULL) {
+ engine_init_display(engine);
+ engine_draw_frame(engine);
+ }
+ break;
+ case APP_CMD_TERM_WINDOW:
+ // The window is being hidden or closed, clean it up.
+ engine_term_display(engine);
+ break;
+ case APP_CMD_GAINED_FOCUS:
+ engine->has_focus = 1;
+ break;
+ case APP_CMD_LOST_FOCUS:
+ engine->has_focus = 0;
+ break;
+ }
+}
+
+}
+
+// Define library entry point.
+extern "C" {
+
+JNIEnv* GetEnvAttachThread(JavaVM* vm)
+{
+ JNIEnv* env;
+ switch (vm->GetEnv((void**)&env, JNI_VERSION_1_6)) {
+ case JNI_OK:
+ break;
+ case JNI_EDETACHED:
+ if (vm->AttachCurrentThread(&env, NULL)!=0) {
+ LOGE("Could not attach current thread");
+ throw std::runtime_error("Could not attach current thread");
+ }
+ break;
+ case JNI_EVERSION:
+ LOGE("Invalid Java version");
+ throw std::runtime_error("Invalid Java version");
+ }
+ return env;
+}
+
+// https://groups.google.com/d/msg/android-ndk/Tk3g00wLKhk/TJQucoaE_asJ
+void displayKeyboard(android_app* app, bool pShow) {
+ jint lFlags = 0;
+ JNIEnv* env = GetEnvAttachThread(app->activity->vm);
+ if(env) {
+ // Retrieves NativeActivity.
+ jobject lNativeActivity = app->activity->clazz;
+ jclass ClassNativeActivity = env->GetObjectClass(lNativeActivity);
+
+ // Retrieves Context.INPUT_METHOD_SERVICE.
+ jclass ClassContext = env->FindClass("android/content/Context");
+ jfieldID FieldINPUT_METHOD_SERVICE = env->GetStaticFieldID(ClassContext, "INPUT_METHOD_SERVICE", "Ljava/lang/String;");
+ jobject INPUT_METHOD_SERVICE = env->GetStaticObjectField(ClassContext, FieldINPUT_METHOD_SERVICE);
+ // jniCheck(INPUT_METHOD_SERVICE);
+
+ // lInputMethodManager = getSystemService(Context.INPUT_METHOD_SERVICE).
+ jclass ClassInputMethodManager = env->FindClass( "android/view/inputmethod/InputMethodManager");
+ jmethodID MethodGetSystemService = env->GetMethodID( ClassNativeActivity, "getSystemService", "(Ljava/lang/String;)Ljava/lang/Object;");
+ jobject lInputMethodManager = env->CallObjectMethod( lNativeActivity, MethodGetSystemService, INPUT_METHOD_SERVICE);
+
+ // lDecorView = getWindow().getDecorView().
+ jmethodID MethodGetWindow = env->GetMethodID( ClassNativeActivity, "getWindow", "()Landroid/view/Window;");
+ jobject lWindow = env->CallObjectMethod(lNativeActivity, MethodGetWindow);
+ jclass ClassWindow = env->FindClass( "android/view/Window");
+ jmethodID MethodGetDecorView = env->GetMethodID( ClassWindow, "getDecorView", "()Landroid/view/View;");
+ jobject lDecorView = env->CallObjectMethod(lWindow, MethodGetDecorView);
+
+ if (pShow) {
+ // Runs lInputMethodManager.showSoftInput(...).
+ jmethodID MethodShowSoftInput = env->GetMethodID( ClassInputMethodManager, "showSoftInput", "(Landroid/view/View;I)Z");
+ /*jboolean lResult = */env->CallBooleanMethod( lInputMethodManager, MethodShowSoftInput, lDecorView, lFlags);
+ } else {
+ // Runs lWindow.getViewToken()
+ jclass ClassView = env->FindClass( "android/view/View");
+ jmethodID MethodGetWindowToken = env->GetMethodID( ClassView, "getWindowToken", "()Landroid/os/IBinder;");
+ jobject lBinder = env->CallObjectMethod(lDecorView, MethodGetWindowToken);
+
+ // lInputMethodManager.hideSoftInput(...).
+ jmethodID MethodHideSoftInput = env->GetMethodID( ClassInputMethodManager, "hideSoftInputFromWindow", "(Landroid/os/IBinder;I)Z");
+ /*jboolean lRes = */env->CallBooleanMethod( lInputMethodManager, MethodHideSoftInput, lBinder, lFlags);
+ }
+
+ // Finished with the JVM.
+ app->activity->vm->DetachCurrentThread();
+ }
+}
+
+pangolin::engine g_engine;
+
+static void free_saved_state(struct android_app* android_app) {
+ pthread_mutex_lock(&android_app->mutex);
+ if (android_app->savedState != NULL) {
+ free(android_app->savedState);
+ android_app->savedState = NULL;
+ android_app->savedStateSize = 0;
+ }
+ pthread_mutex_unlock(&android_app->mutex);
+}
+
+int8_t android_app_read_cmd(struct android_app* android_app) {
+ int8_t cmd;
+ if (read(android_app->msgread, &cmd, sizeof(cmd)) == sizeof(cmd)) {
+ switch (cmd) {
+ case APP_CMD_SAVE_STATE:
+ free_saved_state(android_app);
+ break;
+ }
+ return cmd;
+ } else {
+ LOGE("No data on command pipe!");
+ }
+ return -1;
+}
+
+static void print_cur_config(struct android_app* android_app) {
+ char lang[2], country[2];
+ AConfiguration_getLanguage(android_app->config, lang);
+ AConfiguration_getCountry(android_app->config, country);
+
+ LOGV("Config: mcc=%d mnc=%d lang=%c%c cnt=%c%c orien=%d touch=%d dens=%d "
+ "keys=%d nav=%d keysHid=%d navHid=%d sdk=%d size=%d long=%d "
+ "modetype=%d modenight=%d",
+ AConfiguration_getMcc(android_app->config),
+ AConfiguration_getMnc(android_app->config),
+ lang[0], lang[1], country[0], country[1],
+ AConfiguration_getOrientation(android_app->config),
+ AConfiguration_getTouchscreen(android_app->config),
+ AConfiguration_getDensity(android_app->config),
+ AConfiguration_getKeyboard(android_app->config),
+ AConfiguration_getNavigation(android_app->config),
+ AConfiguration_getKeysHidden(android_app->config),
+ AConfiguration_getNavHidden(android_app->config),
+ AConfiguration_getSdkVersion(android_app->config),
+ AConfiguration_getScreenSize(android_app->config),
+ AConfiguration_getScreenLong(android_app->config),
+ AConfiguration_getUiModeType(android_app->config),
+ AConfiguration_getUiModeNight(android_app->config));
+}
+
+void android_app_pre_exec_cmd(struct android_app* android_app, int8_t cmd) {
+ switch (cmd) {
+ case APP_CMD_INPUT_CHANGED:
+ LOGV("APP_CMD_INPUT_CHANGED\n");
+ pthread_mutex_lock(&android_app->mutex);
+ if (android_app->inputQueue != NULL) {
+ AInputQueue_detachLooper(android_app->inputQueue);
+ }
+ android_app->inputQueue = android_app->pendingInputQueue;
+ if (android_app->inputQueue != NULL) {
+ LOGV("Attaching input queue to looper");
+ AInputQueue_attachLooper(android_app->inputQueue,
+ android_app->looper, LOOPER_ID_INPUT, NULL,
+ &android_app->inputPollSource);
+ }
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ break;
+
+ case APP_CMD_INIT_WINDOW:
+ LOGV("APP_CMD_INIT_WINDOW\n");
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->window = android_app->pendingWindow;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ break;
+
+ case APP_CMD_TERM_WINDOW:
+ LOGV("APP_CMD_TERM_WINDOW\n");
+ pthread_cond_broadcast(&android_app->cond);
+ break;
+
+ case APP_CMD_RESUME:
+ case APP_CMD_START:
+ case APP_CMD_PAUSE:
+ case APP_CMD_STOP:
+ LOGV("activityState=%d\n", cmd);
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->activityState = cmd;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ break;
+
+ case APP_CMD_CONFIG_CHANGED:
+ LOGV("APP_CMD_CONFIG_CHANGED\n");
+ AConfiguration_fromAssetManager(android_app->config,
+ android_app->activity->assetManager);
+ print_cur_config(android_app);
+ break;
+
+ case APP_CMD_DESTROY:
+ LOGV("APP_CMD_DESTROY\n");
+ android_app->destroyRequested = 1;
+ break;
+ }
+}
+
+void android_app_post_exec_cmd(struct android_app* android_app, int8_t cmd) {
+ switch (cmd) {
+ case APP_CMD_TERM_WINDOW:
+ LOGV("APP_CMD_TERM_WINDOW\n");
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->window = NULL;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ break;
+
+ case APP_CMD_SAVE_STATE:
+ LOGV("APP_CMD_SAVE_STATE\n");
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->stateSaved = 1;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ break;
+
+ case APP_CMD_RESUME:
+ free_saved_state(android_app);
+ break;
+ }
+}
+
+static void android_app_destroy(struct android_app* android_app) {
+ LOGV("+android_app_destroy!");
+ free_saved_state(android_app);
+ pthread_mutex_lock(&android_app->mutex);
+ if (android_app->inputQueue != NULL) {
+ AInputQueue_detachLooper(android_app->inputQueue);
+ }
+ AConfiguration_delete(android_app->config);
+ android_app->destroyed = 1;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+ // Can't touch android_app object after this.
+ LOGV("-android_app_destroy!");
+}
+
+static void process_input(struct android_app* app, struct android_poll_source* source) {
+ AInputEvent* event = NULL;
+ if (AInputQueue_getEvent(app->inputQueue, &event) >= 0) {
+
+ // HACK: Override back buttom to show / hide keyboard.
+ int type = AInputEvent_getType(event);
+ if(type == AINPUT_EVENT_TYPE_KEY) {
+ if(AKeyEvent_getAction(event) == AKEY_EVENT_ACTION_DOWN) {
+ static bool keyboard_shown = false;
+ if( AKeyEvent_getKeyCode(event) == AKEYCODE_BACK ) {
+ displayKeyboard(app,!keyboard_shown);
+ keyboard_shown = !keyboard_shown;
+ AInputQueue_finishEvent(app->inputQueue, event, 1);
+ return;
+ }
+ }
+ }
+
+ if (AInputQueue_preDispatchEvent(app->inputQueue, event)) {
+ return;
+ }
+ int32_t handled = 0;
+ if (app->onInputEvent != NULL) handled = app->onInputEvent(app, event);
+ AInputQueue_finishEvent(app->inputQueue, event, handled);
+ } else {
+ LOGE("Failure reading next input event: %s\n", strerror(errno));
+ }
+}
+
+static void process_cmd(struct android_app* app, struct android_poll_source* source) {
+ int8_t cmd = android_app_read_cmd(app);
+ android_app_pre_exec_cmd(app, cmd);
+ if (app->onAppCmd != NULL) app->onAppCmd(app, cmd);
+ android_app_post_exec_cmd(app, cmd);
+}
+
+static void* android_app_entry(void* param) {
+ LOGV("+android_app_entry");
+ struct android_app* android_app = (struct android_app*)param;
+
+ android_app->config = AConfiguration_new();
+ AConfiguration_fromAssetManager(android_app->config, android_app->activity->assetManager);
+
+ print_cur_config(android_app);
+
+ android_app->cmdPollSource.id = LOOPER_ID_MAIN;
+ android_app->cmdPollSource.app = android_app;
+ android_app->cmdPollSource.process = process_cmd;
+ android_app->inputPollSource.id = LOOPER_ID_INPUT;
+ android_app->inputPollSource.app = android_app;
+ android_app->inputPollSource.process = process_input;
+
+ ALooper* looper = ALooper_prepare(ALOOPER_PREPARE_ALLOW_NON_CALLBACKS);
+ ALooper_addFd(looper, android_app->msgread, LOOPER_ID_MAIN, ALOOPER_EVENT_INPUT, NULL,
+ &android_app->cmdPollSource);
+ android_app->looper = looper;
+
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->running = 1;
+ pthread_cond_broadcast(&android_app->cond);
+ pthread_mutex_unlock(&android_app->mutex);
+
+ std::string sargv;
+
+ // Load command line from ARGV parameter
+ JNIEnv *env = GetEnvAttachThread(android_app->activity->vm);
+ if(env) {
+ jobject me = android_app->activity->clazz;
+
+ jclass acl = env->GetObjectClass(me); //class pointer of NativeActivity
+ jmethodID giid = env->GetMethodID(acl, "getIntent", "()Landroid/content/Intent;");
+ jobject intent = env->CallObjectMethod(me, giid); //Got our intent
+
+ jclass icl = env->GetObjectClass(intent); //class pointer of Intent
+ jmethodID gseid = env->GetMethodID(icl, "getStringExtra", "(Ljava/lang/String;)Ljava/lang/String;");
+
+ jstring jsARGV = (jstring)env->CallObjectMethod(intent, gseid, env->NewStringUTF("ARGV"));
+
+
+ if(jsARGV) {
+ const char *chARGV = env->GetStringUTFChars(jsARGV, 0);
+ if(chARGV) {
+ sargv = std::string(chARGV);
+ LOGI("ARGV: pango %s", chARGV);
+ }
+ env->ReleaseStringUTFChars(jsARGV, chARGV);
+ }
+
+ android_app->activity->vm->DetachCurrentThread();
+ }
+
+ // Set up argv/argc to pass to users main
+ std::vector<std::string> vargv;
+ vargv.push_back("pango");
+
+ // Parse parameters from ARGV android intent parameter
+ std::istringstream iss(sargv);
+ std::copy(std::istream_iterator<std::string>(iss),
+ std::istream_iterator<std::string>(),
+ std::back_inserter<std::vector<std::string> >(vargv));
+
+ char* argv[vargv.size()+1];
+ for(size_t ac = 0; ac < vargv.size(); ++ac) {
+ argv[ac] = new char[vargv[ac].size()];
+ strcpy( argv[ac], vargv[ac].c_str() );
+ }
+ argv[vargv.size()] = NULL;
+
+ // Find main symbol
+ void (*main)(int, char**);
+ *(void **) (&main) = dlsym( dlopen(android_app->application_so, RTLD_NOW), "main");
+ if (!main) {
+ LOGE( "undefined symbol main, crap" );
+ exit(1);
+ }
+ // Call users standard main entry point.
+ (*main)(vargv.size(), argv);
+
+ // Clean up parameters
+ for(size_t ac = 0; ac < vargv.size(); ++ac) {
+ delete[] argv[ac];
+ }
+
+ android_app_destroy(android_app);
+
+ LOGV("-android_app_entry");
+
+ return NULL;
+}
+
+static void android_app_write_cmd(struct android_app* android_app, int8_t cmd) {
+ if (write(android_app->msgwrite, &cmd, sizeof(cmd)) != sizeof(cmd)) {
+ LOGE("Failure writing android_app cmd: %s\n", strerror(errno));
+ }
+}
+
+static void android_app_set_input(struct android_app* android_app, AInputQueue* inputQueue) {
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->pendingInputQueue = inputQueue;
+ android_app_write_cmd(android_app, APP_CMD_INPUT_CHANGED);
+ while (android_app->inputQueue != android_app->pendingInputQueue) {
+ pthread_cond_wait(&android_app->cond, &android_app->mutex);
+ }
+ pthread_mutex_unlock(&android_app->mutex);
+}
+
+static void android_app_set_window(struct android_app* android_app, ANativeWindow* window) {
+ pthread_mutex_lock(&android_app->mutex);
+ if (android_app->pendingWindow != NULL) {
+ android_app_write_cmd(android_app, APP_CMD_TERM_WINDOW);
+ }
+ android_app->pendingWindow = window;
+ if (window != NULL) {
+ android_app_write_cmd(android_app, APP_CMD_INIT_WINDOW);
+ }
+ while (android_app->window != android_app->pendingWindow) {
+ pthread_cond_wait(&android_app->cond, &android_app->mutex);
+ }
+ pthread_mutex_unlock(&android_app->mutex);
+}
+
+static void android_app_set_activity_state(struct android_app* android_app, int8_t cmd) {
+ pthread_mutex_lock(&android_app->mutex);
+ android_app_write_cmd(android_app, cmd);
+ while (android_app->activityState != cmd) {
+ pthread_cond_wait(&android_app->cond, &android_app->mutex);
+ }
+ pthread_mutex_unlock(&android_app->mutex);
+}
+
+static void android_app_free(struct android_app* android_app) {
+ pthread_mutex_lock(&android_app->mutex);
+ android_app_write_cmd(android_app, APP_CMD_DESTROY);
+ while (!android_app->destroyed) {
+ pthread_cond_wait(&android_app->cond, &android_app->mutex);
+ }
+ pthread_mutex_unlock(&android_app->mutex);
+
+ close(android_app->msgread);
+ close(android_app->msgwrite);
+ pthread_cond_destroy(&android_app->cond);
+ pthread_mutex_destroy(&android_app->mutex);
+ free(android_app);
+}
+
+static void onDestroy(ANativeActivity* activity) {
+ LOGV("Destroy: %p\n", activity);
+ android_app_free((struct android_app*)activity->instance);
+}
+
+static void onStart(ANativeActivity* activity) {
+ LOGV("Start: %p\n", activity);
+ android_app_set_activity_state((struct android_app*)activity->instance, APP_CMD_START);
+}
+
+static void onResume(ANativeActivity* activity) {
+ LOGV("Resume: %p\n", activity);
+ android_app_set_activity_state((struct android_app*)activity->instance, APP_CMD_RESUME);
+}
+
+static void* onSaveInstanceState(ANativeActivity* activity, size_t* outLen) {
+ struct android_app* android_app = (struct android_app*)activity->instance;
+ void* savedState = NULL;
+
+ LOGV("SaveInstanceState: %p\n", activity);
+ pthread_mutex_lock(&android_app->mutex);
+ android_app->stateSaved = 0;
+ android_app_write_cmd(android_app, APP_CMD_SAVE_STATE);
+ while (!android_app->stateSaved) {
+ pthread_cond_wait(&android_app->cond, &android_app->mutex);
+ }
+
+ if (android_app->savedState != NULL) {
+ savedState = android_app->savedState;
+ *outLen = android_app->savedStateSize;
+ android_app->savedState = NULL;
+ android_app->savedStateSize = 0;
+ }
+
+ pthread_mutex_unlock(&android_app->mutex);
+
+ return savedState;
+}
+
+static void onPause(ANativeActivity* activity) {
+ LOGV("Pause: %p\n", activity);
+ android_app_set_activity_state((struct android_app*)activity->instance, APP_CMD_PAUSE);
+}
+
+static void onStop(ANativeActivity* activity) {
+ LOGV("Stop: %p\n", activity);
+ android_app_set_activity_state((struct android_app*)activity->instance, APP_CMD_STOP);
+}
+
+static void onConfigurationChanged(ANativeActivity* activity) {
+ struct android_app* android_app = (struct android_app*)activity->instance;
+ LOGV("ConfigurationChanged: %p\n", activity);
+ android_app_write_cmd(android_app, APP_CMD_CONFIG_CHANGED);
+}
+
+static void onLowMemory(ANativeActivity* activity) {
+ struct android_app* android_app = (struct android_app*)activity->instance;
+ LOGV("LowMemory: %p\n", activity);
+ android_app_write_cmd(android_app, APP_CMD_LOW_MEMORY);
+}
+
+static void onWindowFocusChanged(ANativeActivity* activity, int focused) {
+ LOGV("WindowFocusChanged: %p -- %d\n", activity, focused);
+ android_app_write_cmd((struct android_app*)activity->instance,
+ focused ? APP_CMD_GAINED_FOCUS : APP_CMD_LOST_FOCUS);
+}
+
+static void onNativeWindowCreated(ANativeActivity* activity, ANativeWindow* window) {
+ LOGV("NativeWindowCreated: %p -- %p\n", activity, window);
+ android_app_set_window((struct android_app*)activity->instance, window);
+}
+
+static void onNativeWindowDestroyed(ANativeActivity* activity, ANativeWindow* window) {
+ LOGV("NativeWindowDestroyed: %p -- %p\n", activity, window);
+ android_app_set_window((struct android_app*)activity->instance, NULL);
+}
+
+static void onInputQueueCreated(ANativeActivity* activity, AInputQueue* queue) {
+ LOGV("InputQueueCreated: %p -- %p\n", activity, queue);
+ android_app_set_input((struct android_app*)activity->instance, queue);
+}
+
+static void onInputQueueDestroyed(ANativeActivity* activity, AInputQueue* queue) {
+ LOGV("InputQueueDestroyed: %p -- %p\n", activity, queue);
+ android_app_set_input((struct android_app*)activity->instance, NULL);
+}
+
+static void onContentRectChanged(ANativeActivity* activity, const ARect* rect) {
+ LOGV("onContentRectChanged: %p -- (%d, %d), (%d, %d)\n", activity, rect->left, rect->top, rect->right, rect->bottom);
+}
+
+void DeferredNativeActivity_onCreate(
+ ANativeActivity* activity,
+ void* savedState,
+ size_t savedStateSize,
+ const char* load_target
+ )
+{
+ activity->callbacks->onDestroy = onDestroy;
+ activity->callbacks->onStart = onStart;
+ activity->callbacks->onResume = onResume;
+ activity->callbacks->onSaveInstanceState = onSaveInstanceState;
+ activity->callbacks->onPause = onPause;
+ activity->callbacks->onStop = onStop;
+ activity->callbacks->onConfigurationChanged = onConfigurationChanged;
+ activity->callbacks->onLowMemory = onLowMemory;
+ activity->callbacks->onWindowFocusChanged = onWindowFocusChanged;
+ activity->callbacks->onNativeWindowCreated = onNativeWindowCreated;
+ activity->callbacks->onNativeWindowDestroyed = onNativeWindowDestroyed;
+ activity->callbacks->onInputQueueCreated = onInputQueueCreated;
+ activity->callbacks->onInputQueueDestroyed = onInputQueueDestroyed;
+ activity->callbacks->onContentRectChanged = onContentRectChanged;
+
+ // Create threaded android_app
+ android_app* app = (struct android_app*)malloc(sizeof(struct android_app));
+ memset(app, 0, sizeof(struct android_app));
+ app->activity = activity;
+ app->application_so = load_target;
+
+ pthread_mutex_init(&app->mutex, NULL);
+ pthread_cond_init(&app->cond, NULL);
+
+ if (savedState != NULL) {
+ app->savedState = malloc(savedStateSize);
+ app->savedStateSize = savedStateSize;
+ memcpy(app->savedState, savedState, savedStateSize);
+ }
+
+ int msgpipe[2];
+ if (pipe(msgpipe)) {
+ LOGE("could not create pipe: %s", strerror(errno));
+ exit(1);
+ }
+ app->msgread = msgpipe[0];
+ app->msgwrite = msgpipe[1];
+
+ pthread_attr_t attr;
+ pthread_attr_init(&attr);
+ pthread_attr_setdetachstate(&attr, PTHREAD_CREATE_DETACHED);
+ pthread_create(&app->thread, &attr, android_app_entry, app);
+
+ // Wait for thread to start.
+ pthread_mutex_lock(&app->mutex);
+ while (!app->running) {
+ pthread_cond_wait(&app->cond, &app->mutex);
+ }
+ pthread_mutex_unlock(&app->mutex);
+
+ activity->instance = app;
+
+ // Save global variables for use later
+ memset(&g_engine, 0, sizeof(pangolin::engine));
+ app->userData = &g_engine;
+ app->onAppCmd = pangolin::engine_handle_cmd;
+ app->onInputEvent = pangolin::engine_handle_input;
+ g_engine.app = app;
+ g_engine.activity = activity;
+
+// // Load existing state if it exists
+// if (app->savedState != NULL) {
+// // We are starting with a previous saved state; restore from it.
+// g_engine.state = *(struct pangolin::saved_state*)app->savedState;
+// }
+}
+}
+
+namespace pangolin
+{
+
+void CreateAndroidWindowAndBind(std::string name)
+{
+ LOGI("*****************************************************************");
+ LOGV("+CreateAndroidWindowAndBind");
+ // Bind and Wait for GL Context
+ pangolin::BindToContext(name);
+ ProcessAndroidEvents();
+ LOGV("-CreateAndroidWindowAndBind");
+}
+
+void ProcessAndroidEvents()
+{
+ do {
+ // Read all pending events.
+ int ident;
+ int events;
+ struct android_poll_source* source;
+
+ // If not animating, we will block forever waiting for events.
+ // If animating, we loop until all events are read, then continue
+ // to draw the next frame of animation.
+ while ((ident=ALooper_pollAll(g_engine.has_focus ? 0 : -1, NULL, &events,
+ (void**)&source)) >= 0) {
+
+ // Process this event.
+ if (source != NULL) {
+ source->process(g_engine.app, source);
+ }
+
+ // Check if we are exiting.
+ if (g_engine.app->destroyRequested != 0) {
+ engine_term_display(&g_engine);
+ context->quit = true;
+ return;
+ }
+ }
+ } while (g_engine.display == NULL);
+}
+
+void FinishAndroidFrame()
+{
+ ProcessAndroidEvents();
+ RenderViews();
+ PostRender();
+ eglSwapBuffers(g_engine.display, g_engine.surface);
+}
+
+// Implement platform agnostic version
+void CreateWindowAndBind(std::string window_title, int /*w*/, int /*h*/, const Params& /*params*/ )
+{
+ CreateAndroidWindowAndBind(window_title);
+
+#ifdef HAVE_GLES_2
+ // Bind default compatibility shader
+ pangolin::glEngine().prog_fixed.Bind();
+#endif
+}
+
+// Implement platform agnostic version
+void FinishFrame()
+{
+ FinishAndroidFrame();
+}
+
+void SetFullscreen(bool /*fullscreen*/)
+{
+ // Do nothing
+}
+
+void PangolinPlatformInit(PangolinGl& /*context*/)
+{
+}
+
+void PangolinPlatformDeinit(PangolinGl& /*context*/)
+{
+}
+
+PANGOLIN_REGISTER_FACTORY(AndroidWindow)
+{
+ struct AndroidWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ CreateAndroidWindowAndBind(window_title);
+#ifdef HAVE_GLES_2
+ // Bind default compatibility shader
+ pangolin::glEngine().prog_fixed.Bind();
+#endif
+ return NULL;
+ }
+ };
+
+ auto factory = std::make_shared<AndroidWindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "android");
+ }
+}
diff --git a/externals/Pangolin/src/display/device/display_headless.cpp b/externals/Pangolin/src/display/device/display_headless.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..c10dcc5606045d07e1cb7940ed6cd23916080050
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_headless.cpp
@@ -0,0 +1,166 @@
+#include <pangolin/display/display_internal.h>
+#include <pangolin/factory/factory_registry.h>
+#include <EGL/egl.h>
+
+namespace pangolin {
+
+extern __thread PangolinGl* context;
+
+namespace headless {
+
+class EGLDisplayHL {
+public:
+ EGLDisplayHL(const int width, const int height);
+
+ ~EGLDisplayHL();
+
+ void swap();
+
+ void makeCurrent();
+
+ void removeCurrent();
+
+private:
+ EGLSurface egl_surface;
+ EGLContext egl_context;
+ EGLDisplay egl_display;
+
+ static constexpr EGLint attribs[] = {
+ EGL_SURFACE_TYPE , EGL_PBUFFER_BIT,
+ EGL_RENDERABLE_TYPE , EGL_OPENGL_BIT,
+ EGL_RED_SIZE , 8,
+ EGL_GREEN_SIZE , 8,
+ EGL_BLUE_SIZE , 8,
+ EGL_ALPHA_SIZE , 8,
+ EGL_DEPTH_SIZE , 24,
+ EGL_STENCIL_SIZE , 8,
+ EGL_NONE
+ };
+};
+
+constexpr EGLint EGLDisplayHL::attribs[];
+
+struct HeadlessWindow : public PangolinGl {
+ HeadlessWindow(const int width, const int height);
+
+ ~HeadlessWindow() override;
+
+ void ToggleFullscreen() override;
+
+ void Move(const int x, const int y) override;
+
+ void Resize(const unsigned int w, const unsigned int h) override;
+
+ void MakeCurrent() override;
+
+ void RemoveCurrent() override;
+
+ void SwapBuffers() override;
+
+ void ProcessEvents() override;
+
+ EGLDisplayHL display;
+};
+
+EGLDisplayHL::EGLDisplayHL(const int width, const int height) {
+ egl_display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+ if(!egl_display) {
+ std::cerr << "Failed to open EGL display" << std::endl;
+ }
+
+ EGLint major, minor;
+ if(eglInitialize(egl_display, &major, &minor)==EGL_FALSE) {
+ std::cerr << "EGL init failed" << std::endl;
+ }
+
+ if(eglBindAPI(EGL_OPENGL_API)==EGL_FALSE) {
+ std::cerr << "EGL bind failed" << std::endl;
+ }
+
+ EGLint count;
+ eglGetConfigs(egl_display, nullptr, 0, &count);
+
+ std::vector<EGLConfig> egl_configs(count);
+
+ EGLint numConfigs;
+ eglChooseConfig(egl_display, attribs, egl_configs.data(), count, &numConfigs);
+
+ egl_context = eglCreateContext(egl_display, egl_configs[0], EGL_NO_CONTEXT, nullptr);
+
+ const EGLint pbufferAttribs[] = {
+ EGL_WIDTH, width,
+ EGL_HEIGHT, height,
+ EGL_NONE,
+ };
+ egl_surface = eglCreatePbufferSurface(egl_display, egl_configs[0], pbufferAttribs);
+ if (egl_surface == EGL_NO_SURFACE) {
+ std::cerr << "Cannot create EGL surface" << std::endl;
+ }
+}
+
+EGLDisplayHL::~EGLDisplayHL() {
+ if(egl_context) eglDestroyContext(egl_display, egl_context);
+ if(egl_surface) eglDestroySurface(egl_display, egl_surface);
+ if(egl_display) eglTerminate(egl_display);
+}
+
+void EGLDisplayHL::swap() {
+ eglSwapBuffers(egl_display, egl_surface);
+}
+
+void EGLDisplayHL::makeCurrent() {
+ eglMakeCurrent(egl_display, egl_surface, egl_surface, egl_context);
+}
+
+void EGLDisplayHL::removeCurrent() {
+ eglMakeCurrent(egl_display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT);
+}
+
+HeadlessWindow::HeadlessWindow(const int w, const int h) : display(w, h) {
+ windowed_size[0] = w;
+ windowed_size[1] = h;
+}
+
+HeadlessWindow::~HeadlessWindow() { }
+
+void HeadlessWindow::MakeCurrent() {
+ display.makeCurrent();
+ context = this;
+}
+
+void HeadlessWindow::RemoveCurrent() {
+ display.removeCurrent();
+}
+
+void HeadlessWindow::ToggleFullscreen() { }
+
+void HeadlessWindow::Move(const int /*x*/, const int /*y*/) { }
+
+void HeadlessWindow::Resize(const unsigned int /*w*/, const unsigned int /*h*/) { }
+
+void HeadlessWindow::ProcessEvents() { }
+
+void HeadlessWindow::SwapBuffers() {
+ display.swap();
+ MakeCurrent();
+}
+
+} // namespace headless
+
+PANGOLIN_REGISTER_FACTORY(NoneWindow) {
+struct HeadlessWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+ return std::unique_ptr<WindowInterface>(new headless::HeadlessWindow(uri.Get<int>("w", 640), uri.Get<int>("h", 480)));
+ }
+
+ virtual ~HeadlessWindowFactory() { }
+};
+
+auto factory = std::make_shared<HeadlessWindowFactory>();
+FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 1, "none");
+FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 1, "nogui");
+FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 1, "headless");
+}
+
+} // namespace pangolin
+
diff --git a/externals/Pangolin/src/display/device/display_osx.mm b/externals/Pangolin/src/display/device/display_osx.mm
new file mode 100644
index 0000000000000000000000000000000000000000..67a1fec0b88ecfb9d3df02ccda6e57d9e0580929
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_osx.mm
@@ -0,0 +1,246 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2018 Steven Lovegrove, Andrey Mnatsakanov
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/display/device/OsxWindow.h>
+#include <pangolin/display/device/PangolinNSGLView.h>
+#include <pangolin/display/device/PangolinNSApplication.h>
+#include <memory>
+
+#if MAC_OS_X_VERSION_MAX_ALLOWED >= MAC_OS_X_VERSION_10_12
+# define NSFullScreenWindowMask NSWindowStyleMaskFullScreen
+# define NSTitledWindowMask NSWindowStyleMaskTitled
+# define NSMiniaturizableWindowMask NSWindowStyleMaskMiniaturizable
+# define NSResizableWindowMask NSWindowStyleMaskResizable
+# define NSClosableWindowMask NSWindowStyleMaskClosable
+#endif
+
+// Hack to fix window focus issue
+// http://www.miscdebris.net/blog/2010/03/30/solution-for-my-mac-os-x-gui-program-doesnt-get-focus-if-its-outside-an-application-bundle/
+extern "C" { void CPSEnableForegroundOperation(ProcessSerialNumber* psn); }
+inline void FixOsxFocus()
+{
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+ ProcessSerialNumber psn;
+ GetCurrentProcess( &psn );
+ CPSEnableForegroundOperation( &psn );
+ SetFrontProcess( &psn );
+#pragma clang diagnostic pop
+}
+
+namespace pangolin
+{
+
+extern __thread PangolinGl* context;
+
+std::unique_ptr<WindowInterface> CreateOsxWindowAndBind(std::string window_title, int w, int h, const bool is_highres)
+{
+
+ OsxWindow* win = new OsxWindow(window_title, w, h, is_highres);
+
+ return std::unique_ptr<WindowInterface>(win);
+}
+
+OsxWindow::OsxWindow(
+ const std::string& title, int width, int height, bool USE_RETINA
+) {
+ context = this;
+
+ PangolinGl::is_double_buffered = true;
+ PangolinGl::windowed_size[0] = width;
+ PangolinGl::windowed_size[1] = height;
+
+// // These are important I think!
+// NSAutoreleasePool *pool = [[NSAutoreleasePool alloc] init];
+// [pool release];
+
+ ///////////////////////////////////////////////////////////////////////
+ // Make sure Application is initialised correctly.
+ // This can be run repeatedly.
+
+ [NSApplication sharedApplication];
+ PangolinAppDelegate *delegate = [[PangolinAppDelegate alloc] init];
+
+ [NSApp setDelegate:delegate];
+ [NSApp setPresentationOptions:NSFullScreenWindowMask];
+
+ [PangolinNSApplication run_pre];
+ [PangolinNSApplication run_step];
+
+ ///////////////////////////////////////////////////////////////////////
+ // Create Window
+
+ NSRect viewRect = NSMakeRect( 0.0, 0.0, width, height );
+
+ _window = [[NSWindow alloc] initWithContentRect:viewRect styleMask:NSTitledWindowMask|NSMiniaturizableWindowMask|NSResizableWindowMask|NSClosableWindowMask backing:NSBackingStoreBuffered defer:YES];
+ [_window setTitle:[NSString stringWithUTF8String:title.c_str()]];
+ [_window setOpaque:YES];
+ [_window makeKeyAndOrderFront:NSApp];
+ [_window setCollectionBehavior: NSWindowCollectionBehaviorFullScreenPrimary];
+
+ PangolinWindowDelegate *windelegate = [[PangolinWindowDelegate alloc] init];
+ [_window setDelegate:windelegate];
+
+ ///////////////////////////////////////////////////////////////////////
+ // Setup Menu
+
+// NSMenu *mainMenuBar;
+// NSMenu *appMenu;
+// NSMenuItem *menuItem;
+
+// mainMenuBar = [[NSMenu alloc] init];
+
+// appMenu = [[NSMenu alloc] initWithTitle:@"Pangolin Application"];
+// menuItem = [appMenu addItemWithTitle:@"Quit Pangolin Application" action:@selector(terminate:) keyEquivalent:@"q"];
+// [menuItem setKeyEquivalentModifierMask:NSCommandKeyMask];
+
+// menuItem = [[NSMenuItem alloc] init];
+// [menuItem setSubmenu:appMenu];
+
+// [mainMenuBar addItem:menuItem];
+
+// //[NSApp performSelector:@selector(setAppleMenu:) withObject:appMenu];
+// [appMenu release];
+// [NSApp setMainMenu:mainMenuBar];
+
+ ///////////////////////////////////////////////////////////////////////
+ // Create OpenGL View for Window
+
+ NSOpenGLPixelFormatAttribute attrs[] =
+ {
+ NSOpenGLPFADoubleBuffer,
+ NSOpenGLPFADepthSize, 32,
+ NSOpenGLPFAOpenGLProfile, NSOpenGLProfileVersionLegacy,
+ 0
+ };
+
+ NSOpenGLPixelFormat *format = [[NSOpenGLPixelFormat alloc] initWithAttributes:attrs];
+ view = [[PangolinNSGLView alloc] initWithFrame:_window.frame pixelFormat:format];
+ [format release];
+#if MAC_OS_X_VERSION_MAX_ALLOWED >= 1070
+ if( USE_RETINA && floor(NSAppKitVersionNumber) > NSAppKitVersionNumber10_6)
+ [view setWantsBestResolutionOpenGLSurface:YES];
+#endif /*MAC_OS_X_VERSION_MAX_ALLOWED*/
+
+ [_window setContentView:view];
+
+ [PangolinNSApplication run_step];
+
+ glewInit();
+
+ FixOsxFocus();
+}
+
+OsxWindow::~OsxWindow()
+{
+ // Not sure how to deallocate...
+}
+
+void OsxWindow::StartFullScreen()
+{
+ if(!is_fullscreen) {
+ [_window toggleFullScreen:nil];
+ is_fullscreen = true;
+ }
+}
+
+void OsxWindow::StopFullScreen()
+{
+ if(is_fullscreen) {
+ [_window toggleFullScreen:nil];
+ is_fullscreen = false;
+ }
+}
+
+void OsxWindow::ToggleFullscreen()
+{
+ [_window toggleFullScreen:nil];
+ PangolinGl::is_fullscreen = !PangolinGl::is_fullscreen;
+}
+
+void OsxWindow::Move(int x, int y)
+{
+ [_window setFrame:CGRectMake(x, y, [_window frame].size.width,
+ [_window frame].size.height) display:NO];
+}
+
+void OsxWindow::Resize(unsigned int w, unsigned int h)
+{
+ [_window setFrame:CGRectMake([_window frame].origin.x,
+ [_window frame].origin.y, w, h) display:NO];
+}
+
+void OsxWindow::MakeCurrent()
+{
+ [[view openGLContext] makeCurrentContext];
+ context = this;
+}
+
+void OsxWindow::RemoveCurrent()
+{
+ [NSOpenGLContext clearCurrentContext];
+}
+
+void OsxWindow::SwapBuffers()
+{
+ [[view openGLContext] flushBuffer];
+// [[view openGLContext] update];
+// [view setNeedsDisplay:YES];
+}
+
+void OsxWindow::ProcessEvents()
+{
+ [PangolinNSApplication run_step];
+}
+
+PANGOLIN_REGISTER_FACTORY(OsxWindow)
+{
+ struct OsxWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ const int w = uri.Get<int>("w", 640);
+ const int h = uri.Get<int>("h", 480);
+ const bool is_highres = uri.Get<bool>(PARAM_HIGHRES, false);
+ return std::unique_ptr<WindowInterface>(CreateOsxWindowAndBind(window_title, w, h, is_highres));
+ }
+ };
+
+ auto factory = std::make_shared<OsxWindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "cocoa");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 100, "default");
+
+}
+
+
+
+}
diff --git a/externals/Pangolin/src/display/device/display_wayland.cpp b/externals/Pangolin/src/display/device/display_wayland.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..1b2912002e56aa73e8dc2e77ed5a04361890183f
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_wayland.cpp
@@ -0,0 +1,930 @@
+#include <pangolin/platform.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/gl/colour.h>
+#include <pangolin/gl/gldraw.h>
+
+#include <wayland-client.h>
+#include <wayland-egl.h>
+#include <EGL/egl.h>
+#include <xkbcommon/xkbcommon.h>
+#include <wayland-cursor.h>
+#include <linux/input.h>
+#include <sys/mman.h>
+
+#include <mutex>
+#include <string.h>
+#include <unistd.h>
+#include <cstdlib>
+
+#define WAYLAND_VERSION_GE(MAJ, MIN) WAYLAND_VERSION_MAJOR >= MAJ && WAYLAND_VERSION_MINOR >= MIN
+
+namespace pangolin {
+
+extern __thread PangolinGl* context;
+
+namespace wayland {
+
+static const std::map<enum wl_shell_surface_resize, std::string> resize_cursor = {
+ {WL_SHELL_SURFACE_RESIZE_NONE, "grabbing"},
+ {WL_SHELL_SURFACE_RESIZE_TOP, "top_side"},
+ {WL_SHELL_SURFACE_RESIZE_BOTTOM, "bottom_side"},
+ {WL_SHELL_SURFACE_RESIZE_LEFT, "left_side"},
+ {WL_SHELL_SURFACE_RESIZE_TOP_LEFT, "top_left_corner"},
+ {WL_SHELL_SURFACE_RESIZE_BOTTOM_LEFT, "bottom_left_corner"},
+ {WL_SHELL_SURFACE_RESIZE_RIGHT, "right_side"},
+ {WL_SHELL_SURFACE_RESIZE_TOP_RIGHT, "top_right_corner"},
+ {WL_SHELL_SURFACE_RESIZE_BOTTOM_RIGHT, "bottom_right_corner"}
+};
+
+struct ButtonSurface {
+ struct wl_surface *surface;
+ struct wl_subsurface *subsurface;
+ struct wl_egl_window *egl_window;
+ EGLSurface egl_surface;
+ EGLContext egl_context;
+ EGLDisplay egl_display;
+ const int32_t x, y;
+ const uint width, height;
+ pangolin::Colour colour;
+
+ enum type {
+ CLOSE = 100,
+ MAXIMISE
+ } function;
+
+ ButtonSurface(wl_compositor* compositor, wl_subcompositor* subcompositor,
+ wl_surface* source, EGLDisplay egl_display, EGLConfig config,
+ int32_t x, int32_t y, uint width, uint height,
+ type fnct, pangolin::Colour colour
+ ) :
+ egl_display(egl_display),
+ x(x), y(y),
+ width(width), height(height),
+ colour(colour),
+ function(fnct)
+ {
+ surface = wl_compositor_create_surface(compositor);
+ subsurface = wl_subcompositor_get_subsurface(subcompositor, surface, source);
+ wl_subsurface_set_desync(subsurface);
+ egl_context = eglCreateContext (egl_display, config, EGL_NO_CONTEXT, nullptr);
+ egl_window = wl_egl_window_create(surface, width, height);
+ egl_surface = eglCreateWindowSurface(egl_display, config, (EGLNativeWindowType)egl_window, nullptr);
+ }
+
+ ~ButtonSurface() {
+ if(egl_surface) eglDestroySurface(egl_display, egl_surface);
+ if(egl_window) wl_egl_window_destroy(egl_window);
+ if(egl_context) eglDestroyContext(egl_display, egl_context);
+
+ if(subsurface) wl_subsurface_destroy(subsurface);
+ if(surface) wl_surface_destroy(surface);
+ }
+
+ void reposition(const int main_w) const {
+ wl_subsurface_set_position(subsurface, main_w-x-width, y);
+ }
+
+ void draw() const {
+ eglMakeCurrent(egl_display, egl_surface, egl_surface, egl_context);
+ glClearColor(colour.r, colour.g, colour.b, colour.a);
+ glClear(GL_COLOR_BUFFER_BIT);
+ switch (function) {
+ case CLOSE:
+ glLineWidth(3);
+ glColor3f(1, 1, 1);
+ glBegin(GL_LINES);
+ glVertex2f(-1, -1);
+ glVertex2f(1, 1);
+ glVertex2f(1, -1);
+ glVertex2f(-1, 1);
+ glEnd();
+ break;
+ case MAXIMISE:
+ glLineWidth(2);
+ glColor3f(0, 0, 0);
+ glBegin(GL_LINE_LOOP);
+ glVertex2f(-0.7f, -0.7f);
+ glVertex2f(0.7f, -0.7f);
+ glVertex2f(0.7f, 0.7f);
+ glVertex2f(-0.7f, 0.7f);
+ glEnd();
+ glLineWidth(3);
+ glBegin(GL_LINES);
+ glVertex2f(+0.7f, +0.7f);
+ glVertex2f(-0.7f, +0.7f);
+ glEnd();
+ break;
+ }
+ eglSwapBuffers(egl_display, egl_surface);
+ }
+};
+
+struct DecorationSurface {
+ struct wl_surface *surface;
+ struct wl_subsurface *subsurface;
+ struct wl_egl_window *egl_window;
+ EGLSurface egl_surface;
+ EGLContext egl_context;
+ EGLDisplay egl_display;
+ pangolin::Colour colour;
+ uint border_size;
+ uint title_bar_size;
+
+ enum wl_shell_surface_resize function;
+
+ DecorationSurface(wl_compositor* compositor, wl_subcompositor* subcompositor,
+ wl_surface* source, EGLDisplay egl_display, EGLConfig config,
+ const uint _border_size, const uint _title_bar_size,
+ enum wl_shell_surface_resize type, pangolin::Colour _colour
+ ) :
+ egl_display(egl_display),
+ colour(_colour),
+ border_size(_border_size),
+ title_bar_size(_title_bar_size),
+ function(type)
+ {
+ surface = wl_compositor_create_surface(compositor);
+ subsurface = wl_subcompositor_get_subsurface(subcompositor, surface, source);
+ wl_subsurface_set_desync(subsurface);
+ egl_context = eglCreateContext(egl_display, config, EGL_NO_CONTEXT, nullptr);
+ egl_window = wl_egl_window_create(surface, 50, 50);
+ egl_surface = eglCreateWindowSurface(egl_display, config, (EGLNativeWindowType)egl_window, nullptr);
+ }
+
+ ~DecorationSurface() {
+ if(egl_surface) eglDestroySurface(egl_display, egl_surface);
+ if(egl_window) wl_egl_window_destroy(egl_window);
+ if(egl_context) eglDestroyContext(egl_display, egl_context);
+
+ if(subsurface) wl_subsurface_destroy(subsurface);
+ if(surface) wl_surface_destroy(surface);
+ }
+
+ void calc_dim(const int main_w, const int main_h, int &x, int &y, int &w, int &h) const {
+ // get position and dimension from type and main surface
+ switch (function) {
+ case WL_SHELL_SURFACE_RESIZE_NONE:
+ x=0; y=-title_bar_size;
+ w=main_w; h=title_bar_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_TOP:
+ x=0; y=-title_bar_size-border_size;
+ w=main_w; h=border_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM:
+ x=0; y=main_h;
+ w=main_w; h=border_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_LEFT:
+ x=-border_size; y=-title_bar_size;
+ w=border_size; h=main_h+title_bar_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_TOP_LEFT:
+ x=-border_size; y=-border_size-title_bar_size;
+ w=border_size; h=border_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM_LEFT:
+ x=-border_size; y=main_h;
+ w=border_size; h=border_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_RIGHT:
+ x=main_w; y=-title_bar_size;
+ w=border_size; h=main_h+title_bar_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_TOP_RIGHT:
+ x=main_w; y=-border_size-title_bar_size;
+ w=border_size; h=border_size;
+ break;
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM_RIGHT:
+ x=main_w; y=main_h;
+ w=border_size; h=border_size;
+ break;
+ }
+ }
+
+ void resize(const int main_w, const int main_h) const {
+ int x=0, y=0, w=0, h=0;
+ calc_dim(main_w, main_h, x, y, w, h);
+ wl_subsurface_set_position(subsurface, x, y);
+ wl_egl_window_resize(egl_window, w, h, 0, 0);
+ }
+
+ void draw() const {
+ eglMakeCurrent(egl_display, egl_surface, egl_surface, egl_context);
+ glClearColor(colour.r, colour.g, colour.b, colour.a);
+ glClear(GL_COLOR_BUFFER_BIT);
+ eglSwapBuffers(egl_display, egl_surface);
+ }
+};
+
+struct Decoration {
+ Decoration(const uint border_size,
+ const uint title_size,
+ const pangolin::Colour colour,
+ wl_compositor* compositor,
+ wl_subcompositor* subcompositor,
+ wl_surface* surface,
+ EGLDisplay egl_display,
+ EGLConfig config
+ ) :
+ border_size(border_size),
+ title_size(title_size),
+ colour(colour),
+ egl_display(egl_display),
+ compositor(compositor),
+ subcompositor(subcompositor),
+ surface(surface),
+ config(config)
+ { }
+
+ void create() {
+ // reserve memory to prevent that DecorationSurface's destructor gets
+ // called by 'emplace_back'
+ decorations.reserve(9);
+
+ // title bar, 2D movement
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_NONE, colour);
+
+ // sides, 1D resizing
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_LEFT, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_RIGHT, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_TOP, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_BOTTOM, colour);
+
+ // corners, 2D resizing
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_TOP_LEFT, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_TOP_RIGHT, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_BOTTOM_LEFT, colour);
+ decorations.emplace_back(compositor, subcompositor, surface, egl_display, config, border_size, title_size, WL_SHELL_SURFACE_RESIZE_BOTTOM_RIGHT, colour);
+
+ // buttons
+ buttons.reserve(2);
+ buttons.emplace_back(compositor, subcompositor, decorations[0].surface, egl_display, config, 5, 1, button_width, button_height, ButtonSurface::type::CLOSE, pangolin::Colour(0, 110/255.0, 182/255.0));
+ buttons.emplace_back(compositor, subcompositor, decorations[0].surface, egl_display, config, 10+button_width, 1, button_width, button_height, ButtonSurface::type::MAXIMISE, pangolin::Colour(1.0, 204/255.0f, 0));
+ }
+
+ void destroy() {
+ decorations.clear();
+ buttons.clear();
+ }
+
+ void resize(const int32_t width, const int32_t height) {
+ for(const DecorationSurface &d : decorations) { d.resize(width, height); }
+ for(const ButtonSurface &b : buttons) { b.reposition(width); }
+ }
+
+ void draw() {
+ for(const DecorationSurface &d : decorations) { d.draw(); }
+ for(const ButtonSurface &b : buttons) { b.draw(); }
+ }
+
+ void setTypeFromSurface(const wl_surface *surface) {
+ for(const DecorationSurface &d : decorations) {
+ if(d.surface==surface) {
+ last_type = d.function;
+ return;
+ }
+ }
+ for(const ButtonSurface &b : buttons) {
+ if(b.surface==surface) {
+ last_type = b.function;
+ return;
+ }
+ }
+ // surface is not part of the window decoration
+ last_type = -1;
+ }
+
+ const std::string getCursorForCurrentSurface() const {
+ return resize_cursor.count((enum wl_shell_surface_resize)last_type) ? resize_cursor.at((enum wl_shell_surface_resize)last_type) : "left_ptr";
+ }
+
+ std::vector<DecorationSurface> decorations;
+ int last_type;
+
+ std::vector<ButtonSurface> buttons;
+
+ const uint border_size;
+ const uint title_size;
+ const pangolin::Colour colour;
+ EGLDisplay egl_display;
+ wl_compositor* compositor;
+ wl_subcompositor* subcompositor;
+ wl_surface* surface;
+ EGLConfig config;
+
+ static const uint button_width;
+ static const uint button_height;
+};
+
+const uint Decoration::button_width = 25;
+const uint Decoration::button_height = 15;
+
+struct WaylandDisplay {
+ WaylandDisplay(const int width, const int height, const std::string title = "");
+
+ ~WaylandDisplay();
+
+ struct wl_display *wdisplay = nullptr;
+ struct wl_registry *wregistry = nullptr;
+ struct wl_compositor *wcompositor = nullptr;
+ struct wl_subcompositor *wsubcompositor = nullptr;
+ struct wl_surface *wsurface = nullptr;
+ struct wl_egl_window *egl_window = nullptr;
+ struct wl_shell *wshell = nullptr;
+ struct wl_shell_surface *wshell_surface = nullptr;
+
+ struct wl_seat *wseat = nullptr;
+ struct wl_keyboard *wkeyboard = nullptr;
+ struct wl_pointer *pointer = nullptr;
+
+ // for cursor
+ struct wl_shm *shm = nullptr;
+ struct wl_cursor_theme *cursor_theme = nullptr;
+ struct wl_surface *cursor_surface = nullptr;
+
+ // xkbcommon
+ struct xkb_context *xkb_context = nullptr;
+ struct xkb_keymap *keymap = nullptr;
+ struct xkb_state *xkb_state = nullptr;
+
+ std::unique_ptr<Decoration> decoration;
+
+ bool pressed = false;
+ int lastx=0;
+ int lasty=0;
+
+ std::vector<EGLConfig> egl_configs;
+ EGLSurface egl_surface = nullptr;
+ EGLContext egl_context = nullptr;
+ EGLDisplay egl_display = nullptr;
+
+ EGLint width, height;
+ bool is_fullscreen;
+ bool is_maximised;
+ static constexpr EGLint attribs[] = {
+ EGL_RENDERABLE_TYPE , EGL_OPENGL_BIT,
+ EGL_RED_SIZE , 8,
+ EGL_GREEN_SIZE , 8,
+ EGL_BLUE_SIZE , 8,
+ EGL_ALPHA_SIZE , 8,
+ EGL_DEPTH_SIZE , 24,
+ EGL_STENCIL_SIZE , 8,
+ EGL_NONE
+ };
+};
+
+constexpr EGLint WaylandDisplay::attribs[];
+
+struct WaylandWindow : public PangolinGl
+{
+ WaylandWindow(const int width, const int height, std::unique_ptr<WaylandDisplay> display);
+
+ ~WaylandWindow() override;
+
+ void ToggleFullscreen() override;
+
+ void Move(const int x, const int y) override;
+
+ void Resize(const unsigned int w, const unsigned int h) override;
+
+ void MakeCurrent() override;
+
+ void RemoveCurrent() override;
+
+ void SwapBuffers() override;
+
+ void ProcessEvents() override;
+
+ std::unique_ptr<WaylandDisplay> display;
+};
+
+// map wayland ids to pangolin ids
+static const std::map<uint,int> wl_button_ids = {
+ {BTN_LEFT, 0},
+ {BTN_MIDDLE, 1},
+ {BTN_RIGHT, 2},
+};
+
+static const std::map<uint,KeyModifier> wl_key_mod_ids = {
+ {KEY_LEFTSHIFT, KeyModifierShift},
+ {KEY_RIGHTSHIFT, KeyModifierShift},
+ {KEY_LEFTCTRL, KeyModifierCtrl},
+ {KEY_RIGHTCTRL, KeyModifierCtrl},
+ {KEY_LEFTALT, KeyModifierAlt},
+ {KEY_RIGHTALT, KeyModifierAlt},
+};
+
+static const std::map<uint,int> wl_key_special_ids = {
+ {KEY_F1, PANGO_KEY_F1},
+ {KEY_F2, PANGO_KEY_F2},
+ {KEY_F3, PANGO_KEY_F3},
+ {KEY_F4, PANGO_KEY_F4},
+ {KEY_F5, PANGO_KEY_F5},
+ {KEY_F6, PANGO_KEY_F6},
+ {KEY_F7, PANGO_KEY_F7},
+ {KEY_F8, PANGO_KEY_F8},
+ {KEY_F9, PANGO_KEY_F9},
+ {KEY_F10, PANGO_KEY_F10},
+ {KEY_F11, PANGO_KEY_F11},
+ {KEY_F12, PANGO_KEY_F12},
+
+ {KEY_LEFT, PANGO_KEY_LEFT},
+ {KEY_UP, PANGO_KEY_UP},
+ {KEY_RIGHT, PANGO_KEY_RIGHT},
+ {KEY_DOWN, PANGO_KEY_DOWN},
+
+ {KEY_PAGEUP, PANGO_KEY_PAGE_UP},
+ {KEY_PAGEDOWN, PANGO_KEY_PAGE_DOWN},
+ {KEY_HOME, PANGO_KEY_HOME},
+ {KEY_END, PANGO_KEY_END},
+ {KEY_INSERT, PANGO_KEY_INSERT},
+};
+
+static void handle_ping(void */*data*/, struct wl_shell_surface *shell_surface, uint32_t serial) {
+ wl_shell_surface_pong(shell_surface, serial);
+}
+
+static void handle_configure(void *data, struct wl_shell_surface */*shell_surface*/, uint32_t /*edges*/, int32_t width, int32_t height) {
+
+ const static uint min_width = 70;
+ const static uint min_height = 70;
+
+ if(width==0 && height==0)
+ return;
+
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ const bool changed = !(width==w->width && height==w->height);
+
+
+ const int main_w = (w->is_fullscreen || !changed) ? width : std::max(width-int(2*w->decoration->border_size), int(min_width));
+ const int main_h = (w->is_fullscreen || !changed) ? height : std::max(height-2*int(w->decoration->border_size)-int(w->decoration->title_size), int(min_height));
+
+ // resize main surface
+ wl_egl_window_resize(w->egl_window, main_w, main_h, 0, 0);
+
+ // set opaque region
+ struct wl_region* wregion = wl_compositor_create_region(w->wcompositor);
+ wl_region_add(wregion, 0, 0, main_w, main_h);
+ wl_surface_set_opaque_region(w->wsurface, wregion);
+ wl_region_destroy(wregion);
+
+ // resize all decoration elements
+ w->decoration->resize(main_w, main_h);
+
+ // notify Panglin views about resized area
+ pangolin::process::Resize(main_w, main_h);
+}
+
+static void handle_popup_done(void */*data*/, struct wl_shell_surface */*shell_surface*/) { }
+
+static const struct wl_shell_surface_listener shell_surface_listener = {
+ handle_ping,
+ handle_configure,
+ handle_popup_done
+};
+
+static void pointer_handle_enter(void *data, struct wl_pointer *pointer, uint32_t serial, struct wl_surface *surface, wl_fixed_t /*sx*/, wl_fixed_t /*sy*/) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+ w->decoration->setTypeFromSurface(surface);
+
+ const std::string cursor = w->decoration->getCursorForCurrentSurface();
+
+ const auto image = wl_cursor_theme_get_cursor(w->cursor_theme, cursor.c_str())->images[0];
+ wl_pointer_set_cursor(pointer, serial, w->cursor_surface, image->hotspot_x, image->hotspot_y);
+ wl_surface_attach(w->cursor_surface, wl_cursor_image_get_buffer(image), 0, 0);
+ wl_surface_damage(w->cursor_surface, 0, 0, image->width, image->height);
+ wl_surface_commit(w->cursor_surface);
+}
+
+static void pointer_handle_leave(void *data, struct wl_pointer */*pointer*/, uint32_t /*serial*/, struct wl_surface */*surface*/) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+ w->pressed = false;
+}
+
+static void pointer_handle_motion(void *data, struct wl_pointer */*pointer*/, uint32_t /*time*/, wl_fixed_t sx, wl_fixed_t sy) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ w->lastx=wl_fixed_to_int(sx);
+ w->lasty=wl_fixed_to_int(sy);
+ if(w->pressed) {
+ pangolin::process::MouseMotion(w->lastx, w->lasty);
+ }
+ else {
+ pangolin::process::PassiveMouseMotion(w->lastx, w->lasty);
+ }
+}
+
+static void pointer_handle_button(void *data, struct wl_pointer */*wl_pointer*/, uint32_t serial, uint32_t /*time*/, uint32_t button, uint32_t state) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ if(w->decoration->last_type<0) {
+ // input goes to pangoling view
+ if(!wl_button_ids.count(button))
+ return;
+
+ w->pressed = (state==WL_POINTER_BUTTON_STATE_PRESSED);
+ pangolin::process::Mouse(wl_button_ids.at(button), (state==WL_POINTER_BUTTON_STATE_RELEASED), w->lastx, w->lasty);
+ }
+ else {
+ // input goes to window decoration
+ // resizing using window decoration
+ if((button==BTN_LEFT) && (state==WL_POINTER_BUTTON_STATE_PRESSED)) {
+ switch (w->decoration->last_type) {
+ case WL_SHELL_SURFACE_RESIZE_NONE:
+ wl_shell_surface_move(w->wshell_surface, w->wseat, serial);
+ break;
+ case WL_SHELL_SURFACE_RESIZE_TOP:
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM:
+ case WL_SHELL_SURFACE_RESIZE_LEFT:
+ case WL_SHELL_SURFACE_RESIZE_TOP_LEFT:
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM_LEFT:
+ case WL_SHELL_SURFACE_RESIZE_RIGHT:
+ case WL_SHELL_SURFACE_RESIZE_TOP_RIGHT:
+ case WL_SHELL_SURFACE_RESIZE_BOTTOM_RIGHT:
+ wl_shell_surface_resize(w->wshell_surface, w->wseat, serial, w->decoration->last_type);
+ break;
+ case ButtonSurface::type::CLOSE:
+ pangolin::QuitAll();
+ break;
+ case ButtonSurface::type::MAXIMISE:
+ w->is_maximised = !w->is_maximised;
+ if(w->is_maximised) {
+ // store original window size
+ wl_egl_window_get_attached_size(w->egl_window, &w->width, &w->height);
+ wl_shell_surface_set_maximized(w->wshell_surface, nullptr);
+ }
+ else {
+ wl_shell_surface_set_toplevel(w->wshell_surface);
+ handle_configure(data, nullptr, 0,
+ w->width+2*w->decoration->border_size,
+ w->height+2*w->decoration->border_size+w->decoration->title_size);
+ }
+
+ break;
+ }
+ }
+ }
+}
+
+static void pointer_handle_axis(void *data, struct wl_pointer */*wl_pointer*/, uint32_t /*time*/, uint32_t axis, wl_fixed_t value) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ int button_id = -1;
+ switch (axis) {
+ case REL_X: button_id = (value<0) ? 3 : 4; break; // up, down
+ case REL_Y: button_id = (value<0) ? 5 : 6; break; // left, right
+ }
+
+ if(button_id>0) {
+ pangolin::process::Mouse(button_id, 0, w->lastx, w->lasty);
+ }
+}
+
+#if WAYLAND_VERSION_GE(1,12)
+
+static void pointer_handle_frame(void */*data*/, struct wl_pointer */*wl_pointer*/) { }
+
+static void pointer_handle_axis_source(void */*data*/, struct wl_pointer */*wl_pointer*/, uint32_t /*axis_source*/) { }
+
+static void pointer_handle_axis_stop(void */*data*/, struct wl_pointer */*wl_pointer*/, uint32_t /*time*/, uint32_t /*axis*/) { }
+
+static void pointer_handle_axis_discrete(void */*data*/, struct wl_pointer */*wl_pointer*/, uint32_t /*axis*/, int32_t /*discrete*/) { }
+
+#endif
+
+static const struct wl_pointer_listener pointer_listener = {
+ pointer_handle_enter,
+ pointer_handle_leave,
+ pointer_handle_motion,
+ pointer_handle_button,
+ pointer_handle_axis,
+#if WAYLAND_VERSION_GE(1,12)
+ pointer_handle_frame,
+ pointer_handle_axis_source,
+ pointer_handle_axis_stop,
+ pointer_handle_axis_discrete,
+#endif
+};
+
+static void keyboard_handle_keymap(void *data, struct wl_keyboard */*keyboard*/, uint32_t /*format*/, int fd, uint32_t size) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ char *keymap_string = static_cast<char*>(mmap(nullptr, size, PROT_READ, MAP_SHARED, fd, 0));
+ xkb_keymap_unref(w->keymap);
+ w->keymap = xkb_keymap_new_from_string(w->xkb_context, keymap_string, XKB_KEYMAP_FORMAT_TEXT_V1, XKB_KEYMAP_COMPILE_NO_FLAGS);
+ munmap(keymap_string, size);
+ close(fd);
+ xkb_state_unref(w->xkb_state);
+ w->xkb_state = xkb_state_new(w->keymap);
+}
+
+static void keyboard_handle_enter(void */*data*/, struct wl_keyboard */*keyboard*/, uint32_t /*serial*/, struct wl_surface */*surface*/, struct wl_array */*keys*/) { }
+
+static void keyboard_handle_leave(void */*data*/, struct wl_keyboard */*keyboard*/, uint32_t /*serial*/, struct wl_surface */*surface*/) { }
+
+static void keyboard_handle_key(void *data, struct wl_keyboard */*keyboard*/, uint32_t /*serial*/, uint32_t /*time*/, uint32_t key, uint32_t state) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ // modifier keys
+ if(wl_key_mod_ids.count(key)) {
+ if(state==WL_KEYBOARD_KEY_STATE_PRESSED) {
+ pangolin::context->mouse_state |= wl_key_mod_ids.at(key);
+ }
+ else if (state==WL_KEYBOARD_KEY_STATE_RELEASED) {
+ pangolin::context->mouse_state &= ~wl_key_mod_ids.at(key);
+ }
+ return;
+ }
+
+ // character and special keys
+ int pango_key = -1;
+ if(wl_key_special_ids.count(key)) {
+ // special keys
+ pango_key = PANGO_SPECIAL + wl_key_special_ids.at(key);
+ }
+ else {
+ // character keys
+ const uint32_t utf32 = xkb_state_key_get_utf32(w->xkb_state, key+8);
+ // filter non-ASCII
+ if(utf32>0 && utf32<=127) {
+ pango_key = int(utf32);
+ }
+ }
+
+ if(pango_key>0) {
+ if(state==WL_KEYBOARD_KEY_STATE_PRESSED) {
+ pangolin::process::Keyboard(uint8_t(pango_key), w->lastx, w->lasty);
+ }else if(state==WL_KEYBOARD_KEY_STATE_RELEASED){
+ pangolin::process::KeyboardUp(uint8_t(pango_key), w->lastx, w->lasty);
+ }
+ }
+}
+
+static void keyboard_handle_modifiers(void *data, struct wl_keyboard */*keyboard*/, uint32_t /*serial*/, uint32_t mods_depressed, uint32_t mods_latched, uint32_t mods_locked, uint32_t group) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ xkb_state_update_mask(w->xkb_state, mods_depressed, mods_latched, mods_locked, 0, 0, group);
+}
+
+#if WAYLAND_VERSION_GE(1,12)
+
+static void keyboard_handle_repeat_info(void */*data*/, struct wl_keyboard */*wl_keyboard*/, int32_t /*rate*/, int32_t /*delay*/) { }
+
+#endif
+
+static const struct wl_keyboard_listener keyboard_listener = {
+ keyboard_handle_keymap,
+ keyboard_handle_enter,
+ keyboard_handle_leave,
+ keyboard_handle_key,
+ keyboard_handle_modifiers,
+#if WAYLAND_VERSION_GE(1,12)
+ keyboard_handle_repeat_info,
+#endif
+};
+
+static void seat_handle_capabilities(void *data, struct wl_seat *seat, uint32_t caps1) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ enum wl_seat_capability caps;
+ caps = (enum wl_seat_capability)caps1;
+ if (caps & WL_SEAT_CAPABILITY_KEYBOARD) {
+ w->wkeyboard = wl_seat_get_keyboard(seat);
+ wl_keyboard_add_listener(w->wkeyboard, &keyboard_listener, data);
+ } else {
+ wl_keyboard_destroy(w->wkeyboard);
+ w->wkeyboard = nullptr;
+ }
+ if (caps & WL_SEAT_CAPABILITY_POINTER) {
+ w->pointer = wl_seat_get_pointer(seat);
+ w->cursor_surface = wl_compositor_create_surface(w->wcompositor);
+ wl_pointer_add_listener(w->pointer, &pointer_listener, data);
+ } else {
+ wl_pointer_destroy(w->pointer);
+ w->pointer = nullptr;
+ }
+}
+
+static void seat_handle_name(void */*data*/, struct wl_seat */*wl_seat*/, const char */*name*/) { }
+
+static const struct wl_seat_listener seat_listener = {
+ seat_handle_capabilities,
+ seat_handle_name,
+};
+
+static void global_registry_handler(void *data, struct wl_registry *registry, uint32_t id, const char *interface, uint32_t version) {
+ WaylandDisplay* const w = static_cast<WaylandDisplay*>(data);
+
+ if (strcmp(interface, wl_compositor_interface.name) == 0) {
+ w->wcompositor = reinterpret_cast<wl_compositor*> (wl_registry_bind(registry, id, &wl_compositor_interface, version));
+ } else if (strcmp(interface, wl_subcompositor_interface.name) == 0) {
+ w->wsubcompositor = static_cast<wl_subcompositor*>(wl_registry_bind(registry, id, &wl_subcompositor_interface, version));
+ } else if (strcmp(interface, wl_shell_interface.name) == 0) {
+ w->wshell = reinterpret_cast<wl_shell*> (wl_registry_bind(registry, id, &wl_shell_interface, version));
+ } else if (strcmp(interface, wl_seat_interface.name) == 0) {
+ w->wseat = reinterpret_cast<wl_seat*>(wl_registry_bind(registry, id, &wl_seat_interface, version));
+ wl_seat_add_listener(w->wseat, &seat_listener, data);
+ } else if (strcmp(interface, wl_shm_interface.name) == 0) {
+ w->shm = static_cast<wl_shm*>(wl_registry_bind(registry, id, &wl_shm_interface, version));
+ w->cursor_theme = wl_cursor_theme_load(nullptr, 16, w->shm);
+ }
+}
+
+static void global_registry_remover(void */*data*/, struct wl_registry */*registry*/, uint32_t /*id*/) { }
+
+static const struct wl_registry_listener wregistry_listener = {
+ global_registry_handler,
+ global_registry_remover
+};
+
+WaylandDisplay::WaylandDisplay(const int width, const int height, const std::string title) : width(width), height(height) {
+ xkb_context = xkb_context_new(XKB_CONTEXT_NO_FLAGS);
+
+ wdisplay = wl_display_connect(nullptr);
+ if (wdisplay == nullptr) { return; }
+
+ wregistry = wl_display_get_registry(wdisplay);
+ wl_registry_add_listener(wregistry, &wregistry_listener, this);
+
+ wl_display_roundtrip(wdisplay);
+
+ egl_display = eglGetDisplay((EGLNativeDisplayType)wdisplay);
+ if(!egl_display) {
+ std::cerr << "Failed to open Wayland display" << std::endl;
+ }
+
+ EGLint major, minor;
+ if(eglInitialize(egl_display, &major, &minor)==EGL_FALSE) {
+ std::cerr << "EGL init failed" << std::endl;
+ }
+
+ if(eglBindAPI(EGL_OPENGL_API)==EGL_FALSE) {
+ std::cerr << "EGL bind failed" << std::endl;
+ }
+
+ EGLint count;
+ eglGetConfigs(egl_display, nullptr, 0, &count);
+
+ egl_configs.resize(count);
+
+ EGLint numConfigs;
+ eglChooseConfig(egl_display, attribs, egl_configs.data(), count, &numConfigs);
+
+ egl_context = eglCreateContext(egl_display, egl_configs[0], EGL_NO_CONTEXT, nullptr);
+
+ wsurface = wl_compositor_create_surface(wcompositor);
+
+ // set opaque background
+ struct wl_region* wregion = wl_compositor_create_region(wcompositor);
+ wl_region_add(wregion, 0, 0, width, height);
+ wl_surface_set_opaque_region(wsurface, wregion);
+ wl_region_destroy(wregion);
+
+ egl_window = wl_egl_window_create(wsurface, width, height);
+ if(!egl_window) {
+ std::cerr << "Cannot create EGL window" << std::endl;
+ }
+
+ egl_surface = eglCreateWindowSurface(egl_display, egl_configs[0], (EGLNativeWindowType)egl_window, nullptr);
+ if (egl_surface == EGL_NO_SURFACE) {
+ std::cerr << "Cannot create EGL surface" << std::endl;
+ }
+
+ wshell_surface = wl_shell_get_shell_surface(wshell, wsurface);
+ wl_shell_surface_add_listener(wshell_surface, &shell_surface_listener, this);
+
+ wl_shell_surface_set_toplevel(wshell_surface);
+ wl_shell_surface_set_title(wshell_surface, title.c_str());
+ wl_shell_surface_set_class(wshell_surface, title.c_str());
+
+ wl_display_sync(wdisplay);
+
+ // construct window decoration
+ const pangolin::Colour grey(0.5f, 0.5f, 0.5f, 0.5f);
+ decoration = std::unique_ptr<Decoration>(new Decoration(5, 20, grey, wcompositor, wsubcompositor, wsurface, egl_display, egl_configs[0]));
+ decoration->create();
+ decoration->resize(width, height);
+}
+
+WaylandDisplay::~WaylandDisplay() {
+ if(decoration) decoration->destroy();
+
+ // cleanup EGL
+ if(egl_context) eglDestroyContext(egl_display, egl_context);
+ if(egl_surface) eglDestroySurface(egl_display, egl_surface);
+ if(egl_window) wl_egl_window_destroy(egl_window);
+ if(egl_display) eglTerminate(egl_display);
+
+ // cleanup Wayland
+ if(wshell_surface) wl_shell_surface_destroy(wshell_surface);
+ if(wsurface) wl_surface_destroy(wsurface);
+ if(wregistry) wl_registry_destroy(wregistry);
+ if(wdisplay) wl_display_disconnect(wdisplay);
+
+ if(xkb_context) xkb_context_unref(xkb_context);
+}
+
+WaylandWindow::WaylandWindow(const int w, const int h, std::unique_ptr<WaylandDisplay> display) : display(std::move(display)){
+ windowed_size[0] = w;
+ windowed_size[1] = h;
+}
+
+WaylandWindow::~WaylandWindow() { }
+
+void WaylandWindow::MakeCurrent() {
+ eglMakeCurrent(display->egl_display, display->egl_surface, display->egl_surface, display->egl_context);
+ context = this;
+}
+
+void WaylandWindow::RemoveCurrent() {
+ eglMakeCurrent(display->egl_display, EGL_NO_SURFACE, EGL_NO_SURFACE, EGL_NO_CONTEXT);
+}
+
+void WaylandWindow::ToggleFullscreen() {
+ is_fullscreen = !is_fullscreen; // state for Pangolin
+ display->is_fullscreen = is_fullscreen; // state for Wayland
+ if(is_fullscreen) {
+ display->decoration->destroy();
+ wl_shell_surface_set_fullscreen(display->wshell_surface, WL_SHELL_SURFACE_FULLSCREEN_METHOD_DEFAULT, 0, nullptr);
+ }
+ else {
+ display->decoration->create();
+ wl_shell_surface_set_toplevel(display->wshell_surface);
+ if(display->is_maximised) {
+ wl_shell_surface_set_maximized(display->wshell_surface, nullptr);
+ }
+ else {
+ handle_configure(static_cast<void*>(display.get()), nullptr, 0,
+ windowed_size[0]+2*display->decoration->border_size,
+ windowed_size[1]+2*display->decoration->border_size+display->decoration->title_size);
+ }
+ }
+
+ wl_display_sync(display->wdisplay);
+}
+
+void WaylandWindow::Move(const int /*x*/, const int /*y*/) { }
+
+void WaylandWindow::Resize(const unsigned int /*w*/, const unsigned int /*h*/) { }
+
+void WaylandWindow::ProcessEvents() { }
+
+void WaylandWindow::SwapBuffers() {
+ eglSwapBuffers(display->egl_display, display->egl_surface);
+
+ // draw all decoration elements
+ display->decoration->draw();
+
+ MakeCurrent();
+
+ wl_display_roundtrip(display->wdisplay);
+}
+
+
+std::unique_ptr<WindowInterface> CreateWaylandWindowAndBind(const std::string window_title, const int w, const int h, const std::string /*display_name*/, const bool /*double_buffered*/, const int /*sample_buffers*/, const int /*samples*/) {
+
+ std::unique_ptr<WaylandDisplay> newdisplay = std::unique_ptr<WaylandDisplay>(new WaylandDisplay(w, h, window_title));
+ // return null pointer for fallback to X11
+ if(!newdisplay->wdisplay) { return nullptr; }
+
+ // glewInit() fails with SIGSEGV for glew < 2.0 since it links to GLX
+ if(atoi((char*)glewGetString(GLEW_VERSION_MAJOR))<2)
+ return nullptr;
+
+ WaylandWindow* win = new WaylandWindow(w, h, std::move(newdisplay));
+
+ return std::unique_ptr<WindowInterface>(win);
+}
+
+} // namespace wayland
+
+PANGOLIN_REGISTER_FACTORY(WaylandWindow)
+{
+ struct WaylandWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ const int w = uri.Get<int>("w", 640);
+ const int h = uri.Get<int>("h", 480);
+ const std::string display_name = uri.Get<std::string>("display_name", "");
+ const bool double_buffered = uri.Get<bool>("double_buffered", true);
+ const int sample_buffers = uri.Get<int>("sample_buffers", 1);
+ const int samples = uri.Get<int>("samples", 1);
+ return std::unique_ptr<WindowInterface>(wayland::CreateWaylandWindowAndBind(window_title, w, h, display_name, double_buffered, sample_buffers, samples));
+ }
+
+ virtual ~WaylandWindowFactory() { }
+ };
+
+ auto factory = std::make_shared<WaylandWindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "wayland");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 9, "linux");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 90, "default");
+}
+
+} // namespace pangolin
+
diff --git a/externals/Pangolin/src/display/device/display_win.cpp b/externals/Pangolin/src/display/device/display_win.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ee125fd60ae1bc92accabe20519647f8b6a7aa93
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_win.cpp
@@ -0,0 +1,595 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2018 Steven Lovegrove, Andrey Mnatsakanov
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+
+#include <pangolin/display/device/WinWindow.h>
+#include <memory>
+
+#define CheckWGLDieOnError() pangolin::_CheckWLDieOnError( __FILE__, __LINE__ );
+namespace pangolin {
+inline void _CheckWLDieOnError( const char *sFile, const int nLine )
+{
+ DWORD errorCode = GetLastError();
+ if(errorCode!=0) {
+ LPVOID lpMsgBuf;
+ FormatMessage(FORMAT_MESSAGE_ALLOCATE_BUFFER | FORMAT_MESSAGE_FROM_SYSTEM | FORMAT_MESSAGE_IGNORE_INSERTS,
+ NULL, errorCode, MAKELANGID(LANG_NEUTRAL, SUBLANG_DEFAULT),(LPTSTR) &lpMsgBuf, 0, NULL);
+ // MessageBox( NULL, (LPCTSTR)lpMsgBuf, ("Error "+std::to_string(errorCode)).c_str(), MB_OK | MB_ICONINFORMATION );
+ pango_print_error("Error %i: %s", errorCode, (char *)lpMsgBuf);
+ pango_print_error("In: %s, line %d\n", sFile, nLine);
+ // exit(EXIT_FAILURE);
+ }
+}
+}
+
+namespace pangolin
+{
+
+const char *className = "Pangolin";
+
+extern __thread PangolinGl* context;
+
+////////////////////////////////////////////////////////////////////////
+// Utils
+////////////////////////////////////////////////////////////////////////
+
+unsigned char GetPangoKey(WPARAM wParam, LPARAM lParam)
+{
+ switch (wParam)
+ {
+ case VK_F1: return PANGO_SPECIAL + PANGO_KEY_F1;
+ case VK_F2: return PANGO_SPECIAL + PANGO_KEY_F2;
+ case VK_F3: return PANGO_SPECIAL + PANGO_KEY_F3;
+ case VK_F4: return PANGO_SPECIAL + PANGO_KEY_F4;
+ case VK_F5: return PANGO_SPECIAL + PANGO_KEY_F5;
+ case VK_F6: return PANGO_SPECIAL + PANGO_KEY_F6;
+ case VK_F7: return PANGO_SPECIAL + PANGO_KEY_F7;
+ case VK_F8: return PANGO_SPECIAL + PANGO_KEY_F8;
+ case VK_F9: return PANGO_SPECIAL + PANGO_KEY_F9;
+ case VK_F10: return PANGO_SPECIAL + PANGO_KEY_F10;
+ case VK_F11: return PANGO_SPECIAL + PANGO_KEY_F11;
+ case VK_F12: return PANGO_SPECIAL + PANGO_KEY_F12;
+ case VK_LEFT: return PANGO_SPECIAL + PANGO_KEY_LEFT;
+ case VK_UP: return PANGO_SPECIAL + PANGO_KEY_UP;
+ case VK_RIGHT: return PANGO_SPECIAL + PANGO_KEY_RIGHT;
+ case VK_DOWN: return PANGO_SPECIAL + PANGO_KEY_DOWN;
+ case VK_HOME: return PANGO_SPECIAL + PANGO_KEY_HOME;
+ case VK_END: return PANGO_SPECIAL + PANGO_KEY_END;
+ case VK_INSERT: return PANGO_SPECIAL + PANGO_KEY_INSERT;
+ case VK_DELETE: return 127;
+ default:
+ const int lBufferSize = 2;
+ WCHAR lBuffer[lBufferSize];
+
+ BYTE State[256];
+ GetKeyboardState(State);
+
+ const UINT scanCode = (lParam >> 8) & 0xFFFFFF00;
+ if( ToUnicode((UINT)wParam, scanCode, State, lBuffer, lBufferSize, 0) >=1 ) {
+ return (unsigned char)lBuffer[0];
+ }
+ }
+ return 0;
+}
+
+int GetMouseModifierKey(WPARAM wParam)
+{
+ //maps windows key modifier to glutGetModifiers values
+ int gluKeyModVal = 0;
+ if (wParam & MK_SHIFT) gluKeyModVal += 1;
+ if (wParam & MK_CONTROL) gluKeyModVal += 2;
+ if (HIBYTE(GetKeyState(VK_MENU))) gluKeyModVal += 4;
+ return gluKeyModVal << 4;
+}
+
+////////////////////////////////////////////////////////////////////////
+// WinWindow Implementation
+////////////////////////////////////////////////////////////////////////
+
+void WinWindow::SetupPixelFormat(HDC hDC)
+{
+ PIXELFORMATDESCRIPTOR pfd = {
+ sizeof(PIXELFORMATDESCRIPTOR), /* size */
+ 1, /* version */
+ PFD_SUPPORT_OPENGL |
+ PFD_DRAW_TO_WINDOW |
+ PFD_DOUBLEBUFFER, /* support double-buffering */
+ PFD_TYPE_RGBA, /* color type */
+ 24, /* prefered color depth */
+ 0, 0, 0, 0, 0, 0, /* color bits (ignored) */
+ 8, /* alpha bits */
+ 0, /* alpha shift (ignored) */
+ 0, /* no accumulation buffer */
+ 0, 0, 0, 0, /* accum bits (ignored) */
+ 32, /* depth buffer */
+ 0, /* no stencil buffer */
+ 0, /* no auxiliary buffers */
+ PFD_MAIN_PLANE, /* main layer */
+ 0, /* reserved */
+ 0, 0, 0, /* no layer, visible, damage masks */
+ };
+ int pixelFormat;
+
+ pixelFormat = ChoosePixelFormat(hDC, &pfd);
+ if (pixelFormat == 0) {
+ MessageBox(WindowFromDC(hDC), "ChoosePixelFormat failed.", "Error", MB_ICONERROR | MB_OK);
+ CheckWGLDieOnError();
+ exit(1);
+ }
+
+ if (SetPixelFormat(hDC, pixelFormat, &pfd) != TRUE) {
+ MessageBox(WindowFromDC(hDC), "SetPixelFormat failed.", "Error", MB_ICONERROR | MB_OK);
+ CheckWGLDieOnError();
+ exit(1);
+ }
+}
+
+void WinWindow::SetupPalette(HDC hDC)
+{
+ int pixelFormat = GetPixelFormat(hDC);
+ if(!pixelFormat) {
+ std::cerr << "GetPixelFormat() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ PIXELFORMATDESCRIPTOR pfd;
+ LOGPALETTE* pPal;
+ int paletteSize;
+
+ if(!DescribePixelFormat(hDC, pixelFormat, sizeof(PIXELFORMATDESCRIPTOR), &pfd)) {
+ std::cerr << "DescribePixelFormat() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ if (pfd.dwFlags & PFD_NEED_PALETTE) {
+ paletteSize = 1 << pfd.cColorBits;
+ }
+ else {
+ return;
+ }
+
+ pPal = (LOGPALETTE*) malloc(sizeof(LOGPALETTE) + paletteSize * sizeof(PALETTEENTRY));
+ pPal->palVersion = 0x300;
+ pPal->palNumEntries = paletteSize;
+
+ /* build a simple RGB color palette */
+ {
+ int redMask = (1 << pfd.cRedBits) - 1;
+ int greenMask = (1 << pfd.cGreenBits) - 1;
+ int blueMask = (1 << pfd.cBlueBits) - 1;
+ int i;
+
+ for (i = 0; i<paletteSize; ++i) {
+ pPal->palPalEntry[i].peRed =
+ (((i >> pfd.cRedShift) & redMask) * 255) / redMask;
+ pPal->palPalEntry[i].peGreen =
+ (((i >> pfd.cGreenShift) & greenMask) * 255) / greenMask;
+ pPal->palPalEntry[i].peBlue =
+ (((i >> pfd.cBlueShift) & blueMask) * 255) / blueMask;
+ pPal->palPalEntry[i].peFlags = 0;
+ }
+ }
+
+ hPalette = CreatePalette(pPal);
+ free(pPal);
+
+ if (hPalette) {
+ SelectPalette(hDC, hPalette, FALSE);
+ RealizePalette(hDC);
+ }
+ else {
+ std::cerr << "CreatePalette() failed" << std::endl;
+ }
+}
+
+WinWindow::WinWindow(
+ const std::string& window_title, int width, int height
+) : hWnd(0)
+{
+ const HMODULE hCurrentInst = GetModuleHandle(0);
+ if(hCurrentInst==NULL) {
+ std::cerr << "GetModuleHandle() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ RegisterThisClass(hCurrentInst);
+
+ PangolinGl::windowed_size[0] = width;
+ PangolinGl::windowed_size[1] = height;
+
+ HWND thishwnd = CreateWindow(
+ className, window_title.c_str(),
+ WS_OVERLAPPEDWINDOW | WS_CLIPCHILDREN | WS_CLIPSIBLINGS,
+ 0, 0, width, height,
+ NULL, NULL, hCurrentInst, this);
+ if(thishwnd==NULL) {
+ std::cerr << "CreateWindow() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ if( thishwnd != hWnd ) {
+ throw std::runtime_error("Pangolin Window Creation Failed.");
+ }
+
+ // Display Window
+ ShowWindow(hWnd, SW_SHOW);
+ PangolinGl::is_double_buffered = true;
+}
+
+WinWindow::~WinWindow()
+{
+ if(!DestroyWindow(hWnd)) {
+ std::cerr << "DestroyWindow() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::RegisterThisClass(HMODULE hCurrentInst)
+{
+ WNDCLASS wndClass;
+ wndClass.style = CS_OWNDC | CS_HREDRAW | CS_VREDRAW;
+ wndClass.lpfnWndProc = WinWindow::WndProc;
+ wndClass.cbClsExtra = 0;
+ wndClass.cbWndExtra = 0;
+ wndClass.hInstance = hCurrentInst;
+ wndClass.hIcon = LoadIcon(NULL, IDI_APPLICATION);
+ wndClass.hCursor = LoadCursor(NULL, IDC_ARROW);
+ wndClass.hbrBackground = (HBRUSH)GetStockObject(BLACK_BRUSH);
+ wndClass.lpszMenuName = NULL;
+ wndClass.lpszClassName = className;
+ if(!RegisterClass(&wndClass)) {
+ std::cerr << "RegisterClass() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+LRESULT APIENTRY
+WinWindow::WndProc(HWND hwnd, UINT uMsg, WPARAM wParam, LPARAM lParam)
+{
+ WinWindow* self = 0;
+
+ if (uMsg == WM_NCCREATE) {
+ LPCREATESTRUCT lpcs = reinterpret_cast<LPCREATESTRUCT>(lParam);
+ self = reinterpret_cast<WinWindow*>(lpcs->lpCreateParams);
+ if(self) {
+ self->hWnd = hwnd;
+ SetWindowLongPtr(hwnd, GWLP_USERDATA, reinterpret_cast<LPARAM>(self));
+ }
+ } else {
+ self = reinterpret_cast<WinWindow*> (GetWindowLongPtr(hwnd, GWLP_USERDATA));
+ }
+
+ if (self) {
+ return self->HandleWinMessages(uMsg, wParam, lParam);
+ } else {
+ return DefWindowProc(hwnd, uMsg, wParam, lParam);
+ }
+}
+
+LRESULT WinWindow::HandleWinMessages(UINT message, WPARAM wParam, LPARAM lParam)
+{
+ switch (message) {
+ case WM_CREATE:
+ /* initialize OpenGL rendering */
+ hDC = GetDC(hWnd);
+ if(hDC==NULL) {
+ std::cerr << "WM_CREATE GetDC() failed" << std::endl;
+ }
+ SetupPixelFormat(hDC);
+ SetupPalette(hDC);
+ hGLRC = wglCreateContext(hDC);
+ if(!hGLRC) {
+ std::cerr << "WM_CREATE wglCreateContext() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ if(!wglMakeCurrent(hDC, hGLRC)) {
+ std::cerr << "WM_CREATE wglMakeCurrent() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ return 0;
+ case WM_DESTROY:
+ /* finish OpenGL rendering */
+ if (hGLRC) {
+ if(!wglMakeCurrent(NULL, NULL)) {
+ std::cerr << "WM_DESTROY wglMakeCurrent() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ if(!wglDeleteContext(hGLRC)) {
+ std::cerr << "WM_DESTROY wglDeleteContext() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ }
+ if (hPalette) {
+ DeleteObject(hPalette);
+ }
+ ReleaseDC(hWnd, hDC);
+ PostQuitMessage(0);
+ return 0;
+ case WM_SIZE:
+ /* track window size changes */
+ if (context == this) {
+ process::Resize((int)LOWORD(lParam), (int)HIWORD(lParam));
+ }
+ return 0;
+ case WM_PALETTECHANGED:
+ /* realize palette if this is *not* the current window */
+ if (hGLRC && hPalette && (HWND)wParam != hWnd) {
+ if(!UnrealizeObject(hPalette)) {
+ std::cerr << "WM_PALETTECHANGED UnrealizeObject() failed" << std::endl;
+ }
+ if(!SelectPalette(hDC, hPalette, FALSE)) {
+ std::cerr << "WM_PALETTECHANGED SelectPalette() failed" << std::endl;
+ }
+ if(RealizePalette(hDC)==GDI_ERROR) {
+ std::cerr << "WM_PALETTECHANGED RealizePalette() failed" << std::endl;
+ }
+ //redraw();
+ break;
+ }
+ break;
+ case WM_QUERYNEWPALETTE:
+ /* realize palette if this is the current window */
+ if (hGLRC && hPalette) {
+ if(!UnrealizeObject(hPalette)) {
+ std::cerr << "WM_QUERYNEWPALETTE UnrealizeObject() failed" << std::endl;
+ }
+ if(!SelectPalette(hDC, hPalette, FALSE)) {
+ std::cerr << "WM_QUERYNEWPALETTE SelectPalette() failed" << std::endl;
+ }
+ if(RealizePalette(hDC)==GDI_ERROR) {
+ std::cerr << "WM_QUERYNEWPALETTE RealizePalette() failed" << std::endl;
+ }
+ //redraw();
+ return TRUE;
+ }
+ break;
+ case WM_PAINT:
+ {
+ //PAINTSTRUCT ps;
+ //BeginPaint(hWnd, &ps);
+ //if (hGLRC) {
+ // redraw();
+ //}
+ //EndPaint(hWnd, &ps);
+ //return 0;
+ }
+ break;
+ case WM_KEYDOWN:
+ {
+ unsigned char key = GetPangoKey(wParam, lParam);
+ if(key>0) process::Keyboard(key, 1, 1);
+ return 0;
+ }
+ case WM_KEYUP:
+ {
+ unsigned char key = GetPangoKey(wParam, lParam);
+ if (key>0) process::KeyboardUp(key, 1, 1);
+ return 0;
+ }
+ case WM_LBUTTONDOWN:
+ process::Mouse(0 | GetMouseModifierKey(wParam), 0, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+ case WM_MBUTTONDOWN:
+ process::Mouse(1 | GetMouseModifierKey(wParam), 0, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+ case WM_RBUTTONDOWN:
+ process::Mouse(2 | GetMouseModifierKey(wParam), 0, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+
+ case WM_LBUTTONUP:
+ process::Mouse(0 | GetMouseModifierKey(wParam), 1, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+ case WM_MBUTTONUP:
+ process::Mouse(1 | GetMouseModifierKey(wParam), 1, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+ case WM_RBUTTONUP:
+ process::Mouse(2 | GetMouseModifierKey(wParam), 1, GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ return 0;
+
+ case WM_MOUSEMOVE:
+ if (wParam & (MK_LBUTTON | MK_MBUTTON | MK_RBUTTON) ) {
+ process::MouseMotion(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ } else{
+ process::PassiveMouseMotion(GET_X_LPARAM(lParam), GET_Y_LPARAM(lParam));
+ }
+ return 0;
+
+ case WM_MOUSEWHEEL:
+ process::Scroll(0.0f, GET_WHEEL_DELTA_WPARAM(wParam) / 5.0f );
+ return 0;
+ case WM_MOUSEHWHEEL:
+ process::Scroll(GET_WHEEL_DELTA_WPARAM(wParam) / 5.0f, 0.0f);
+ return 0;
+ default:
+ break;
+ }
+ return DefWindowProc(hWnd, message, wParam, lParam);
+}
+
+void WinWindow::StartFullScreen() {
+ LONG dwExStyle = GetWindowLong(hWnd, GWL_EXSTYLE)
+ & ~(WS_EX_DLGMODALFRAME | WS_EX_CLIENTEDGE | WS_EX_STATICEDGE);
+ if(dwExStyle==0) {
+ std::cerr << "GetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ LONG dwStyle = GetWindowLong(hWnd, GWL_STYLE)
+ & ~(WS_CAPTION | WS_THICKFRAME | WS_MINIMIZE | WS_MAXIMIZE | WS_SYSMENU);
+ if(dwStyle==0) {
+ std::cerr << "GetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ if(!SetWindowLong(hWnd, GWL_EXSTYLE, dwExStyle)) {
+ std::cerr << "SetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ if(!SetWindowLong(hWnd, GWL_STYLE, dwStyle)) {
+ std::cerr << "SetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ GLint prev[2];
+ std::memcpy(prev, context->windowed_size, sizeof(prev));
+ ShowWindow(hWnd, SW_SHOWMAXIMIZED);
+ std::memcpy(context->windowed_size, prev, sizeof(prev));
+}
+
+void WinWindow::StopFullScreen() {
+ ChangeDisplaySettings(NULL, 0);
+ ShowCursor(TRUE);
+
+ LONG dwExStyle = GetWindowLong(hWnd, GWL_EXSTYLE) | WS_EX_APPWINDOW | WS_EX_WINDOWEDGE;
+ LONG dwStyle = GetWindowLong(hWnd, GWL_STYLE) | WS_OVERLAPPEDWINDOW;
+
+ if(dwExStyle==0) {
+ std::cerr << "GetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ if(dwStyle==0) {
+ std::cerr << "GetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ if(!SetWindowLong(hWnd, GWL_EXSTYLE, dwExStyle)) {
+ std::cerr << "SetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ if(!SetWindowLong(hWnd, GWL_STYLE, dwStyle)) {
+ std::cerr << "SetWindowLong() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ if(!SetWindowPos(hWnd, HWND_TOP, 0, 0, context->windowed_size[0], context->windowed_size[1], SWP_FRAMECHANGED)) {
+ std::cerr << "SetWindowPos() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::ToggleFullscreen()
+{
+ if(!context->is_fullscreen) {
+ StartFullScreen();
+ context->is_fullscreen = true;
+ }else{
+ StopFullScreen();
+ context->is_fullscreen = false;
+ }
+}
+
+void WinWindow::Move(int x, int y)
+{
+ if( !SetWindowPos(hWnd, 0, x, y, 0, 0, SWP_NOSIZE) ) {
+ std::cerr << "WinWindow::Move failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::Resize(unsigned int w, unsigned int h)
+{
+ if( !SetWindowPos(hWnd, 0, 0, 0, w, h, SWP_NOMOVE) ) {
+ std::cerr << "WinWindow::Resize failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::MakeCurrent()
+{
+ if(wglMakeCurrent(hDC, hGLRC)==FALSE) {
+ std::cerr << "wglMakeCurrent() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+
+ // Setup threadlocal context as this
+ context = this;
+
+ RECT rect;
+ if(!GetWindowRect(hWnd, &rect)) {
+ std::cerr << "GetWindowRect() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+ Resize(rect.right - rect.left, rect.bottom - rect.top);
+}
+
+void WinWindow::RemoveCurrent()
+{
+ if(wglMakeCurrent(NULL, NULL)==0) {
+ std::cerr << "wglMakeCurrent() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::SwapBuffers()
+{
+ if(!::SwapBuffers(hDC)) {
+ std::cerr << "SwapBuffers() failed" << std::endl;
+ CheckWGLDieOnError();
+ }
+}
+
+void WinWindow::ProcessEvents()
+{
+ MSG msg;
+ while (PeekMessage(&msg, NULL, 0, 0, PM_REMOVE)) {
+ if (msg.message == WM_QUIT) {
+ pangolin::Quit();
+ break;
+ }
+ TranslateMessage(&msg);
+ DispatchMessage(&msg);
+ }
+}
+
+std::unique_ptr<WindowInterface> CreateWinWindowAndBind(std::string window_title, int w, int h)
+{
+ WinWindow* win = new WinWindow(window_title, w, h);
+
+ return std::unique_ptr<WindowInterface>(win);
+}
+
+PANGOLIN_REGISTER_FACTORY(WinWindow)
+{
+ struct WinWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ const int w = uri.Get<int>("w", 640);
+ const int h = uri.Get<int>("h", 480);
+ return std::unique_ptr<WindowInterface>(CreateWinWindowAndBind(window_title, w, h));
+ }
+ };
+
+ auto factory = std::make_shared<WinWindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "winapi");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 100, "default");
+}
+
+}
diff --git a/externals/Pangolin/src/display/device/display_x11.cpp b/externals/Pangolin/src/display/device/display_x11.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fff02c6ba0afebb9c83888c086e948b859b301a3
--- /dev/null
+++ b/externals/Pangolin/src/display/device/display_x11.cpp
@@ -0,0 +1,531 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2018 Steven Lovegrove, Andrey Mnatsakanov
+ *
+ * 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.
+ */
+
+
+// Code based on public domain sample at
+// https://www.opengl.org/wiki/Tutorial:_OpenGL_3.0_Context_Creation_%28GLX%29
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/display/window.h>
+
+#include <pangolin/display/device/X11Window.h>
+
+#include <mutex>
+#include <stdexcept>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <GL/glx.h>
+
+namespace pangolin
+{
+
+extern __thread PangolinGl* context;
+
+std::mutex window_mutex;
+std::weak_ptr<X11GlContext> global_gl_context;
+
+const long EVENT_MASKS = ButtonPressMask|ButtonReleaseMask|StructureNotifyMask|ButtonMotionMask|PointerMotionMask|KeyPressMask|KeyReleaseMask|FocusChangeMask;
+
+#define GLX_CONTEXT_MAJOR_VERSION_ARB 0x2091
+#define GLX_CONTEXT_MINOR_VERSION_ARB 0x2092
+typedef GLXContext (*glXCreateContextAttribsARBProc)(::Display*, ::GLXFBConfig, ::GLXContext, Bool, const int*);
+
+// Adapted from: http://www.opengl.org/resources/features/OGLextensions/
+bool isExtensionSupported(const char *extList, const char *extension)
+{
+ /* Extension names should not have spaces. */
+ const char* where = strchr(extension, ' ');
+ if (where || *extension == '\0') {
+ return false;
+ }
+
+ for(const char* start=extList;;) {
+ where = strstr(start, extension);
+ if (!where) {
+ break;
+ }
+
+ const char *terminator = where + strlen(extension);
+
+ if ( where == start || *(where - 1) == ' ' ) {
+ if ( *terminator == ' ' || *terminator == '\0' ) {
+ return true;
+ }
+ }
+
+ start = terminator;
+ }
+
+ return false;
+}
+
+::GLXFBConfig ChooseFrameBuffer(
+ ::Display *display, bool glx_doublebuffer,
+ int glx_sample_buffers, int glx_samples
+) {
+ // Desired attributes
+ int visual_attribs[] =
+ {
+ GLX_X_RENDERABLE , True,
+ GLX_DRAWABLE_TYPE , GLX_WINDOW_BIT,
+ GLX_RENDER_TYPE , GLX_RGBA_BIT,
+ GLX_X_VISUAL_TYPE , GLX_TRUE_COLOR,
+ GLX_RED_SIZE , 8,
+ GLX_GREEN_SIZE , 8,
+ GLX_BLUE_SIZE , 8,
+ GLX_ALPHA_SIZE , 8,
+ GLX_DEPTH_SIZE , 24,
+ GLX_STENCIL_SIZE , 8,
+ GLX_DOUBLEBUFFER , glx_doublebuffer ? True : False,
+ None
+ };
+
+ int fbcount;
+ GLXFBConfig* fbc = glXChooseFBConfig(display, DefaultScreen(display), visual_attribs, &fbcount);
+ if (!fbc) {
+ throw std::runtime_error("Pangolin X11: Unable to retrieve framebuffer options");
+ }
+
+ int best_fbc = -1;
+ int worst_fbc = -1;
+ int best_num_samp = -1;
+ int worst_num_samp = 999;
+
+ // Enumerate framebuffer options, storing the best and worst that match our attribs
+ for (int i=0; i<fbcount; ++i)
+ {
+ XVisualInfo *vi = glXGetVisualFromFBConfig( display, fbc[i] );
+ if ( vi )
+ {
+ int samp_buf, samples;
+ glXGetFBConfigAttrib( display, fbc[i], GLX_SAMPLE_BUFFERS, &samp_buf );
+ glXGetFBConfigAttrib( display, fbc[i], GLX_SAMPLES , &samples );
+
+ // Filter for the best available.
+ if ( samples > best_num_samp ) {
+ best_fbc = i;
+ best_num_samp = samples;
+ }
+
+ // Filter lowest settings which match minimum user requirement.
+ if ( samp_buf >= glx_sample_buffers && samples >= glx_samples && samples < worst_num_samp ) {
+ worst_fbc = i;
+ worst_num_samp = samples;
+ }
+ }
+ XFree( vi );
+ }
+
+ // Select the minimum suitable option. The 'best' is often too slow.
+ int chosen_fbc_id = worst_fbc;
+
+ // If minimum requested isn't available, return the best that is.
+ if(chosen_fbc_id < 0) {
+ pango_print_warn("Framebuffer with requested attributes not available. Using available framebuffer. You may see visual artifacts.");
+ chosen_fbc_id = best_fbc;
+ }
+
+ ::GLXFBConfig chosenFbc = fbc[ chosen_fbc_id ];
+ XFree( fbc );
+ return chosenFbc;
+}
+
+static bool ctxErrorOccurred = false;
+static int ctxErrorHandler( ::Display * /*dpy*/, ::XErrorEvent * ev )
+{
+ const int buffer_size = 10240;
+ char buffer[buffer_size];
+ XGetErrorText(ev->display, ev->error_code, buffer, buffer_size );
+ pango_print_error("X11 Error: %s\n", buffer);
+ ctxErrorOccurred = true;
+ return 0;
+}
+
+GLXContext CreateGlContext(::Display *display, ::GLXFBConfig chosenFbc, GLXContext share_context = 0)
+{
+ int glx_major, glx_minor;
+ if ( !glXQueryVersion( display, &glx_major, &glx_minor ) ||
+ ( ( glx_major == 1 ) && ( glx_minor < 3 ) ) || ( glx_major < 1 ) )
+ {
+ throw std::runtime_error("Pangolin X11: Invalid GLX version. Require GLX >= 1.3");
+ }
+
+ GLXContext new_ctx;
+
+ // Get the default screen's GLX extension list
+ const char *glxExts = glXQueryExtensionsString( display, DefaultScreen( display ) );
+
+ glXCreateContextAttribsARBProc glXCreateContextAttribsARB =
+ (glXCreateContextAttribsARBProc) glXGetProcAddressARB(
+ (const GLubyte *) "glXCreateContextAttribsARB"
+ );
+
+ // Install an X error handler so the application won't exit if GL 3.0
+ // context allocation fails. Handler is global and shared across all threads.
+ ctxErrorOccurred = false;
+ int (*oldHandler)(::Display*, ::XErrorEvent*) = XSetErrorHandler(&ctxErrorHandler);
+
+ if ( isExtensionSupported( glxExts, "GLX_ARB_create_context" ) && glXCreateContextAttribsARB )
+ {
+ int context_attribs[] = {
+ GLX_CONTEXT_MAJOR_VERSION_ARB, 3,
+ GLX_CONTEXT_MINOR_VERSION_ARB, 0,
+ //GLX_CONTEXT_FLAGS_ARB , GLX_CONTEXT_FORWARD_COMPATIBLE_BIT_ARB,
+ None
+ };
+
+ new_ctx = glXCreateContextAttribsARB( display, chosenFbc, share_context, True, context_attribs );
+
+ // Sync to ensure any errors generated are processed.
+ XSync( display, False );
+ if ( ctxErrorOccurred || !new_ctx ) {
+ ctxErrorOccurred = false;
+ // Fall back to old-style 2.x context. Implementations will return the newest
+ // context version compatible with OpenGL versions less than version 3.0.
+ context_attribs[1] = 1; // GLX_CONTEXT_MAJOR_VERSION_ARB = 1
+ context_attribs[3] = 0; // GLX_CONTEXT_MINOR_VERSION_ARB = 0
+ new_ctx = glXCreateContextAttribsARB( display, chosenFbc, share_context, True, context_attribs );
+ }
+ } else {
+ // Fallback to GLX 1.3 Context
+ new_ctx = glXCreateNewContext( display, chosenFbc, GLX_RGBA_TYPE, share_context, True );
+ }
+
+ // Sync to ensure any errors generated are processed.
+ XSync( display, False );
+
+ // Restore the original error handler
+ XSetErrorHandler( oldHandler );
+
+ if ( ctxErrorOccurred || !new_ctx ) {
+ throw std::runtime_error("Pangolin X11: Failed to create an OpenGL context");
+ }
+
+ // Verifying that context is a direct context
+ if ( ! glXIsDirect ( display, new_ctx ) ) {
+ pango_print_warn("Pangolin X11: Indirect GLX rendering context obtained\n");
+ }
+
+ return new_ctx;
+}
+
+X11GlContext::X11GlContext(std::shared_ptr<X11Display>& d, ::GLXFBConfig chosenFbc, std::shared_ptr<X11GlContext> shared_context)
+ : display(d), shared_context(shared_context)
+{
+ // prevent chained sharing
+ while(shared_context && shared_context->shared_context) {
+ shared_context = shared_context->shared_context;
+ }
+
+ // Contexts can't be shared across different displays.
+ if(shared_context && shared_context->display != d) {
+ shared_context.reset();
+ }
+
+ glcontext = CreateGlContext(display->display, chosenFbc, shared_context ? shared_context->glcontext : 0);
+}
+
+X11GlContext::~X11GlContext()
+{
+ glXDestroyContext( display->display, glcontext );
+}
+
+X11Window::X11Window(
+ const std::string& title, int width, int height,
+ std::shared_ptr<X11Display>& display, ::GLXFBConfig chosenFbc
+) : display(display), glcontext(0), win(0), cmap(0)
+{
+ PangolinGl::windowed_size[0] = width;
+ PangolinGl::windowed_size[1] = height;
+
+ // Get a visual
+ XVisualInfo *vi = glXGetVisualFromFBConfig( display->display, chosenFbc );
+
+ // Create colourmap
+ XSetWindowAttributes swa;
+ swa.background_pixmap = None;
+ swa.border_pixel = 0;
+ swa.event_mask = StructureNotifyMask;
+ swa.colormap = cmap = XCreateColormap( display->display,
+ RootWindow( display->display, vi->screen ),
+ vi->visual, AllocNone );
+
+ // Create window
+ win = XCreateWindow( display->display, RootWindow( display->display, vi->screen ),
+ 0, 0, width, height, 0, vi->depth, InputOutput,
+ vi->visual,
+ CWBorderPixel|CWColormap|CWEventMask, &swa );
+
+ XFree( vi );
+
+ if ( !win ) {
+ throw std::runtime_error("Pangolin X11: Failed to create window." );
+ }
+
+ XStoreName( display->display, win, title.c_str() );
+ XMapWindow( display->display, win );
+
+ // Request to be notified of these events
+ XSelectInput(display->display, win, EVENT_MASKS );
+
+ delete_message = XInternAtom(display->display, "WM_DELETE_WINDOW", False);
+ XSetWMProtocols(display->display, win, &delete_message, 1);
+}
+
+X11Window::~X11Window()
+{
+ glXMakeCurrent( display->display, 0, 0 );
+ XDestroyWindow( display->display, win );
+ XFreeColormap( display->display, cmap );
+}
+
+void X11Window::MakeCurrent(GLXContext ctx)
+{
+ glXMakeCurrent( display->display, win, ctx );
+ context = this;
+}
+
+void X11Window::MakeCurrent()
+{
+ MakeCurrent(glcontext ? glcontext->glcontext : global_gl_context.lock()->glcontext);
+}
+
+void X11Window::RemoveCurrent()
+{
+ glXMakeCurrent(display->display, 0, nullptr);
+}
+
+void X11Window::ToggleFullscreen()
+{
+ const Atom _NET_WM_STATE_FULLSCREEN = XInternAtom(display->display, "_NET_WM_STATE_FULLSCREEN", True);
+ const Atom _NET_WM_STATE = XInternAtom(display->display, "_NET_WM_STATE", True);
+ XEvent e;
+ e.xclient.type = ClientMessage;
+ e.xclient.window = win;
+ e.xclient.message_type = _NET_WM_STATE;
+ e.xclient.format = 32;
+ e.xclient.data.l[0] = 2; // Toggle
+ e.xclient.data.l[1] = _NET_WM_STATE_FULLSCREEN;
+ e.xclient.data.l[2] = 0;
+ e.xclient.data.l[3] = 1;
+ e.xclient.data.l[4] = 0;
+
+ XSendEvent(display->display, DefaultRootWindow(display->display), False, SubstructureRedirectMask | SubstructureNotifyMask, &e);
+ XMoveResizeWindow(display->display, win, 0, 0, windowed_size[0], windowed_size[1]);
+}
+
+void X11Window::Move(int x, int y)
+{
+ XMoveWindow(display->display, win, x, y);
+}
+
+void X11Window::Resize(unsigned int w, unsigned int h)
+{
+ XResizeWindow(display->display, win, w, h);
+}
+
+void X11Window::ProcessEvents()
+{
+ XEvent ev;
+ while(!pangolin::ShouldQuit() && XPending(display->display) > 0)
+ {
+ XNextEvent(display->display, &ev);
+
+ switch(ev.type){
+ case ConfigureNotify:
+ pangolin::process::Resize(ev.xconfigure.width, ev.xconfigure.height);
+ break;
+ case ClientMessage:
+ // We've only registered to receive WM_DELETE_WINDOW, so no further checks needed.
+ pangolin::Quit();
+ break;
+ case ButtonPress:
+ case ButtonRelease:
+ {
+ const int button = ev.xbutton.button-1;
+ const int mask = Button1Mask << button;
+ pangolin::process::Mouse(
+ button,
+ ev.xbutton.state & mask,
+ ev.xbutton.x, ev.xbutton.y
+ );
+ break;
+ }
+ case FocusOut:
+ pangolin::context->mouse_state = 0;
+ break;
+ case MotionNotify:
+ if(ev.xmotion.state & (Button1Mask|Button2Mask|Button3Mask) ) {
+ pangolin::process::MouseMotion(ev.xmotion.x, ev.xmotion.y);
+ }else{
+ pangolin::process::PassiveMouseMotion(ev.xmotion.x, ev.xmotion.y);
+ }
+ break;
+ case KeyPress:
+ case KeyRelease:
+ int key;
+ char ch;
+ KeySym sym;
+
+ if( XLookupString(&ev.xkey,&ch,1,&sym,0) == 0) {
+ switch (sym) {
+ case XK_F1: key = PANGO_SPECIAL + PANGO_KEY_F1 ; break;
+ case XK_F2: key = PANGO_SPECIAL + PANGO_KEY_F2 ; break;
+ case XK_F3: key = PANGO_SPECIAL + PANGO_KEY_F3 ; break;
+ case XK_F4: key = PANGO_SPECIAL + PANGO_KEY_F4 ; break;
+ case XK_F5: key = PANGO_SPECIAL + PANGO_KEY_F5 ; break;
+ case XK_F6: key = PANGO_SPECIAL + PANGO_KEY_F6 ; break;
+ case XK_F7: key = PANGO_SPECIAL + PANGO_KEY_F7 ; break;
+ case XK_F8: key = PANGO_SPECIAL + PANGO_KEY_F8 ; break;
+ case XK_F9: key = PANGO_SPECIAL + PANGO_KEY_F9 ; break;
+ case XK_F10: key = PANGO_SPECIAL + PANGO_KEY_F10 ; break;
+ case XK_F11: key = PANGO_SPECIAL + PANGO_KEY_F11 ; break;
+ case XK_F12: key = PANGO_SPECIAL + PANGO_KEY_F12 ; break;
+ case XK_Left: key = PANGO_SPECIAL + PANGO_KEY_LEFT ; break;
+ case XK_Up: key = PANGO_SPECIAL + PANGO_KEY_UP ; break;
+ case XK_Right: key = PANGO_SPECIAL + PANGO_KEY_RIGHT ; break;
+ case XK_Down: key = PANGO_SPECIAL + PANGO_KEY_DOWN ; break;
+ case XK_Page_Up: key = PANGO_SPECIAL + PANGO_KEY_PAGE_UP ; break;
+ case XK_Page_Down: key = PANGO_SPECIAL + PANGO_KEY_PAGE_DOWN ; break;
+ case XK_Home: key = PANGO_SPECIAL + PANGO_KEY_HOME ; break;
+ case XK_End: key = PANGO_SPECIAL + PANGO_KEY_END ; break;
+ case XK_Insert: key = PANGO_SPECIAL + PANGO_KEY_INSERT ; break;
+ case XK_Shift_L:
+ case XK_Shift_R:
+ key = -1;
+ if(ev.type==KeyPress) {
+ pangolin::context->mouse_state |= pangolin::KeyModifierShift;
+ }else{
+ pangolin::context->mouse_state &= ~pangolin::KeyModifierShift;
+ }
+ break;
+ case XK_Control_L:
+ case XK_Control_R:
+ key = -1;
+ if(ev.type==KeyPress) {
+ pangolin::context->mouse_state |= pangolin::KeyModifierCtrl;
+ }else{
+ pangolin::context->mouse_state &= ~pangolin::KeyModifierCtrl;
+ }
+ break;
+ case XK_Alt_L:
+ case XK_Alt_R:
+ key = -1;
+ if(ev.type==KeyPress) {
+ pangolin::context->mouse_state |= pangolin::KeyModifierAlt;
+ }else{
+ pangolin::context->mouse_state &= ~pangolin::KeyModifierAlt;
+ }
+ break;
+ case XK_Super_L:
+ case XK_Super_R:
+ key = -1;
+ if(ev.type==KeyPress) {
+ pangolin::context->mouse_state |= pangolin::KeyModifierCmd;
+ }else{
+ pangolin::context->mouse_state &= ~pangolin::KeyModifierCmd;
+ }
+ break;
+ default: key = -1; break;
+ }
+ }else{
+ key = ch;
+ }
+
+ if(key >=0) {
+ if(ev.type == KeyPress) {
+ pangolin::process::Keyboard(key, ev.xkey.x, ev.xkey.y);
+ }else{
+ pangolin::process::KeyboardUp(key, ev.xkey.x, ev.xkey.y);
+ }
+ }
+
+ break;
+ }
+ }
+}
+
+void X11Window::SwapBuffers() {
+ glXSwapBuffers(display->display, win);
+}
+
+std::unique_ptr<WindowInterface> CreateX11WindowAndBind(const std::string& window_title, const int w, const int h, const std::string& display_name, const bool double_buffered, const int sample_buffers, const int samples)
+{
+ std::shared_ptr<X11Display> newdisplay = std::make_shared<X11Display>(display_name.empty() ? NULL : display_name.c_str() );
+ if (!newdisplay) {
+ throw std::runtime_error("Pangolin X11: Failed to open X display");
+ }
+ ::GLXFBConfig newfbc = ChooseFrameBuffer(newdisplay->display, double_buffered, sample_buffers, samples);
+
+ window_mutex.lock();
+ std::shared_ptr<X11GlContext> newglcontext = std::make_shared<X11GlContext>(
+ newdisplay, newfbc, global_gl_context.lock()
+ );
+
+ if(!global_gl_context.lock()) {
+ global_gl_context = newglcontext;
+ }
+ window_mutex.unlock();
+
+ X11Window* win = new X11Window(window_title, w, h, newdisplay, newfbc);
+ win->glcontext = newglcontext;
+ win->is_double_buffered = double_buffered;
+
+ return std::unique_ptr<WindowInterface>(win);
+}
+
+PANGOLIN_REGISTER_FACTORY(X11Window)
+{
+ struct X11WindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ const int w = uri.Get<int>("w", 640);
+ const int h = uri.Get<int>("h", 480);
+ const std::string display_name = uri.Get<std::string>("display_name", "");
+ const bool double_buffered = uri.Get<bool>("double_buffered", true);
+ const int sample_buffers = uri.Get<int>("sample_buffers", 1);
+ const int samples = uri.Get<int>("samples", 1);
+ return std::unique_ptr<WindowInterface>(CreateX11WindowAndBind(window_title, w, h, display_name, double_buffered, sample_buffers, samples));
+ }
+ };
+
+ auto factory = std::make_shared<X11WindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "x11");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "linux");
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 100, "default");
+}
+
+}
+
diff --git a/externals/Pangolin/src/display/display.cpp b/externals/Pangolin/src/display/display.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d08d208d7ea0f4932aab2d72920e1945c804f0a4
--- /dev/null
+++ b/externals/Pangolin/src/display/display.cpp
@@ -0,0 +1,660 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove, Richard Newcombe
+ *
+ * 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.
+ */
+
+#include <pangolin/platform.h>
+
+#ifdef HAVE_PYTHON
+#include <pangolin/python/pyinterpreter.h>
+#include <pangolin/console/ConsoleView.h>
+#endif // HAVE_PYTHON
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <map>
+#include <mutex>
+#include <cstdlib>
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/window_frameworks.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/handler/handler.h>
+#include <pangolin/utils/simple_math.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/utils/type_convert.h>
+#include <pangolin/image/image_io.h>
+
+#ifdef BUILD_PANGOLIN_VARS
+ #include <pangolin/var/var.h>
+#endif
+
+namespace pangolin
+{
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ // Forward declaration.
+ void SaveFramebuffer(VideoOutput& video, const Viewport& v);
+#endif // BUILD_PANGOLIN_VIDEO
+
+const char* PARAM_DISPLAYNAME = "DISPLAYNAME";
+const char* PARAM_DOUBLEBUFFER = "DOUBLEBUFFER";
+const char* PARAM_SAMPLE_BUFFERS = "SAMPLE_BUFFERS";
+const char* PARAM_SAMPLES = "SAMPLES";
+const char* PARAM_HIGHRES = "HIGHRES";
+
+
+typedef std::map<std::string,std::shared_ptr<PangolinGl> > ContextMap;
+
+// Map of active contexts
+ContextMap contexts;
+std::recursive_mutex contexts_mutex;
+bool one_time_window_frameworks_init = false;
+
+// Context active for current thread
+__thread PangolinGl* context = 0;
+
+PangolinGl::PangolinGl()
+ : user_app(0), is_high_res(false), quit(false), mouse_state(0),activeDisplay(0)
+#ifdef BUILD_PANGOLIN_VIDEO
+ , record_view(0)
+#endif
+#ifdef HAVE_PYTHON
+ , console_view(0)
+#endif
+{
+}
+
+PangolinGl::~PangolinGl()
+{
+ // Free displays owned by named_managed_views
+ for(ViewMap::iterator iv = named_managed_views.begin(); iv != named_managed_views.end(); ++iv) {
+ delete iv->second;
+ }
+ named_managed_views.clear();
+}
+
+PangolinGl* GetCurrentContext()
+{
+ return context;
+}
+
+PangolinGl *FindContext(const std::string& name)
+{
+ contexts_mutex.lock();
+ ContextMap::iterator ic = contexts.find(name);
+ PangolinGl* context = (ic == contexts.end()) ? 0 : ic->second.get();
+ contexts_mutex.unlock();
+ return context;
+}
+
+WindowInterface& CreateWindowAndBind(std::string window_title, int w, int h, const Params& params)
+{
+ std::unique_lock<std::recursive_mutex> l(contexts_mutex);
+
+ if(!one_time_window_frameworks_init) {
+ one_time_window_frameworks_init = LoadBuiltInWindowFrameworks();
+ }
+
+ pangolin::Uri win_uri;
+
+ if(const char* extra_params = std::getenv("PANGOLIN_WINDOW_URI"))
+ {
+ // Take any defaults from the environment
+ win_uri = pangolin::ParseUri(extra_params);
+ }else{
+ // Otherwise revert to 'default' scheme.
+ win_uri.scheme = "default";
+ }
+
+ // Allow params to override
+ win_uri.scheme = params.Get("scheme", win_uri.scheme);
+
+ // Override with anything the program specified
+ win_uri.params.insert(std::end(win_uri.params), std::begin(params.params), std::end(params.params));
+ win_uri.Set("w", w);
+ win_uri.Set("h", h);
+ win_uri.Set("window_title", window_title);
+
+ std::unique_ptr<WindowInterface> window = FactoryRegistry<WindowInterface>::I().Open(win_uri);
+
+ // We're expecting not only a WindowInterface, but a PangolinGl.
+ if(!window || !dynamic_cast<PangolinGl*>(window.get())) {
+ throw WindowExceptionNoKnownHandler(win_uri.scheme);
+ }
+
+ std::shared_ptr<PangolinGl> context(dynamic_cast<PangolinGl*>(window.release()));
+ RegisterNewContext(window_title, context );
+ // is_high_res will alter default font size and a few other gui elements.
+ context->is_high_res = win_uri.Get(PARAM_HIGHRES,false);
+ context->MakeCurrent();
+ context->ProcessEvents();
+ glewInit();
+
+ return *context;
+}
+
+// Assumption: unique lock is held on contexts_mutex for multi-threaded operation
+void RegisterNewContext(const std::string& name, std::shared_ptr<PangolinGl> newcontext)
+{
+ // Set defaults
+ newcontext->base.left = 0.0;
+ newcontext->base.bottom = 0.0;
+ newcontext->base.top = 1.0;
+ newcontext->base.right = 1.0;
+ newcontext->base.aspect = 0;
+ newcontext->base.handler = &StaticHandler;
+ newcontext->is_fullscreen = false;
+
+ // Create and add
+ if( contexts.find(name) != contexts.end() ) {
+ throw std::runtime_error("Context already exists.");
+ }
+ contexts[name] = newcontext;
+
+ // Process the following as if this context is now current.
+ PangolinGl *oldContext = context;
+ context = newcontext.get();
+ process::Resize(
+ newcontext->windowed_size[0],
+ newcontext->windowed_size[1]
+ );
+
+ // Default key bindings can be overridden
+ RegisterKeyPressCallback(PANGO_KEY_ESCAPE, Quit );
+ RegisterKeyPressCallback('\t', ToggleFullscreen );
+ RegisterKeyPressCallback('`', ToggleConsole );
+
+ context = oldContext;
+}
+
+WindowInterface* GetBoundWindow()
+{
+ return context;
+}
+
+void DestroyWindow(const std::string& name)
+{
+ contexts_mutex.lock();
+ ContextMap::iterator ic = contexts.find(name);
+ PangolinGl *context_to_destroy = (ic == contexts.end()) ? 0 : ic->second.get();
+ if (context_to_destroy == context) {
+ context = nullptr;
+ }
+ size_t erased = contexts.erase(name);
+ if(erased == 0) {
+ pango_print_warn("Context '%s' doesn't exist for deletion.\n", name.c_str());
+ }
+ contexts_mutex.unlock();
+}
+
+WindowInterface& BindToContext(std::string name)
+{
+ std::unique_lock<std::recursive_mutex> l(contexts_mutex);
+
+ // N.B. context is modified prior to invoking MakeCurrent so that
+ // state management callbacks (such as Resize()) can be correctly
+ // processed.
+ PangolinGl *context_to_bind = FindContext(name);
+ if( !context_to_bind )
+ {
+ std::shared_ptr<PangolinGl> newcontext(new PangolinGl());
+ RegisterNewContext(name, newcontext);
+ return *(newcontext.get());
+ }else{
+ context_to_bind->MakeCurrent();
+ return *context_to_bind;
+ }
+}
+
+void Quit()
+{
+ context->quit = true;
+}
+
+void QuitAll()
+{
+ for(auto nc : contexts) {
+ nc.second->quit = true;
+ }
+}
+
+bool ShouldQuit()
+{
+ return !context || context->quit;
+}
+
+bool HadInput()
+{
+ if( context->had_input > 0 )
+ {
+ --context->had_input;
+ return true;
+ }
+ return false;
+}
+
+bool HasResized()
+{
+ if( context->has_resized > 0 )
+ {
+ --context->has_resized;
+ return true;
+ }
+ return false;
+}
+
+void StartFullScreen() {
+ if(!context->is_fullscreen) {
+ context->ToggleFullscreen();
+ context->is_fullscreen = true;
+ }
+}
+
+void StopFullScreen() {
+ if(context->is_fullscreen) {
+ context->ToggleFullscreen();
+ context->is_fullscreen = false;
+ }
+}
+
+void SetFullscreen(bool fullscreen)
+{
+ if(fullscreen) {
+ StartFullScreen();
+ }else{
+ StopFullScreen();
+ }
+}
+
+void RenderViews()
+{
+ Viewport::DisableScissor();
+ DisplayBase().Render();
+}
+
+void RenderRecordGraphic(const Viewport& v)
+{
+ const float r = 7;
+ v.ActivatePixelOrthographic();
+ glRecordGraphic(v.w-2*r, v.h-2*r, r);
+}
+
+void PostRender()
+{
+ while(context->screen_capture.size()) {
+ std::pair<std::string,Viewport> fv = context->screen_capture.front();
+ context->screen_capture.pop();
+ SaveFramebuffer(fv.first, fv.second);
+ }
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ if(context->recorder.IsOpen()) {
+ SaveFramebuffer(context->recorder, context->record_view->GetBounds() );
+ RenderRecordGraphic(context->record_view->GetBounds());
+ }
+#endif // BUILD_PANGOLIN_VIDEO
+
+ // Disable scissor each frame
+ Viewport::DisableScissor();
+}
+
+void FinishFrame()
+{
+ RenderViews();
+ PostRender();
+ context->SwapBuffers();
+ context->ProcessEvents();
+}
+
+View& DisplayBase()
+{
+ return context->base;
+}
+
+View& CreateDisplay()
+{
+ int iguid = rand();
+ std::stringstream ssguid;
+ ssguid << iguid;
+ return Display(ssguid.str());
+}
+
+void ToggleConsole()
+{
+#ifdef HAVE_PYTHON
+ if( !context->console_view) {
+ // Create console and let the pangolin context take ownership
+ context->console_view = new ConsoleView(new PyInterpreter());
+ context->named_managed_views["pangolin_console"] = context->console_view;
+ context->console_view->SetFocus();
+ context->console_view->zorder = std::numeric_limits<int>::max();
+ DisplayBase().AddDisplay(*context->console_view);
+ }else{
+ context->console_view->ToggleShow();
+ if(context->console_view->IsShown()) {
+ context->console_view->SetFocus();
+ }
+ }
+#endif
+}
+
+void ToggleFullscreen()
+{
+ SetFullscreen(!context->is_fullscreen);
+}
+
+View& Display(const std::string& name)
+{
+ // Get / Create View
+ ViewMap::iterator vi = context->named_managed_views.find(name);
+ if( vi != context->named_managed_views.end() )
+ {
+ return *(vi->second);
+ }else{
+ View * v = new View();
+ context->named_managed_views[name] = v;
+ v->handler = &StaticHandler;
+ context->base.views.push_back(v);
+ return *v;
+ }
+}
+
+void RegisterKeyPressCallback(int key, std::function<void(void)> func)
+{
+ context->keypress_hooks[key] = func;
+}
+
+void SaveWindowOnRender(std::string prefix)
+{
+ context->screen_capture.push(std::pair<std::string,Viewport>(prefix, context->base.v) );
+}
+
+void SaveFramebuffer(std::string prefix, const Viewport& v)
+{
+ PANGOLIN_UNUSED(prefix);
+ PANGOLIN_UNUSED(v);
+
+#ifndef HAVE_GLES
+
+#ifdef HAVE_PNG
+ PixelFormat fmt = PixelFormatFromString("RGBA32");
+ TypedImage buffer(v.w, v.h, fmt );
+ glReadBuffer(GL_BACK);
+ glPixelStorei(GL_PACK_ALIGNMENT, 1); // TODO: Avoid this?
+ glReadPixels(v.l, v.b, v.w, v.h, GL_RGBA, GL_UNSIGNED_BYTE, buffer.ptr );
+ SaveImage(buffer, fmt, prefix + ".png", false);
+#endif // HAVE_PNG
+
+#endif // HAVE_GLES
+}
+
+#ifdef BUILD_PANGOLIN_VIDEO
+void SaveFramebuffer(VideoOutput& video, const Viewport& v)
+{
+#ifndef HAVE_GLES
+ const StreamInfo& si = video.Streams()[0];
+ if(video.Streams().size()==0 || (int)si.Width() != v.w || (int)si.Height() != v.h) {
+ video.Close();
+ return;
+ }
+
+ static basetime last_time = TimeNow();
+ const basetime time_now = TimeNow();
+ last_time = time_now;
+
+ static std::vector<unsigned char> img;
+ img.resize(v.w*v.h*4);
+
+ glReadBuffer(GL_BACK);
+ glPixelStorei(GL_PACK_ALIGNMENT, 1); // TODO: Avoid this?
+ glReadPixels(v.l, v.b, v.w, v.h, GL_RGB, GL_UNSIGNED_BYTE, &img[0] );
+ video.WriteStreams(&img[0]);
+#endif // HAVE_GLES
+}
+#endif // BUILD_PANGOLIN_VIDEO
+
+
+namespace process
+{
+float last_x = 0;
+float last_y = 0;
+
+void Keyboard( unsigned char key, int x, int y)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+
+#ifdef HAVE_APPLE_OPENGL_FRAMEWORK
+ // Switch backspace and delete for OSX!
+ if(key== '\b') {
+ key = 127;
+ }else if(key == 127) {
+ key = '\b';
+ }
+#endif
+
+ context->had_input = context->is_double_buffered ? 2 : 1;
+
+ // Check if global key hook exists
+ const KeyhookMap::iterator hook = context->keypress_hooks.find(key);
+
+#ifdef HAVE_PYTHON
+ // Console receives all input when it is open
+ if( context->console_view && context->console_view->IsShown() ) {
+ context->console_view->Keyboard(*(context->console_view),key,x,y,true);
+ }else
+#endif
+ if(hook != context->keypress_hooks.end() ) {
+ hook->second();
+ } else if(context->activeDisplay && context->activeDisplay->handler) {
+ context->activeDisplay->handler->Keyboard(*(context->activeDisplay),key,x,y,true);
+ }
+}
+
+void KeyboardUp(unsigned char key, int x, int y)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+
+ if(context->activeDisplay && context->activeDisplay->handler)
+ {
+ context->activeDisplay->handler->Keyboard(*(context->activeDisplay),key,x,y,false);
+ }
+}
+
+void SpecialFunc(int key, int x, int y)
+{
+ Keyboard(key+128,x,y);
+}
+
+void SpecialFuncUp(int key, int x, int y)
+{
+ KeyboardUp(key+128,x,y);
+}
+
+
+void Mouse( int button_raw, int state, int x, int y)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+
+ last_x = (float)x;
+ last_y = (float)y;
+
+ const MouseButton button = (MouseButton)(1 << (button_raw & 0xf) );
+ const bool pressed = (state == 0);
+
+ context->had_input = context->is_double_buffered ? 2 : 1;
+
+ const bool fresh_input = ( (context->mouse_state & 7) == 0);
+
+ if( pressed ) {
+ context->mouse_state |= (button&7);
+ }else{
+ context->mouse_state &= ~(button&7);
+ }
+
+#if defined(_WIN_)
+ context->mouse_state &= 0x0000ffff;
+ context->mouse_state |= (button_raw >> 4) << 16;
+#endif
+
+ if(fresh_input) {
+ context->base.handler->Mouse(context->base,button,x,y,pressed,context->mouse_state);
+ }else if(context->activeDisplay && context->activeDisplay->handler) {
+ context->activeDisplay->handler->Mouse(*(context->activeDisplay),button,x,y,pressed,context->mouse_state);
+ }
+}
+
+void MouseMotion( int x, int y)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+
+ last_x = (float)x;
+ last_y = (float)y;
+
+ context->had_input = context->is_double_buffered ? 2 : 1;
+
+ if( context->activeDisplay)
+ {
+ if( context->activeDisplay->handler )
+ context->activeDisplay->handler->MouseMotion(*(context->activeDisplay),x,y,context->mouse_state);
+ }else{
+ context->base.handler->MouseMotion(context->base,x,y,context->mouse_state);
+ }
+}
+
+void PassiveMouseMotion(int x, int y)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+
+ context->base.handler->PassiveMouseMotion(context->base,x,y,context->mouse_state);
+
+ last_x = (float)x;
+ last_y = (float)y;
+}
+
+void Display()
+{
+ // No implementation
+}
+
+void Resize( int width, int height )
+{
+ if( !context->is_fullscreen )
+ {
+ context->windowed_size[0] = width;
+ context->windowed_size[1] = height;
+ }
+ // TODO: Fancy display managers seem to cause this to mess up?
+ context->had_input = 20; //context->is_double_buffered ? 2 : 1;
+ context->has_resized = 20; //context->is_double_buffered ? 2 : 1;
+ Viewport win(0,0,width,height);
+ context->base.Resize(win);
+}
+
+void SpecialInput(InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4)
+{
+ // Assume coords already match OpenGl Window Coords
+
+ context->had_input = context->is_double_buffered ? 2 : 1;
+
+ const bool fresh_input = (context->mouse_state == 0);
+
+ if(fresh_input) {
+ context->base.handler->Special(context->base,inType,x,y,p1,p2,p3,p4,context->mouse_state);
+ }else if(context->activeDisplay && context->activeDisplay->handler) {
+ context->activeDisplay->handler->Special(*(context->activeDisplay),inType,x,y,p1,p2,p3,p4,context->mouse_state);
+ }
+}
+
+void Scroll(float x, float y)
+{
+ SpecialInput(InputSpecialScroll, last_x, last_y, x, y, 0, 0);
+}
+
+void Zoom(float m)
+{
+ SpecialInput(InputSpecialZoom, last_x, last_y, m, 0, 0, 0);
+}
+
+void Rotate(float r)
+{
+ SpecialInput(InputSpecialRotate, last_x, last_y, r, 0, 0, 0);
+}
+
+void SubpixMotion(float x, float y, float pressure, float rotation, float tiltx, float tilty)
+{
+ // Force coords to match OpenGl Window Coords
+ y = context->base.v.h - y;
+ SpecialInput(InputSpecialTablet, x, y, pressure, rotation, tiltx, tilty);
+}
+}
+
+void DrawTextureToViewport(GLuint texid)
+{
+ OpenGlRenderState::ApplyIdentity();
+ glBindTexture(GL_TEXTURE_2D, texid);
+ glEnable(GL_TEXTURE_2D);
+
+ GLfloat sq_vert[] = { -1,-1, 1,-1, 1, 1, -1, 1 };
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ GLfloat sq_tex[] = { 0,0, 1,0, 1,1, 0,1 };
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+
+ glDisable(GL_TEXTURE_2D);
+}
+
+ToggleViewFunctor::ToggleViewFunctor(View& view)
+ : view(view)
+{
+}
+
+ToggleViewFunctor::ToggleViewFunctor(const std::string& name)
+ : view(Display(name))
+{
+}
+
+void ToggleViewFunctor::operator()()
+{
+ view.ToggleShow();
+}
+
+}
diff --git a/externals/Pangolin/src/display/image_view.cpp b/externals/Pangolin/src/display/image_view.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5b56ca59ac33a83c3b5f3b4f80f50829cccb42e7
--- /dev/null
+++ b/externals/Pangolin/src/display/image_view.cpp
@@ -0,0 +1,223 @@
+#include <pangolin/display/image_view.h>
+#include <pangolin/image/image_utils.h>
+#include <pangolin/image/image_convert.h>
+
+namespace pangolin
+{
+
+ImageView::ImageView()
+ : offset_scale(0.0f, 1.0f), lastPressed(false), mouseReleased(false), mousePressed(false), overlayRender(true)
+{
+ SetHandler(this);
+}
+
+ImageView::~ImageView()
+{
+}
+
+void ImageView::Render()
+{
+ LoadPending();
+
+ glPushAttrib(GL_DEPTH_BITS);
+ glDisable(GL_DEPTH_TEST);
+
+ Activate();
+ this->UpdateView();
+ this->glSetViewOrtho();
+
+ if(tex.IsValid())
+ {
+ if(offset_scale.first != 0.0 || offset_scale.second != 1.0)
+ {
+ pangolin::GlSlUtilities::OffsetAndScale(offset_scale.first, offset_scale.second);
+ }
+ else
+ {
+ glColor4f(1, 1, 1, 1);
+ }
+
+ this->glRenderTexture(tex);
+ pangolin::GlSlUtilities::UseNone();
+ }
+
+ if(overlayRender)
+ {
+ this->glRenderOverlay();
+ }
+
+ if(extern_draw_function)
+ {
+ extern_draw_function(*this);
+ }
+
+ glPopAttrib();
+}
+
+void ImageView::Mouse(View& view, pangolin::MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ ImageViewHandler::Mouse(view, button, x, y, pressed, button_state);
+
+ mouseReleased = (!pressed && lastPressed);
+
+ mousePressed = lastPressed = (pressed && button == pangolin::MouseButtonLeft);
+}
+
+void ImageView::Keyboard(View& view, unsigned char key, int x, int y, bool pressed)
+{
+ if(key == 'a')
+ {
+ if(!tex.IsValid())
+ {
+ std::cerr << "ImageViewHandler does not contain valid texture." << std::endl;
+ return;
+ }
+
+ // compute scale
+ const bool have_selection = std::isfinite(GetSelection().Area()) && std::abs(GetSelection().Area()) >= 4;
+ const pangolin::XYRangef froi = have_selection ? GetSelection() : GetViewToRender();
+
+ // Download texture so that we can take min / max
+ pangolin::TypedImage img;
+ tex.Download(img);
+ offset_scale = pangolin::GetOffsetScale(img, pangolin::Round(froi), img.fmt);
+ }
+ else if(key == 'b')
+ {
+ if(!tex.IsValid())
+ {
+ std::cerr << "ImageViewHandler does not contain valid texture." << std::endl;
+ return;
+ }
+
+ // compute scale
+ const bool have_selection = std::isfinite(GetSelection().Area()) && std::abs(GetSelection().Area()) >= 4;
+ const pangolin::XYRangef froi = have_selection ? GetSelection() : GetViewToRender();
+
+ // Download texture so that we can take min / max
+ pangolin::TypedImage img;
+ tex.Download(img);
+ std::pair<float, float> mm = pangolin::GetMinMax(img, pangolin::Round(froi), img.fmt);
+
+ printf("Min / Max in Region: %f / %f\n", mm.first, mm.second);
+ }
+ else
+ {
+ pangolin::ImageViewHandler::Keyboard(view, key, x, y, pressed);
+ }
+}
+
+pangolin::GlTexture& ImageView::Tex() {
+ return tex;
+}
+
+ImageView& ImageView::SetImage(void* ptr, size_t w, size_t h, size_t pitch, pangolin::GlPixFormat img_fmt, bool delayed_upload )
+{
+ const size_t pix_bytes =
+ pangolin::GlFormatChannels(img_fmt.glformat) * pangolin::GlDataTypeBytes(img_fmt.gltype);
+
+ const bool convert_first = (img_fmt.gltype == GL_DOUBLE);
+
+ if(delayed_upload || !pangolin::GetBoundWindow() || IsDevicePtr(ptr) || convert_first )
+ {
+ texlock.lock();
+ if(!convert_first) {
+ img_to_load = ManagedImage<unsigned char>(w,h,w*pix_bytes);
+ PitchedCopy((char*)img_to_load.ptr, img_to_load.pitch, (char*)ptr, pitch, w * pix_bytes, h);
+ img_fmt_to_load = img_fmt;
+ }else if(img_fmt.gltype == GL_DOUBLE) {
+ Image<double> double_image( (double*)ptr, w, h, pitch);
+ img_to_load.OwnAndReinterpret(ImageConvert<float>(double_image));
+ img_fmt_to_load = GlPixFormat::FromType<float>();
+ }else{
+ pango_print_warn("TextureView: Unable to display image.\n");
+ }
+ texlock.unlock();
+ return *this;
+ }
+
+ PANGO_ASSERT(pitch % pix_bytes == 0);
+ const size_t stride = pitch / pix_bytes;
+ glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
+ glPixelStorei(GL_UNPACK_ROW_LENGTH, stride);
+
+ // Initialise if it didn't already exist or the size was too small
+ if(!tex.tid || tex.width != (int)w || tex.height != (int)h ||
+ tex.internal_format != img_fmt.scalable_internal_format)
+ {
+ fmt = img_fmt;
+ SetDimensions(w, h);
+ SetAspect((float)w / (float)h);
+ tex.Reinitialise(w, h, img_fmt.scalable_internal_format, true, 0, img_fmt.glformat, img_fmt.gltype, ptr);
+ }
+ else
+ {
+ tex.Upload(ptr, img_fmt.glformat, img_fmt.gltype);
+ }
+
+ glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+ return *this;
+}
+
+ImageView& ImageView::SetImage(const pangolin::Image<unsigned char>& img, const pangolin::GlPixFormat& glfmt, bool delayed_upload )
+{
+ return SetImage(img.ptr, img.w, img.h, img.pitch, glfmt, delayed_upload);
+}
+
+ImageView& ImageView::SetImage(const pangolin::TypedImage& img, bool delayed_upload )
+{
+ return SetImage(img.ptr, img.w, img.h, img.pitch, pangolin::GlPixFormat(img.fmt), delayed_upload);
+}
+
+ImageView& ImageView::SetImage(const pangolin::GlTexture& texture)
+{
+ // Initialise if it didn't already exist or the size was too small
+ if(!tex.tid || tex.width != texture.width || tex.height != texture.height ||
+ tex.internal_format != texture.internal_format)
+ {
+ SetDimensions(texture.width, texture.height);
+ SetAspect((float)texture.width / (float)texture.height);
+ tex.Reinitialise(texture.width, texture.height, texture.internal_format, true);
+ }
+
+ glCopyImageSubData(
+ texture.tid, GL_TEXTURE_2D, 0, 0, 0, 0, tex.tid, GL_TEXTURE_2D, 0, 0, 0, 0, tex.width, tex.height, 1);
+
+ return *this;
+}
+
+void ImageView::LoadPending()
+{
+ if(img_to_load.ptr)
+ {
+ // Scoped lock
+ texlock.lock();
+ SetImage(img_to_load, img_fmt_to_load, false);
+ img_to_load.Deallocate();
+ texlock.unlock();
+ }
+}
+
+ImageView& ImageView::Clear()
+{
+ tex.Delete();
+ return *this;
+}
+
+std::pair<float, float>& ImageView::GetOffsetScale() {
+ return offset_scale;
+}
+
+bool ImageView::MouseReleased() const {
+ return mouseReleased;
+}
+
+bool ImageView::MousePressed() const {
+ return mousePressed;
+}
+
+void ImageView::SetRenderOverlay(const bool& val) {
+ overlayRender = val;
+}
+
+}
diff --git a/externals/Pangolin/src/display/opengl_render_state.cpp b/externals/Pangolin/src/display/opengl_render_state.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cdc8f911ad1e1f44e6835eb238d05d1e0ba2334a
--- /dev/null
+++ b/externals/Pangolin/src/display/opengl_render_state.cpp
@@ -0,0 +1,707 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove, Richard Newcombe
+ *
+ * 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.
+ */
+
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/gl/glinclude.h>
+
+#include <stdexcept>
+
+namespace pangolin
+{
+
+inline void glLoadMatrix(const float* m) { glLoadMatrixf(m); }
+inline void glMultMatrix(const float* m) { glMultMatrixf(m); }
+
+#ifndef HAVE_GLES
+inline void glLoadMatrix(const double* m) { glLoadMatrixd(m); }
+inline void glMultMatrix(const double* m) { glMultMatrixd(m); }
+#endif
+
+OpenGlMatrix OpenGlMatrix::Translate(GLprecision x, GLprecision y, GLprecision z)
+{
+ OpenGlMatrix mat;
+ mat.SetIdentity();
+ mat(0, 3) = x;
+ mat(1, 3) = y;
+ mat(2, 3) = z;
+ return mat;
+}
+
+OpenGlMatrix OpenGlMatrix::Scale(GLprecision x, GLprecision y, GLprecision z)
+{
+ OpenGlMatrix mat;
+ mat.SetIdentity();
+ mat(0, 0) = x;
+ mat(1, 1) = y;
+ mat(2, 2) = z;
+ return mat;
+}
+
+OpenGlMatrix OpenGlMatrix::RotateX(GLprecision theta_rad)
+{
+ OpenGlMatrix mat;
+ mat.SetIdentity();
+ const GLprecision costh = cos(theta_rad);
+ const GLprecision sinth = sin(theta_rad);
+ mat(1, 1) = costh;
+ mat(1, 2) = -sinth;
+ mat(2, 1) = sinth;
+ mat(2, 2) = costh;
+ return mat;
+}
+
+OpenGlMatrix OpenGlMatrix::RotateY(GLprecision theta_rad)
+{
+ OpenGlMatrix mat;
+ mat.SetIdentity();
+ const GLprecision costh = cos(theta_rad);
+ const GLprecision sinth = sin(theta_rad);
+ mat(0, 0) = costh;
+ mat(0, 2) = sinth;
+ mat(2, 0) = -sinth;
+ mat(2, 2) = costh;
+ return mat;
+}
+
+OpenGlMatrix OpenGlMatrix::RotateZ(GLprecision theta_rad)
+{
+ OpenGlMatrix mat;
+ mat.SetIdentity();
+ const GLprecision costh = cos(theta_rad);
+ const GLprecision sinth = sin(theta_rad);
+ mat(0, 0) = costh;
+ mat(0, 1) = -sinth;
+ mat(1, 0) = sinth;
+ mat(1, 1) = costh;
+ return mat;
+}
+
+void OpenGlMatrix::Load() const
+{
+ glLoadMatrix(m);
+}
+
+void OpenGlMatrix::Multiply() const
+{
+ glMultMatrix(m);
+}
+
+void OpenGlMatrix::SetIdentity()
+{
+ m[0] = 1.0f; m[1] = 0.0f; m[2] = 0.0f; m[3] = 0.0f;
+ m[4] = 0.0f; m[5] = 1.0f; m[6] = 0.0f; m[7] = 0.0f;
+ m[8] = 0.0f; m[9] = 0.0f; m[10] = 1.0f; m[11] = 0.0f;
+ m[12] = 0.0f; m[13] = 0.0f; m[14] = 0.0f; m[15] = 1.0f;
+}
+
+OpenGlMatrix OpenGlMatrix::Transpose() const
+{
+ OpenGlMatrix trans;
+ trans.m[0] = m[0]; trans.m[4] = m[1]; trans.m[8] = m[2]; trans.m[12] = m[3];
+ trans.m[1] = m[4]; trans.m[5] = m[5]; trans.m[9] = m[6]; trans.m[13] = m[7];
+ trans.m[2] = m[8]; trans.m[6] = m[9]; trans.m[10] = m[10]; trans.m[14] = m[11];
+ trans.m[3] = m[12]; trans.m[7] = m[13]; trans.m[11] = m[14]; trans.m[15] = m[15];
+ return trans;
+}
+
+OpenGlMatrix OpenGlMatrix::Inverse() const
+{
+ OpenGlMatrix inv;
+ inv.m[0] = m[0]; inv.m[4] = m[1]; inv.m[8] = m[2]; inv.m[12] = -(m[0]*m[12] + m[1]*m[13] + m[2]*m[14]);
+ inv.m[1] = m[4]; inv.m[5] = m[5]; inv.m[9] = m[6]; inv.m[13] = -(m[4]*m[12] + m[5]*m[13] + m[6]*m[14]);
+ inv.m[2] = m[8]; inv.m[6] = m[9]; inv.m[10] = m[10]; inv.m[14] = -(m[8]*m[12] + m[9]*m[13] + m[10]*m[14]);
+ inv.m[3] = 0; inv.m[7] = 0; inv.m[11] = 0; inv.m[15] = 1;
+ return inv;
+}
+
+std::ostream& operator<<(std::ostream& os, const OpenGlMatrix& mat)
+{
+ for(int r=0; r< 4; ++r) {
+ for(int c=0; c<4; ++c) {
+ std::cout << mat.m[4*c+r] << '\t';
+ }
+ std::cout << std::endl;
+ }
+ return os;
+}
+
+void OpenGlRenderState::Apply() const
+{
+ glMatrixMode(GL_PROJECTION);
+ projection[0].Load();
+
+ // Leave in MODEVIEW mode
+ glMatrixMode(GL_MODELVIEW);
+ modelview.Load();
+
+ if(follow) {
+ T_cw.Multiply();
+ }
+}
+
+OpenGlMatrix& OpenGlRenderState::GetProjectionMatrix(unsigned int view)
+{
+ if( projection.size() <= view ) {
+ projection.resize(view+1);
+ }
+ return projection[view];
+}
+
+OpenGlMatrix OpenGlRenderState::GetProjectionMatrix(unsigned int view) const
+{
+ if( projection.size() <= view ) {
+ return IdentityMatrix();
+ }
+ return projection[view];
+}
+
+OpenGlMatrix& OpenGlRenderState::GetViewOffset(unsigned int view)
+{
+ if( modelview_premult.size() <= view ) {
+ modelview_premult.resize(view+1);
+ }
+ return modelview_premult[view];
+}
+
+OpenGlMatrix OpenGlRenderState::GetViewOffset(unsigned int view) const
+{
+ if( modelview_premult.size() <= view ) {
+ return IdentityMatrix();
+ }
+ return modelview_premult[view];
+}
+
+void OpenGlRenderState::ApplyNView(int view) const
+{
+ glMatrixMode(GL_PROJECTION);
+ projection[view].Load();
+
+ // Leave in MODEVIEW mode
+ glMatrixMode(GL_MODELVIEW);
+ OpenGlMatrix m = GetModelViewMatrix(view);
+ m.Load();
+
+ if(follow) {
+ T_cw.Multiply();
+ }
+}
+
+OpenGlRenderState::OpenGlRenderState()
+ : modelview(IdentityMatrix()), follow(false)
+{
+ projection.push_back( IdentityMatrix() );
+}
+
+OpenGlRenderState::OpenGlRenderState(const OpenGlMatrix& projection_matrix)
+ : modelview(IdentityMatrix()), follow(false)
+{
+ projection.push_back( projection_matrix );
+}
+
+OpenGlRenderState::OpenGlRenderState(const OpenGlMatrix& projection_matrix, const OpenGlMatrix& modelview_matrx)
+ : modelview(modelview_matrx), follow(false)
+{
+ projection.push_back( projection_matrix );
+}
+
+void OpenGlRenderState::ApplyIdentity()
+{
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+}
+
+OpenGlRenderState& OpenGlRenderState::SetProjectionMatrix(OpenGlMatrix m)
+{
+ projection[0] = m;
+ return *this;
+}
+
+OpenGlRenderState& OpenGlRenderState::SetModelViewMatrix(OpenGlMatrix m)
+{
+ modelview = m;
+ return *this;
+}
+
+OpenGlRenderState& OpenGlRenderState::Set(OpenGlMatrixSpec m)
+{
+ switch (m.type) {
+ case GlProjectionStack:
+ projection[0] = m;
+ break;
+ case GlModelViewStack:
+ modelview = m;
+ break;
+ default:
+ throw std::runtime_error("Unexpected matrix type");
+ break;
+ }
+ return *this;
+}
+
+OpenGlMatrix operator*(const OpenGlMatrix& lhs, const OpenGlMatrix& rhs)
+{
+ OpenGlMatrix ret;
+ pangolin::MatMul<4,4,4>(ret.m, lhs.m, rhs.m);
+ return ret;
+}
+
+OpenGlMatrix& OpenGlRenderState::GetProjectionMatrix()
+{
+ // guarenteed to have at least one projection matrix element
+ return projection[0];
+}
+
+OpenGlMatrix OpenGlRenderState::GetProjectionMatrix() const
+{
+ // guarenteed to have at least one projection matrix element
+ return projection[0];
+}
+
+OpenGlMatrix& OpenGlRenderState::GetModelViewMatrix()
+{
+ return modelview;
+}
+
+OpenGlMatrix OpenGlRenderState::GetModelViewMatrix() const
+{
+ return modelview;
+}
+
+OpenGlMatrix OpenGlRenderState::GetModelViewMatrix(int i) const
+{
+ return modelview_premult[i] * modelview;
+}
+
+OpenGlMatrix OpenGlRenderState::GetProjectionModelViewMatrix() const
+{
+ return GetProjectionMatrix() * GetModelViewMatrix();
+}
+
+OpenGlMatrix OpenGlRenderState::GetProjectiveTextureMatrix() const
+{
+ return OpenGlMatrix::Translate(0.5,0.5,0.5) * OpenGlMatrix::Scale(0.5,0.5,0.5) * GetProjectionModelViewMatrix();
+}
+
+void OpenGlRenderState::EnableProjectiveTexturing() const
+{
+#ifndef HAVE_GLES
+ const pangolin::OpenGlMatrix projmattrans = GetProjectiveTextureMatrix().Transpose();
+ glEnable(GL_TEXTURE_GEN_S);
+ glEnable(GL_TEXTURE_GEN_T);
+ glEnable(GL_TEXTURE_GEN_R);
+ glEnable(GL_TEXTURE_GEN_Q);
+ glTexGendv(GL_S, GL_EYE_PLANE, projmattrans.m);
+ glTexGendv(GL_T, GL_EYE_PLANE, projmattrans.m+4);
+ glTexGendv(GL_R, GL_EYE_PLANE, projmattrans.m+8);
+ glTexGendv(GL_Q, GL_EYE_PLANE, projmattrans.m+12);
+ glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
+ glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
+ glTexGeni(GL_R, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
+ glTexGeni(GL_Q, GL_TEXTURE_GEN_MODE, GL_EYE_LINEAR);
+#endif
+}
+
+void OpenGlRenderState::DisableProjectiveTexturing() const
+{
+#ifndef HAVE_GLES
+ glDisable(GL_TEXTURE_GEN_S);
+ glDisable(GL_TEXTURE_GEN_T);
+ glDisable(GL_TEXTURE_GEN_R);
+ glDisable(GL_TEXTURE_GEN_Q);
+#endif
+}
+
+void OpenGlRenderState::Follow(const OpenGlMatrix& T_wc, bool follow)
+{
+ this->T_cw = T_wc.Inverse();
+
+ if(follow != this->follow) {
+ if(follow) {
+ const OpenGlMatrix T_vc = GetModelViewMatrix() * T_wc;
+ SetModelViewMatrix(T_vc);
+ this->follow = true;
+ }else{
+ Unfollow();
+ }
+ }
+}
+
+void OpenGlRenderState::Unfollow()
+{
+ const OpenGlMatrix T_vw = GetModelViewMatrix() * T_cw;
+ SetModelViewMatrix(T_vw);
+ this->follow = false;
+}
+
+// Use OpenGl's default frame of reference
+OpenGlMatrixSpec ProjectionMatrix(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ return ProjectionMatrixRUB_BottomLeft(w,h,fu,fv,u0,v0,zNear,zFar);
+}
+
+OpenGlMatrixSpec ProjectionMatrixOrthographic(GLprecision l, GLprecision r, GLprecision b, GLprecision t, GLprecision n, GLprecision f )
+{
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+
+ P.m[0] = 2/(r-l);
+ P.m[1] = 0;
+ P.m[2] = 0;
+ P.m[3] = 0;
+
+ P.m[4] = 0;
+ P.m[5] = 2/(t-b);
+ P.m[6] = 0;
+ P.m[7] = 0;
+
+ P.m[8] = 0;
+ P.m[9] = 0;
+ P.m[10] = -2/(f-n);
+ P.m[11] = 0;
+
+ P.m[12] = -(r+l)/(r-l);
+ P.m[13] = -(t+b)/(t-b);
+ P.m[14] = -(f+n)/(f-n);
+ P.m[15] = 1;
+
+ return P;
+}
+
+
+// Camera Axis:
+// X - Right, Y - Up, Z - Back
+// Image Origin:
+// Bottom Left
+// Caution: Principal point defined with respect to image origin (0,0) at
+// top left of top-left pixel (not center, and in different frame
+// of reference to projection function image)
+OpenGlMatrixSpec ProjectionMatrixRUB_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision L = +(u0) * zNear / -fu;
+ const GLprecision T = +(v0) * zNear / fv;
+ const GLprecision R = -(w-u0) * zNear / -fu;
+ const GLprecision B = -(h-v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+ P.m[2*4+2] = -(zFar +zNear) / (zFar - zNear);
+ P.m[2*4+0] = (R+L)/(R-L);
+ P.m[2*4+1] = (T+B)/(T-B);
+ P.m[2*4+3] = -1.0;
+ P.m[3*4+2] = -(2*zFar*zNear)/(zFar-zNear);
+
+ return P;
+}
+
+// Camera Axis:
+// X - Right, Y - Up, Z - Back
+// Image Origin:
+// Top Left
+// Caution: Principal point defined with respect to image origin (0,0) at
+// top left of top-left pixel (not center, and in different frame
+// of reference to projection function image)
+OpenGlMatrixSpec ProjectionMatrixRUB_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision L = +(u0) * zNear / -fu;
+ const GLprecision R = -(w-u0) * zNear / -fu;
+ const GLprecision T = -(h-v0) * zNear / fv;
+ const GLprecision B = +(v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+ P.m[2*4+2] = -(zFar +zNear) / (zFar - zNear);
+ P.m[2*4+0] = (R+L)/(R-L);
+ P.m[2*4+1] = (T+B)/(T-B);
+ P.m[2*4+3] = -1.0;
+ P.m[3*4+2] = -(2*zFar*zNear)/(zFar-zNear);
+
+ return P;
+}
+
+// Camera Axis:
+// X - Right, Y - Down, Z - Forward
+// Image Origin:
+// Top Left
+// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
+OpenGlMatrixSpec ProjectionMatrixRDF_TopLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision L = -(u0) * zNear / fu;
+ const GLprecision R = +(w-u0) * zNear / fu;
+ const GLprecision T = -(v0) * zNear / fv;
+ const GLprecision B = +(h-v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+
+ P.m[2*4+0] = (R+L)/(L-R);
+ P.m[2*4+1] = (T+B)/(B-T);
+ P.m[2*4+2] = (zFar +zNear) / (zFar - zNear);
+ P.m[2*4+3] = 1.0;
+
+ P.m[3*4+2] = (2*zFar*zNear)/(zNear - zFar);
+ return P;
+}
+
+// Camera Axis:
+// X - Right, Y - Down, Z - Forward
+// Image Origin:
+// Top Right
+// Pricipal point specified with image origin (0,0) at top right of top-right pixel (not center)
+OpenGlMatrixSpec ProjectionMatrixRDF_TopRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision L = +(w-u0) * zNear / fu;
+ const GLprecision R = -(u0) * zNear / fu;
+ const GLprecision T = -(v0) * zNear / fv;
+ const GLprecision B = +(h-v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+
+ P.m[2*4+0] = (R+L)/(L-R);
+ P.m[2*4+1] = (T+B)/(B-T);
+ P.m[2*4+2] = (zFar +zNear) / (zFar - zNear);
+ P.m[2*4+3] = 1.0;
+
+ P.m[3*4+2] = (2*zFar*zNear)/(zNear - zFar);
+ return P;
+}
+
+// Camera Axis:
+// X - Right, Y - Down, Z - Forward
+// Image Origin:
+// Bottom Left
+// Pricipal point specified with image origin (0,0) at top left of top-left pixel (not center)
+OpenGlMatrixSpec ProjectionMatrixRDF_BottomLeft(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision L = -(u0) * zNear / fu;
+ const GLprecision R = +(w-u0) * zNear / fu;
+ const GLprecision B = -(v0) * zNear / fv;
+ const GLprecision T = +(h-v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+
+ P.m[2*4+0] = (R+L)/(L-R);
+ P.m[2*4+1] = (T+B)/(B-T);
+ P.m[2*4+2] = (zFar +zNear) / (zFar - zNear);
+ P.m[2*4+3] = 1.0;
+
+ P.m[3*4+2] = (2*zFar*zNear)/(zNear - zFar);
+ return P;
+}
+
+// Camera Axis:
+// X - Right, Y - Down, Z - Forward
+// Image Origin:
+// Bottom Right
+// Pricipal point specified with image origin (0,0) at top right of top-right pixel (not center)
+OpenGlMatrixSpec ProjectionMatrixRDF_BottomRight(int w, int h, GLprecision fu, GLprecision fv, GLprecision u0, GLprecision v0, GLprecision zNear, GLprecision zFar )
+{
+ // http://www.songho.ca/opengl/gl_projectionmatrix.html
+ const GLprecision R = -(u0) * zNear / fu;
+ const GLprecision L = +(w-u0) * zNear / fu;
+ const GLprecision B = -(v0) * zNear / fv;
+ const GLprecision T = +(h-v0) * zNear / fv;
+
+ OpenGlMatrixSpec P;
+ P.type = GlProjectionStack;
+ std::fill_n(P.m,4*4,0);
+
+ P.m[0*4+0] = 2 * zNear / (R-L);
+ P.m[1*4+1] = 2 * zNear / (T-B);
+
+ P.m[2*4+0] = (R+L)/(L-R);
+ P.m[2*4+1] = (T+B)/(B-T);
+ P.m[2*4+2] = (zFar +zNear) / (zFar - zNear);
+ P.m[2*4+3] = 1.0;
+
+ P.m[3*4+2] = (2*zFar*zNear)/(zNear - zFar);
+ return P;
+}
+
+OpenGlMatrix ModelViewLookAtRUB(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz)
+{
+ OpenGlMatrix mat;
+ GLprecision* m = mat.m;
+
+ const GLprecision u_o[3] = {ux,uy,uz};
+
+ GLprecision x[3], y[3];
+ GLprecision z[] = {ex - lx, ey - ly, ez - lz};
+ Normalise<3>(z);
+
+ CrossProduct(x,u_o,z);
+ CrossProduct(y,z,x);
+
+ // Normalize x, y
+ const GLprecision lenx = Length<3>(x);
+ const GLprecision leny = Length<3>(y);
+
+ if( lenx > 0 && leny > 0) {
+ for(size_t r = 0; r < 3; ++r ) {
+ x[r] /= lenx;
+ y[r] /= leny;
+ }
+ #define M(row,col) m[col*4+row]
+ M(0,0) = x[0];
+ M(0,1) = x[1];
+ M(0,2) = x[2];
+ M(1,0) = y[0];
+ M(1,1) = y[1];
+ M(1,2) = y[2];
+ M(2,0) = z[0];
+ M(2,1) = z[1];
+ M(2,2) = z[2];
+ M(3,0) = 0.0;
+ M(3,1) = 0.0;
+ M(3,2) = 0.0;
+ M(0,3) = -(M(0,0)*ex + M(0,1)*ey + M(0,2)*ez);
+ M(1,3) = -(M(1,0)*ex + M(1,1)*ey + M(1,2)*ez);
+ M(2,3) = -(M(2,0)*ex + M(2,1)*ey + M(2,2)*ez);
+ M(3,3) = 1.0;
+ #undef M
+ return mat;
+ }else{
+ throw std::invalid_argument("'Look' and 'up' vectors cannot be parallel when calling ModelViewLookAt.");
+ }
+
+}
+
+OpenGlMatrix ModelViewLookAtRDF(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz)
+{
+ OpenGlMatrix mat;
+ GLprecision* m = mat.m;
+
+ const GLprecision u_o[3] = {ux,uy,uz};
+
+ GLprecision x[3], y[3];
+ GLprecision z[] = {lx - ex, ly - ey, lz - ez};
+ Normalise<3>(z);
+
+ CrossProduct(x,z,u_o);
+ CrossProduct(y,z,x);
+
+ // Normalize x, y
+ const GLprecision lenx = Length<3>(x);
+ const GLprecision leny = Length<3>(y);
+
+ if( lenx > 0 && leny > 0) {
+ for(size_t r = 0; r < 3; ++r ) {
+ x[r] /= lenx;
+ y[r] /= leny;
+ }
+ #define M(row,col) m[col*4+row]
+ M(0,0) = x[0];
+ M(0,1) = x[1];
+ M(0,2) = x[2];
+ M(1,0) = y[0];
+ M(1,1) = y[1];
+ M(1,2) = y[2];
+ M(2,0) = z[0];
+ M(2,1) = z[1];
+ M(2,2) = z[2];
+ M(3,0) = 0.0;
+ M(3,1) = 0.0;
+ M(3,2) = 0.0;
+ M(0,3) = -(M(0,0)*ex + M(0,1)*ey + M(0,2)*ez);
+ M(1,3) = -(M(1,0)*ex + M(1,1)*ey + M(1,2)*ez);
+ M(2,3) = -(M(2,0)*ex + M(2,1)*ey + M(2,2)*ez);
+ M(3,3) = 1.0;
+ #undef M
+ return mat;
+ }else{
+ throw std::invalid_argument("'Look' and 'up' vectors cannot be parallel when calling ModelViewLookAt.");
+ }
+}
+
+OpenGlMatrix ModelViewLookAt(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, GLprecision ux, GLprecision uy, GLprecision uz)
+{
+ return ModelViewLookAtRUB(ex,ey,ez,lx,ly,lz,ux,uy,uz);
+}
+
+OpenGlMatrix ModelViewLookAt(GLprecision ex, GLprecision ey, GLprecision ez, GLprecision lx, GLprecision ly, GLprecision lz, AxisDirection up)
+{
+ const GLprecision* u = AxisDirectionVector[up];
+ return ModelViewLookAtRUB(ex,ey,ez,lx,ly,lz,u[0],u[1],u[2]);
+}
+
+OpenGlMatrix IdentityMatrix()
+{
+ OpenGlMatrix P;
+ std::fill_n(P.m,4*4,0);
+ for( int i=0; i<4; ++i ) P.m[i*4+i] = 1;
+ return P;
+}
+
+OpenGlMatrixSpec IdentityMatrix(OpenGlStack type)
+{
+ OpenGlMatrixSpec P;
+ P.type = type;
+ std::fill_n(P.m,4*4,0);
+ for( int i=0; i<4; ++i ) P.m[i*4+i] = 1;
+ return P;
+}
+
+OpenGlMatrixSpec negIdentityMatrix(OpenGlStack type)
+{
+ OpenGlMatrixSpec P;
+ P.type = type;
+ std::fill_n(P.m,4*4,0);
+ for( int i=0; i<4; ++i ) P.m[i*4+i] = -1;
+
+ P.m[3*4+3] =1;
+ return P;
+}
+
+}
diff --git a/externals/Pangolin/src/display/view.cpp b/externals/Pangolin/src/display/view.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5dbee396e90226767ddbbe8b3e4384d8efa877da
--- /dev/null
+++ b/externals/Pangolin/src/display/view.cpp
@@ -0,0 +1,582 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/display/opengl_render_state.h>
+#include <pangolin/display/view.h>
+#include <pangolin/display/viewport.h>
+#include <pangolin/gl/gl.h>
+#include <pangolin/platform.h>
+
+#include <stdexcept>
+
+namespace pangolin
+{
+
+// Pointer to context defined in display.cpp
+extern __thread PangolinGl* context;
+
+const int panal_v_margin = 6;
+
+int AttachAbs( int low, int high, Attach a)
+{
+ if( a.unit == Pixel ) return low + (int)a.p;
+ if( a.unit == ReversePixel ) return high - (int)a.p;
+ return (int)(low + a.p * (high - low));
+}
+
+double AspectAreaWithinTarget(double target, double test)
+{
+ if( test < target )
+ return test / target;
+ else
+ return target / test;
+}
+
+void SaveViewFromFbo(std::string prefix, View& view, float scale)
+{
+ PANGOLIN_UNUSED(prefix);
+
+#ifndef HAVE_GLES
+ const Viewport orig = view.v;
+ view.v.l = 0;
+ view.v.b = 0;
+ view.v.w = (int)(view.v.w * scale);
+ view.v.h = (int)(view.v.h * scale);
+
+ const int w = view.v.w;
+ const int h = view.v.h;
+
+ float origLineWidth;
+ glGetFloatv(GL_LINE_WIDTH, &origLineWidth);
+ glLineWidth(origLineWidth * scale);
+
+ float origPointSize;
+ glGetFloatv(GL_POINT_SIZE, &origPointSize);
+ glPointSize(origPointSize * scale);
+
+ // Create FBO
+ GlTexture color(w,h);
+ GlRenderBuffer depth(w,h);
+ GlFramebuffer fbo(color, depth);
+
+ // Render into FBO
+ fbo.Bind();
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ view.Render();
+ glFlush();
+
+#ifdef HAVE_PNG
+ const PixelFormat fmt = PixelFormatFromString("RGBA32");
+ TypedImage buffer(w, h, fmt );
+ glReadBuffer(GL_BACK);
+ glPixelStorei(GL_PACK_ALIGNMENT, 1); // TODO: Avoid this?
+ glReadPixels(0,0,w,h, GL_RGBA, GL_UNSIGNED_BYTE, buffer.ptr );
+ SaveImage(buffer, fmt, prefix + ".png", false);
+#endif // HAVE_PNG
+
+ // unbind FBO
+ fbo.Unbind();
+
+ // restore viewport / line width
+ view.v = orig;
+ glLineWidth(origLineWidth);
+ glPointSize(origPointSize);
+#endif // HAVE_GLES
+}
+
+void View::Resize(const Viewport& p)
+{
+ // Compute Bounds based on specification
+ v.l = AttachAbs(p.l,p.r(),left);
+ v.b = AttachAbs(p.b,p.t(),bottom);
+ int r = AttachAbs(p.l,p.r(),right);
+ int t = AttachAbs(p.b,p.t(),top);
+
+ // Make sure left and right, top and bottom are correct order
+ if( t < v.b ) std::swap(t,v.b);
+ if( r < v.l ) std::swap(r,v.l);
+
+ v.w = r - v.l;
+ v.h = t - v.b;
+
+ vp = v;
+
+ // Adjust based on aspect requirements
+ if( aspect != 0 )
+ {
+ const float current_aspect = (float)v.w / (float)v.h;
+ if( aspect > 0 )
+ {
+ // Fit to space
+ if( current_aspect < aspect )
+ {
+ //Adjust height
+ const int nh = (int)(v.w / aspect);
+ v.b += vlock == LockBottom ? 0 : (vlock == LockCenter ? (v.h-nh)/2 : (v.h-nh) );
+ v.h = nh;
+ }else if( current_aspect > aspect )
+ {
+ //Adjust width
+ const int nw = (int)(v.h * aspect);
+ v.l += hlock == LockLeft? 0 : (hlock == LockCenter ? (v.w-nw)/2 : (v.w-nw) );
+ v.w = nw;
+ }
+ }else{
+ // Overfit
+ double true_aspect = -aspect;
+ if( current_aspect < true_aspect )
+ {
+ //Adjust width
+ const int nw = (int)(v.h * true_aspect);
+ v.l += hlock == LockLeft? 0 : (hlock == LockCenter ? (v.w-nw)/2 : (v.w-nw) );
+ v.w = nw;
+ }else if( current_aspect > true_aspect )
+ {
+ //Adjust height
+ const int nh = (int)(v.w / true_aspect);
+ v.b += vlock == LockBottom ? 0 : (vlock == LockCenter ? (v.h-nh)/2 : (v.h-nh) );
+ v.h = nh;
+ }
+ }
+ }
+
+ ResizeChildren();
+}
+
+inline int zcompare(const View* lhs, const View* rhs)
+{
+ return lhs->zorder < rhs->zorder;
+}
+
+void View::ResizeChildren()
+{
+ if( layout == LayoutOverlay )
+ {
+ // Sort children into z-order
+ std::sort(views.begin(), views.end(), zcompare);
+
+ for(std::vector<View*>::iterator iv = views.begin(); iv != views.end(); ++iv ) {
+ (*iv)->Resize(v);
+ }
+ }else if( layout == LayoutVertical )
+ {
+ // Allocate space incrementally
+ Viewport space = v.Inset(panal_v_margin);
+ int num_children = 0;
+ for(std::vector<View*>::iterator iv = views.begin(); iv != views.end(); ++iv )
+ {
+ num_children++;
+ if(scroll_offset > num_children ) {
+ (*iv)->show = false;
+ }else{
+ (*iv)->show = true;
+ (*iv)->Resize(space);
+ space.h = (*iv)->v.b - panal_v_margin - space.b;
+ }
+ }
+ }else if(layout == LayoutHorizontal )
+ {
+ // Allocate space incrementally
+ const int margin = 8;
+ Viewport space = v.Inset(margin);
+ for(std::vector<View*>::iterator iv = views.begin(); iv != views.end(); ++iv )
+ {
+ (*iv)->Resize(space);
+ space.w = (*iv)->v.l + margin + space.l;
+ }
+ }else if(layout == LayoutEqualVertical )
+ {
+ // Allocate vertical space equally
+ const size_t visiblechildren = NumVisibleChildren();
+ const float height = (float)v.h / (float)visiblechildren;
+
+ for( size_t i=0; i < visiblechildren; ++i) {
+ Viewport space(v.l, (GLint)(v.b+(visiblechildren-1-i)*height), v.w, (GLint)(height) );
+ VisibleChild(i).Resize(space);
+ }
+ }else if(layout == LayoutEqualHorizontal )
+ {
+ // Allocate vertical space equally
+ const size_t visiblechildren = NumVisibleChildren();
+ const float width = (float)v.w / (float)visiblechildren;
+
+ for( size_t i=0; i < visiblechildren; ++i) {
+ Viewport space( (GLint)(v.l+i*width), v.b, (GLint)width, v.h);
+ VisibleChild(i).Resize(space);
+ }
+ }else if(layout == LayoutEqual )
+ {
+ const size_t visiblechildren = NumVisibleChildren();
+ // TODO: Make this neater, and make fewer assumptions!
+ if( visiblechildren > 0 )
+ {
+ // This containers aspect
+ const double this_a = std::fabs(v.aspect());
+
+ // Use first child with fixed aspect for all children
+ double child_a = std::fabs(VisibleChild(0).aspect);
+ for(size_t i=1; (child_a==0) && i < visiblechildren; ++i ) {
+ child_a = std::fabs(VisibleChild(i).aspect);
+ }
+
+ if(child_a == 0) {
+ child_a = 1;
+ }
+
+ double a = visiblechildren*child_a;
+ double area = AspectAreaWithinTarget(this_a, a);
+
+ size_t cols = visiblechildren-1;
+ for(; cols > 0; --cols)
+ {
+ const size_t rows = visiblechildren / cols + (visiblechildren % cols == 0 ? 0 : 1);
+ const double na = cols * child_a / rows;
+ const double new_area = visiblechildren*AspectAreaWithinTarget(this_a,na)/(rows*cols);
+ if( new_area <= area )
+ break;
+ area = new_area;
+ a = na;
+ }
+
+ cols++;
+ const size_t rows = visiblechildren / cols + (visiblechildren % cols == 0 ? 0 : 1);
+ size_t cw, ch;
+ if( a > this_a )
+ {
+ cw = v.w / cols;
+ ch = (int)(cw / child_a); //v.h / rows;
+ }else{
+ ch = v.h / rows;
+ cw = (int)(ch * child_a);
+ }
+
+ for(size_t i=0; i< visiblechildren; ++i )
+ {
+ size_t c = i % cols;
+ size_t r = i / cols;
+ Viewport space( GLint(v.l + c*cw), GLint(v.t() - (r+1)*ch), GLint(cw), GLint(ch) );
+ VisibleChild(i).Resize(space);
+ }
+ }
+ }
+
+}
+
+void View::Render()
+{
+ if(extern_draw_function && show) {
+ extern_draw_function(*this);
+ }
+ RenderChildren();
+}
+
+void View::RenderChildren()
+{
+ for(std::vector<View*>::iterator iv = views.begin(); iv != views.end(); ++iv )
+ {
+ if((*iv)->show) (*iv)->Render();
+ }
+}
+
+void View::Activate() const
+{
+ v.Activate();
+}
+
+void View::ActivateAndScissor() const
+{
+ vp.Scissor();
+ v.Activate();
+}
+
+void View::ActivateScissorAndClear() const
+{
+ vp.Scissor();
+ v.Activate();
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+}
+
+void View::Activate(const OpenGlRenderState& state ) const
+{
+ v.Activate();
+ state.Apply();
+}
+
+void View::ActivateAndScissor(const OpenGlRenderState& state) const
+{
+ vp.Scissor();
+ v.Activate();
+ state.Apply();
+}
+
+void View::ActivateScissorAndClear(const OpenGlRenderState& state ) const
+{
+ vp.Scissor();
+ v.Activate();
+ state.Apply();
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+}
+
+void View::ActivatePixelOrthographic() const
+{
+ v.ActivatePixelOrthographic();
+}
+
+void View::ActivateIdentity() const
+{
+ v.ActivateIdentity();
+}
+
+GLfloat View::GetClosestDepth(int x, int y, int radius) const
+{
+ // TODO: Get to work on android
+
+#ifdef _MSVC_
+ // MSVC Requires fixed sized arrays on stack
+ radius = 5;
+ const int zl = (5*2+1);
+#else
+ const int zl = (radius*2+1);
+#endif
+
+ const int zsize = zl*zl;
+ GLfloat zs[zsize];
+
+#ifndef HAVE_GLES
+ glReadBuffer(GL_FRONT);
+ glReadPixels(x-radius,y-radius,zl,zl,GL_DEPTH_COMPONENT,GL_FLOAT,zs);
+#else
+ std::fill(zs,zs+zsize, 0.8);
+#endif
+
+ const GLfloat mindepth = *(std::min_element(zs,zs+zsize));
+ return mindepth;
+}
+
+void View::GetObjectCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const
+{
+ const GLint viewport[4] = {v.l,v.b,v.w,v.h};
+ const OpenGlMatrix proj = cam_state.GetProjectionMatrix();
+ const OpenGlMatrix mv = cam_state.GetModelViewMatrix();
+ glUnProject(winx, winy, winzdepth, mv.m, proj.m, viewport, &x, &y, &z);
+}
+
+void View::GetCamCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const
+{
+ v.GetCamCoordinates(cam_state, winx, winy, winzdepth, x, y, z);
+}
+
+View& View::SetFocus()
+{
+ context->activeDisplay = this;
+ return *this;
+}
+
+bool View::HasFocus() const
+{
+ return context->activeDisplay == this;
+}
+
+View& View::SetBounds(Attach bottom, Attach top, Attach left, Attach right)
+{
+ this->left = left;
+ this->top = top;
+ this->right = right;
+ this->bottom = bottom;
+ context->base.ResizeChildren();
+ return *this;
+}
+
+View& View::SetBounds(Attach bottom, Attach top, Attach left, Attach right, bool keep_aspect)
+{
+ aspect = keep_aspect ? v.aspect() : 0;
+ SetBounds(top,bottom,left,right);
+ return *this;
+}
+
+View& View::SetBounds(Attach bottom, Attach top, Attach left, Attach right, double aspect)
+{
+ this->aspect = aspect;
+ SetBounds(top,bottom,left,right);
+ return *this;
+}
+
+View& View::SetAspect(double aspect)
+{
+ this->aspect = aspect;
+ context->base.ResizeChildren();
+ return *this;
+}
+
+View& View::SetLock(Lock horizontal, Lock vertical )
+{
+ vlock = vertical;
+ hlock = horizontal;
+ return *this;
+}
+
+View& View::SetLayout(Layout l)
+{
+ layout = l;
+ return *this;
+}
+
+
+View& View::AddDisplay(View& child)
+{
+ // detach child from any other view, and add to this
+ std::vector<View*>::iterator f = std::find(
+ context->base.views.begin(), context->base.views.end(), &child
+ );
+
+ if( f != context->base.views.end() )
+ context->base.views.erase(f);
+
+ views.push_back(&child);
+ context->base.ResizeChildren();
+ return *this;
+}
+
+View& View::Show(bool show)
+{
+ this->show = show;
+ context->base.ResizeChildren();
+ return *this;
+}
+
+void View::ToggleShow()
+{
+ Show(!show);
+}
+
+bool View::IsShown() const
+{
+ return show;
+}
+
+Viewport View::GetBounds() const
+{
+ return Viewport( std::max(v.l, vp.l), std::max(v.b, vp.b), std::min(v.w, vp.w), std::min(v.h, vp.h) );
+}
+
+void View::SaveOnRender(const std::string& filename_prefix)
+{
+ const Viewport tosave = this->v.Intersect(this->vp);
+ context->screen_capture.push(std::pair<std::string,Viewport>(filename_prefix,tosave ) );
+}
+
+void View::RecordOnRender(const std::string& record_uri)
+{
+ PANGOLIN_UNUSED(record_uri);
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ if(!context->recorder.IsOpen()) {
+ Viewport area = GetBounds();
+ context->record_view = this;
+ try{
+ context->recorder.Open(record_uri);
+ std::vector<StreamInfo> streams;
+ const PixelFormat fmt = PixelFormatFromString("RGB24");
+ streams.push_back( StreamInfo(fmt, area.w, area.h, area.w * fmt.bpp / 8) );
+ context->recorder.SetStreams(streams);
+ }catch(const std::exception& e) {
+ pango_print_error("Unable to open VideoRecorder:\n\t%s\n", e.what());
+ }
+ }else{
+ context->recorder.Close();
+ }
+#else
+ std::cerr << "Error: Video Support hasn't been built into this library." << std::endl;
+#endif // BUILD_PANGOLIN_VIDEO
+}
+
+void View::SaveRenderNow(const std::string& filename_prefix, float scale)
+{
+ SaveViewFromFbo(filename_prefix, *this, scale);
+}
+
+View& View::operator[](size_t i)
+{
+ return *views[i];
+}
+
+size_t View::NumChildren() const
+{
+ return views.size();
+}
+
+size_t View::NumVisibleChildren() const
+{
+ int numvis = 0;
+ for(std::vector<View*>::const_iterator i=views.begin(); i!=views.end(); ++i)
+ {
+ if((*i)->show) {
+ numvis++;
+ }
+ }
+ return numvis;
+}
+
+View& View::VisibleChild(size_t i)
+{
+ size_t numvis = 0;
+ for(size_t v=0; v < views.size(); ++v ) {
+ if(views[v]->show) {
+ if( i == numvis ) {
+ return *views[v];
+ }
+ numvis++;
+ }
+ }
+ // Shouldn't get here
+ throw std::out_of_range("No such child.");
+}
+
+View* View::FindChild(int x, int y)
+{
+ // Find in reverse order to mirror draw order
+ for( std::vector<View*>::const_reverse_iterator i = views.rbegin(); i != views.rend(); ++i )
+ if( (*i)->show && (*i)->GetBounds().Contains(x,y) )
+ return (*i);
+ return 0;
+}
+
+View& View::SetHandler(Handler* h)
+{
+ handler = h;
+ return *this;
+}
+
+View& View::SetDrawFunction(const std::function<void(View&)>& drawFunc)
+{
+ extern_draw_function = drawFunc;
+ return *this;
+}
+
+}
diff --git a/externals/Pangolin/src/display/viewport.cpp b/externals/Pangolin/src/display/viewport.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d8ac8c16a246bb94c953102d16f836e4fbb11fe2
--- /dev/null
+++ b/externals/Pangolin/src/display/viewport.cpp
@@ -0,0 +1,113 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/display/viewport.h>
+#include <algorithm>
+#include <pangolin/utils/simple_math.h>
+
+namespace pangolin {
+
+void Viewport::Activate() const
+{
+ glViewport(l,b,w,h);
+}
+
+void Viewport::Scissor() const
+{
+ glEnable(GL_SCISSOR_TEST);
+ glScissor(l,b,w,h);
+}
+
+void Viewport::ActivateAndScissor() const
+{
+ glViewport(l,b,w,h);
+ glEnable(GL_SCISSOR_TEST);
+ glScissor(l,b,w,h);
+}
+
+
+void Viewport::DisableScissor()
+{
+ glDisable(GL_SCISSOR_TEST);
+}
+
+bool Viewport::Contains(int x, int y) const
+{
+ return l <= x && x < (l+w) && b <= y && y < (b+h);
+}
+
+void Viewport::ActivatePixelOrthographic() const
+{
+ Activate();
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(-0.5, w-0.5, -0.5, h-0.5, -1, 1);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+}
+
+void Viewport::ActivateIdentity() const
+{
+ Activate();
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+}
+
+
+Viewport Viewport::Inset(int i) const
+{
+ return Viewport(l+i, b+i, w-2*i, h-2*i);
+}
+
+Viewport Viewport::Inset(int horiz, int vert) const
+{
+ return Viewport(l+horiz, b+vert, w-horiz, h-vert);
+}
+
+Viewport Viewport::Intersect(const Viewport& vp) const
+{
+ GLint nl = std::max(l,vp.l);
+ GLint nr = std::min(r(),vp.r());
+ GLint nb = std::max(b,vp.b);
+ GLint nt = std::min(t(),vp.t());
+ return Viewport(nl,nb, nr-nl, nt-nb);
+}
+
+void Viewport::GetCamCoordinates(const OpenGlRenderState& cam_state, double winx, double winy, double winzdepth, GLdouble& x, GLdouble& y, GLdouble& z) const
+{
+ const GLint viewport[4] = {l, b, w, h};
+ const OpenGlMatrix proj = cam_state.GetProjectionMatrix();
+#ifndef HAVE_GLES
+ glUnProject(winx, winy, winzdepth, Identity4d, proj.m, viewport, &x, &y, &z);
+#else
+ glUnProject(winx, winy, winzdepth, Identity4f, proj.m, viewport, &x, &y, &z);
+#endif
+}
+
+}
diff --git a/externals/Pangolin/src/display/widgets/widgets.cpp b/externals/Pangolin/src/display/widgets/widgets.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c9a8a82fcd2e8c01af47c1dc5faa1c525c3eb2db
--- /dev/null
+++ b/externals/Pangolin/src/display/widgets/widgets.cpp
@@ -0,0 +1,680 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/display/widgets/widgets.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/var/varextra.h>
+#include <pangolin/utils/file_utils.h>
+
+#include <thread>
+#include <mutex>
+#include <iostream>
+#include <iomanip>
+
+using namespace std;
+
+namespace pangolin
+{
+
+// Pointer to context defined in display.cpp
+extern __thread PangolinGl* context;
+
+
+const static GLfloat colour_s1[4] = {0.2f, 0.2f, 0.2f, 1.0f};
+const static GLfloat colour_s2[4] = {0.6f, 0.6f, 0.6f, 1.0f};
+const static GLfloat colour_bg[4] = {0.9f, 0.9f, 0.9f, 1.0f};
+const static GLfloat colour_fg[4] = {1.0f, 1.0f, 1.0f, 1.0f};
+const static GLfloat colour_tx[4] = {0.0f, 0.0f, 0.0f, 1.0f};
+const static GLfloat colour_dn[4] = {1.0f, 0.7f, 0.7f, 1.0f};
+
+static inline GlFont& font()
+{
+ return GlFont::I();
+}
+
+static inline int cb_height()
+{
+ return (int)(font().Height() * 1.0);
+}
+
+static inline int tab_h()
+{
+ return (int)(font().Height() * 1.4);
+}
+
+std::mutex display_mutex;
+
+template<typename T>
+void GuiVarChanged( Var<T>& var)
+{
+ VarState::I().FlagVarChanged();
+ var.Meta().gui_changed = true;
+
+ for(std::vector<GuiVarChangedCallback>::iterator igvc = VarState::I().gui_var_changed_callbacks.begin(); igvc != VarState::I().gui_var_changed_callbacks.end(); ++igvc) {
+ if( StartsWith(var.Meta().full_name, igvc->filter) ) {
+ igvc->fn( igvc->data, var.Meta().full_name, var.Ref() );
+ }
+ }
+}
+
+void glRect(Viewport v)
+{
+ GLfloat vs[] = { (float)v.l,(float)v.b,
+ (float)v.l,(float)v.t(),
+ (float)v.r(),(float)v.t(),
+ (float)v.r(),(float)v.b };
+
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, vs);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisableClientState(GL_VERTEX_ARRAY);
+}
+
+void glRect(Viewport v, int inset)
+{
+ glRect(v.Inset(inset));
+}
+
+void DrawShadowRect(Viewport& v)
+{
+ glColor4fv(colour_s2);
+ glDrawRectPerimeter((GLfloat)v.l, (GLfloat)v.b, (GLfloat)v.r(), (GLfloat)v.t());
+}
+
+void DrawShadowRect(Viewport& v, bool pushed)
+{
+ const GLfloat* c1 = pushed ? colour_s1 : colour_s2;
+ const GLfloat* c2 = pushed ? colour_s2 : colour_s1;
+
+ GLfloat vs[] = { (float)v.l,(float)v.b,
+ (float)v.l,(float)v.t(),
+ (float)v.r(),(float)v.t(),
+ (float)v.r(),(float)v.b,
+ (float)v.l,(float)v.b };
+
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, vs);
+ glColor4fv(c1);
+ glDrawArrays(GL_LINE_STRIP, 0, 3);
+
+ glColor4fv(c2);
+ glDrawArrays(GL_LINE_STRIP, 2, 3);
+ glDisableClientState(GL_VERTEX_ARRAY);
+
+}
+
+Panel::Panel()
+{
+ handler = &StaticHandlerScroll;
+ layout = LayoutVertical;
+}
+
+Panel::Panel(const std::string& auto_register_var_prefix)
+{
+ handler = &StaticHandlerScroll;
+ layout = LayoutVertical;
+ RegisterNewVarCallback(&Panel::AddVariable,(void*)this,auto_register_var_prefix);
+ ProcessHistoricCallbacks(&Panel::AddVariable,(void*)this,auto_register_var_prefix);
+}
+
+void Panel::AddVariable(void* data, const std::string& name, VarValueGeneric& var, bool /*brand_new*/)
+{
+ Panel* thisptr = (Panel*)data;
+
+ const string& title = var.Meta().friendly;
+
+ display_mutex.lock();
+
+ ViewMap::iterator pnl = context->named_managed_views.find(name);
+
+ // Only add if a widget by the same name doesn't
+ // already exist
+ if( pnl == context->named_managed_views.end() )
+ {
+ View* nv = NULL;
+ if( !strcmp(var.TypeId(), typeid(bool).name()) ) {
+ nv = (var.Meta().flags & META_FLAG_TOGGLE) ? (View*)new Checkbox(title,var) : (View*)new Button(title,var);
+ } else if (!strcmp(var.TypeId(), typeid(double).name()) ||
+ !strcmp(var.TypeId(), typeid(float).name()) ||
+ !strcmp(var.TypeId(), typeid(int).name()) ||
+ !strcmp(var.TypeId(), typeid(unsigned int).name()))
+ {
+ nv = new Slider(title, var);
+ } else if (!strcmp(var.TypeId(), typeid(std::function<void(void)>).name() ) ) {
+ nv = (View*)new FunctionButton(title, var);
+ }else{
+ nv = new TextInput(title,var);
+ }
+ if(nv) {
+ context->named_managed_views[name] = nv;
+ thisptr->views.push_back( nv );
+ thisptr->ResizeChildren();
+ }
+ }
+
+ display_mutex.unlock();
+}
+
+void Panel::Render()
+{
+#ifndef HAVE_GLES
+ glPushAttrib(GL_CURRENT_BIT | GL_ENABLE_BIT | GL_DEPTH_BUFFER_BIT | GL_SCISSOR_BIT | GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_TRANSFORM_BIT);
+#endif
+ glEnable (GL_BLEND);
+ glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ DisplayBase().ActivatePixelOrthographic();
+ glDisable(GL_DEPTH_TEST);
+ glDisable(GL_LIGHTING);
+ glDisable(GL_SCISSOR_TEST);
+ glDisable(GL_LINE_SMOOTH);
+ glDisable( GL_COLOR_MATERIAL );
+ glLineWidth(1.0);
+
+ glColor4fv(colour_bg);
+ glRect(v);
+ DrawShadowRect(v);
+
+ RenderChildren();
+
+#ifndef HAVE_GLES
+ glPopAttrib();
+#else
+ glEnable(GL_LINE_SMOOTH);
+ glEnable(GL_DEPTH_TEST);
+#endif
+}
+
+void Panel::ResizeChildren()
+{
+ View::ResizeChildren();
+}
+
+
+View& CreatePanel(const std::string& name)
+{
+ if(context->named_managed_views.find(name) != context->named_managed_views.end()) {
+ throw std::runtime_error("Panel already registered with this name.");
+ }
+ Panel * p = new Panel(name);
+ context->named_managed_views[name] = p;
+ context->base.views.push_back(p);
+ return *p;
+}
+
+Button::Button(string title, VarValueGeneric& tv)
+ : Widget<bool>(title,tv), down(false)
+{
+ top = 1.0; bottom = Attach::Pix(-tab_h());
+ left = 0.0; right = 1.0;
+ hlock = LockLeft;
+ vlock = LockBottom;
+ gltext = font().Text(title);
+}
+
+void Button::Mouse(View&, MouseButton button, int /*x*/, int /*y*/, bool pressed, int /*mouse_state*/)
+{
+ if(button == MouseButtonLeft )
+ {
+ down = pressed;
+ if( !pressed ) {
+ var->Set(!var->Get());
+ GuiVarChanged(*this);
+ }
+ }
+}
+
+void Button::Render()
+{
+ glColor4fv(colour_fg );
+ glRect(v);
+ glColor4fv(colour_tx);
+ gltext.DrawWindow(raster[0],raster[1]-down);
+ DrawShadowRect(v, down);
+}
+
+void Button::ResizeChildren()
+{
+ raster[0] = floor(v.l + (v.w-gltext.Width())/2.0f);
+ raster[1] = floor(v.b + (v.h-gltext.Height())/2.0f);
+}
+
+FunctionButton::FunctionButton(string title, VarValueGeneric& tv)
+ : Widget<std::function<void(void)> >(title, tv), down(false)
+{
+ top = 1.0; bottom = Attach::Pix(-tab_h());
+ left = 0.0; right = 1.0;
+ hlock = LockLeft;
+ vlock = LockBottom;
+ gltext = font().Text(title);
+}
+
+void FunctionButton::Mouse(View&, MouseButton button, int /*x*/, int /*y*/, bool pressed, int /*mouse_state*/)
+{
+ if (button == MouseButtonLeft)
+ {
+ down = pressed;
+ if (!pressed) {
+ var->Get()();
+ GuiVarChanged(*this);
+ }
+ }
+}
+
+void FunctionButton::Render()
+{
+ glColor4fv(colour_fg);
+ glRect(v);
+ glColor4fv(colour_tx);
+ gltext.DrawWindow(raster[0],raster[1]-down);
+ DrawShadowRect(v, down);
+}
+
+void FunctionButton::ResizeChildren()
+{
+ raster[0] = v.l + (v.w - gltext.Width()) / 2.0f;
+ raster[1] = v.b + (v.h - gltext.Height()) / 2.0f;
+}
+
+Checkbox::Checkbox(std::string title, VarValueGeneric& tv)
+ : Widget<bool>(title,tv)
+{
+ top = 1.0; bottom = Attach::Pix(-tab_h());
+ left = 0.0; right = 1.0;
+ hlock = LockLeft;
+ vlock = LockBottom;
+ handler = this;
+ gltext = font().Text(title);
+}
+
+void Checkbox::Mouse(View&, MouseButton button, int /*x*/, int /*y*/, bool pressed, int /*mouse_state*/)
+{
+ if( button == MouseButtonLeft && pressed ) {
+ var->Set(!var->Get());
+ GuiVarChanged(*this);
+ }
+}
+
+void Checkbox::ResizeChildren()
+{
+ raster[0] = v.l + cb_height() + 4.0f;
+ raster[1] = v.b + (v.h-gltext.Height())/2.0f;
+ const int h = v.h;
+ const int t = (int)((h-cb_height()) / 2.0f);
+ vcb = Viewport(v.l,v.b+t,cb_height(),cb_height());
+}
+
+void Checkbox::Render()
+{
+ const bool val = var->Get();
+
+ if( val )
+ {
+ glColor4fv(colour_dn);
+ glRect(vcb);
+ }
+ glColor4fv(colour_tx);
+ gltext.DrawWindow(raster[0],raster[1]);
+ DrawShadowRect(vcb, val);
+}
+
+inline bool IsIntegral(const char* typeidname)
+{
+ // TODO: There must be a better way of doing this...
+ return !strcmp(typeidname, typeid(char).name()) ||
+ !strcmp(typeidname, typeid(short).name()) ||
+ !strcmp(typeidname, typeid(int).name()) ||
+ !strcmp(typeidname, typeid(long).name()) ||
+ !strcmp(typeidname, typeid(unsigned char).name()) ||
+ !strcmp(typeidname, typeid(unsigned short).name()) ||
+ !strcmp(typeidname, typeid(unsigned int).name()) ||
+ !strcmp(typeidname, typeid(unsigned long).name());
+}
+
+Slider::Slider(std::string title, VarValueGeneric& tv)
+ : Widget<double>(title+":", tv), lock_bounds(true)
+{
+ top = 1.0; bottom = Attach::Pix(-tab_h());
+ left = 0.0; right = 1.0;
+ hlock = LockLeft;
+ vlock = LockBottom;
+ handler = this;
+ logscale = (int)tv.Meta().logscale;
+ gltext = font().Text(title);
+ is_integral_type = IsIntegral(tv.TypeId());
+}
+
+void Slider::Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed)
+{
+ if( pressed && var->Meta().range[0] < var->Meta().range[1] )
+ {
+ double val = !logscale ? var->Get() : log(var->Get());
+
+ if(key=='-' || key=='_' || key=='=' || key=='+') {
+ double inc = var->Meta().increment;
+ if (key == '-') inc *= -1.0;
+ if (key == '_') inc *= -0.1;
+ if (key == '+') inc *= 0.1;
+ const double newval = max(var->Meta().range[0], min(var->Meta().range[1], val + inc));
+ var->Set( logscale ? exp(newval) : newval );
+ }else if(key == 'r'){
+ Reset();
+ }else{
+ return;
+ }
+ GuiVarChanged(*this);
+ }
+}
+
+void Slider::Mouse(View& view, MouseButton button, int x, int y, bool pressed, int mouse_state)
+{
+ if(pressed)
+ {
+ // Wheel
+ if( button == MouseWheelUp || button == MouseWheelDown )
+ {
+ // Change scale around current value
+ const double frac = max(0.0,min(1.0,(double)(x - v.l)/(double)v.w));
+ double val = frac * (var->Meta().range[1] - var->Meta().range[0]) + var->Meta().range[0];
+
+ if (logscale)
+ {
+ if (val<=0)
+ val = std::numeric_limits<double>::min();
+ else
+ val = log(val);
+ }
+
+ const double scale = (button == MouseWheelUp ? 1.2 : 1.0 / 1.2 );
+ var->Meta().range[1] = val + (var->Meta().range[1] - val)*scale;
+ var->Meta().range[0] = val - (val - var->Meta().range[0])*scale;
+ }else{
+ lock_bounds = (button == MouseButtonLeft);
+ MouseMotion(view,x,y,mouse_state);
+ }
+ }else{
+ if(!lock_bounds)
+ {
+ double val = !logscale ? var->Get() : log(var->Get());
+
+ var->Meta().range[0] = min(var->Meta().range[0], val);
+ var->Meta().range[1] = max(var->Meta().range[1], val);
+ }
+ }
+}
+
+void Slider::MouseMotion(View&, int x, int /*y*/, int /*mouse_state*/)
+{
+ if( var->Meta().range[0] != var->Meta().range[1] )
+ {
+ const double range = (var->Meta().range[1] - var->Meta().range[0]);
+ const double frac = (double)(x - v.l)/(double)v.w;
+ double val;
+
+ if( lock_bounds )
+ {
+ const double bfrac = max(0.0,min(1.0,frac));
+ val = bfrac * range + var->Meta().range[0] ;
+ }else{
+ val = frac * range + var->Meta().range[0];
+ }
+
+ if (logscale) {
+ val = exp(val);
+ }
+
+ if( is_integral_type ) {
+ val = std::round(val);
+ }
+
+ var->Set(val);
+ GuiVarChanged(*this);
+ }
+}
+
+
+void Slider::ResizeChildren()
+{
+ raster[0] = v.l + 2.0f;
+ raster[1] = v.b + (v.h-gltext.Height())/2.0f;
+}
+
+void Slider::Render()
+{
+ const double val = var->Get();
+
+ if( var->Meta().range[0] != var->Meta().range[1] )
+ {
+ double rval = val;
+ if (logscale)
+ {
+ rval = log(val);
+ }
+ glColor4fv(colour_fg);
+ glRect(v);
+ glColor4fv(colour_dn);
+ const double norm_val = max(0.0,min(1.0,(rval - var->Meta().range[0]) / (var->Meta().range[1] - var->Meta().range[0])));
+ glRect(Viewport(v.l,v.b, (int)(v.w*norm_val),v.h));
+ DrawShadowRect(v);
+ }
+
+ glColor4fv(colour_tx);
+ if(gltext.Text() != var->Meta().friendly) {
+ gltext = font().Text(var->Meta().friendly);
+ }
+ gltext.DrawWindow(raster[0], raster[1]);
+
+ std::ostringstream oss;
+ oss << setprecision(4) << val;
+ string str = oss.str();
+ GlText glval = font().Text(str);
+ const float l = glval.Width() + 2.0f;
+ glval.DrawWindow( v.l + v.w - l, raster[1] );
+}
+
+
+TextInput::TextInput(std::string title, VarValueGeneric& tv)
+ : Widget<std::string>(title+":", tv), can_edit(!(tv.Meta().flags & META_FLAG_READONLY)), do_edit(false)
+{
+ top = 1.0; bottom = Attach::Pix(-tab_h());
+ left = 0.0; right = 1.0;
+ hlock = LockLeft;
+ vlock = LockBottom;
+ handler = this;
+ sel[0] = -1;
+ sel[1] = -1;
+ gltext = font().Text(title);
+}
+
+void TextInput::Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed)
+{
+ if(can_edit && pressed && do_edit)
+ {
+ const bool selection = sel[1] > sel[0] && sel[0] >= 0;
+
+ if(key == 13)
+ {
+ var->Set(edit);
+ GuiVarChanged(*this);
+
+ do_edit = false;
+ sel[0] = sel[1] = -1;
+ }else if(key == 8) {
+ // backspace
+ if(selection)
+ {
+ edit = edit.substr(0,sel[0]) + edit.substr(sel[1],edit.length()-sel[1]);
+ sel[1] = sel[0];
+ }else{
+ if(sel[0] >0)
+ {
+ edit = edit.substr(0,sel[0]-1) + edit.substr(sel[0],edit.length()-sel[0]);
+ sel[0]--;
+ sel[1]--;
+ }
+ }
+ }else if(key == 127){
+ // delete
+ if(selection)
+ {
+ edit = edit.substr(0,sel[0]) + edit.substr(sel[1],edit.length()-sel[1]);
+ sel[1] = sel[0];
+ }else{
+ if(sel[0] < (int)edit.length())
+ {
+ edit = edit.substr(0,sel[0]) + edit.substr(sel[0]+1,edit.length()-sel[0]+1);
+ }
+ }
+ }else if(key == 230){
+ // right
+ sel[0] = min((int)edit.length(),sel[0]+1);
+ sel[1] = sel[0];
+ }else if(key == 228){
+ // left
+ sel[0] = max(0,sel[0]-1);
+ sel[1] = sel[0];
+ }else if(key == 234){
+ // home
+ sel[0] = sel[1] = 0;
+ }else if(key == 235){
+ // end
+ sel[0] = sel[1] = (int)edit.length();
+ }else if(key < PANGO_SPECIAL){
+ edit = edit.substr(0,sel[0]).append(1,key) + edit.substr(sel[1],edit.length()-sel[1]);
+ sel[1] = sel[0];
+ sel[0]++;
+ sel[1]++;
+ }
+ }
+}
+
+void TextInput::Mouse(View& /*view*/, MouseButton button, int x, int /*y*/, bool pressed, int /*mouse_state*/)
+{
+ if(can_edit && button != MouseWheelUp && button != MouseWheelDown )
+ {
+
+ if(do_edit)
+ {
+ const int sl = (int)gledit.Width() + 2;
+ const int rl = v.l + v.w - sl;
+ int ep = (int)edit.length();
+
+ if( x < rl )
+ {
+ ep = 0;
+ }else{
+ for( unsigned i=0; i<edit.length(); ++i )
+ {
+ const int tl = (int)(rl + font().Text(edit.substr(0,i)).Width());
+ if(x < tl+2)
+ {
+ ep = i;
+ break;
+ }
+ }
+ }
+ if(pressed)
+ {
+ sel[0] = sel[1] = ep;
+ }else{
+ sel[1] = ep;
+ }
+
+ if(sel[0] > sel[1])
+ std::swap(sel[0],sel[1]);
+ }else{
+ do_edit = !pressed;
+ sel[0] = 0;
+ sel[1] = (int)edit.length();
+ }
+ }
+}
+
+void TextInput::MouseMotion(View&, int x, int /*y*/, int /*mouse_state*/)
+{
+ if(can_edit && do_edit)
+ {
+ const int sl = (int)gledit.Width() + 2;
+ const int rl = v.l + v.w - sl;
+ int ep = (int)edit.length();
+
+ if( x < rl )
+ {
+ ep = 0;
+ }else{
+ for( unsigned i=0; i<edit.length(); ++i )
+ {
+ const int tl = (int)(rl + font().Text(edit.substr(0,i)).Width());
+ if(x < tl+2)
+ {
+ ep = i;
+ break;
+ }
+ }
+ }
+
+ sel[1] = ep;
+ }
+}
+
+
+void TextInput::ResizeChildren()
+{
+ raster[0] = v.l + 2.0f;
+ raster[1] = v.b + (v.h-gltext.Height()) / 2.0f;
+}
+
+void TextInput::Render()
+{
+ if(!do_edit) edit = var->Get();
+
+ gledit = font().Text(edit);
+
+ glColor4fv(colour_fg);
+ if(can_edit) glRect(v);
+
+ const int sl = (int)gledit.Width() + 2;
+ const int rl = v.l + v.w - sl;
+
+ if( do_edit && sel[0] >= 0)
+ {
+ const int tl = (int)(rl + font().Text(edit.substr(0,sel[0])).Width());
+ const int tr = (int)(rl + font().Text(edit.substr(0,sel[1])).Width());
+ glColor4fv(colour_dn);
+ glRect(Viewport(tl,v.b,tr-tl,v.h));
+ }
+
+ glColor4fv(colour_tx);
+ gltext.DrawWindow(raster[0], raster[1]);
+
+ gledit.DrawWindow((GLfloat)(rl), raster[1]);
+ if(can_edit) DrawShadowRect(v);
+}
+
+}
diff --git a/externals/Pangolin/src/display/window_factory.cpp b/externals/Pangolin/src/display/window_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..26cf67dcade09474270e7e3716969bcf50019b2c
--- /dev/null
+++ b/externals/Pangolin/src/display/window_factory.cpp
@@ -0,0 +1,42 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2017 Steven Lovegrove, Andrey Mnatsakanov
+ *
+ * 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.
+ */
+
+#include <pangolin/display/window.h>
+#include <pangolin/factory/factory_registry.h>
+
+namespace pangolin
+{
+
+template<>
+FactoryRegistry<WindowInterface>& FactoryRegistry<WindowInterface>::I()
+{
+ // Singleton instance
+ static FactoryRegistry instance;
+ return instance;
+}
+
+}
\ No newline at end of file
diff --git a/externals/Pangolin/src/geometry/geometry.cpp b/externals/Pangolin/src/geometry/geometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..60bf7e85f16c4dd1b41c8e4d5ef7beee443ab962
--- /dev/null
+++ b/externals/Pangolin/src/geometry/geometry.cpp
@@ -0,0 +1,73 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/geometry/geometry.h>
+#include <pangolin/geometry/geometry_ply.h>
+#include <pangolin/geometry/geometry_obj.h>
+
+#include <pangolin/utils/file_extension.h>
+#include <pangolin/utils/file_utils.h>
+
+// TODO: Should really get rid of this from API
+#include <pangolin/gl/gl.h>
+
+namespace pangolin {
+
+// TODO: Replace this with proper factory registry
+pangolin::Geometry LoadGeometry(const std::string& filename)
+{
+ const std::string expanded_filename = PathExpand(filename);
+ const ImageFileType ft = FileType(expanded_filename);
+ if(ft == ImageFileTypePly) {
+ return LoadGeometryPly(expanded_filename);
+ }else if(ft == ImageFileTypeObj) {
+ return LoadGeometryObj(expanded_filename);
+ }else{
+ throw std::runtime_error("Unsupported geometry file type.");
+ }
+}
+
+pangolin::Geometry::Element::Attribute MakeAttribute(GLenum datatype, size_t num_items, size_t count_per_item, void* ptr, size_t pitch_bytes)
+{
+ switch(datatype) {
+ case GL_FLOAT:
+ return Image<float> ( (float*)ptr, count_per_item, num_items, pitch_bytes);
+ case GL_INT:
+ case GL_UNSIGNED_INT:
+ return Image<uint32_t>( (uint32_t*)ptr, count_per_item, num_items, pitch_bytes);
+ case GL_SHORT:
+ case GL_UNSIGNED_SHORT:
+ return Image<uint16_t>( (uint16_t*)ptr, count_per_item, num_items, pitch_bytes);
+ case GL_BYTE:
+ case GL_UNSIGNED_BYTE:
+ return Image<uint8_t> ( (uint8_t*)ptr, count_per_item, num_items, pitch_bytes);
+ default:
+ throw std::runtime_error("Unsupported type");
+ };
+}
+
+}
diff --git a/externals/Pangolin/src/geometry/geometry_obj.cpp b/externals/Pangolin/src/geometry/geometry_obj.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7299ba8cc4d415e385d2f906c9657645c10b5119
--- /dev/null
+++ b/externals/Pangolin/src/geometry/geometry_obj.cpp
@@ -0,0 +1,207 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/geometry/geometry_obj.h>
+#include <tinyobj/tiny_obj_loader.h>
+#include <pangolin/utils/variadic_all.h>
+#include <pangolin/utils/simple_math.h>
+
+// Needed for enum type. TODO: Make this independent of GL
+#include <pangolin/gl/glplatform.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/utils/file_utils.h>
+
+namespace pangolin {
+
+template<typename T>
+Geometry::Element ConvertVectorToGeometryElement(const std::vector<T>& v, size_t count_per_element, const std::string& attribute_name )
+{
+ PANGO_ASSERT(v.size() % count_per_element == 0);
+ const size_t num_elements = v.size() / count_per_element;
+
+ // Allocate buffer and copy into it
+ Geometry::Element el(sizeof(T) * count_per_element, num_elements);
+ el.CopyFrom(Image<T>(v.data(), count_per_element, num_elements));
+ el.attributes[attribute_name] = el.template UnsafeReinterpret<T>();
+ return el;
+}
+
+template<typename T>
+Image<T> GetImageWrapper(std::vector<T>& vec, size_t count_per_element)
+{
+ PANGO_ASSERT(vec.size() % count_per_element == 0);
+ if(vec.size()) {
+ return Image<T>(vec.data(), count_per_element, vec.size() / count_per_element, count_per_element * sizeof(T));
+ }else{
+ return Image<T>();
+ }
+}
+
+
+pangolin::Geometry LoadGeometryObj(const std::string& filename)
+{
+ pangolin::Geometry geom;
+
+ tinyobj::attrib_t attrib;
+ std::vector<tinyobj::shape_t> shapes;
+ std::vector<tinyobj::material_t> materials;
+
+ std::string warn;
+ std::string err;
+ if(tinyobj::LoadObj(&attrib, &shapes, &materials, &warn, &err, filename.c_str(), PathParent(filename).c_str())) {
+ PANGO_ASSERT(attrib.vertices.size() % 3 == 0);
+ PANGO_ASSERT(attrib.normals.size() % 3 == 0);
+ PANGO_ASSERT(attrib.colors.size() % 3 == 0);
+ PANGO_ASSERT(attrib.texcoords.size() % 2 == 0);
+
+ // Load textures - a bit of a hack for now.
+ for(size_t i=0; i < materials.size(); ++i) {
+ if(!materials[i].diffuse_texname.empty()) {
+ geom.textures[FormatString("texture_%",i)] = LoadImage(PathParent(filename) + "/" + materials[i].diffuse_texname);
+ }
+ }
+
+ const size_t num_verts = attrib.vertices.size() / 3;
+// PANGO_ASSERT(all_of(
+// [&](const size_t& v){return (v == 0) || (v == num_verts);},
+// attrib.normals.size() / 3,
+// attrib.colors.size() / 3));
+
+ // Get rid of color buffer if all elements are equal.
+ if ( std::adjacent_find( attrib.colors.begin(), attrib.colors.end(), std::not_equal_to<float>() ) == attrib.colors.end() )
+ {
+ attrib.colors.clear();
+ }
+
+ Image<float> tiny_vs = GetImageWrapper(attrib.vertices, 3);
+ Image<float> tiny_ns = GetImageWrapper(attrib.normals, 3);
+ Image<float> tiny_cs = GetImageWrapper(attrib.colors, 3);
+ Image<float> tinu_ts = GetImageWrapper(attrib.texcoords, 2);
+
+
+ // Create unified verts attribute
+ auto& verts = geom.buffers["geometry"];
+ Image<float> new_vs, new_ns, new_cs, new_ts;
+ {
+ verts.Reinitialise(sizeof(float)*(tiny_vs.w + tiny_ns.w + tiny_cs.w + tinu_ts.w), num_verts);
+ size_t float_offset = 0;
+ if(tiny_vs.IsValid()) {
+ new_vs = verts.UnsafeReinterpret<float>().SubImage(float_offset,0,3,num_verts);
+ verts.attributes["vertex"] = new_vs;
+ float_offset += 3;
+ new_vs.CopyFrom(tiny_vs);
+ }
+ if(tiny_ns.IsValid()) {
+ new_ns = verts.UnsafeReinterpret<float>().SubImage(float_offset,0,3,num_verts);
+ verts.attributes["normal"] = new_ns;
+ float_offset += 3;
+ // Don't copy - we'll have to re-order
+ }
+ if(tiny_cs.IsValid()) {
+ new_cs = verts.UnsafeReinterpret<float>().SubImage(float_offset,0,3,num_verts);
+ verts.attributes["color"] = new_cs;
+ float_offset += 3;
+ new_cs.CopyFrom(tiny_cs);
+ }
+ if(tinu_ts.IsValid()) {
+ new_ts = verts.UnsafeReinterpret<float>().SubImage(float_offset,0,3,num_verts);
+ verts.attributes["uv"] = new_ts;
+ float_offset += 2;
+ // Don't copy - we'll have to re-order
+ }
+ PANGO_ASSERT(float_offset * sizeof(float) == verts.w);
+ }
+
+ for(auto& shape : shapes) {
+ if(shape.mesh.indices.size() == 0) {
+ continue;
+ }
+
+ auto faces = geom.objects.emplace(shape.name, Geometry::Element());
+
+ if(std::all_of( shape.mesh.num_face_vertices.begin(), shape.mesh.num_face_vertices.end(),
+ [](unsigned char num){return num==3;}
+ )) {
+ // tri mesh
+ const size_t num_indices = shape.mesh.indices.size() ;
+ const size_t num_faces = shape.mesh.indices.size() / 3;
+ PANGO_ASSERT(num_indices % 3 == 0);
+
+ Image<uint32_t> ibo_vs((uint32_t*)&shape.mesh.indices[0].vertex_index, 1, num_indices, sizeof(tinyobj::index_t));
+ Image<uint32_t> ibo_ns((uint32_t*)&shape.mesh.indices[0].normal_index, 1, num_indices, sizeof(tinyobj::index_t));
+ Image<uint32_t> ibo_ts((uint32_t*)&shape.mesh.indices[0].texcoord_index, 1, num_indices, sizeof(tinyobj::index_t));
+
+ // Use vert_ibo as our new IBO
+ faces->second.Reinitialise(3*sizeof(uint32_t), num_indices);
+ Image<uint32_t> new_ibo = faces->second.UnsafeReinterpret<uint32_t>().SubImage(0,0,3,num_faces);
+ for(size_t f=0; f < num_faces; ++f) {
+ for(size_t v=0; v < 3; ++v) {
+ new_ibo(v,f) = ibo_vs(0,3*f+v);
+ }
+ }
+ faces->second.attributes["vertex_indices"] = new_ibo;
+
+ // Reorder normals
+ if(new_ns.IsValid()) {
+ for(size_t f=0; f < num_faces; ++f) {
+ for(size_t v=0; v < 3; ++v) {
+ size_t vi = ibo_vs(0,3*f+v);
+ size_t ni = ibo_ns(0,3*f+v);
+ new_ns.Row(vi).CopyFrom(tiny_ns.Row(ni));
+ Normalise<3>(new_ns.RowPtr(vi));
+ }
+ }
+ }
+
+ // Reorder uvs
+ if(new_ts.IsValid()) {
+ for(size_t f=0; f < num_faces; ++f) {
+ for(size_t v=0; v < 3; ++v) {
+ size_t vi = ibo_vs(0,3*f+v);
+ size_t ti = ibo_ts(0,3*f+v);
+ new_ts.Row(vi).CopyFrom(tinu_ts.Row(ti));
+ }
+ }
+ }
+ }else if(std::all_of( shape.mesh.num_face_vertices.begin(), shape.mesh.num_face_vertices.end(),
+ [](unsigned char num){return num==4;}
+ )) {
+ // Quad mesh
+ throw std::runtime_error("Do not support quad meshes yet.");
+ }else{
+ // ???
+ throw std::runtime_error("Do not support meshes with mixed triangles and quads.");
+ }
+ }
+ }else{
+ throw std::runtime_error(FormatString("Unable to load OBJ file '%'. Error: '%'", filename, err));
+ }
+
+ return geom;
+}
+
+}
diff --git a/externals/Pangolin/src/geometry/geometry_ply.cpp b/externals/Pangolin/src/geometry/geometry_ply.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..397a1f66c7b89dd09c9e9153b078ed1cd88080eb
--- /dev/null
+++ b/externals/Pangolin/src/geometry/geometry_ply.cpp
@@ -0,0 +1,483 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/geometry/geometry_ply.h>
+
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/variadic_all.h>
+#include <pangolin/utils/parse.h>
+#include <pangolin/utils/type_convert.h>
+#include <pangolin/utils/simple_math.h>
+
+// TODO: Should really remove need for GL here.
+#include <pangolin/gl/gl.h>
+
+namespace pangolin {
+
+#define FORMAT_STRING_LIST(x) #x,
+
+const char* PlyHeaderString[] = {
+ PLY_HEADER_LIST(FORMAT_STRING_LIST)
+};
+
+const char* PlyFormatString[] = {
+ PLY_FORMAT_LIST(FORMAT_STRING_LIST)
+};
+
+const char* PlyTypeString[] = {
+ PLY_TYPE_LIST(FORMAT_STRING_LIST)
+};
+
+#undef FORMAT_STRING_LIST
+
+PLY_GROUP_LIST(PANGOLIN_DEFINE_PARSE_TOKEN)
+
+void ParsePlyHeader(PlyHeaderDetails& ply, std::istream& is)
+{
+ // 'Active' element for property definitions.
+ int current_element = -1;
+
+ // Check header is correct
+ PlyHeader token = ParseTokenPlyHeader(is);
+ if( token != PlyHeader_ply) {
+ throw std::runtime_error("Bad PLY header magic.");
+ }
+ ConsumeToNewline(is);
+
+ while(is.good() && token != PlyHeader_end_header) {
+ token = ParseTokenPlyHeader(is);
+ switch (token) {
+ case PlyHeader_format:
+ // parse PLY format and version
+ ConsumeWhitespace(is);
+ ply.format = ParseTokenPlyFormat(is);
+ ConsumeWhitespace(is);
+ ply.version = ReadToken(is);
+ break;
+ case PlyHeader_element:
+ {
+ current_element = ply.elements.size();
+ PlyElementDetails el;
+ el.stride_bytes = 0;
+ ConsumeWhitespace(is);
+ el.name = ReadToken(is);
+ ConsumeWhitespace(is);
+ el.num_items = FromString<int>(ReadToken(is));
+ ply.elements.push_back(el);
+ break;
+ }
+ case PlyHeader_property:
+ if(current_element >= 0) {
+ PlyElementDetails& el = ply.elements[current_element];
+ PlyPropertyDetails prop;
+ ConsumeWhitespace(is);
+ const PlyType t = ParseTokenPlyType(is);
+ if( t == PlyType_list) {
+ ConsumeWhitespace(is);
+ const PlyType idtype = ParseTokenPlyType(is);
+ ConsumeWhitespace(is);
+ const PlyType itemtype = ParseTokenPlyType(is);
+ prop.list_index_type = PlyTypeGl[idtype];
+ prop.type = PlyTypeGl[itemtype];
+ prop.offset_bytes = el.stride_bytes;
+ prop.num_items = -1;
+ el.stride_bytes = -1;
+ }else{
+ prop.type = PlyTypeGl[t];
+ prop.list_index_type = GL_NONE;
+ prop.offset_bytes = el.stride_bytes;
+ prop.num_items = 1;
+ const size_t size_bytes = GlDataTypeBytes(prop.type);
+ if( el.stride_bytes >= 0) {
+ el.stride_bytes += size_bytes;
+ }
+ }
+ ConsumeWhitespace(is);
+ prop.name = ReadToken(is);
+ el.properties.push_back(prop);
+ }else{
+ pango_print_warn("PLY Parser: property declaration before element. Ignoring line.");
+ }
+ break;
+ case PlyHeader_comment:
+ case PlyHeader_end_header:
+ break;
+ default:
+ pango_print_warn("PLY Parser: Unknown token - ignoring line.");
+ }
+ ConsumeToNewline(is);
+ }
+}
+
+void ParsePlyAscii(pangolin::Geometry& /*geom*/, const PlyHeaderDetails& /*ply*/, std::istream& /*is*/)
+{
+ throw std::runtime_error("Not implemented.");
+}
+
+void AddVertexNormals(pangolin::Geometry& geom)
+{
+ auto it_geom = geom.buffers.find("geometry");
+ auto it_face = geom.objects.find("default");
+
+ if(it_geom != geom.buffers.end() && it_face != geom.objects.end())
+ {
+ const auto it_vbo = it_geom->second.attributes.find("vertex");
+ const auto it_ibo = it_face->second.attributes.find("vertex_indices");
+
+ if(it_vbo != it_geom->second.attributes.end() && it_ibo != it_face->second.attributes.end()) {
+ const auto& ibo = get<Image<uint32_t>>(it_ibo->second);
+ const auto& vbo = get<Image<float>>(it_vbo->second);
+
+ // Assume we have triangles.
+ PANGO_ASSERT(ibo.w == 3 && vbo.w == 3);
+
+ ManagedImage<float> vert_normals(3, vbo.h);
+ ManagedImage<size_t> vert_face_count(1, vbo.h);
+ vert_normals.Fill(0.0f);
+ vert_face_count.Fill(0);
+
+ float ab[3];
+ float ac[3];
+ float fn[3];
+
+ for(size_t i=0; i < ibo.h; ++i) {
+ uint32_t i0 = ibo(0,i);
+ uint32_t i1 = ibo(1,i);
+ uint32_t i2 = ibo(2,i);
+ MatSub<3,1>(ab, vbo.RowPtr(i1), vbo.RowPtr(i0));
+ MatSub<3,1>(ac, vbo.RowPtr(i2), vbo.RowPtr(i0));
+ VecCross3(fn, ab, ac);
+ Normalise<3>(fn);
+ for(size_t v=0; v < 3; ++v) {
+ MatAdd<3,1>(vert_normals.RowPtr(ibo(v,i)), vert_normals.RowPtr(ibo(v,i)), fn);
+ ++vert_face_count(0,ibo(v,i));
+ }
+ }
+
+ for(size_t v=0; v < vert_normals.h; ++v) {
+ // Compute average
+ MatMul<3,1>(vert_normals.RowPtr(v), 1.0f / vert_face_count(0,v));
+ }
+
+ auto& el = geom.buffers["normal"];
+ (ManagedImage<float>&)el = std::move(vert_normals);
+ auto& attr_norm = el.attributes["normal"];
+ attr_norm = MakeAttribute(GL_FLOAT, el.h, 3, el.ptr, el.pitch);
+ }
+ }
+
+}
+
+void StandardizeXyzToVertex(pangolin::Geometry& geom)
+{
+ auto it_verts = geom.buffers.find("geometry");
+
+ if(it_verts != geom.buffers.end()) {
+ auto& verts = it_verts->second;
+ auto it_x = verts.attributes.find("x");
+ auto it_y = verts.attributes.find("y");
+ auto it_z = verts.attributes.find("z");
+ if(all_found(verts.attributes, it_x, it_y, it_z)) {
+ if(verts.attributes.find("vertex") == verts.attributes.end()) {
+ auto& vertex = verts.attributes["vertex"];
+ auto& imx = get<Image<float>>(it_x->second);
+ auto& imy = get<Image<float>>(it_y->second);
+ auto& imz = get<Image<float>>(it_z->second);
+
+ if(imx.ptr + 1 == imy.ptr && imy.ptr + 1 == imz.ptr) {
+ vertex = MakeAttribute(GL_FLOAT, verts.h, 3, imx.ptr, imx.pitch);
+ }else{
+ throw std::runtime_error("Ooops");
+ }
+ }
+ verts.attributes.erase(it_x);
+ verts.attributes.erase(it_y);
+ verts.attributes.erase(it_z);
+ }
+ }
+}
+
+void StandardizeRgbToColor(pangolin::Geometry& geom)
+{
+ auto it_verts = geom.buffers.find("geometry");
+
+ if(it_verts != geom.buffers.end()) {
+ auto& verts = it_verts->second;
+ auto it_r = verts.attributes.find("r");
+ auto it_g = verts.attributes.find("g");
+ auto it_b = verts.attributes.find("b");
+ auto it_a = verts.attributes.find("a");
+
+ if(!all_found(verts.attributes, it_r, it_b, it_g)) {
+ it_r = verts.attributes.find("red");
+ it_g = verts.attributes.find("green");
+ it_b = verts.attributes.find("blue");
+ it_a = verts.attributes.find("alpha");
+ }
+
+ if(all_found(verts.attributes, it_r, it_g, it_b)) {
+ const bool have_alpha = it_a != verts.attributes.end();
+
+ if(verts.attributes.find("color") == verts.attributes.end()) {
+ Geometry::Element::Attribute& red = it_r->second;
+ Geometry::Element::Attribute& color = verts.attributes["color"];
+
+ // TODO: Check that these really are contiguous in memory...
+ if(auto attrib = mpark::get_if<Image<float>>(&red)) {
+ color = Image<float>(attrib->ptr, have_alpha ? 4 : 3, verts.h, verts.pitch);
+ }else if(auto attrib = get_if<Image<uint8_t>>(&red)) {
+ color = Image<uint8_t>(attrib->ptr, have_alpha ? 4 : 3, verts.h, verts.pitch);
+ }else if(auto attrib = get_if<Image<uint16_t>>(&red)) {
+ color = Image<uint16_t>(attrib->ptr, have_alpha ? 4 : 3, verts.h, verts.pitch);
+ }else if(auto attrib = get_if<Image<uint32_t>>(&red)) {
+ color = Image<uint32_t>(attrib->ptr, have_alpha ? 4 : 3, verts.h, verts.pitch);
+ }
+ }
+ verts.attributes.erase(it_r);
+ verts.attributes.erase(it_g);
+ verts.attributes.erase(it_b);
+ if(have_alpha) verts.attributes.erase(it_a);
+ }
+ }
+}
+
+void StandardizeMultiTextureFaceToXyzuv(pangolin::Geometry& geom)
+{
+ const auto it_multi_texture_face = geom.buffers.find("multi_texture_face");
+ const auto it_multi_texture_vertex = geom.buffers.find("multi_texture_vertex");
+ const auto it_geom = geom.buffers.find("geometry");
+ const auto it_face = geom.objects.find("default");
+
+ if(it_geom != geom.buffers.end() && it_face != geom.objects.end())
+ {
+ const auto it_vbo = it_geom->second.attributes.find("vertex");
+ const auto it_ibo = it_face->second.attributes.find("vertex_indices");
+
+ if(all_found(geom.buffers, it_multi_texture_face, it_multi_texture_vertex) &&
+ it_vbo != it_geom->second.attributes.end() &&
+ it_ibo != it_face->second.attributes.end()
+ ) {
+ const auto it_uv_ibo = it_multi_texture_face->second.attributes.find("texture_vertex_indices");
+ const auto it_tx = it_multi_texture_face->second.attributes.find("tx");
+ const auto it_tn = it_multi_texture_face->second.attributes.find("tn");
+
+ const auto it_u = it_multi_texture_vertex->second.attributes.find("u");
+ const auto it_v = it_multi_texture_vertex->second.attributes.find("v");
+
+ if(all_found(it_multi_texture_vertex->second.attributes, it_u, it_v) &&
+ it_uv_ibo != it_multi_texture_face->second.attributes.end()
+ ) {
+ // We're going to create a new vertex buffer to hold uv's too
+ auto& orig_ibo = get<Image<uint32_t>>(it_ibo->second);
+ const auto& orig_xyz = get<Image<float>>(it_vbo->second);
+ const auto& uv_ibo = get<Image<uint32_t>>(it_uv_ibo->second);
+ const auto& u = get<Image<float>>(it_u->second);
+ const auto& v = get<Image<float>>(it_v->second);
+ const auto& tx = get<Image<uint8_t>>(it_tx->second);
+ const auto& tn = get<Image<uint32_t>>(it_tn->second);
+
+ PANGO_ASSERT(u.h == v.h);
+ PANGO_ASSERT(orig_ibo.w == 3 && uv_ibo.w == 3);
+
+ pangolin::Geometry::Element new_xyzuv(5*sizeof(float), u.h);
+ Image<float> new_xyz = new_xyzuv.UnsafeReinterpret<float>().SubImage(0,0,3,new_xyzuv.h);
+ Image<float> new_uv = new_xyzuv.UnsafeReinterpret<float>().SubImage(3,0,2,new_xyzuv.h);
+ new_xyzuv.attributes["vertex"] = new_xyz;
+ new_xyzuv.attributes["uv"] = new_uv;
+
+ for(size_t face=0; face < orig_ibo.h; ++face) {
+ uint32_t vtn = tn(0,face);
+ uint8_t vtx = tx(0,face);
+ PANGO_ASSERT(vtx==0, "Haven't implemented multi-texture yet.");
+
+ for(size_t vert=0; vert < 3; ++vert)
+ {
+ uint32_t& orig_xyz_index = orig_ibo(vert,vtn);
+ const uint32_t uv_index = uv_ibo(vert,face);
+ PANGO_ASSERT(uv_index < new_xyzuv.h && orig_xyz_index < orig_xyz.h);
+
+ for(int el=0; el < 3; ++el) {
+ new_xyz(el,uv_index) = orig_xyz(el,orig_xyz_index);
+ }
+ new_uv(0,uv_index) = u(0,uv_index);
+ new_uv(1,uv_index) = v(0,uv_index);
+ orig_xyz_index = uv_index;
+ }
+ }
+
+ geom.buffers["geometry"] = std::move(new_xyzuv);
+ geom.buffers.erase(it_multi_texture_face);
+ geom.buffers.erase(it_multi_texture_vertex);
+ }
+
+ }
+ }
+}
+
+void Standardize(pangolin::Geometry& geom)
+{
+ StandardizeXyzToVertex(geom);
+ StandardizeRgbToColor(geom);
+ StandardizeMultiTextureFaceToXyzuv(geom);
+ AddVertexNormals(geom);
+}
+
+inline int ReadGlIntType(GLenum type, std::istream& is)
+{
+ // TODO: This seems really dodgey...
+ // int may not be big enough and if the datatype is smaller will it be padded?
+ int v = 0;
+ is.read( (char*)&v, GlDataTypeBytes(type));
+ return v;
+}
+
+inline void ReadInto(std::vector<unsigned char>& vec, std::istream& is, size_t num_bytes)
+{
+ const size_t current_size = vec.size();
+ vec.resize(current_size + num_bytes);
+ is.read((char*)vec.data() + current_size, num_bytes);
+}
+
+void ParsePlyLE(pangolin::Geometry& geom, PlyHeaderDetails& ply, std::istream& is)
+{
+ std::vector<uint8_t> buffer;
+
+ for(auto& el : ply.elements) {
+ pangolin::Geometry::Element geom_el;
+
+ if(el.stride_bytes > 0) {
+ // This will usually be the case for vertex buffers with a known number of attributes
+ PANGO_ASSERT(el.num_items > 0);
+ geom_el.Reinitialise(el.stride_bytes, el.num_items);
+ is.read((char*)geom_el.ptr, geom_el.Area());
+ }else {
+ // This will generally be the case for face data (containing a list of vertex indices)
+
+ // Reserve enough space for a list of quads
+ buffer.clear();
+ buffer.reserve(el.num_items * el.properties.size() * 4);
+
+ for(int i=0; i< el.num_items; ++i) {
+ size_t offset_bytes = 0;
+ for(auto& prop : el.properties) {
+ if(prop.isList()) {
+ const int list_items = ReadGlIntType(prop.list_index_type, is);
+ if(prop.num_items == -1) {
+ prop.num_items = list_items;
+ prop.offset_bytes = offset_bytes;
+ }else{
+ PANGO_ASSERT(prop.num_items == list_items);
+ }
+ }
+ const size_t num_bytes = prop.num_items * GlDataTypeBytes(prop.type);
+ ReadInto(buffer, is, num_bytes);
+ offset_bytes += num_bytes;
+ }
+ }
+
+ // Update element stride now we know
+ el.stride_bytes = 0;
+ for(auto& prop : el.properties) {
+ el.stride_bytes += prop.num_items * GlDataTypeBytes(prop.type);
+ }
+
+ geom_el.Reinitialise(el.stride_bytes, el.num_items);
+ PANGO_ASSERT(geom_el.SizeBytes() == buffer.size());
+ std::memcpy(geom_el.ptr, buffer.data(), buffer.size());
+ }
+
+ for(auto& prop : el.properties) {
+ geom_el.attributes[prop.name] = MakeAttribute(
+ prop.type, el.num_items, prop.num_items, geom_el.ptr + prop.offset_bytes, geom_el.pitch
+ );
+ }
+ if(el.name == "vertex") {
+ geom.buffers["geometry"] = std::move(geom_el);
+ }else if(el.name == "face") {
+ geom.objects.emplace("default", std::move(geom_el));
+ }else{
+ geom.buffers[el.name] = std::move(geom_el);
+ }
+ }
+
+ Standardize(geom);
+}
+
+void ParsePlyBE(pangolin::Geometry& /*geom*/, const PlyHeaderDetails& /*ply*/, std::istream& /*is*/)
+{
+ throw std::runtime_error("Not implemented.");
+}
+
+void AttachAssociatedTexturesPly(pangolin::Geometry& geom, const std::string& filename)
+{
+ // For PLY, texture names are generally implicit
+ auto dot = filename.find_last_of('.');
+ if(dot != filename.npos) {
+ const std::string base = filename.substr(0, dot);
+ for(int i=0; i < 10; ++i) {
+ const std::string glob = FormatString("%_%.*", base, i);
+ std::vector<std::string> file_vec;
+ if(FilesMatchingWildcard(glob, file_vec)) {
+ for(const auto& file : file_vec) {
+ try {
+ geom.textures[FormatString("texture_%",i)] = LoadImage(file);
+ break;
+ }catch(std::runtime_error&)
+ {
+ }
+ }
+ }
+ }
+ }
+}
+
+pangolin::Geometry LoadGeometryPly(const std::string& filename)
+{
+ std::ifstream bFile( filename.c_str(), std::ios::in | std::ios::binary );
+ if( !bFile.is_open() ) throw std::runtime_error("Unable to open PLY file: " + filename);
+
+ PlyHeaderDetails ply;
+ ParsePlyHeader(ply, bFile);
+
+ // Initialise geom object
+ pangolin::Geometry geom;
+
+ // Fill in geometry from file.
+ if(ply.format == PlyFormat_ascii) {
+ ParsePlyAscii(geom, ply, bFile);
+ }else if(ply.format == PlyFormat_binary_little_endian) {
+ ParsePlyLE(geom, ply, bFile);
+ }else if(ply.format == PlyFormat_binary_big_endian) {
+ ParsePlyBE(geom, ply, bFile);
+ }
+
+ AttachAssociatedTexturesPly(geom, filename);
+
+ return geom;
+}
+
+}
diff --git a/externals/Pangolin/src/geometry/glgeometry.cpp b/externals/Pangolin/src/geometry/glgeometry.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..084b609c20c2e798e79b063d17bebaf84d826f7f
--- /dev/null
+++ b/externals/Pangolin/src/geometry/glgeometry.cpp
@@ -0,0 +1,142 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/geometry/glgeometry.h>
+
+#include <pangolin/gl/glformattraits.h>
+
+namespace pangolin {
+
+GlGeometry::Element ToGlGeometryElement(const Geometry::Element& el, GlBufferType buffertype)
+{
+ GlGeometry::Element glel(buffertype, el.SizeBytes(), GL_STATIC_DRAW, el.ptr );
+ for(const auto& attrib_variant : el.attributes) {
+ visit([&](auto&& attrib){
+ using T = std::decay_t<decltype(attrib)>;
+ auto& glattrib = glel.attributes[attrib_variant.first];
+ glattrib.gltype = GlFormatTraits<typename T::PixelType>::gltype;
+ glattrib.count_per_element = attrib.w;
+ glattrib.num_elements = attrib.h;
+ glattrib.offset = (uint8_t*)attrib.ptr - el.ptr;
+ glattrib.stride_bytes = attrib.pitch;
+ }, attrib_variant.second);
+ }
+ return glel;
+}
+
+GlGeometry ToGlGeometry(const Geometry& geom)
+{
+ GlGeometry gl;
+ for(const auto& b : geom.buffers)
+ gl.buffers[b.first] = ToGlGeometryElement(b.second, GlArrayBuffer);
+
+ for(const auto& b : geom.objects)
+ gl.objects.emplace(b.first, ToGlGeometryElement(b.second, GlElementArrayBuffer));
+
+ for(const auto& tex : geom.textures) {
+ auto& gltex = gl.textures[tex.first];
+ gltex.Load(tex.second);
+ }
+ return gl;
+}
+
+void BindGlElement(GlSlProgram& prog, const GlGeometry::Element& el)
+{
+ el.Bind();
+ for(auto& a : el.attributes) {
+ const GLint attrib_handle = prog.GetAttributeHandle(a.first);
+ const GlGeometry::Element::Attribute& attr = a.second;
+ if(attrib_handle >= 0) {
+ glEnableVertexAttribArray(attrib_handle);
+ glVertexAttribPointer(
+ attrib_handle, attr.count_per_element, attr.gltype, GL_TRUE,
+ attr.stride_bytes,
+ (uint8_t*)0 + attr.offset
+ );
+ }
+ }
+}
+
+void UnbindGlElements(GlSlProgram& prog, const GlGeometry::Element& el)
+{
+ for(auto& a : el.attributes) {
+ const GLint attrib_handle = prog.GetAttributeHandle(a.first);
+ if(attrib_handle >= 0) {
+ glDisableVertexAttribArray(attrib_handle);
+ }
+ }
+ el.Unbind();
+}
+
+void GlDraw(GlSlProgram& prog, const GlGeometry& geom, const GlTexture* matcap)
+{
+ // Bind textures
+ int num_tex_bound = 0;
+ for(auto& tex : geom.textures) {
+ glActiveTexture(GL_TEXTURE0 + num_tex_bound);
+ tex.second.Bind();
+ prog.SetUniform(tex.first, (int)num_tex_bound);
+ ++num_tex_bound;
+ }
+
+ if(matcap) {
+ glActiveTexture(GL_TEXTURE0 + num_tex_bound);
+ matcap->Bind();
+ prog.SetUniform("matcap", (int)num_tex_bound);
+ ++num_tex_bound;
+ }
+
+ // Bind all attribute buffers
+ for(auto& buffer : geom.buffers) {
+ BindGlElement(prog, buffer.second);
+ }
+
+ // Draw all geometry
+ for(auto& buffer : geom.objects) {
+ auto it_indices = buffer.second.attributes.find("vertex_indices");
+ if(it_indices != buffer.second.attributes.end()) {
+ buffer.second.Bind();
+ auto& attrib = it_indices->second;
+ glDrawElements(
+ GL_TRIANGLES, attrib.count_per_element * attrib.num_elements,
+ attrib.gltype, (uint8_t*)0 + attrib.offset
+ );
+ buffer.second.Unbind();
+ }
+ }
+
+ // Unbind attribute buffers
+ for(auto& buffer : geom.buffers) {
+ UnbindGlElements(prog, buffer.second);
+ }
+
+ // Unbind textures
+ glBindTexture(GL_TEXTURE_2D, 0);
+ glActiveTexture(GL_TEXTURE0);
+}
+
+}
diff --git a/externals/Pangolin/src/geometry/tinyobj.cpp b/externals/Pangolin/src/geometry/tinyobj.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4cc64913a916a973ba839a686b1d5ad4fa5c0bfa
--- /dev/null
+++ b/externals/Pangolin/src/geometry/tinyobj.cpp
@@ -0,0 +1,29 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#define TINYOBJLOADER_IMPLEMENTATION
+#include <tinyobj/tiny_obj_loader.h>
diff --git a/externals/Pangolin/src/gl/compat/gl2engine.cpp b/externals/Pangolin/src/gl/compat/gl2engine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..96728a04d4dbaa03c7ff8f761b3fa204a11f6298
--- /dev/null
+++ b/externals/Pangolin/src/gl/compat/gl2engine.cpp
@@ -0,0 +1,39 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/gl/glinclude.h>
+
+namespace pangolin
+{
+
+GlEngine& glEngine()
+{
+ static GlEngine engine;
+ return engine;
+}
+
+}
diff --git a/externals/Pangolin/src/gl/glchar.cpp b/externals/Pangolin/src/gl/glchar.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..db7ff74b97b60b2f4c12597efd8770425e754039
--- /dev/null
+++ b/externals/Pangolin/src/gl/glchar.cpp
@@ -0,0 +1,70 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/gl/glchar.h>
+
+namespace pangolin
+{
+
+GlChar::GlChar()
+ : x_step(0.0f)
+{
+ // Uninitialised
+}
+
+GlChar::GlChar(int tw, int th, int x, int y, int w, int h, GLfloat advance, GLfloat ox, GLfloat oy)
+ : x_step(advance)
+{
+ const GLfloat u = (GLfloat)x / (GLfloat)tw;
+ const GLfloat v = (GLfloat)y / (GLfloat)th;
+ const GLfloat u2 = (GLfloat)(x + w) / (GLfloat)tw;
+ const GLfloat v2 = (GLfloat)(y + h) / (GLfloat)th;
+
+ // Setup u,v tex coords
+ vs[0] = XYUV(ox, oy, u,v );
+ vs[1] = XYUV(ox, oy-h, u,v2 );
+ vs[2] = XYUV(w+ox, oy-h, u2,v2 );
+ vs[3] = XYUV(w+ox, oy, u2,v );
+
+ y_min = oy-h;
+ y_max = oy;
+}
+
+void GlChar::Draw() const
+{
+ glVertexPointer(2, GL_FLOAT, sizeof(XYUV), &vs[0].x);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glTexCoordPointer(2, GL_FLOAT, sizeof(XYUV), &vs[0].tu);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+ glEnable(GL_TEXTURE_2D);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisable(GL_TEXTURE_2D);
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+}
+
+}
diff --git a/externals/Pangolin/src/gl/gldraw.cpp b/externals/Pangolin/src/gl/gldraw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3a5b49efec21c4e0f7b64f4410e4edc8ee97d939
--- /dev/null
+++ b/externals/Pangolin/src/gl/gldraw.cpp
@@ -0,0 +1,51 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove, Richard Newcombe
+ *
+ * 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.
+ */
+
+
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/utils/timer.h>
+
+namespace pangolin
+{
+
+void glRecordGraphic(float x, float y, float radius)
+{
+ const int ticks = static_cast<int>(TimeNow_s());
+ if( ticks % 2 )
+ {
+ // now, render a little red "recording" dot
+ glPushAttrib(GL_ENABLE_BIT);
+ glDisable(GL_LIGHTING);
+ glDisable(GL_DEPTH_TEST);
+ glColor3ub( 255, 0, 0 );
+ glDrawCircle( x, y, radius );
+ glPopAttrib();
+ }
+
+}
+
+}
diff --git a/externals/Pangolin/src/gl/glfont.cpp b/externals/Pangolin/src/gl/glfont.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc38c7c29fc2cf2282fb7f9a6d8e219dbcb4c442
--- /dev/null
+++ b/externals/Pangolin/src/gl/glfont.cpp
@@ -0,0 +1,214 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/display/display_internal.h>
+#include <pangolin/gl/glfont.h>
+#include <pangolin/gl/glstate.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/utils/type_convert.h>
+
+#if !defined(HAVE_GLES) || defined(HAVE_GLES_2)
+#include <pangolin/gl/glsl.h>
+#endif
+
+#define STB_TRUETYPE_IMPLEMENTATION
+#define STBTT_STATIC
+
+#if defined(_GCC_) || defined(_CLANG_)
+# pragma GCC diagnostic push
+# pragma GCC diagnostic ignored "-Wunused-function"
+# include "stb_truetype.h"
+# pragma GCC diagnostic pop
+#else
+# include "stb_truetype.h"
+#endif
+
+#define MAX_TEXT_LENGTH 500
+
+// Embedded fonts:
+extern const unsigned char AnonymousPro_ttf[];
+
+namespace pangolin
+{
+
+extern __thread PangolinGl* context;
+
+GlFont& GlFont::I()
+{
+ if (!context->font) {
+ context->font.reset(new GlFont(AnonymousPro_ttf, context->is_high_res ? 30 : 15));
+ }
+ return *context->font.get();
+}
+
+GlFont::GlFont(const unsigned char* ttf_buffer, float pixel_height, int tex_w, int tex_h)
+{
+ InitialiseFont(ttf_buffer, pixel_height, tex_w, tex_h);
+}
+
+GlFont::GlFont(const std::string& filename, float pixel_height, int tex_w, int tex_h)
+{
+ unsigned char* ttf_buffer = new unsigned char[1<<20];
+ const size_t bytes_read = fread(ttf_buffer, 1, 1<<20, fopen(filename.c_str(), "rb"));
+ if(bytes_read > 0) {
+ InitialiseFont(ttf_buffer, pixel_height, tex_w, tex_h);
+ }else{
+ throw std::runtime_error("Unable to read font from file.");
+ }
+ delete[] ttf_buffer;
+}
+
+GlFont::~GlFont()
+{
+ delete[] font_bitmap;
+}
+
+void GlFont::InitialiseFont(const unsigned char* ttf_buffer, float pixel_height, int tex_w, int tex_h)
+{
+ font_height_px = pixel_height;
+ this->tex_w = tex_w;
+ this->tex_h = tex_h;
+ font_bitmap = new unsigned char[tex_w*tex_h];
+ const int offset = 0;
+
+ stbtt_fontinfo f;
+ if (!stbtt_InitFont(&f, ttf_buffer, offset)) {
+ throw std::runtime_error("Unable to initialise font");
+ }
+
+ float scale = stbtt_ScaleForPixelHeight(&f, pixel_height);
+
+ STBTT_memset(font_bitmap, 0, tex_w*tex_h);
+ int x = 1;
+ int y = 1;
+ int bottom_y = 1;
+
+ // Generate bitmap and char indices
+ for (int i=0; i < NUM_CHARS; ++i) {
+ int advance, lsb, x0,y0,x1,y1,gw,gh;
+ int g = stbtt_FindGlyphIndex(&f, FIRST_CHAR + i);
+ stbtt_GetGlyphHMetrics(&f, g, &advance, &lsb);
+ stbtt_GetGlyphBitmapBox(&f, g, scale,scale, &x0,&y0,&x1,&y1);
+ gw = x1-x0;
+ gh = y1-y0;
+ if (x + gw + 1 >= tex_w)
+ y = bottom_y, x = 1; // advance to next row
+ if (y + gh + 1 >= tex_h) // check if it fits vertically AFTER potentially moving to next row
+ throw std::runtime_error("Unable to initialise font");
+ STBTT_assert(x+gw < tex_w);
+ STBTT_assert(y+gh < tex_h);
+ stbtt_MakeGlyphBitmap(&f, font_bitmap+x+y*tex_w, gw,gh,tex_w, scale,scale, g);
+
+ // Adjust offset for edges of pixels
+ chardata[i] = GlChar(tex_w,tex_h, x, y, gw, gh, scale*advance, x0 -0.5f, -y0 -0.5f);
+
+ x = x + gw + 1;
+ if (y+gh+1 > bottom_y)
+ bottom_y = y+gh+1;
+ }
+
+ // Generate kern table
+ for (int i=0; i < NUM_CHARS; ++i) {
+ for (int j=0; j < NUM_CHARS; ++j) {
+ kern_table[i*NUM_CHARS+j] = scale * stbtt_GetCodepointKernAdvance(&f,i,j);
+ }
+ }
+}
+
+void GlFont::InitialiseGlTexture()
+{
+ if(font_bitmap) {
+ mTex.Reinitialise(tex_w,tex_h, GL_ALPHA, true, 0, GL_ALPHA,GL_UNSIGNED_BYTE, font_bitmap);
+// mTex.SetNearestNeighbour();
+ delete[] font_bitmap;
+ font_bitmap = 0;
+ }
+}
+
+GlText GlFont::Text( const char* fmt, ... )
+{
+ if(!mTex.IsValid()) InitialiseGlTexture();
+
+ GlText ret(mTex);
+
+ char text[MAX_TEXT_LENGTH];
+ va_list ap;
+
+ if( fmt != NULL ) {
+ va_start( ap, fmt );
+ vsnprintf( text, MAX_TEXT_LENGTH, fmt, ap );
+ va_end( ap );
+
+ const size_t len = strlen(text);
+ char lc = ' ' - FIRST_CHAR;
+ for(size_t i=0; i < len; ++i) {
+ char c = text[i];
+ if( !(FIRST_CHAR <= c /*&& c <FIRST_CHAR+NUM_CHARS*/) ) {
+ c = ' ';
+ }
+ GlChar& ch = chardata[c-32];
+
+ // Kerning
+ if(i) {
+ const GLfloat kern = kern_table[ (lc-32)*NUM_CHARS + (c-32) ];
+ ret.AddSpace(kern);
+ }
+
+ ret.Add(c,ch);
+ lc = c;
+ }
+ }
+ return ret;
+}
+
+GlText GlFont::Text( const std::string& str )
+{
+ if(!mTex.IsValid()) InitialiseGlTexture();
+
+ GlText ret(mTex);
+
+ char lc = ' ' - FIRST_CHAR;
+ for(size_t i=0; i < str.length(); ++i) {
+ char c = str[i];
+ if( !(FIRST_CHAR <= c /*&& c <FIRST_CHAR+NUM_CHARS*/) ) {
+ c = ' ';
+ }
+ GlChar& ch = chardata[c-32];
+
+ // Kerning
+ if(i) {
+ const GLfloat kern = kern_table[ (lc-32)*NUM_CHARS + (c-32) ];
+ ret.AddSpace(kern);
+ }
+
+ ret.Add(c,ch);
+ lc = c;
+ }
+ return ret;
+}
+
+}
diff --git a/externals/Pangolin/src/gl/glpangoglu.cpp b/externals/Pangolin/src/gl/glpangoglu.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7f31389c8a9c7ab9fa38313e57fd25a07efdb38e
--- /dev/null
+++ b/externals/Pangolin/src/gl/glpangoglu.cpp
@@ -0,0 +1,264 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/gl/glpangoglu.h>
+#include <pangolin/utils/simple_math.h>
+
+namespace pangolin {
+
+const GLubyte gNotErrorLookup[] = "XX";
+
+const GLubyte* glErrorString(GLenum /*error*/)
+{
+ // TODO: Implement glErrorString
+ return gNotErrorLookup;
+}
+
+// Based on glu implementation.
+template<typename P>
+int InvertMatrix(const P m[16], P invOut[16])
+{
+ P inv[16], det;
+ int i;
+
+ inv[0] = m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15]
+ + m[9]*m[7]*m[14] + m[13]*m[6]*m[11] - m[13]*m[7]*m[10];
+ inv[4] = -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15]
+ - m[8]*m[7]*m[14] - m[12]*m[6]*m[11] + m[12]*m[7]*m[10];
+ inv[8] = m[4]*m[9]*m[15] - m[4]*m[11]*m[13] - m[8]*m[5]*m[15]
+ + m[8]*m[7]*m[13] + m[12]*m[5]*m[11] - m[12]*m[7]*m[9];
+ inv[12] = -m[4]*m[9]*m[14] + m[4]*m[10]*m[13] + m[8]*m[5]*m[14]
+ - m[8]*m[6]*m[13] - m[12]*m[5]*m[10] + m[12]*m[6]*m[9];
+ inv[1] = -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15]
+ - m[9]*m[3]*m[14] - m[13]*m[2]*m[11] + m[13]*m[3]*m[10];
+ inv[5] = m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15]
+ + m[8]*m[3]*m[14] + m[12]*m[2]*m[11] - m[12]*m[3]*m[10];
+ inv[9] = -m[0]*m[9]*m[15] + m[0]*m[11]*m[13] + m[8]*m[1]*m[15]
+ - m[8]*m[3]*m[13] - m[12]*m[1]*m[11] + m[12]*m[3]*m[9];
+ inv[13] = m[0]*m[9]*m[14] - m[0]*m[10]*m[13] - m[8]*m[1]*m[14]
+ + m[8]*m[2]*m[13] + m[12]*m[1]*m[10] - m[12]*m[2]*m[9];
+ inv[2] = m[1]*m[6]*m[15] - m[1]*m[7]*m[14] - m[5]*m[2]*m[15]
+ + m[5]*m[3]*m[14] + m[13]*m[2]*m[7] - m[13]*m[3]*m[6];
+ inv[6] = -m[0]*m[6]*m[15] + m[0]*m[7]*m[14] + m[4]*m[2]*m[15]
+ - m[4]*m[3]*m[14] - m[12]*m[2]*m[7] + m[12]*m[3]*m[6];
+ inv[10] = m[0]*m[5]*m[15] - m[0]*m[7]*m[13] - m[4]*m[1]*m[15]
+ + m[4]*m[3]*m[13] + m[12]*m[1]*m[7] - m[12]*m[3]*m[5];
+ inv[14] = -m[0]*m[5]*m[14] + m[0]*m[6]*m[13] + m[4]*m[1]*m[14]
+ - m[4]*m[2]*m[13] - m[12]*m[1]*m[6] + m[12]*m[2]*m[5];
+ inv[3] = -m[1]*m[6]*m[11] + m[1]*m[7]*m[10] + m[5]*m[2]*m[11]
+ - m[5]*m[3]*m[10] - m[9]*m[2]*m[7] + m[9]*m[3]*m[6];
+ inv[7] = m[0]*m[6]*m[11] - m[0]*m[7]*m[10] - m[4]*m[2]*m[11]
+ + m[4]*m[3]*m[10] + m[8]*m[2]*m[7] - m[8]*m[3]*m[6];
+ inv[11] = -m[0]*m[5]*m[11] + m[0]*m[7]*m[9] + m[4]*m[1]*m[11]
+ - m[4]*m[3]*m[9] - m[8]*m[1]*m[7] + m[8]*m[3]*m[5];
+ inv[15] = m[0]*m[5]*m[10] - m[0]*m[6]*m[9] - m[4]*m[1]*m[10]
+ + m[4]*m[2]*m[9] + m[8]*m[1]*m[6] - m[8]*m[2]*m[5];
+
+ det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12];
+ if (det == 0)
+ return GL_FALSE;
+
+ det=1.0f/det;
+
+ for (i = 0; i < 16; i++)
+ invOut[i] = inv[i] * det;
+
+ return GL_TRUE;
+}
+
+// Based on glu implementation
+GLint glProject(
+ float objx, float objy, float objz,
+ const float modelMatrix[16],
+ const float projMatrix[16],
+ const GLint viewport[4],
+ float* winx, float* winy, float* winz)
+{
+ float t1[4] = {objx, objy, objz, 1.0f};
+ float t2[4];
+
+ MatMul<4,4,1,float>(t2, modelMatrix, t1);
+ MatMul<4,4,1,float>(t1, projMatrix, t2);
+
+ if (t1[3] == 0.0) {
+ return(GL_FALSE);
+ }
+
+ // Normalise
+ t1[0]/=t1[3];
+ t1[1]/=t1[3];
+ t1[2]/=t1[3];
+
+ // Map x, y and z to range 0-1
+ t1[0]=t1[0]*0.5f+0.5f;
+ t1[1]=t1[1]*0.5f+0.5f;
+ t1[2]=t1[2]*0.5f+0.5f;
+
+ // Map x,y to viewport
+ t1[0]=t1[0] * viewport[2] + viewport[0];
+ t1[1]=t1[1] * viewport[3] + viewport[1];
+
+ *winx=t1[0];
+ *winy=t1[1];
+ *winz=t1[2];
+
+ return GL_TRUE;
+}
+
+// Based on glu implementation
+GLint glUnProject(
+ float winx, float winy, float winz,
+ const float mv[16],
+ const float proj[16],
+ const GLint viewport[4],
+ float* objx, float* objy, float* objz)
+{
+ float t1[16];
+
+ MatMul<4,4,4,float>(t1, proj, mv);
+
+ if (!InvertMatrix<float>(t1, t1)) {
+ return(GL_FALSE);
+ }
+
+ // Map x and y from window coordinates
+ float in[4] = {winx, winy, winz, 1.0f};
+ in[0] = (in[0] - viewport[0]) / viewport[2];
+ in[1] = (in[1] - viewport[1]) / viewport[3];
+
+ // Map to range -1 to 1
+ in[0] = in[0] * 2 - 1;
+ in[1] = in[1] * 2 - 1;
+ in[2] = in[2] * 2 - 1;
+
+ float out[4];
+ MatMul<4,4,1,float>(out, t1, in);
+
+ if (out[3] == 0.0) {
+ return(GL_FALSE);
+ }
+
+ // Normalise
+ out[0] /= out[3];
+ out[1] /= out[3];
+ out[2] /= out[3];
+
+ // Copy out
+ *objx = out[0];
+ *objy = out[1];
+ *objz = out[2];
+
+ return GL_TRUE;
+}
+
+// Based on glu implementation
+GLint glProject(
+ double objx, double objy, double objz,
+ const double modelMatrix[16],
+ const double projMatrix[16],
+ const GLint viewport[4],
+ double* winx, double* winy, double* winz)
+{
+ double t1[4] = {objx, objy, objz, 1.0f};
+ double t2[4];
+
+ MatMul<4,4,1,double>(t2, modelMatrix, t1);
+ MatMul<4,4,1,double>(t1, projMatrix, t2);
+
+ if (t1[3] == 0.0) {
+ return(GL_FALSE);
+ }
+
+ // Normalise
+ t1[0]/=t1[3];
+ t1[1]/=t1[3];
+ t1[2]/=t1[3];
+
+ // Map x, y and z to range 0-1
+ t1[0]=t1[0]*0.5f+0.5f;
+ t1[1]=t1[1]*0.5f+0.5f;
+ t1[2]=t1[2]*0.5f+0.5f;
+
+ // Map x,y to viewport
+ t1[0]=t1[0] * viewport[2] + viewport[0];
+ t1[1]=t1[1] * viewport[3] + viewport[1];
+
+ *winx=t1[0];
+ *winy=t1[1];
+ *winz=t1[2];
+
+ return GL_TRUE;
+}
+
+// Based on glu implementation
+GLint glUnProject(
+ double winx, double winy, double winz,
+ const double mv[16],
+ const double proj[16],
+ const GLint viewport[4],
+ double* objx, double* objy, double* objz)
+{
+ double t1[16];
+
+ MatMul<4,4,4,double>(t1, proj, mv);
+
+ if (!InvertMatrix<double>(t1, t1)) {
+ return(GL_FALSE);
+ }
+
+ // Map x and y from window coordinates
+ double in[4] = {winx, winy, winz, 1.0f};
+ in[0] = (in[0] - viewport[0]) / viewport[2];
+ in[1] = (in[1] - viewport[1]) / viewport[3];
+
+ // Map to range -1 to 1
+ in[0] = in[0] * 2 - 1;
+ in[1] = in[1] * 2 - 1;
+ in[2] = in[2] * 2 - 1;
+
+ double out[4];
+ MatMul<4,4,1,double>(out, t1, in);
+
+ if (out[3] == 0.0) {
+ return(GL_FALSE);
+ }
+
+ // Normalise
+ out[0] /= out[3];
+ out[1] /= out[3];
+ out[2] /= out[3];
+
+ // Copy out
+ *objx = out[0];
+ *objy = out[1];
+ *objz = out[2];
+
+ return GL_TRUE;
+}
+
+
+}
diff --git a/externals/Pangolin/src/gl/gltext.cpp b/externals/Pangolin/src/gl/gltext.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4f33d5dacddd09b27aae5ffa5e04261a7b02a78
--- /dev/null
+++ b/externals/Pangolin/src/gl/gltext.cpp
@@ -0,0 +1,202 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/gl/gltext.h>
+#include <pangolin/gl/glsl.h>
+
+#ifdef BUILD_PANGOLIN_GUI
+#include <pangolin/display/display.h>
+#include <pangolin/display/view.h>
+#endif
+
+namespace pangolin
+{
+
+GlText::GlText()
+ : tex(NULL), width(0),
+ ymin(std::numeric_limits<GLfloat>::max()),
+ ymax(-std::numeric_limits<GLfloat>::max())
+{
+
+}
+
+GlText::GlText(const GlText& txt)
+ : tex(txt.tex), str(txt.str), width(txt.width),
+ ymin(txt.ymin), ymax(txt.ymax), vs(txt.vs)
+{
+}
+
+GlText::GlText(const GlTexture& font_tex)
+ : tex(&font_tex), width(0),
+ ymin(std::numeric_limits<GLfloat>::max()),
+ ymax(-std::numeric_limits<GLfloat>::max())
+{
+}
+
+void GlText::AddSpace(GLfloat s)
+{
+ width += s;
+}
+
+void GlText::Add(unsigned char c, const GlChar& glc)
+{
+ GLfloat x = width;
+
+ vs.push_back(glc.GetVert(0) + x);
+ vs.push_back(glc.GetVert(1) + x);
+ vs.push_back(glc.GetVert(2) + x);
+ vs.push_back(glc.GetVert(0) + x);
+ vs.push_back(glc.GetVert(2) + x);
+ vs.push_back(glc.GetVert(3) + x);
+
+ ymin = std::min(ymin, glc.YMin());
+ ymax = std::max(ymax, glc.YMax());
+ width = x + glc.StepX();
+
+ str.append(1,c);
+}
+
+void GlText::Clear()
+{
+ str.clear();
+ vs.clear();
+ width = 0;
+ ymin = +std::numeric_limits<GLfloat>::max();
+ ymax = -std::numeric_limits<GLfloat>::max();
+}
+
+void GlText::DrawGlSl() const
+{
+#if !defined(HAVE_GLES) || defined(HAVE_GLES_2)
+ if(vs.size() && tex) {
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_POSITION);
+ glEnableVertexAttribArray(pangolin::DEFAULT_LOCATION_TEXCOORD);
+
+ glVertexAttribPointer(pangolin::DEFAULT_LOCATION_POSITION, 2, GL_FLOAT, GL_FALSE, sizeof(XYUV), &vs[0].x);
+ glVertexAttribPointer(pangolin::DEFAULT_LOCATION_TEXCOORD, 2, GL_FLOAT, GL_FALSE, sizeof(XYUV), &vs[0].tu);
+
+ tex->Bind();
+ glEnable(GL_TEXTURE_2D);
+ glDrawArrays(GL_TRIANGLES, 0, (GLsizei)vs.size() );
+ glDisable(GL_TEXTURE_2D);
+
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_POSITION);
+ glDisableVertexAttribArray(pangolin::DEFAULT_LOCATION_TEXCOORD);
+ }
+#endif
+}
+
+void GlText::Draw() const
+{
+ if(vs.size() && tex) {
+ glVertexPointer(2, GL_FLOAT, sizeof(XYUV), &vs[0].x);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glTexCoordPointer(2, GL_FLOAT, sizeof(XYUV), &vs[0].tu);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+ tex->Bind();
+ glEnable(GL_TEXTURE_2D);
+ glDrawArrays(GL_TRIANGLES, 0, (GLsizei)vs.size() );
+ glDisable(GL_TEXTURE_2D);
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+ }
+}
+
+#ifdef BUILD_PANGOLIN_GUI
+void GlText::Draw(GLfloat x, GLfloat y, GLfloat z) const
+{
+ // find object point (x,y,z)' in pixel coords
+ GLdouble projection[16];
+ GLdouble modelview[16];
+ GLint view[4];
+ GLdouble scrn[3];
+
+#ifdef HAVE_GLES_2
+ std::copy(glEngine().projection.top().m, glEngine().projection.top().m+16, projection);
+ std::copy(glEngine().modelview.top().m, glEngine().modelview.top().m+16, modelview);
+#else
+ glGetDoublev(GL_PROJECTION_MATRIX, projection );
+ glGetDoublev(GL_MODELVIEW_MATRIX, modelview );
+#endif
+ glGetIntegerv(GL_VIEWPORT, view );
+
+ pangolin::glProject(x, y, z, modelview, projection, view,
+ scrn, scrn + 1, scrn + 2);
+
+ DisplayBase().Activate();
+ glMatrixMode(GL_PROJECTION);
+ glPushMatrix();
+ glLoadIdentity();
+ glOrtho(-0.5, DisplayBase().v.w-0.5, -0.5, DisplayBase().v.h-0.5, -1, 1);
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+ glLoadIdentity();
+
+ glTranslatef(std::floor((GLfloat)scrn[0]), std::floor((GLfloat)scrn[1]), (GLfloat)scrn[2]);
+ Draw();
+
+ // Restore viewport
+ glViewport(view[0],view[1],view[2],view[3]);
+
+ // Restore modelview / project matrices
+ glMatrixMode(GL_PROJECTION);
+ glPopMatrix();
+ glMatrixMode(GL_MODELVIEW);
+ glPopMatrix();
+}
+
+// Render at (x,y) in window coordinates.
+void GlText::DrawWindow(GLfloat x, GLfloat y, GLfloat z) const
+{
+ // Backup viewport
+ GLint view[4];
+ glGetIntegerv(GL_VIEWPORT, view );
+
+ glMatrixMode(GL_PROJECTION);
+ glPushMatrix();
+ glMatrixMode(GL_MODELVIEW);
+ glPushMatrix();
+
+ DisplayBase().ActivatePixelOrthographic();
+
+ glTranslatef( std::floor(x), std::floor(y), z);
+ Draw();
+
+ // Restore viewport
+ glViewport(view[0],view[1],view[2],view[3]);
+
+ // Restore modelview / project matrices
+ glMatrixMode(GL_PROJECTION);
+ glPopMatrix();
+ glMatrixMode(GL_MODELVIEW);
+ glPopMatrix();
+}
+
+#endif // BUILD_PANGOLIN_GUI
+
+
+}
diff --git a/externals/Pangolin/src/gl/gltexturecache.cpp b/externals/Pangolin/src/gl/gltexturecache.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b4fe9313d9e2078c99866f45e3b38038767afcde
--- /dev/null
+++ b/externals/Pangolin/src/gl/gltexturecache.cpp
@@ -0,0 +1,38 @@
+/* This file is part of the Pangolin Project.
+* http://github.com/stevenlovegrove/Pangolin
+*
+* Copyright (c) 2013 Steven Lovegrove
+*
+* 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.
+*/
+
+#include <pangolin/gl/gltexturecache.h>
+
+namespace pangolin
+{
+
+TextureCache& TextureCache::I() {
+ static TextureCache instance;
+ return instance;
+}
+
+}
\ No newline at end of file
diff --git a/externals/Pangolin/src/gl/stb_truetype.h b/externals/Pangolin/src/gl/stb_truetype.h
new file mode 100644
index 0000000000000000000000000000000000000000..b3c2582cd3dfa1ea7d4a1a579247b3e89288ba4b
--- /dev/null
+++ b/externals/Pangolin/src/gl/stb_truetype.h
@@ -0,0 +1,3222 @@
+#pragma once
+
+// stb_truetype.h - v1.07 - public domain
+// authored from 2009-2015 by Sean Barrett / RAD Game Tools
+//
+// This library processes TrueType files:
+// parse files
+// extract glyph metrics
+// extract glyph shapes
+// render glyphs to one-channel bitmaps with antialiasing (box filter)
+//
+// Todo:
+// non-MS cmaps
+// crashproof on bad data
+// hinting? (no longer patented)
+// cleartype-style AA?
+// optimize: use simple memory allocator for intermediates
+// optimize: build edge-list directly from curves
+// optimize: rasterize directly from curves?
+//
+// ADDITIONAL CONTRIBUTORS
+//
+// Mikko Mononen: compound shape support, more cmap formats
+// Tor Andersson: kerning, subpixel rendering
+//
+// Bug/warning reports/fixes:
+// "Zer" on mollyrocket (with fix)
+// Cass Everitt
+// stoiko (Haemimont Games)
+// Brian Hook
+// Walter van Niftrik
+// David Gow
+// David Given
+// Ivan-Assen Ivanov
+// Anthony Pesch
+// Johan Duparc
+// Hou Qiming
+// Fabian "ryg" Giesen
+// Martins Mozeiko
+// Cap Petschulat
+// Omar Cornut
+// github:aloucks
+// Peter LaValle
+// Giumo X. Clanjor
+//
+// Misc other:
+// Ryan Gordon
+//
+// VERSION HISTORY
+//
+// 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints;
+// variant PackFontRanges to pack and render in separate phases;
+// fix stbtt_GetFontOFfsetForIndex (never worked for non-0 input?);
+// fixed an assert() bug in the new rasterizer
+// replace assert() with STBTT_assert() in new rasterizer
+// 1.06 (2015-07-14) performance improvements (~35% faster on x86 and x64 on test machine)
+// also more precise AA rasterizer, except if shapes overlap
+// remove need for STBTT_sort
+// 1.05 (2015-04-15) fix misplaced definitions for STBTT_STATIC
+// 1.04 (2015-04-15) typo in example
+// 1.03 (2015-04-12) STBTT_STATIC, fix memory leak in new packing, various fixes
+//
+// Full history can be found at the end of this file.
+//
+// LICENSE
+//
+// This software is in the public domain. Where that dedication is not
+// recognized, you are granted a perpetual, irrevocable license to copy,
+// distribute, and modify this file as you see fit.
+//
+// USAGE
+//
+// Include this file in whatever places neeed to refer to it. In ONE C/C++
+// file, write:
+// #define STB_TRUETYPE_IMPLEMENTATION
+// before the #include of this file. This expands out the actual
+// implementation into that C/C++ file.
+//
+// To make the implementation private to the file that generates the implementation,
+// #define STBTT_STATIC
+//
+// Simple 3D API (don't ship this, but it's fine for tools and quick start)
+// stbtt_BakeFontBitmap() -- bake a font to a bitmap for use as texture
+// stbtt_GetBakedQuad() -- compute quad to draw for a given char
+//
+// Improved 3D API (more shippable):
+// #include "stb_rect_pack.h" -- optional, but you really want it
+// stbtt_PackBegin()
+// stbtt_PackSetOversample() -- for improved quality on small fonts
+// stbtt_PackFontRanges() -- pack and renders
+// stbtt_PackEnd()
+// stbtt_GetPackedQuad()
+//
+// "Load" a font file from a memory buffer (you have to keep the buffer loaded)
+// stbtt_InitFont()
+// stbtt_GetFontOffsetForIndex() -- use for TTC font collections
+//
+// Render a unicode codepoint to a bitmap
+// stbtt_GetCodepointBitmap() -- allocates and returns a bitmap
+// stbtt_MakeCodepointBitmap() -- renders into bitmap you provide
+// stbtt_GetCodepointBitmapBox() -- how big the bitmap must be
+//
+// Character advance/positioning
+// stbtt_GetCodepointHMetrics()
+// stbtt_GetFontVMetrics()
+// stbtt_GetCodepointKernAdvance()
+//
+// Starting with version 1.06, the rasterizer was replaced with a new,
+// faster and generally-more-precise rasterizer. The new rasterizer more
+// accurately measures pixel coverage for anti-aliasing, except in the case
+// where multiple shapes overlap, in which case it overestimates the AA pixel
+// coverage. Thus, anti-aliasing of intersecting shapes may look wrong. If
+// this turns out to be a problem, you can re-enable the old rasterizer with
+// #define STBTT_RASTERIZER_VERSION 1
+// which will incur about a 15% speed hit.
+//
+// ADDITIONAL DOCUMENTATION
+//
+// Immediately after this block comment are a series of sample programs.
+//
+// After the sample programs is the "header file" section. This section
+// includes documentation for each API function.
+//
+// Some important concepts to understand to use this library:
+//
+// Codepoint
+// Characters are defined by unicode codepoints, e.g. 65 is
+// uppercase A, 231 is lowercase c with a cedilla, 0x7e30 is
+// the hiragana for "ma".
+//
+// Glyph
+// A visual character shape (every codepoint is rendered as
+// some glyph)
+//
+// Glyph index
+// A font-specific integer ID representing a glyph
+//
+// Baseline
+// Glyph shapes are defined relative to a baseline, which is the
+// bottom of uppercase characters. Characters extend both above
+// and below the baseline.
+//
+// Current Point
+// As you draw text to the screen, you keep track of a "current point"
+// which is the origin of each character. The current point's vertical
+// position is the baseline. Even "baked fonts" use this model.
+//
+// Vertical Font Metrics
+// The vertical qualities of the font, used to vertically position
+// and space the characters. See docs for stbtt_GetFontVMetrics.
+//
+// Font Size in Pixels or Points
+// The preferred interface for specifying font sizes in stb_truetype
+// is to specify how tall the font's vertical extent should be in pixels.
+// If that sounds good enough, skip the next paragraph.
+//
+// Most font APIs instead use "points", which are a common typographic
+// measurement for describing font size, defined as 72 points per inch.
+// stb_truetype provides a point API for compatibility. However, true
+// "per inch" conventions don't make much sense on computer displays
+// since they different monitors have different number of pixels per
+// inch. For example, Windows traditionally uses a convention that
+// there are 96 pixels per inch, thus making 'inch' measurements have
+// nothing to do with inches, and thus effectively defining a point to
+// be 1.333 pixels. Additionally, the TrueType font data provides
+// an explicit scale factor to scale a given font's glyphs to points,
+// but the author has observed that this scale factor is often wrong
+// for non-commercial fonts, thus making fonts scaled in points
+// according to the TrueType spec incoherently sized in practice.
+//
+// ADVANCED USAGE
+//
+// Quality:
+//
+// - Use the functions with Subpixel at the end to allow your characters
+// to have subpixel positioning. Since the font is anti-aliased, not
+// hinted, this is very import for quality. (This is not possible with
+// baked fonts.)
+//
+// - Kerning is now supported, and if you're supporting subpixel rendering
+// then kerning is worth using to give your text a polished look.
+//
+// Performance:
+//
+// - Convert Unicode codepoints to glyph indexes and operate on the glyphs;
+// if you don't do this, stb_truetype is forced to do the conversion on
+// every call.
+//
+// - There are a lot of memory allocations. We should modify it to take
+// a temp buffer and allocate from the temp buffer (without freeing),
+// should help performance a lot.
+//
+// NOTES
+//
+// The system uses the raw data found in the .ttf file without changing it
+// and without building auxiliary data structures. This is a bit inefficient
+// on little-endian systems (the data is big-endian), but assuming you're
+// caching the bitmaps or glyph shapes this shouldn't be a big deal.
+//
+// It appears to be very hard to programmatically determine what font a
+// given file is in a general way. I provide an API for this, but I don't
+// recommend it.
+//
+//
+// SOURCE STATISTICS (based on v0.6c, 2050 LOC)
+//
+// Documentation & header file 520 LOC \___ 660 LOC documentation
+// Sample code 140 LOC /
+// Truetype parsing 620 LOC ---- 620 LOC TrueType
+// Software rasterization 240 LOC \ .
+// Curve tesselation 120 LOC \__ 550 LOC Bitmap creation
+// Bitmap management 100 LOC /
+// Baked bitmap interface 70 LOC /
+// Font name matching & access 150 LOC ---- 150
+// C runtime library abstraction 60 LOC ---- 60
+//
+//
+// PERFORMANCE MEASUREMENTS FOR 1.06:
+//
+// 32-bit 64-bit
+// Previous release: 8.83 s 7.68 s
+// Pool allocations: 7.72 s 6.34 s
+// Inline sort : 6.54 s 5.65 s
+// New rasterizer : 5.63 s 5.00 s
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+////
+//// SAMPLE PROGRAMS
+////
+//
+// Incomplete text-in-3d-api example, which draws quads properly aligned to be lossless
+//
+#if 0
+#define STB_TRUETYPE_IMPLEMENTATION // force following include to generate implementation
+#include "stb_truetype.h"
+
+unsigned char ttf_buffer[1<<20];
+unsigned char temp_bitmap[512*512];
+
+stbtt_bakedchar cdata[96]; // ASCII 32..126 is 95 glyphs
+GLuint ftex;
+
+void my_stbtt_initfont(void)
+{
+ fread(ttf_buffer, 1, 1<<20, fopen("c:/windows/fonts/times.ttf", "rb"));
+ stbtt_BakeFontBitmap(ttf_buffer,0, 32.0, temp_bitmap,512,512, 32,96, cdata); // no guarantee this fits!
+ // can free ttf_buffer at this point
+ glGenTextures(1, &ftex);
+ glBindTexture(GL_TEXTURE_2D, ftex);
+ glTexImage2D(GL_TEXTURE_2D, 0, GL_ALPHA, 512,512, 0, GL_ALPHA, GL_UNSIGNED_BYTE, temp_bitmap);
+ // can free temp_bitmap at this point
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+}
+
+void my_stbtt_print(float x, float y, char *text)
+{
+ // assume orthographic projection with units = screen pixels, origin at top left
+ glEnable(GL_TEXTURE_2D);
+ glBindTexture(GL_TEXTURE_2D, ftex);
+ glBegin(GL_QUADS);
+ while (*text) {
+ if (*text >= 32 && *text < 128) {
+ stbtt_aligned_quad q;
+ stbtt_GetBakedQuad(cdata, 512,512, *text-32, &x,&y,&q,1);//1=opengl & d3d10+,0=d3d9
+ glTexCoord2f(q.s0,q.t1); glVertex2f(q.x0,q.y0);
+ glTexCoord2f(q.s1,q.t1); glVertex2f(q.x1,q.y0);
+ glTexCoord2f(q.s1,q.t0); glVertex2f(q.x1,q.y1);
+ glTexCoord2f(q.s0,q.t0); glVertex2f(q.x0,q.y1);
+ }
+ ++text;
+ }
+ glEnd();
+}
+#endif
+//
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Complete program (this compiles): get a single bitmap, print as ASCII art
+//
+#if 0
+#include <stdio.h>
+#define STB_TRUETYPE_IMPLEMENTATION // force following include to generate implementation
+#include "stb_truetype.h"
+
+char ttf_buffer[1<<25];
+
+int main(int argc, char **argv)
+{
+ stbtt_fontinfo font;
+ unsigned char *bitmap;
+ int w,h,i,j,c = (argc > 1 ? atoi(argv[1]) : 'a'), s = (argc > 2 ? atoi(argv[2]) : 20);
+
+ fread(ttf_buffer, 1, 1<<25, fopen(argc > 3 ? argv[3] : "c:/windows/fonts/arialbd.ttf", "rb"));
+
+ stbtt_InitFont(&font, ttf_buffer, stbtt_GetFontOffsetForIndex(ttf_buffer,0));
+ bitmap = stbtt_GetCodepointBitmap(&font, 0,stbtt_ScaleForPixelHeight(&font, s), c, &w, &h, 0,0);
+
+ for (j=0; j < h; ++j) {
+ for (i=0; i < w; ++i)
+ putchar(" .:ioVM@"[bitmap[j*w+i]>>5]);
+ putchar('\n');
+ }
+ return 0;
+}
+#endif
+//
+// Output:
+//
+// .ii.
+// @@@@@@.
+// V@Mio@@o
+// :i. V@V
+// :oM@@M
+// :@@@MM@M
+// @@o o@M
+// :@@. M@M
+// @@@o@@@@
+// :M@@V:@@.
+//
+//////////////////////////////////////////////////////////////////////////////
+//
+// Complete program: print "Hello World!" banner, with bugs
+//
+#if 0
+char buffer[24<<20];
+unsigned char screen[20][79];
+
+int main(int arg, char **argv)
+{
+ stbtt_fontinfo font;
+ int i,j,ascent,baseline,ch=0;
+ float scale, xpos=2; // leave a little padding in case the character extends left
+ char *text = "Heljo World!"; // intentionally misspelled to show 'lj' brokenness
+
+ fread(buffer, 1, 1000000, fopen("c:/windows/fonts/arialbd.ttf", "rb"));
+ stbtt_InitFont(&font, buffer, 0);
+
+ scale = stbtt_ScaleForPixelHeight(&font, 15);
+ stbtt_GetFontVMetrics(&font, &ascent,0,0);
+ baseline = (int) (ascent*scale);
+
+ while (text[ch]) {
+ int advance,lsb,x0,y0,x1,y1;
+ float x_shift = xpos - (float) floor(xpos);
+ stbtt_GetCodepointHMetrics(&font, text[ch], &advance, &lsb);
+ stbtt_GetCodepointBitmapBoxSubpixel(&font, text[ch], scale,scale,x_shift,0, &x0,&y0,&x1,&y1);
+ stbtt_MakeCodepointBitmapSubpixel(&font, &screen[baseline + y0][(int) xpos + x0], x1-x0,y1-y0, 79, scale,scale,x_shift,0, text[ch]);
+ // note that this stomps the old data, so where character boxes overlap (e.g. 'lj') it's wrong
+ // because this API is really for baking character bitmaps into textures. if you want to render
+ // a sequence of characters, you really need to render each bitmap to a temp buffer, then
+ // "alpha blend" that into the working buffer
+ xpos += (advance * scale);
+ if (text[ch+1])
+ xpos += scale*stbtt_GetCodepointKernAdvance(&font, text[ch],text[ch+1]);
+ ++ch;
+ }
+
+ for (j=0; j < 20; ++j) {
+ for (i=0; i < 78; ++i)
+ putchar(" .:ioVM@"[screen[j][i]>>5]);
+ putchar('\n');
+ }
+
+ return 0;
+}
+#endif
+
+
+//////////////////////////////////////////////////////////////////////////////
+//////////////////////////////////////////////////////////////////////////////
+////
+//// INTEGRATION WITH YOUR CODEBASE
+////
+//// The following sections allow you to supply alternate definitions
+//// of C library functions used by stb_truetype.
+
+#ifdef STB_TRUETYPE_IMPLEMENTATION
+ // #define your own (u)stbtt_int8/16/32 before including to override this
+ #ifndef stbtt_uint8
+ typedef unsigned char stbtt_uint8;
+ typedef signed char stbtt_int8;
+ typedef unsigned short stbtt_uint16;
+ typedef signed short stbtt_int16;
+ typedef unsigned int stbtt_uint32;
+ typedef signed int stbtt_int32;
+ #endif
+
+ typedef char stbtt__check_size32[sizeof(stbtt_int32)==4 ? 1 : -1];
+ typedef char stbtt__check_size16[sizeof(stbtt_int16)==2 ? 1 : -1];
+
+ // #define your own STBTT_ifloor/STBTT_iceil() to avoid math.h
+ #ifndef STBTT_ifloor
+ #include <math.h>
+ #define STBTT_ifloor(x) ((int) floor(x))
+ #define STBTT_iceil(x) ((int) ceil(x))
+ #endif
+
+ #ifndef STBTT_sqrt
+ #include <math.h>
+ #define STBTT_sqrt(x) sqrt(x)
+ #endif
+
+ // #define your own functions "STBTT_malloc" / "STBTT_free" to avoid malloc.h
+ #ifndef STBTT_malloc
+ #include <stdlib.h>
+ #define STBTT_malloc(x,u) ((void)(u),malloc(x))
+ #define STBTT_free(x,u) ((void)(u),free(x))
+ #endif
+
+ #ifndef STBTT_assert
+ #include <assert.h>
+ #define STBTT_assert(x) assert(x)
+ #endif
+
+ #ifndef STBTT_strlen
+ #include <string.h>
+ #define STBTT_strlen(x) strlen(x)
+ #endif
+
+ #ifndef STBTT_memcpy
+ #include <memory.h>
+ #define STBTT_memcpy memcpy
+ #define STBTT_memset memset
+ #endif
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+////
+//// INTERFACE
+////
+////
+
+#ifndef __STB_INCLUDE_STB_TRUETYPE_H__
+#define __STB_INCLUDE_STB_TRUETYPE_H__
+
+#ifdef STBTT_STATIC
+#define STBTT_DEF static
+#else
+#define STBTT_DEF extern
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// TEXTURE BAKING API
+//
+// If you use this API, you only have to call two functions ever.
+//
+
+typedef struct
+{
+ unsigned short x0,y0,x1,y1; // coordinates of bbox in bitmap
+ float xoff,yoff,xadvance;
+} stbtt_bakedchar;
+
+STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, // font location (use offset=0 for plain .ttf)
+ float pixel_height, // height of font in pixels
+ unsigned char *pixels, int pw, int ph, // bitmap to be filled in
+ int first_char, int num_chars, // characters to bake
+ stbtt_bakedchar *chardata); // you allocate this, it's num_chars long
+// if return is positive, the first unused row of the bitmap
+// if return is negative, returns the negative of the number of characters that fit
+// if return is 0, no characters fit and no rows were used
+// This uses a very crappy packing.
+
+typedef struct
+{
+ float x0,y0,s0,t0; // top-left
+ float x1,y1,s1,t1; // bottom-right
+} stbtt_aligned_quad;
+
+STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, // same data as above
+ int char_index, // character to display
+ float *xpos, float *ypos, // pointers to current position in screen pixel space
+ stbtt_aligned_quad *q, // output: quad to draw
+ int opengl_fillrule); // true if opengl fill rule; false if DX9 or earlier
+// Call GetBakedQuad with char_index = 'character - first_char', and it
+// creates the quad you need to draw and advances the current position.
+//
+// The coordinate system used assumes y increases downwards.
+//
+// Characters will extend both above and below the current position;
+// see discussion of "BASELINE" above.
+//
+// It's inefficient; you might want to c&p it and optimize it.
+
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// NEW TEXTURE BAKING API
+//
+// This provides options for packing multiple fonts into one atlas, not
+// perfectly but better than nothing.
+
+typedef struct
+{
+ unsigned short x0,y0,x1,y1; // coordinates of bbox in bitmap
+ float xoff,yoff,xadvance;
+ float xoff2,yoff2;
+} stbtt_packedchar;
+
+typedef struct stbtt_pack_context stbtt_pack_context;
+typedef struct stbtt_fontinfo stbtt_fontinfo;
+#ifndef STB_RECT_PACK_VERSION
+typedef struct stbrp_rect stbrp_rect;
+#endif
+
+STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, int width, int height, int stride_in_bytes, int padding, void *alloc_context);
+// Initializes a packing context stored in the passed-in stbtt_pack_context.
+// Future calls using this context will pack characters into the bitmap passed
+// in here: a 1-channel bitmap that is weight x height. stride_in_bytes is
+// the distance from one row to the next (or 0 to mean they are packed tightly
+// together). "padding" is the amount of padding to leave between each
+// character (normally you want '1' for bitmaps you'll use as textures with
+// bilinear filtering).
+//
+// Returns 0 on failure, 1 on success.
+
+STBTT_DEF void stbtt_PackEnd (stbtt_pack_context *spc);
+// Cleans up the packing context and frees all memory.
+
+#define STBTT_POINT_SIZE(x) (-(x))
+
+STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, float font_size,
+ int first_unicode_char_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range);
+// Creates character bitmaps from the font_index'th font found in fontdata (use
+// font_index=0 if you don't know what that is). It creates num_chars_in_range
+// bitmaps for characters with unicode values starting at first_unicode_char_in_range
+// and increasing. Data for how to render them is stored in chardata_for_range;
+// pass these to stbtt_GetPackedQuad to get back renderable quads.
+//
+// font_size is the full height of the character from ascender to descender,
+// as computed by stbtt_ScaleForPixelHeight. To use a point size as computed
+// by stbtt_ScaleForMappingEmToPixels, wrap the point size in STBTT_POINT_SIZE()
+// and pass that result as 'font_size':
+// ..., 20 , ... // font max minus min y is 20 pixels tall
+// ..., STBTT_POINT_SIZE(20), ... // 'M' is 20 pixels tall
+
+typedef struct
+{
+ float font_size;
+ int first_unicode_codepoint_in_range; // if non-zero, then the chars are continuous, and this is the first codepoint
+ int *array_of_unicode_codepoints; // if non-zero, then this is an array of unicode codepoints
+ int num_chars;
+ stbtt_packedchar *chardata_for_range; // output
+ unsigned char h_oversample, v_oversample; // don't set these, they're used internally
+} stbtt_pack_range;
+
+STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges);
+// Creates character bitmaps from multiple ranges of characters stored in
+// ranges. This will usually create a better-packed bitmap than multiple
+// calls to stbtt_PackFontRange. Note that you can call this multiple
+// times within a single PackBegin/PackEnd.
+
+STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h_oversample, unsigned int v_oversample);
+// Oversampling a font increases the quality by allowing higher-quality subpixel
+// positioning, and is especially valuable at smaller text sizes.
+//
+// This function sets the amount of oversampling for all following calls to
+// stbtt_PackFontRange(s) or stbtt_PackFontRangesGatherRects for a given
+// pack context. The default (no oversampling) is achieved by h_oversample=1
+// and v_oversample=1. The total number of pixels required is
+// h_oversample*v_oversample larger than the default; for example, 2x2
+// oversampling requires 4x the storage of 1x1. For best results, render
+// oversampled textures with bilinear filtering. Look at the readme in
+// stb/tests/oversample for information about oversampled fonts
+//
+// To use with PackFontRangesGather etc., you must set it before calls
+// call to PackFontRangesGatherRects.
+
+STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, // same data as above
+ int char_index, // character to display
+ float *xpos, float *ypos, // pointers to current position in screen pixel space
+ stbtt_aligned_quad *q, // output: quad to draw
+ int align_to_integer);
+
+STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects);
+STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect *rects, int num_rects);
+STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects);
+// Calling these functions in sequence is roughly equivalent to calling
+// stbtt_PackFontRanges(). If you more control over the packing of multiple
+// fonts, or if you want to pack custom data into a font texture, take a look
+// at the source to of stbtt_PackFontRanges() and create a custom version
+// using these functions, e.g. call GatherRects multiple times,
+// building up a single array of rects, then call PackRects once,
+// then call RenderIntoRects repeatedly. This may result in a
+// better packing than calling PackFontRanges multiple times
+// (or it may not).
+
+// this is an opaque structure that you shouldn't mess with which holds
+// all the context needed from PackBegin to PackEnd.
+struct stbtt_pack_context {
+ void *user_allocator_context;
+ void *pack_info;
+ int width;
+ int height;
+ int stride_in_bytes;
+ int padding;
+ unsigned int h_oversample, v_oversample;
+ unsigned char *pixels;
+ void *nodes;
+};
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// FONT LOADING
+//
+//
+
+STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *data, int index);
+// Each .ttf/.ttc file may have more than one font. Each font has a sequential
+// index number starting from 0. Call this function to get the font offset for
+// a given index; it returns -1 if the index is out of range. A regular .ttf
+// file will only define one font and it always be at offset 0, so it will
+// return '0' for index 0, and -1 for all other indices. You can just skip
+// this step if you know it's that kind of font.
+
+
+// The following structure is defined publically so you can declare one on
+// the stack or as a global or etc, but you should treat it as opaque.
+typedef struct stbtt_fontinfo
+{
+ void * userdata;
+ unsigned char * data; // pointer to .ttf file
+ int fontstart; // offset of start of font
+
+ int numGlyphs; // number of glyphs, needed for range checking
+
+ int loca,head,glyf,hhea,hmtx,kern; // table locations as offset from start of .ttf
+ int index_map; // a cmap mapping for our chosen character encoding
+ int indexToLocFormat; // format needed to map from glyph index to glyph
+} stbtt_fontinfo;
+
+STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data, int offset);
+// Given an offset into the file that defines a font, this function builds
+// the necessary cached info for the rest of the system. You must allocate
+// the stbtt_fontinfo yourself, and stbtt_InitFont will fill it out. You don't
+// need to do anything special to free it, because the contents are pure
+// value data with no additional data structures. Returns 0 on failure.
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// CHARACTER TO GLYPH-INDEX CONVERSIOn
+
+STBTT_DEF int stbtt_FindGlyphIndex(const stbtt_fontinfo *info, int unicode_codepoint);
+// If you're going to perform multiple operations on the same character
+// and you want a speed-up, call this function with the character you're
+// going to process, then use glyph-based functions instead of the
+// codepoint-based functions.
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// CHARACTER PROPERTIES
+//
+
+STBTT_DEF float stbtt_ScaleForPixelHeight(const stbtt_fontinfo *info, float pixels);
+// computes a scale factor to produce a font whose "height" is 'pixels' tall.
+// Height is measured as the distance from the highest ascender to the lowest
+// descender; in other words, it's equivalent to calling stbtt_GetFontVMetrics
+// and computing:
+// scale = pixels / (ascent - descent)
+// so if you prefer to measure height by the ascent only, use a similar calculation.
+
+STBTT_DEF float stbtt_ScaleForMappingEmToPixels(const stbtt_fontinfo *info, float pixels);
+// computes a scale factor to produce a font whose EM size is mapped to
+// 'pixels' tall. This is probably what traditional APIs compute, but
+// I'm not positive.
+
+STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, int *descent, int *lineGap);
+// ascent is the coordinate above the baseline the font extends; descent
+// is the coordinate below the baseline the font extends (i.e. it is typically negative)
+// lineGap is the spacing between one row's descent and the next row's ascent...
+// so you should advance the vertical position by "*ascent - *descent + *lineGap"
+// these are expressed in unscaled coordinates, so you must multiply by
+// the scale factor for a given size
+
+STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1);
+// the bounding box around all possible characters
+
+STBTT_DEF void stbtt_GetCodepointHMetrics(const stbtt_fontinfo *info, int codepoint, int *advanceWidth, int *leftSideBearing);
+// leftSideBearing is the offset from the current horizontal position to the left edge of the character
+// advanceWidth is the offset from the current horizontal position to the next horizontal position
+// these are expressed in unscaled coordinates
+
+STBTT_DEF int stbtt_GetCodepointKernAdvance(const stbtt_fontinfo *info, int ch1, int ch2);
+// an additional amount to add to the 'advance' value between ch1 and ch2
+
+STBTT_DEF int stbtt_GetCodepointBox(const stbtt_fontinfo *info, int codepoint, int *x0, int *y0, int *x1, int *y1);
+// Gets the bounding box of the visible part of the glyph, in unscaled coordinates
+
+STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_index, int *advanceWidth, int *leftSideBearing);
+STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2);
+STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1);
+// as above, but takes one or more glyph indices for greater efficiency
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// GLYPH SHAPES (you probably don't need these, but they have to go before
+// the bitmaps for C declaration-order reasons)
+//
+
+#ifndef STBTT_vmove // you can predefine these to use different values (but why?)
+ enum {
+ STBTT_vmove=1,
+ STBTT_vline,
+ STBTT_vcurve
+ };
+#endif
+
+#ifndef stbtt_vertex // you can predefine this to use different values
+ // (we share this with other code at RAD)
+ #define stbtt_vertex_type short // can't use stbtt_int16 because that's not visible in the header file
+ typedef struct
+ {
+ stbtt_vertex_type x,y,cx,cy;
+ unsigned char type,padding;
+ } stbtt_vertex;
+#endif
+
+STBTT_DEF int stbtt_IsGlyphEmpty(const stbtt_fontinfo *info, int glyph_index);
+// returns non-zero if nothing is drawn for this glyph
+
+STBTT_DEF int stbtt_GetCodepointShape(const stbtt_fontinfo *info, int unicode_codepoint, stbtt_vertex **vertices);
+STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **vertices);
+// returns # of vertices and fills *vertices with the pointer to them
+// these are expressed in "unscaled" coordinates
+//
+// The shape is a series of countours. Each one starts with
+// a STBTT_moveto, then consists of a series of mixed
+// STBTT_lineto and STBTT_curveto segments. A lineto
+// draws a line from previous endpoint to its x,y; a curveto
+// draws a quadratic bezier from previous endpoint to
+// its x,y, using cx,cy as the bezier control point.
+
+STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *vertices);
+// frees the data allocated above
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// BITMAP RENDERING
+//
+
+STBTT_DEF void stbtt_FreeBitmap(unsigned char *bitmap, void *userdata);
+// frees the bitmap allocated below
+
+STBTT_DEF unsigned char *stbtt_GetCodepointBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int codepoint, int *width, int *height, int *xoff, int *yoff);
+// allocates a large-enough single-channel 8bpp bitmap and renders the
+// specified character/glyph at the specified scale into it, with
+// antialiasing. 0 is no coverage (transparent), 255 is fully covered (opaque).
+// *width & *height are filled out with the width & height of the bitmap,
+// which is stored left-to-right, top-to-bottom.
+//
+// xoff/yoff are the offset it pixel space from the glyph origin to the top-left of the bitmap
+
+STBTT_DEF unsigned char *stbtt_GetCodepointBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint, int *width, int *height, int *xoff, int *yoff);
+// the same as stbtt_GetCodepoitnBitmap, but you can specify a subpixel
+// shift for the character
+
+STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int codepoint);
+// the same as stbtt_GetCodepointBitmap, but you pass in storage for the bitmap
+// in the form of 'output', with row spacing of 'out_stride' bytes. the bitmap
+// is clipped to out_w/out_h bytes. Call stbtt_GetCodepointBitmapBox to get the
+// width and height and positioning info for it first.
+
+STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint);
+// same as stbtt_MakeCodepointBitmap, but you can specify a subpixel
+// shift for the character
+
+STBTT_DEF void stbtt_GetCodepointBitmapBox(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1);
+// get the bbox of the bitmap centered around the glyph origin; so the
+// bitmap width is ix1-ix0, height is iy1-iy0, and location to place
+// the bitmap top left is (leftSideBearing*scale,iy0).
+// (Note that the bitmap uses y-increases-down, but the shape uses
+// y-increases-up, so CodepointBitmapBox and CodepointBox are inverted.)
+
+STBTT_DEF void stbtt_GetCodepointBitmapBoxSubpixel(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1);
+// same as stbtt_GetCodepointBitmapBox, but you can specify a subpixel
+// shift for the character
+
+// the following functions are equivalent to the above functions, but operate
+// on glyph indices instead of Unicode codepoints (for efficiency)
+STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int glyph, int *width, int *height, int *xoff, int *yoff);
+STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int glyph, int *width, int *height, int *xoff, int *yoff);
+STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int glyph);
+STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int glyph);
+STBTT_DEF void stbtt_GetGlyphBitmapBox(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1);
+STBTT_DEF void stbtt_GetGlyphBitmapBoxSubpixel(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y,float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1);
+
+
+// @TODO: don't expose this structure
+typedef struct
+{
+ int w,h,stride;
+ unsigned char *pixels;
+} stbtt__bitmap;
+
+STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, float flatness_in_pixels, stbtt_vertex *vertices, int num_verts, float scale_x, float scale_y, float shift_x, float shift_y, int x_off, int y_off, int invert, void *userdata);
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Finding the right font...
+//
+// You should really just solve this offline, keep your own tables
+// of what font is what, and don't try to get it out of the .ttf file.
+// That's because getting it out of the .ttf file is really hard, because
+// the names in the file can appear in many possible encodings, in many
+// possible languages, and e.g. if you need a case-insensitive comparison,
+// the details of that depend on the encoding & language in a complex way
+// (actually underspecified in truetype, but also gigantic).
+//
+// But you can use the provided functions in two possible ways:
+// stbtt_FindMatchingFont() will use *case-sensitive* comparisons on
+// unicode-encoded names to try to find the font you want;
+// you can run this before calling stbtt_InitFont()
+//
+// stbtt_GetFontNameString() lets you get any of the various strings
+// from the file yourself and do your own comparisons on them.
+// You have to have called stbtt_InitFont() first.
+
+
+STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *fontdata, const char *name, int flags);
+// returns the offset (not index) of the font that matches, or -1 if none
+// if you use STBTT_MACSTYLE_DONTCARE, use a font name like "Arial Bold".
+// if you use any other flag, use a font name like "Arial"; this checks
+// the 'macStyle' header field; i don't know if fonts set this consistently
+#define STBTT_MACSTYLE_DONTCARE 0
+#define STBTT_MACSTYLE_BOLD 1
+#define STBTT_MACSTYLE_ITALIC 2
+#define STBTT_MACSTYLE_UNDERSCORE 4
+#define STBTT_MACSTYLE_NONE 8 // <= not same as 0, this makes us check the bitfield is 0
+
+STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2);
+// returns 1/0 whether the first string interpreted as utf8 is identical to
+// the second string interpreted as big-endian utf16... useful for strings from next func
+
+STBTT_DEF const char *stbtt_GetFontNameString(const stbtt_fontinfo *font, int *length, int platformID, int encodingID, int languageID, int nameID);
+// returns the string (which may be big-endian double byte, e.g. for unicode)
+// and puts the length in bytes in *length.
+//
+// some of the values for the IDs are below; for more see the truetype spec:
+// http://developer.apple.com/textfonts/TTRefMan/RM06/Chap6name.html
+// http://www.microsoft.com/typography/otspec/name.htm
+
+enum { // platformID
+ STBTT_PLATFORM_ID_UNICODE =0,
+ STBTT_PLATFORM_ID_MAC =1,
+ STBTT_PLATFORM_ID_ISO =2,
+ STBTT_PLATFORM_ID_MICROSOFT =3
+};
+
+enum { // encodingID for STBTT_PLATFORM_ID_UNICODE
+ STBTT_UNICODE_EID_UNICODE_1_0 =0,
+ STBTT_UNICODE_EID_UNICODE_1_1 =1,
+ STBTT_UNICODE_EID_ISO_10646 =2,
+ STBTT_UNICODE_EID_UNICODE_2_0_BMP=3,
+ STBTT_UNICODE_EID_UNICODE_2_0_FULL=4
+};
+
+enum { // encodingID for STBTT_PLATFORM_ID_MICROSOFT
+ STBTT_MS_EID_SYMBOL =0,
+ STBTT_MS_EID_UNICODE_BMP =1,
+ STBTT_MS_EID_SHIFTJIS =2,
+ STBTT_MS_EID_UNICODE_FULL =10
+};
+
+enum { // encodingID for STBTT_PLATFORM_ID_MAC; same as Script Manager codes
+ STBTT_MAC_EID_ROMAN =0, STBTT_MAC_EID_ARABIC =4,
+ STBTT_MAC_EID_JAPANESE =1, STBTT_MAC_EID_HEBREW =5,
+ STBTT_MAC_EID_CHINESE_TRAD =2, STBTT_MAC_EID_GREEK =6,
+ STBTT_MAC_EID_KOREAN =3, STBTT_MAC_EID_RUSSIAN =7
+};
+
+enum { // languageID for STBTT_PLATFORM_ID_MICROSOFT; same as LCID...
+ // problematic because there are e.g. 16 english LCIDs and 16 arabic LCIDs
+ STBTT_MS_LANG_ENGLISH =0x0409, STBTT_MS_LANG_ITALIAN =0x0410,
+ STBTT_MS_LANG_CHINESE =0x0804, STBTT_MS_LANG_JAPANESE =0x0411,
+ STBTT_MS_LANG_DUTCH =0x0413, STBTT_MS_LANG_KOREAN =0x0412,
+ STBTT_MS_LANG_FRENCH =0x040c, STBTT_MS_LANG_RUSSIAN =0x0419,
+ STBTT_MS_LANG_GERMAN =0x0407, STBTT_MS_LANG_SPANISH =0x0409,
+ STBTT_MS_LANG_HEBREW =0x040d, STBTT_MS_LANG_SWEDISH =0x041D
+};
+
+enum { // languageID for STBTT_PLATFORM_ID_MAC
+ STBTT_MAC_LANG_ENGLISH =0 , STBTT_MAC_LANG_JAPANESE =11,
+ STBTT_MAC_LANG_ARABIC =12, STBTT_MAC_LANG_KOREAN =23,
+ STBTT_MAC_LANG_DUTCH =4 , STBTT_MAC_LANG_RUSSIAN =32,
+ STBTT_MAC_LANG_FRENCH =1 , STBTT_MAC_LANG_SPANISH =6 ,
+ STBTT_MAC_LANG_GERMAN =2 , STBTT_MAC_LANG_SWEDISH =5 ,
+ STBTT_MAC_LANG_HEBREW =10, STBTT_MAC_LANG_CHINESE_SIMPLIFIED =33,
+ STBTT_MAC_LANG_ITALIAN =3 , STBTT_MAC_LANG_CHINESE_TRAD =19
+};
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // __STB_INCLUDE_STB_TRUETYPE_H__
+
+///////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////
+////
+//// IMPLEMENTATION
+////
+////
+
+#ifdef STB_TRUETYPE_IMPLEMENTATION
+
+#ifndef STBTT_MAX_OVERSAMPLE
+#define STBTT_MAX_OVERSAMPLE 8
+#endif
+
+#if STBTT_MAX_OVERSAMPLE > 255
+#error "STBTT_MAX_OVERSAMPLE cannot be > 255"
+#endif
+
+typedef int stbtt__test_oversample_pow2[(STBTT_MAX_OVERSAMPLE & (STBTT_MAX_OVERSAMPLE-1)) == 0 ? 1 : -1];
+
+#ifndef STBTT_RASTERIZER_VERSION
+#define STBTT_RASTERIZER_VERSION 2
+#endif
+
+//////////////////////////////////////////////////////////////////////////
+//
+// accessors to parse data from file
+//
+
+// on platforms that don't allow misaligned reads, if we want to allow
+// truetype fonts that aren't padded to alignment, define ALLOW_UNALIGNED_TRUETYPE
+
+#define ttBYTE(p) (* (stbtt_uint8 *) (p))
+#define ttCHAR(p) (* (stbtt_int8 *) (p))
+#define ttFixed(p) ttLONG(p)
+
+#if defined(STB_TRUETYPE_BIGENDIAN) && !defined(ALLOW_UNALIGNED_TRUETYPE)
+
+ #define ttUSHORT(p) (* (stbtt_uint16 *) (p))
+ #define ttSHORT(p) (* (stbtt_int16 *) (p))
+ #define ttULONG(p) (* (stbtt_uint32 *) (p))
+ #define ttLONG(p) (* (stbtt_int32 *) (p))
+
+#else
+
+ static stbtt_uint16 ttUSHORT(const stbtt_uint8 *p) { return p[0]*256 + p[1]; }
+ static stbtt_int16 ttSHORT(const stbtt_uint8 *p) { return p[0]*256 + p[1]; }
+ static stbtt_uint32 ttULONG(const stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; }
+ static stbtt_int32 ttLONG(const stbtt_uint8 *p) { return (p[0]<<24) + (p[1]<<16) + (p[2]<<8) + p[3]; }
+
+#endif
+
+#define stbtt_tag4(p,c0,c1,c2,c3) ((p)[0] == (c0) && (p)[1] == (c1) && (p)[2] == (c2) && (p)[3] == (c3))
+#define stbtt_tag(p,str) stbtt_tag4(p,str[0],str[1],str[2],str[3])
+
+static int stbtt__isfont(const stbtt_uint8 *font)
+{
+ // check the version number
+ if (stbtt_tag4(font, '1',0,0,0)) return 1; // TrueType 1
+ if (stbtt_tag(font, "typ1")) return 1; // TrueType with type 1 font -- we don't support this!
+ if (stbtt_tag(font, "OTTO")) return 1; // OpenType with CFF
+ if (stbtt_tag4(font, 0,1,0,0)) return 1; // OpenType 1.0
+ return 0;
+}
+
+// @OPTIMIZE: binary search
+static stbtt_uint32 stbtt__find_table(stbtt_uint8 *data, stbtt_uint32 fontstart, const char *tag)
+{
+ stbtt_int32 num_tables = ttUSHORT(data+fontstart+4);
+ stbtt_uint32 tabledir = fontstart + 12;
+ stbtt_int32 i;
+ for (i=0; i < num_tables; ++i) {
+ stbtt_uint32 loc = tabledir + 16*i;
+ if (stbtt_tag(data+loc+0, tag))
+ return ttULONG(data+loc+8);
+ }
+ return 0;
+}
+
+STBTT_DEF int stbtt_GetFontOffsetForIndex(const unsigned char *font_collection, int index)
+{
+ // if it's just a font, there's only one valid index
+ if (stbtt__isfont(font_collection))
+ return index == 0 ? 0 : -1;
+
+ // check if it's a TTC
+ if (stbtt_tag(font_collection, "ttcf")) {
+ // version 1?
+ if (ttULONG(font_collection+4) == 0x00010000 || ttULONG(font_collection+4) == 0x00020000) {
+ stbtt_int32 n = ttLONG(font_collection+8);
+ if (index >= n)
+ return -1;
+ return ttULONG(font_collection+12+index*4);
+ }
+ }
+ return -1;
+}
+
+STBTT_DEF int stbtt_InitFont(stbtt_fontinfo *info, const unsigned char *data2, int fontstart)
+{
+ stbtt_uint8 *data = (stbtt_uint8 *) data2;
+ stbtt_uint32 cmap, t;
+ stbtt_int32 i,numTables;
+
+ info->data = data;
+ info->fontstart = fontstart;
+
+ cmap = stbtt__find_table(data, fontstart, "cmap"); // required
+ info->loca = stbtt__find_table(data, fontstart, "loca"); // required
+ info->head = stbtt__find_table(data, fontstart, "head"); // required
+ info->glyf = stbtt__find_table(data, fontstart, "glyf"); // required
+ info->hhea = stbtt__find_table(data, fontstart, "hhea"); // required
+ info->hmtx = stbtt__find_table(data, fontstart, "hmtx"); // required
+ info->kern = stbtt__find_table(data, fontstart, "kern"); // not required
+ if (!cmap || !info->loca || !info->head || !info->glyf || !info->hhea || !info->hmtx)
+ return 0;
+
+ t = stbtt__find_table(data, fontstart, "maxp");
+ if (t)
+ info->numGlyphs = ttUSHORT(data+t+4);
+ else
+ info->numGlyphs = 0xffff;
+
+ // find a cmap encoding table we understand *now* to avoid searching
+ // later. (todo: could make this installable)
+ // the same regardless of glyph.
+ numTables = ttUSHORT(data + cmap + 2);
+ info->index_map = 0;
+ for (i=0; i < numTables; ++i) {
+ stbtt_uint32 encoding_record = cmap + 4 + 8 * i;
+ // find an encoding we understand:
+ switch(ttUSHORT(data+encoding_record)) {
+ case STBTT_PLATFORM_ID_MICROSOFT:
+ switch (ttUSHORT(data+encoding_record+2)) {
+ case STBTT_MS_EID_UNICODE_BMP:
+ case STBTT_MS_EID_UNICODE_FULL:
+ // MS/Unicode
+ info->index_map = cmap + ttULONG(data+encoding_record+4);
+ break;
+ }
+ break;
+ case STBTT_PLATFORM_ID_UNICODE:
+ // Mac/iOS has these
+ // all the encodingIDs are unicode, so we don't bother to check it
+ info->index_map = cmap + ttULONG(data+encoding_record+4);
+ break;
+ }
+ }
+ if (info->index_map == 0)
+ return 0;
+
+ info->indexToLocFormat = ttUSHORT(data+info->head + 50);
+ return 1;
+}
+
+STBTT_DEF int stbtt_FindGlyphIndex(const stbtt_fontinfo *info, int unicode_codepoint)
+{
+ stbtt_uint8 *data = info->data;
+ stbtt_uint32 index_map = info->index_map;
+
+ stbtt_uint16 format = ttUSHORT(data + index_map + 0);
+ if (format == 0) { // apple byte encoding
+ stbtt_int32 bytes = ttUSHORT(data + index_map + 2);
+ if (unicode_codepoint < bytes-6)
+ return ttBYTE(data + index_map + 6 + unicode_codepoint);
+ return 0;
+ } else if (format == 6) {
+ stbtt_uint32 first = ttUSHORT(data + index_map + 6);
+ stbtt_uint32 count = ttUSHORT(data + index_map + 8);
+ if ((stbtt_uint32) unicode_codepoint >= first && (stbtt_uint32) unicode_codepoint < first+count)
+ return ttUSHORT(data + index_map + 10 + (unicode_codepoint - first)*2);
+ return 0;
+ } else if (format == 2) {
+ STBTT_assert(0); // @TODO: high-byte mapping for japanese/chinese/korean
+ return 0;
+ } else if (format == 4) { // standard mapping for windows fonts: binary search collection of ranges
+ stbtt_uint16 segcount = ttUSHORT(data+index_map+6) >> 1;
+ stbtt_uint16 searchRange = ttUSHORT(data+index_map+8) >> 1;
+ stbtt_uint16 entrySelector = ttUSHORT(data+index_map+10);
+ stbtt_uint16 rangeShift = ttUSHORT(data+index_map+12) >> 1;
+
+ // do a binary search of the segments
+ stbtt_uint32 endCount = index_map + 14;
+ stbtt_uint32 search = endCount;
+
+ if (unicode_codepoint > 0xffff)
+ return 0;
+
+ // they lie from endCount .. endCount + segCount
+ // but searchRange is the nearest power of two, so...
+ if (unicode_codepoint >= ttUSHORT(data + search + rangeShift*2))
+ search += rangeShift*2;
+
+ // now decrement to bias correctly to find smallest
+ search -= 2;
+ while (entrySelector) {
+ stbtt_uint16 end;
+ searchRange >>= 1;
+ end = ttUSHORT(data + search + searchRange*2);
+ if (unicode_codepoint > end)
+ search += searchRange*2;
+ --entrySelector;
+ }
+ search += 2;
+
+ {
+ stbtt_uint16 offset, start;
+ stbtt_uint16 item = (stbtt_uint16) ((search - endCount) >> 1);
+
+ STBTT_assert(unicode_codepoint <= ttUSHORT(data + endCount + 2*item));
+ start = ttUSHORT(data + index_map + 14 + segcount*2 + 2 + 2*item);
+ if (unicode_codepoint < start)
+ return 0;
+
+ offset = ttUSHORT(data + index_map + 14 + segcount*6 + 2 + 2*item);
+ if (offset == 0)
+ return (stbtt_uint16) (unicode_codepoint + ttSHORT(data + index_map + 14 + segcount*4 + 2 + 2*item));
+
+ return ttUSHORT(data + offset + (unicode_codepoint-start)*2 + index_map + 14 + segcount*6 + 2 + 2*item);
+ }
+ } else if (format == 12 || format == 13) {
+ stbtt_uint32 ngroups = ttULONG(data+index_map+12);
+ stbtt_int32 low,high;
+ low = 0; high = (stbtt_int32)ngroups;
+ // Binary search the right group.
+ while (low < high) {
+ stbtt_int32 mid = low + ((high-low) >> 1); // rounds down, so low <= mid < high
+ stbtt_uint32 start_char = ttULONG(data+index_map+16+mid*12);
+ stbtt_uint32 end_char = ttULONG(data+index_map+16+mid*12+4);
+ if ((stbtt_uint32) unicode_codepoint < start_char)
+ high = mid;
+ else if ((stbtt_uint32) unicode_codepoint > end_char)
+ low = mid+1;
+ else {
+ stbtt_uint32 start_glyph = ttULONG(data+index_map+16+mid*12+8);
+ if (format == 12)
+ return start_glyph + unicode_codepoint-start_char;
+ else // format == 13
+ return start_glyph;
+ }
+ }
+ return 0; // not found
+ }
+ // @TODO
+ STBTT_assert(0);
+ return 0;
+}
+
+STBTT_DEF int stbtt_GetCodepointShape(const stbtt_fontinfo *info, int unicode_codepoint, stbtt_vertex **vertices)
+{
+ return stbtt_GetGlyphShape(info, stbtt_FindGlyphIndex(info, unicode_codepoint), vertices);
+}
+
+static void stbtt_setvertex(stbtt_vertex *v, stbtt_uint8 type, stbtt_int32 x, stbtt_int32 y, stbtt_int32 cx, stbtt_int32 cy)
+{
+ v->type = type;
+ v->x = (stbtt_int16) x;
+ v->y = (stbtt_int16) y;
+ v->cx = (stbtt_int16) cx;
+ v->cy = (stbtt_int16) cy;
+}
+
+static int stbtt__GetGlyfOffset(const stbtt_fontinfo *info, int glyph_index)
+{
+ int g1,g2;
+
+ if (glyph_index >= info->numGlyphs) return -1; // glyph index out of range
+ if (info->indexToLocFormat >= 2) return -1; // unknown index->glyph map format
+
+ if (info->indexToLocFormat == 0) {
+ g1 = info->glyf + ttUSHORT(info->data + info->loca + glyph_index * 2) * 2;
+ g2 = info->glyf + ttUSHORT(info->data + info->loca + glyph_index * 2 + 2) * 2;
+ } else {
+ g1 = info->glyf + ttULONG (info->data + info->loca + glyph_index * 4);
+ g2 = info->glyf + ttULONG (info->data + info->loca + glyph_index * 4 + 4);
+ }
+
+ return g1==g2 ? -1 : g1; // if length is 0, return -1
+}
+
+STBTT_DEF int stbtt_GetGlyphBox(const stbtt_fontinfo *info, int glyph_index, int *x0, int *y0, int *x1, int *y1)
+{
+ int g = stbtt__GetGlyfOffset(info, glyph_index);
+ if (g < 0) return 0;
+
+ if (x0) *x0 = ttSHORT(info->data + g + 2);
+ if (y0) *y0 = ttSHORT(info->data + g + 4);
+ if (x1) *x1 = ttSHORT(info->data + g + 6);
+ if (y1) *y1 = ttSHORT(info->data + g + 8);
+ return 1;
+}
+
+STBTT_DEF int stbtt_GetCodepointBox(const stbtt_fontinfo *info, int codepoint, int *x0, int *y0, int *x1, int *y1)
+{
+ return stbtt_GetGlyphBox(info, stbtt_FindGlyphIndex(info,codepoint), x0,y0,x1,y1);
+}
+
+STBTT_DEF int stbtt_IsGlyphEmpty(const stbtt_fontinfo *info, int glyph_index)
+{
+ stbtt_int16 numberOfContours;
+ int g = stbtt__GetGlyfOffset(info, glyph_index);
+ if (g < 0) return 1;
+ numberOfContours = ttSHORT(info->data + g);
+ return numberOfContours == 0;
+}
+
+static int stbtt__close_shape(stbtt_vertex *vertices, int num_vertices, int was_off, int start_off,
+ stbtt_int32 sx, stbtt_int32 sy, stbtt_int32 scx, stbtt_int32 scy, stbtt_int32 cx, stbtt_int32 cy)
+{
+ if (start_off) {
+ if (was_off)
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, (cx+scx)>>1, (cy+scy)>>1, cx,cy);
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, sx,sy,scx,scy);
+ } else {
+ if (was_off)
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve,sx,sy,cx,cy);
+ else
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vline,sx,sy,0,0);
+ }
+ return num_vertices;
+}
+
+STBTT_DEF int stbtt_GetGlyphShape(const stbtt_fontinfo *info, int glyph_index, stbtt_vertex **pvertices)
+{
+ stbtt_int16 numberOfContours;
+ stbtt_uint8 *endPtsOfContours;
+ stbtt_uint8 *data = info->data;
+ stbtt_vertex *vertices=0;
+ int num_vertices=0;
+ int g = stbtt__GetGlyfOffset(info, glyph_index);
+
+ *pvertices = NULL;
+
+ if (g < 0) return 0;
+
+ numberOfContours = ttSHORT(data + g);
+
+ if (numberOfContours > 0) {
+ stbtt_uint8 flags=0,flagcount;
+ stbtt_int32 ins, i,j=0,m,n, next_move, was_off=0, off, start_off=0;
+ stbtt_int32 x,y,cx,cy,sx,sy, scx,scy;
+ stbtt_uint8 *points;
+ endPtsOfContours = (data + g + 10);
+ ins = ttUSHORT(data + g + 10 + numberOfContours * 2);
+ points = data + g + 10 + numberOfContours * 2 + 2 + ins;
+
+ n = 1+ttUSHORT(endPtsOfContours + numberOfContours*2-2);
+
+ m = n + 2*numberOfContours; // a loose bound on how many vertices we might need
+ vertices = (stbtt_vertex *) STBTT_malloc(m * sizeof(vertices[0]), info->userdata);
+ if (vertices == 0)
+ return 0;
+
+ next_move = 0;
+ flagcount=0;
+
+ // in first pass, we load uninterpreted data into the allocated array
+ // above, shifted to the end of the array so we won't overwrite it when
+ // we create our final data starting from the front
+
+ off = m - n; // starting offset for uninterpreted data, regardless of how m ends up being calculated
+
+ // first load flags
+
+ for (i=0; i < n; ++i) {
+ if (flagcount == 0) {
+ flags = *points++;
+ if (flags & 8)
+ flagcount = *points++;
+ } else
+ --flagcount;
+ vertices[off+i].type = flags;
+ }
+
+ // now load x coordinates
+ x=0;
+ for (i=0; i < n; ++i) {
+ flags = vertices[off+i].type;
+ if (flags & 2) {
+ stbtt_int16 dx = *points++;
+ x += (flags & 16) ? dx : -dx; // ???
+ } else {
+ if (!(flags & 16)) {
+ x = x + (stbtt_int16) (points[0]*256 + points[1]);
+ points += 2;
+ }
+ }
+ vertices[off+i].x = (stbtt_int16) x;
+ }
+
+ // now load y coordinates
+ y=0;
+ for (i=0; i < n; ++i) {
+ flags = vertices[off+i].type;
+ if (flags & 4) {
+ stbtt_int16 dy = *points++;
+ y += (flags & 32) ? dy : -dy; // ???
+ } else {
+ if (!(flags & 32)) {
+ y = y + (stbtt_int16) (points[0]*256 + points[1]);
+ points += 2;
+ }
+ }
+ vertices[off+i].y = (stbtt_int16) y;
+ }
+
+ // now convert them to our format
+ num_vertices=0;
+ sx = sy = cx = cy = scx = scy = 0;
+ for (i=0; i < n; ++i) {
+ flags = vertices[off+i].type;
+ x = (stbtt_int16) vertices[off+i].x;
+ y = (stbtt_int16) vertices[off+i].y;
+
+ if (next_move == i) {
+ if (i != 0)
+ num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy);
+
+ // now start the new one
+ start_off = !(flags & 1);
+ if (start_off) {
+ // if we start off with an off-curve point, then when we need to find a point on the curve
+ // where we can start, and we need to save some state for when we wraparound.
+ scx = x;
+ scy = y;
+ if (!(vertices[off+i+1].type & 1)) {
+ // next point is also a curve point, so interpolate an on-point curve
+ sx = (x + (stbtt_int32) vertices[off+i+1].x) >> 1;
+ sy = (y + (stbtt_int32) vertices[off+i+1].y) >> 1;
+ } else {
+ // otherwise just use the next point as our start point
+ sx = (stbtt_int32) vertices[off+i+1].x;
+ sy = (stbtt_int32) vertices[off+i+1].y;
+ ++i; // we're using point i+1 as the starting point, so skip it
+ }
+ } else {
+ sx = x;
+ sy = y;
+ }
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vmove,sx,sy,0,0);
+ was_off = 0;
+ next_move = 1 + ttUSHORT(endPtsOfContours+j*2);
+ ++j;
+ } else {
+ if (!(flags & 1)) { // if it's a curve
+ if (was_off) // two off-curve control points in a row means interpolate an on-curve midpoint
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, (cx+x)>>1, (cy+y)>>1, cx, cy);
+ cx = x;
+ cy = y;
+ was_off = 1;
+ } else {
+ if (was_off)
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vcurve, x,y, cx, cy);
+ else
+ stbtt_setvertex(&vertices[num_vertices++], STBTT_vline, x,y,0,0);
+ was_off = 0;
+ }
+ }
+ }
+ num_vertices = stbtt__close_shape(vertices, num_vertices, was_off, start_off, sx,sy,scx,scy,cx,cy);
+ } else if (numberOfContours == -1) {
+ // Compound shapes.
+ int more = 1;
+ stbtt_uint8 *comp = data + g + 10;
+ num_vertices = 0;
+ vertices = 0;
+ while (more) {
+ stbtt_uint16 flags, gidx;
+ int comp_num_verts = 0, i;
+ stbtt_vertex *comp_verts = 0, *tmp = 0;
+ float mtx[6] = {1,0,0,1,0,0}, m, n;
+
+ flags = ttSHORT(comp); comp+=2;
+ gidx = ttSHORT(comp); comp+=2;
+
+ if (flags & 2) { // XY values
+ if (flags & 1) { // shorts
+ mtx[4] = ttSHORT(comp); comp+=2;
+ mtx[5] = ttSHORT(comp); comp+=2;
+ } else {
+ mtx[4] = ttCHAR(comp); comp+=1;
+ mtx[5] = ttCHAR(comp); comp+=1;
+ }
+ }
+ else {
+ // @TODO handle matching point
+ STBTT_assert(0);
+ }
+ if (flags & (1<<3)) { // WE_HAVE_A_SCALE
+ mtx[0] = mtx[3] = ttSHORT(comp)/16384.0f; comp+=2;
+ mtx[1] = mtx[2] = 0;
+ } else if (flags & (1<<6)) { // WE_HAVE_AN_X_AND_YSCALE
+ mtx[0] = ttSHORT(comp)/16384.0f; comp+=2;
+ mtx[1] = mtx[2] = 0;
+ mtx[3] = ttSHORT(comp)/16384.0f; comp+=2;
+ } else if (flags & (1<<7)) { // WE_HAVE_A_TWO_BY_TWO
+ mtx[0] = ttSHORT(comp)/16384.0f; comp+=2;
+ mtx[1] = ttSHORT(comp)/16384.0f; comp+=2;
+ mtx[2] = ttSHORT(comp)/16384.0f; comp+=2;
+ mtx[3] = ttSHORT(comp)/16384.0f; comp+=2;
+ }
+
+ // Find transformation scales.
+ m = (float) STBTT_sqrt(mtx[0]*mtx[0] + mtx[1]*mtx[1]);
+ n = (float) STBTT_sqrt(mtx[2]*mtx[2] + mtx[3]*mtx[3]);
+
+ // Get indexed glyph.
+ comp_num_verts = stbtt_GetGlyphShape(info, gidx, &comp_verts);
+ if (comp_num_verts > 0) {
+ // Transform vertices.
+ for (i = 0; i < comp_num_verts; ++i) {
+ stbtt_vertex* v = &comp_verts[i];
+ stbtt_vertex_type x,y;
+ x=v->x; y=v->y;
+ v->x = (stbtt_vertex_type)(m * (mtx[0]*x + mtx[2]*y + mtx[4]));
+ v->y = (stbtt_vertex_type)(n * (mtx[1]*x + mtx[3]*y + mtx[5]));
+ x=v->cx; y=v->cy;
+ v->cx = (stbtt_vertex_type)(m * (mtx[0]*x + mtx[2]*y + mtx[4]));
+ v->cy = (stbtt_vertex_type)(n * (mtx[1]*x + mtx[3]*y + mtx[5]));
+ }
+ // Append vertices.
+ tmp = (stbtt_vertex*)STBTT_malloc((num_vertices+comp_num_verts)*sizeof(stbtt_vertex), info->userdata);
+ if (!tmp) {
+ if (vertices) STBTT_free(vertices, info->userdata);
+ if (comp_verts) STBTT_free(comp_verts, info->userdata);
+ return 0;
+ }
+ if (num_vertices > 0) STBTT_memcpy(tmp, vertices, num_vertices*sizeof(stbtt_vertex));
+ STBTT_memcpy(tmp+num_vertices, comp_verts, comp_num_verts*sizeof(stbtt_vertex));
+ if (vertices) STBTT_free(vertices, info->userdata);
+ vertices = tmp;
+ STBTT_free(comp_verts, info->userdata);
+ num_vertices += comp_num_verts;
+ }
+ // More components ?
+ more = flags & (1<<5);
+ }
+ } else if (numberOfContours < 0) {
+ // @TODO other compound variations?
+ STBTT_assert(0);
+ } else {
+ // numberOfCounters == 0, do nothing
+ }
+
+ *pvertices = vertices;
+ return num_vertices;
+}
+
+STBTT_DEF void stbtt_GetGlyphHMetrics(const stbtt_fontinfo *info, int glyph_index, int *advanceWidth, int *leftSideBearing)
+{
+ stbtt_uint16 numOfLongHorMetrics = ttUSHORT(info->data+info->hhea + 34);
+ if (glyph_index < numOfLongHorMetrics) {
+ if (advanceWidth) *advanceWidth = ttSHORT(info->data + info->hmtx + 4*glyph_index);
+ if (leftSideBearing) *leftSideBearing = ttSHORT(info->data + info->hmtx + 4*glyph_index + 2);
+ } else {
+ if (advanceWidth) *advanceWidth = ttSHORT(info->data + info->hmtx + 4*(numOfLongHorMetrics-1));
+ if (leftSideBearing) *leftSideBearing = ttSHORT(info->data + info->hmtx + 4*numOfLongHorMetrics + 2*(glyph_index - numOfLongHorMetrics));
+ }
+}
+
+STBTT_DEF int stbtt_GetGlyphKernAdvance(const stbtt_fontinfo *info, int glyph1, int glyph2)
+{
+ stbtt_uint8 *data = info->data + info->kern;
+ stbtt_uint32 needle, straw;
+ int l, r, m;
+
+ // we only look at the first table. it must be 'horizontal' and format 0.
+ if (!info->kern)
+ return 0;
+ if (ttUSHORT(data+2) < 1) // number of tables, need at least 1
+ return 0;
+ if (ttUSHORT(data+8) != 1) // horizontal flag must be set in format
+ return 0;
+
+ l = 0;
+ r = ttUSHORT(data+10) - 1;
+ needle = glyph1 << 16 | glyph2;
+ while (l <= r) {
+ m = (l + r) >> 1;
+ straw = ttULONG(data+18+(m*6)); // note: unaligned read
+ if (needle < straw)
+ r = m - 1;
+ else if (needle > straw)
+ l = m + 1;
+ else
+ return ttSHORT(data+22+(m*6));
+ }
+ return 0;
+}
+
+STBTT_DEF int stbtt_GetCodepointKernAdvance(const stbtt_fontinfo *info, int ch1, int ch2)
+{
+ if (!info->kern) // if no kerning table, don't waste time looking up both codepoint->glyphs
+ return 0;
+ return stbtt_GetGlyphKernAdvance(info, stbtt_FindGlyphIndex(info,ch1), stbtt_FindGlyphIndex(info,ch2));
+}
+
+STBTT_DEF void stbtt_GetCodepointHMetrics(const stbtt_fontinfo *info, int codepoint, int *advanceWidth, int *leftSideBearing)
+{
+ stbtt_GetGlyphHMetrics(info, stbtt_FindGlyphIndex(info,codepoint), advanceWidth, leftSideBearing);
+}
+
+STBTT_DEF void stbtt_GetFontVMetrics(const stbtt_fontinfo *info, int *ascent, int *descent, int *lineGap)
+{
+ if (ascent ) *ascent = ttSHORT(info->data+info->hhea + 4);
+ if (descent) *descent = ttSHORT(info->data+info->hhea + 6);
+ if (lineGap) *lineGap = ttSHORT(info->data+info->hhea + 8);
+}
+
+STBTT_DEF void stbtt_GetFontBoundingBox(const stbtt_fontinfo *info, int *x0, int *y0, int *x1, int *y1)
+{
+ *x0 = ttSHORT(info->data + info->head + 36);
+ *y0 = ttSHORT(info->data + info->head + 38);
+ *x1 = ttSHORT(info->data + info->head + 40);
+ *y1 = ttSHORT(info->data + info->head + 42);
+}
+
+STBTT_DEF float stbtt_ScaleForPixelHeight(const stbtt_fontinfo *info, float height)
+{
+ int fheight = ttSHORT(info->data + info->hhea + 4) - ttSHORT(info->data + info->hhea + 6);
+ return (float) height / fheight;
+}
+
+STBTT_DEF float stbtt_ScaleForMappingEmToPixels(const stbtt_fontinfo *info, float pixels)
+{
+ int unitsPerEm = ttUSHORT(info->data + info->head + 18);
+ return pixels / unitsPerEm;
+}
+
+STBTT_DEF void stbtt_FreeShape(const stbtt_fontinfo *info, stbtt_vertex *v)
+{
+ STBTT_free(v, info->userdata);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// antialiasing software rasterizer
+//
+
+STBTT_DEF void stbtt_GetGlyphBitmapBoxSubpixel(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y,float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1)
+{
+ int x0,y0,x1,y1;
+ if (!stbtt_GetGlyphBox(font, glyph, &x0,&y0,&x1,&y1)) {
+ // e.g. space character
+ if (ix0) *ix0 = 0;
+ if (iy0) *iy0 = 0;
+ if (ix1) *ix1 = 0;
+ if (iy1) *iy1 = 0;
+ } else {
+ // move to integral bboxes (treating pixels as little squares, what pixels get touched)?
+ if (ix0) *ix0 = STBTT_ifloor( x0 * scale_x + shift_x);
+ if (iy0) *iy0 = STBTT_ifloor(-y1 * scale_y + shift_y);
+ if (ix1) *ix1 = STBTT_iceil ( x1 * scale_x + shift_x);
+ if (iy1) *iy1 = STBTT_iceil (-y0 * scale_y + shift_y);
+ }
+}
+
+STBTT_DEF void stbtt_GetGlyphBitmapBox(const stbtt_fontinfo *font, int glyph, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1)
+{
+ stbtt_GetGlyphBitmapBoxSubpixel(font, glyph, scale_x, scale_y,0.0f,0.0f, ix0, iy0, ix1, iy1);
+}
+
+STBTT_DEF void stbtt_GetCodepointBitmapBoxSubpixel(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, float shift_x, float shift_y, int *ix0, int *iy0, int *ix1, int *iy1)
+{
+ stbtt_GetGlyphBitmapBoxSubpixel(font, stbtt_FindGlyphIndex(font,codepoint), scale_x, scale_y,shift_x,shift_y, ix0,iy0,ix1,iy1);
+}
+
+STBTT_DEF void stbtt_GetCodepointBitmapBox(const stbtt_fontinfo *font, int codepoint, float scale_x, float scale_y, int *ix0, int *iy0, int *ix1, int *iy1)
+{
+ stbtt_GetCodepointBitmapBoxSubpixel(font, codepoint, scale_x, scale_y,0.0f,0.0f, ix0,iy0,ix1,iy1);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// Rasterizer
+
+typedef struct stbtt__hheap_chunk
+{
+ struct stbtt__hheap_chunk *next;
+} stbtt__hheap_chunk;
+
+typedef struct stbtt__hheap
+{
+ struct stbtt__hheap_chunk *head;
+ void *first_free;
+ int num_remaining_in_head_chunk;
+} stbtt__hheap;
+
+static void *stbtt__hheap_alloc(stbtt__hheap *hh, size_t size, void *userdata)
+{
+ if (hh->first_free) {
+ void *p = hh->first_free;
+ hh->first_free = * (void **) p;
+ return p;
+ } else {
+ if (hh->num_remaining_in_head_chunk == 0) {
+ int count = (size < 32 ? 2000 : size < 128 ? 800 : 100);
+ stbtt__hheap_chunk *c = (stbtt__hheap_chunk *) STBTT_malloc(sizeof(stbtt__hheap_chunk) + size * count, userdata);
+ if (c == NULL)
+ return NULL;
+ c->next = hh->head;
+ hh->head = c;
+ hh->num_remaining_in_head_chunk = count;
+ }
+ --hh->num_remaining_in_head_chunk;
+ return (char *) (hh->head) + size * hh->num_remaining_in_head_chunk;
+ }
+}
+
+static void stbtt__hheap_free(stbtt__hheap *hh, void *p)
+{
+ *(void **) p = hh->first_free;
+ hh->first_free = p;
+}
+
+static void stbtt__hheap_cleanup(stbtt__hheap *hh, void *userdata)
+{
+ stbtt__hheap_chunk *c = hh->head;
+ while (c) {
+ stbtt__hheap_chunk *n = c->next;
+ STBTT_free(c, userdata);
+ c = n;
+ }
+}
+
+typedef struct stbtt__edge {
+ float x0,y0, x1,y1;
+ int invert;
+} stbtt__edge;
+
+
+typedef struct stbtt__active_edge
+{
+ struct stbtt__active_edge *next;
+ #if STBTT_RASTERIZER_VERSION==1
+ int x,dx;
+ float ey;
+ int direction;
+ #elif STBTT_RASTERIZER_VERSION==2
+ float fx,fdx,fdy;
+ float direction;
+ float sy;
+ float ey;
+ #else
+ #error "Unrecognized value of STBTT_RASTERIZER_VERSION"
+ #endif
+} stbtt__active_edge;
+
+#if STBTT_RASTERIZER_VERSION == 1
+#define STBTT_FIXSHIFT 10
+#define STBTT_FIX (1 << STBTT_FIXSHIFT)
+#define STBTT_FIXMASK (STBTT_FIX-1)
+
+static stbtt__active_edge *stbtt__new_active(stbtt__hheap *hh, stbtt__edge *e, int off_x, float start_point, void *userdata)
+{
+ stbtt__active_edge *z = (stbtt__active_edge *) stbtt__hheap_alloc(hh, sizeof(*z), userdata);
+ float dxdy = (e->x1 - e->x0) / (e->y1 - e->y0);
+ if (!z) return z;
+
+ // round dx down to avoid overshooting
+ if (dxdy < 0)
+ z->dx = -STBTT_ifloor(STBTT_FIX * -dxdy);
+ else
+ z->dx = STBTT_ifloor(STBTT_FIX * dxdy);
+
+ z->x = STBTT_ifloor(STBTT_FIX * e->x0 + z->dx * (start_point - e->y0)); // use z->dx so when we offset later it's by the same amount
+ z->x -= off_x * STBTT_FIX;
+
+ z->ey = e->y1;
+ z->next = 0;
+ z->direction = e->invert ? 1 : -1;
+ return z;
+}
+#elif STBTT_RASTERIZER_VERSION == 2
+static stbtt__active_edge *stbtt__new_active(stbtt__hheap *hh, stbtt__edge *e, int off_x, float start_point, void *userdata)
+{
+ stbtt__active_edge *z = (stbtt__active_edge *) stbtt__hheap_alloc(hh, sizeof(*z), userdata);
+ float dxdy = (e->x1 - e->x0) / (e->y1 - e->y0);
+ //STBTT_assert(e->y0 <= start_point);
+ if (!z) return z;
+ z->fdx = dxdy;
+ z->fdy = (1/dxdy);
+ z->fx = e->x0 + dxdy * (start_point - e->y0);
+ z->fx -= off_x;
+ z->direction = e->invert ? 1.0f : -1.0f;
+ z->sy = e->y0;
+ z->ey = e->y1;
+ z->next = 0;
+ return z;
+}
+#else
+#error "Unrecognized value of STBTT_RASTERIZER_VERSION"
+#endif
+
+#if STBTT_RASTERIZER_VERSION == 1
+// note: this routine clips fills that extend off the edges... ideally this
+// wouldn't happen, but it could happen if the truetype glyph bounding boxes
+// are wrong, or if the user supplies a too-small bitmap
+static void stbtt__fill_active_edges(unsigned char *scanline, int len, stbtt__active_edge *e, int max_weight)
+{
+ // non-zero winding fill
+ int x0=0, w=0;
+
+ while (e) {
+ if (w == 0) {
+ // if we're currently at zero, we need to record the edge start point
+ x0 = e->x; w += e->direction;
+ } else {
+ int x1 = e->x; w += e->direction;
+ // if we went to zero, we need to draw
+ if (w == 0) {
+ int i = x0 >> STBTT_FIXSHIFT;
+ int j = x1 >> STBTT_FIXSHIFT;
+
+ if (i < len && j >= 0) {
+ if (i == j) {
+ // x0,x1 are the same pixel, so compute combined coverage
+ scanline[i] = scanline[i] + (stbtt_uint8) ((x1 - x0) * max_weight >> STBTT_FIXSHIFT);
+ } else {
+ if (i >= 0) // add antialiasing for x0
+ scanline[i] = scanline[i] + (stbtt_uint8) (((STBTT_FIX - (x0 & STBTT_FIXMASK)) * max_weight) >> STBTT_FIXSHIFT);
+ else
+ i = -1; // clip
+
+ if (j < len) // add antialiasing for x1
+ scanline[j] = scanline[j] + (stbtt_uint8) (((x1 & STBTT_FIXMASK) * max_weight) >> STBTT_FIXSHIFT);
+ else
+ j = len; // clip
+
+ for (++i; i < j; ++i) // fill pixels between x0 and x1
+ scanline[i] = scanline[i] + (stbtt_uint8) max_weight;
+ }
+ }
+ }
+ }
+
+ e = e->next;
+ }
+}
+
+static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int n, int vsubsample, int off_x, int off_y, void *userdata)
+{
+ stbtt__hheap hh = { 0 };
+ stbtt__active_edge *active = NULL;
+ int y,j=0;
+ int max_weight = (255 / vsubsample); // weight per vertical scanline
+ int s; // vertical subsample index
+ unsigned char scanline_data[512], *scanline;
+
+ if (result->w > 512)
+ scanline = (unsigned char *) STBTT_malloc(result->w, userdata);
+ else
+ scanline = scanline_data;
+
+ y = off_y * vsubsample;
+ e[n].y0 = (off_y + result->h) * (float) vsubsample + 1;
+
+ while (j < result->h) {
+ STBTT_memset(scanline, 0, result->w);
+ for (s=0; s < vsubsample; ++s) {
+ // find center of pixel for this scanline
+ float scan_y = y + 0.5f;
+ stbtt__active_edge **step = &active;
+
+ // update all active edges;
+ // remove all active edges that terminate before the center of this scanline
+ while (*step) {
+ stbtt__active_edge * z = *step;
+ if (z->ey <= scan_y) {
+ *step = z->next; // delete from list
+ STBTT_assert(z->direction);
+ z->direction = 0;
+ stbtt__hheap_free(&hh, z);
+ } else {
+ z->x += z->dx; // advance to position for current scanline
+ step = &((*step)->next); // advance through list
+ }
+ }
+
+ // resort the list if needed
+ for(;;) {
+ int changed=0;
+ step = &active;
+ while (*step && (*step)->next) {
+ if ((*step)->x > (*step)->next->x) {
+ stbtt__active_edge *t = *step;
+ stbtt__active_edge *q = t->next;
+
+ t->next = q->next;
+ q->next = t;
+ *step = q;
+ changed = 1;
+ }
+ step = &(*step)->next;
+ }
+ if (!changed) break;
+ }
+
+ // insert all edges that start before the center of this scanline -- omit ones that also end on this scanline
+ while (e->y0 <= scan_y) {
+ if (e->y1 > scan_y) {
+ stbtt__active_edge *z = stbtt__new_active(&hh, e, off_x, scan_y, userdata);
+ // find insertion point
+ if (active == NULL)
+ active = z;
+ else if (z->x < active->x) {
+ // insert at front
+ z->next = active;
+ active = z;
+ } else {
+ // find thing to insert AFTER
+ stbtt__active_edge *p = active;
+ while (p->next && p->next->x < z->x)
+ p = p->next;
+ // at this point, p->next->x is NOT < z->x
+ z->next = p->next;
+ p->next = z;
+ }
+ }
+ ++e;
+ }
+
+ // now process all active edges in XOR fashion
+ if (active)
+ stbtt__fill_active_edges(scanline, result->w, active, max_weight);
+
+ ++y;
+ }
+ STBTT_memcpy(result->pixels + j * result->stride, scanline, result->w);
+ ++j;
+ }
+
+ stbtt__hheap_cleanup(&hh, userdata);
+
+ if (scanline != scanline_data)
+ STBTT_free(scanline, userdata);
+}
+
+#elif STBTT_RASTERIZER_VERSION == 2
+
+// the edge passed in here does not cross the vertical line at x or the vertical line at x+1
+// (i.e. it has already been clipped to those)
+static void stbtt__handle_clipped_edge(float *scanline, int x, stbtt__active_edge *e, float x0, float y0, float x1, float y1)
+{
+ if (y0 == y1) return;
+ STBTT_assert(y0 < y1);
+ STBTT_assert(e->sy <= e->ey);
+ if (y0 > e->ey) return;
+ if (y1 < e->sy) return;
+ if (y0 < e->sy) {
+ x0 += (x1-x0) * (e->sy - y0) / (y1-y0);
+ y0 = e->sy;
+ }
+ if (y1 > e->ey) {
+ x1 += (x1-x0) * (e->ey - y1) / (y1-y0);
+ y1 = e->ey;
+ }
+
+ if (x0 == x)
+ STBTT_assert(x1 <= x+1);
+ else if (x0 == x+1)
+ STBTT_assert(x1 >= x);
+ else if (x0 <= x)
+ STBTT_assert(x1 <= x);
+ else if (x0 >= x+1)
+ STBTT_assert(x1 >= x+1);
+ else
+ STBTT_assert(x1 >= x && x1 <= x+1);
+
+ if (x0 <= x && x1 <= x)
+ scanline[x] += e->direction * (y1-y0);
+ else if (x0 >= x+1 && x1 >= x+1)
+ ;
+ else {
+ STBTT_assert(x0 >= x && x0 <= x+1 && x1 >= x && x1 <= x+1);
+ scanline[x] += e->direction * (y1-y0) * (1-((x0-x)+(x1-x))/2); // coverage = 1 - average x position
+ }
+}
+
+static void stbtt__fill_active_edges_new(float *scanline, float *scanline_fill, int len, stbtt__active_edge *e, float y_top)
+{
+ float y_bottom = y_top+1;
+
+ while (e) {
+ // brute force every pixel
+
+ // compute intersection points with top & bottom
+ STBTT_assert(e->ey >= y_top);
+
+ if (e->fdx == 0) {
+ float x0 = e->fx;
+ if (x0 < len) {
+ if (x0 >= 0) {
+ stbtt__handle_clipped_edge(scanline,(int) x0,e, x0,y_top, x0,y_bottom);
+ stbtt__handle_clipped_edge(scanline_fill-1,(int) x0+1,e, x0,y_top, x0,y_bottom);
+ } else {
+ stbtt__handle_clipped_edge(scanline_fill-1,0,e, x0,y_top, x0,y_bottom);
+ }
+ }
+ } else {
+ float x0 = e->fx;
+ float dx = e->fdx;
+ float xb = x0 + dx;
+ float x_top, x_bottom;
+ float y0,y1;
+ float dy = e->fdy;
+ STBTT_assert(e->sy <= y_bottom && e->ey >= y_top);
+
+ // compute endpoints of line segment clipped to this scanline (if the
+ // line segment starts on this scanline. x0 is the intersection of the
+ // line with y_top, but that may be off the line segment.
+ if (e->sy > y_top) {
+ x_top = x0 + dx * (e->sy - y_top);
+ y0 = e->sy;
+ } else {
+ x_top = x0;
+ y0 = y_top;
+ }
+ if (e->ey < y_bottom) {
+ x_bottom = x0 + dx * (e->ey - y_top);
+ y1 = e->ey;
+ } else {
+ x_bottom = xb;
+ y1 = y_bottom;
+ }
+
+ if (x_top >= 0 && x_bottom >= 0 && x_top < len && x_bottom < len) {
+ // from here on, we don't have to range check x values
+
+ if ((int) x_top == (int) x_bottom) {
+ float height;
+ // simple case, only spans one pixel
+ int x = (int) x_top;
+ height = y1 - y0;
+ STBTT_assert(x >= 0 && x < len);
+ scanline[x] += e->direction * (1-((x_top - x) + (x_bottom-x))/2) * height;
+ scanline_fill[x] += e->direction * height; // everything right of this pixel is filled
+ } else {
+ int x,x1,x2;
+ float y_crossing, step, sign, area;
+ // covers 2+ pixels
+ if (x_top > x_bottom) {
+ // flip scanline vertically; signed area is the same
+ float t;
+ y0 = y_bottom - (y0 - y_top);
+ y1 = y_bottom - (y1 - y_top);
+ t = y0, y0 = y1, y1 = t;
+ t = x_bottom, x_bottom = x_top, x_top = t;
+ dx = -dx;
+ dy = -dy;
+ t = x0, x0 = xb, xb = t;
+ }
+
+ x1 = (int) x_top;
+ x2 = (int) x_bottom;
+ // compute intersection with y axis at x1+1
+ y_crossing = (x1+1 - x0) * dy + y_top;
+
+ sign = e->direction;
+ // area of the rectangle covered from y0..y_crossing
+ area = sign * (y_crossing-y0);
+ // area of the triangle (x_top,y0), (x+1,y0), (x+1,y_crossing)
+ scanline[x1] += area * (1-((x_top - x1)+(x1+1-x1))/2);
+
+ step = sign * dy;
+ for (x = x1+1; x < x2; ++x) {
+ scanline[x] += area + step/2;
+ area += step;
+ }
+ y_crossing += dy * (x2 - (x1+1));
+
+ STBTT_assert(fabs(area) <= 1.01f);
+
+ scanline[x2] += area + sign * (1-((x2-x2)+(x_bottom-x2))/2) * (y1-y_crossing);
+
+ scanline_fill[x2] += sign * (y1-y0);
+ }
+ } else {
+ // if edge goes outside of box we're drawing, we require
+ // clipping logic. since this does not match the intended use
+ // of this library, we use a different, very slow brute
+ // force implementation
+ int x;
+ for (x=0; x < len; ++x) {
+ // cases:
+ //
+ // there can be up to two intersections with the pixel. any intersection
+ // with left or right edges can be handled by splitting into two (or three)
+ // regions. intersections with top & bottom do not necessitate case-wise logic.
+ //
+ // the old way of doing this found the intersections with the left & right edges,
+ // then used some simple logic to produce up to three segments in sorted order
+ // from top-to-bottom. however, this had a problem: if an x edge was epsilon
+ // across the x border, then the corresponding y position might not be distinct
+ // from the other y segment, and it might ignored as an empty segment. to avoid
+ // that, we need to explicitly produce segments based on x positions.
+
+ // rename variables to clear pairs
+ float y0 = y_top;
+ float x1 = (float) (x);
+ float x2 = (float) (x+1);
+ float x3 = xb;
+ float y3 = y_bottom;
+ float y1,y2;
+
+ // x = e->x + e->dx * (y-y_top)
+ // (y-y_top) = (x - e->x) / e->dx
+ // y = (x - e->x) / e->dx + y_top
+ y1 = (x - x0) / dx + y_top;
+ y2 = (x+1 - x0) / dx + y_top;
+
+ if (x0 < x1 && x3 > x2) { // three segments descending down-right
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1);
+ stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x2,y2);
+ stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3);
+ } else if (x3 < x1 && x0 > x2) { // three segments descending down-left
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2);
+ stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x1,y1);
+ stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3);
+ } else if (x0 < x1 && x3 > x1) { // two segments across x, down-right
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1);
+ stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3);
+ } else if (x3 < x1 && x0 > x1) { // two segments across x, down-left
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x1,y1);
+ stbtt__handle_clipped_edge(scanline,x,e, x1,y1, x3,y3);
+ } else if (x0 < x2 && x3 > x2) { // two segments across x+1, down-right
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2);
+ stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3);
+ } else if (x3 < x2 && x0 > x2) { // two segments across x+1, down-left
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x2,y2);
+ stbtt__handle_clipped_edge(scanline,x,e, x2,y2, x3,y3);
+ } else { // one segment
+ stbtt__handle_clipped_edge(scanline,x,e, x0,y0, x3,y3);
+ }
+ }
+ }
+ }
+ e = e->next;
+ }
+}
+
+// directly AA rasterize edges w/o supersampling
+static void stbtt__rasterize_sorted_edges(stbtt__bitmap *result, stbtt__edge *e, int n, int /*vsubsample*/, int off_x, int off_y, void *userdata)
+{
+ stbtt__hheap hh = { 0, 0, 0 };
+ stbtt__active_edge *active = NULL;
+ int y,j=0, i;
+ float scanline_data[129], *scanline, *scanline2;
+
+ if (result->w > 64)
+ scanline = (float *) STBTT_malloc((result->w*2+1) * sizeof(float), userdata);
+ else
+ scanline = scanline_data;
+
+ scanline2 = scanline + result->w;
+
+ y = off_y;
+ e[n].y0 = (float) (off_y + result->h) + 1;
+
+ while (j < result->h) {
+ // find center of pixel for this scanline
+ float scan_y_top = y + 0.0f;
+ float scan_y_bottom = y + 1.0f;
+ stbtt__active_edge **step = &active;
+
+ STBTT_memset(scanline , 0, result->w*sizeof(scanline[0]));
+ STBTT_memset(scanline2, 0, (result->w+1)*sizeof(scanline[0]));
+
+ // update all active edges;
+ // remove all active edges that terminate before the top of this scanline
+ while (*step) {
+ stbtt__active_edge * z = *step;
+ if (z->ey <= scan_y_top) {
+ *step = z->next; // delete from list
+ STBTT_assert(z->direction);
+ z->direction = 0;
+ stbtt__hheap_free(&hh, z);
+ } else {
+ step = &((*step)->next); // advance through list
+ }
+ }
+
+ // insert all edges that start before the bottom of this scanline
+ while (e->y0 <= scan_y_bottom) {
+ stbtt__active_edge *z = stbtt__new_active(&hh, e, off_x, scan_y_top, userdata);
+ STBTT_assert(z->ey >= scan_y_top);
+ // insert at front
+ z->next = active;
+ active = z;
+ ++e;
+ }
+
+ // now process all active edges
+ if (active)
+ stbtt__fill_active_edges_new(scanline, scanline2+1, result->w, active, scan_y_top);
+
+ {
+ float sum = 0;
+ for (i=0; i < result->w; ++i) {
+ float k;
+ int m;
+ sum += scanline2[i];
+ k = scanline[i] + sum;
+ k = (float) fabs(k)*255 + 0.5f;
+ m = (int) k;
+ if (m > 255) m = 255;
+ result->pixels[j*result->stride + i] = (unsigned char) m;
+ }
+ }
+ // advance all the edges
+ step = &active;
+ while (*step) {
+ stbtt__active_edge *z = *step;
+ z->fx += z->fdx; // advance to position for current scanline
+ step = &((*step)->next); // advance through list
+ }
+
+ ++y;
+ ++j;
+ }
+
+ stbtt__hheap_cleanup(&hh, userdata);
+
+ if (scanline != scanline_data)
+ STBTT_free(scanline, userdata);
+}
+#else
+#error "Unrecognized value of STBTT_RASTERIZER_VERSION"
+#endif
+
+#define STBTT__COMPARE(a,b) ((a)->y0 < (b)->y0)
+
+static void stbtt__sort_edges_ins_sort(stbtt__edge *p, int n)
+{
+ int i,j;
+ for (i=1; i < n; ++i) {
+ stbtt__edge t = p[i], *a = &t;
+ j = i;
+ while (j > 0) {
+ stbtt__edge *b = &p[j-1];
+ int c = STBTT__COMPARE(a,b);
+ if (!c) break;
+ p[j] = p[j-1];
+ --j;
+ }
+ if (i != j)
+ p[j] = t;
+ }
+}
+
+static void stbtt__sort_edges_quicksort(stbtt__edge *p, int n)
+{
+ /* threshhold for transitioning to insertion sort */
+ while (n > 12) {
+ stbtt__edge t;
+ int c01,c12,c,m,i,j;
+
+ /* compute median of three */
+ m = n >> 1;
+ c01 = STBTT__COMPARE(&p[0],&p[m]);
+ c12 = STBTT__COMPARE(&p[m],&p[n-1]);
+ /* if 0 >= mid >= end, or 0 < mid < end, then use mid */
+ if (c01 != c12) {
+ /* otherwise, we'll need to swap something else to middle */
+ int z;
+ c = STBTT__COMPARE(&p[0],&p[n-1]);
+ /* 0>mid && mid<n: 0>n => n; 0<n => 0 */
+ /* 0<mid && mid>n: 0>n => 0; 0<n => n */
+ z = (c == c12) ? 0 : n-1;
+ t = p[z];
+ p[z] = p[m];
+ p[m] = t;
+ }
+ /* now p[m] is the median-of-three */
+ /* swap it to the beginning so it won't move around */
+ t = p[0];
+ p[0] = p[m];
+ p[m] = t;
+
+ /* partition loop */
+ i=1;
+ j=n-1;
+ for(;;) {
+ /* handling of equality is crucial here */
+ /* for sentinels & efficiency with duplicates */
+ for (;;++i) {
+ if (!STBTT__COMPARE(&p[i], &p[0])) break;
+ }
+ for (;;--j) {
+ if (!STBTT__COMPARE(&p[0], &p[j])) break;
+ }
+ /* make sure we haven't crossed */
+ if (i >= j) break;
+ t = p[i];
+ p[i] = p[j];
+ p[j] = t;
+
+ ++i;
+ --j;
+ }
+ /* recurse on smaller side, iterate on larger */
+ if (j < (n-i)) {
+ stbtt__sort_edges_quicksort(p,j);
+ p = p+i;
+ n = n-i;
+ } else {
+ stbtt__sort_edges_quicksort(p+i, n-i);
+ n = j;
+ }
+ }
+}
+
+static void stbtt__sort_edges(stbtt__edge *p, int n)
+{
+ stbtt__sort_edges_quicksort(p, n);
+ stbtt__sort_edges_ins_sort(p, n);
+}
+
+typedef struct
+{
+ float x,y;
+} stbtt__point;
+
+static void stbtt__rasterize(stbtt__bitmap *result, stbtt__point *pts, int *wcount, int windings, float scale_x, float scale_y, float shift_x, float shift_y, int off_x, int off_y, int invert, void *userdata)
+{
+ float y_scale_inv = invert ? -scale_y : scale_y;
+ stbtt__edge *e;
+ int n,i,j,k,m;
+#if STBTT_RASTERIZER_VERSION == 1
+ int vsubsample = result->h < 8 ? 15 : 5;
+#elif STBTT_RASTERIZER_VERSION == 2
+ int vsubsample = 1;
+#else
+ #error "Unrecognized value of STBTT_RASTERIZER_VERSION"
+#endif
+ // vsubsample should divide 255 evenly; otherwise we won't reach full opacity
+
+ // now we have to blow out the windings into explicit edge lists
+ n = 0;
+ for (i=0; i < windings; ++i)
+ n += wcount[i];
+
+ e = (stbtt__edge *) STBTT_malloc(sizeof(*e) * (n+1), userdata); // add an extra one as a sentinel
+ if (e == 0) return;
+ n = 0;
+
+ m=0;
+ for (i=0; i < windings; ++i) {
+ stbtt__point *p = pts + m;
+ m += wcount[i];
+ j = wcount[i]-1;
+ for (k=0; k < wcount[i]; j=k++) {
+ int a=k,b=j;
+ // skip the edge if horizontal
+ if (p[j].y == p[k].y)
+ continue;
+ // add edge from j to k to the list
+ e[n].invert = 0;
+ if (invert ? p[j].y > p[k].y : p[j].y < p[k].y) {
+ e[n].invert = 1;
+ a=j,b=k;
+ }
+ e[n].x0 = p[a].x * scale_x + shift_x;
+ e[n].y0 = (p[a].y * y_scale_inv + shift_y) * vsubsample;
+ e[n].x1 = p[b].x * scale_x + shift_x;
+ e[n].y1 = (p[b].y * y_scale_inv + shift_y) * vsubsample;
+ ++n;
+ }
+ }
+
+ // now sort the edges by their highest point (should snap to integer, and then by x)
+ //STBTT_sort(e, n, sizeof(e[0]), stbtt__edge_compare);
+ stbtt__sort_edges(e, n);
+
+ // now, traverse the scanlines and find the intersections on each scanline, use xor winding rule
+ stbtt__rasterize_sorted_edges(result, e, n, vsubsample, off_x, off_y, userdata);
+
+ STBTT_free(e, userdata);
+}
+
+static void stbtt__add_point(stbtt__point *points, int n, float x, float y)
+{
+ if (!points) return; // during first pass, it's unallocated
+ points[n].x = x;
+ points[n].y = y;
+}
+
+// tesselate until threshhold p is happy... @TODO warped to compensate for non-linear stretching
+static int stbtt__tesselate_curve(stbtt__point *points, int *num_points, float x0, float y0, float x1, float y1, float x2, float y2, float objspace_flatness_squared, int n)
+{
+ // midpoint
+ float mx = (x0 + 2*x1 + x2)/4;
+ float my = (y0 + 2*y1 + y2)/4;
+ // versus directly drawn line
+ float dx = (x0+x2)/2 - mx;
+ float dy = (y0+y2)/2 - my;
+ if (n > 16) // 65536 segments on one curve better be enough!
+ return 1;
+ if (dx*dx+dy*dy > objspace_flatness_squared) { // half-pixel error allowed... need to be smaller if AA
+ stbtt__tesselate_curve(points, num_points, x0,y0, (x0+x1)/2.0f,(y0+y1)/2.0f, mx,my, objspace_flatness_squared,n+1);
+ stbtt__tesselate_curve(points, num_points, mx,my, (x1+x2)/2.0f,(y1+y2)/2.0f, x2,y2, objspace_flatness_squared,n+1);
+ } else {
+ stbtt__add_point(points, *num_points,x2,y2);
+ *num_points = *num_points+1;
+ }
+ return 1;
+}
+
+// returns number of contours
+static stbtt__point *stbtt_FlattenCurves(stbtt_vertex *vertices, int num_verts, float objspace_flatness, int **contour_lengths, int *num_contours, void *userdata)
+{
+ stbtt__point *points=0;
+ int num_points=0;
+
+ float objspace_flatness_squared = objspace_flatness * objspace_flatness;
+ int i,n=0,start=0, pass;
+
+ // count how many "moves" there are to get the contour count
+ for (i=0; i < num_verts; ++i)
+ if (vertices[i].type == STBTT_vmove)
+ ++n;
+
+ *num_contours = n;
+ if (n == 0) return 0;
+
+ *contour_lengths = (int *) STBTT_malloc(sizeof(**contour_lengths) * n, userdata);
+
+ if (*contour_lengths == 0) {
+ *num_contours = 0;
+ return 0;
+ }
+
+ // make two passes through the points so we don't need to realloc
+ for (pass=0; pass < 2; ++pass) {
+ float x=0,y=0;
+ if (pass == 1) {
+ points = (stbtt__point *) STBTT_malloc(num_points * sizeof(points[0]), userdata);
+ if (points == NULL) goto error;
+ }
+ num_points = 0;
+ n= -1;
+ for (i=0; i < num_verts; ++i) {
+ switch (vertices[i].type) {
+ case STBTT_vmove:
+ // start the next contour
+ if (n >= 0)
+ (*contour_lengths)[n] = num_points - start;
+ ++n;
+ start = num_points;
+
+ x = vertices[i].x, y = vertices[i].y;
+ stbtt__add_point(points, num_points++, x,y);
+ break;
+ case STBTT_vline:
+ x = vertices[i].x, y = vertices[i].y;
+ stbtt__add_point(points, num_points++, x, y);
+ break;
+ case STBTT_vcurve:
+ stbtt__tesselate_curve(points, &num_points, x,y,
+ vertices[i].cx, vertices[i].cy,
+ vertices[i].x, vertices[i].y,
+ objspace_flatness_squared, 0);
+ x = vertices[i].x, y = vertices[i].y;
+ break;
+ }
+ }
+ (*contour_lengths)[n] = num_points - start;
+ }
+
+ return points;
+error:
+ STBTT_free(points, userdata);
+ STBTT_free(*contour_lengths, userdata);
+ *contour_lengths = 0;
+ *num_contours = 0;
+ return NULL;
+}
+
+STBTT_DEF void stbtt_Rasterize(stbtt__bitmap *result, float flatness_in_pixels, stbtt_vertex *vertices, int num_verts, float scale_x, float scale_y, float shift_x, float shift_y, int x_off, int y_off, int invert, void *userdata)
+{
+ float scale = scale_x > scale_y ? scale_y : scale_x;
+ int winding_count, *winding_lengths;
+ stbtt__point *windings = stbtt_FlattenCurves(vertices, num_verts, flatness_in_pixels / scale, &winding_lengths, &winding_count, userdata);
+ if (windings) {
+ stbtt__rasterize(result, windings, winding_lengths, winding_count, scale_x, scale_y, shift_x, shift_y, x_off, y_off, invert, userdata);
+ STBTT_free(winding_lengths, userdata);
+ STBTT_free(windings, userdata);
+ }
+}
+
+STBTT_DEF void stbtt_FreeBitmap(unsigned char *bitmap, void *userdata)
+{
+ STBTT_free(bitmap, userdata);
+}
+
+STBTT_DEF unsigned char *stbtt_GetGlyphBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int glyph, int *width, int *height, int *xoff, int *yoff)
+{
+ int ix0,iy0,ix1,iy1;
+ stbtt__bitmap gbm;
+ stbtt_vertex *vertices;
+ int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices);
+
+ if (scale_x == 0) scale_x = scale_y;
+ if (scale_y == 0) {
+ if (scale_x == 0) return NULL;
+ scale_y = scale_x;
+ }
+
+ stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale_x, scale_y, shift_x, shift_y, &ix0,&iy0,&ix1,&iy1);
+
+ // now we get the size
+ gbm.w = (ix1 - ix0);
+ gbm.h = (iy1 - iy0);
+ gbm.pixels = NULL; // in case we error
+
+ if (width ) *width = gbm.w;
+ if (height) *height = gbm.h;
+ if (xoff ) *xoff = ix0;
+ if (yoff ) *yoff = iy0;
+
+ if (gbm.w && gbm.h) {
+ gbm.pixels = (unsigned char *) STBTT_malloc(gbm.w * gbm.h, info->userdata);
+ if (gbm.pixels) {
+ gbm.stride = gbm.w;
+
+ stbtt_Rasterize(&gbm, 0.35f, vertices, num_verts, scale_x, scale_y, shift_x, shift_y, ix0, iy0, 1, info->userdata);
+ }
+ }
+ STBTT_free(vertices, info->userdata);
+ return gbm.pixels;
+}
+
+STBTT_DEF unsigned char *stbtt_GetGlyphBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int glyph, int *width, int *height, int *xoff, int *yoff)
+{
+ return stbtt_GetGlyphBitmapSubpixel(info, scale_x, scale_y, 0.0f, 0.0f, glyph, width, height, xoff, yoff);
+}
+
+STBTT_DEF void stbtt_MakeGlyphBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int glyph)
+{
+ int ix0,iy0;
+ stbtt_vertex *vertices;
+ int num_verts = stbtt_GetGlyphShape(info, glyph, &vertices);
+ stbtt__bitmap gbm;
+
+ stbtt_GetGlyphBitmapBoxSubpixel(info, glyph, scale_x, scale_y, shift_x, shift_y, &ix0,&iy0,0,0);
+ gbm.pixels = output;
+ gbm.w = out_w;
+ gbm.h = out_h;
+ gbm.stride = out_stride;
+
+ if (gbm.w && gbm.h)
+ stbtt_Rasterize(&gbm, 0.35f, vertices, num_verts, scale_x, scale_y, shift_x, shift_y, ix0,iy0, 1, info->userdata);
+
+ STBTT_free(vertices, info->userdata);
+}
+
+STBTT_DEF void stbtt_MakeGlyphBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int glyph)
+{
+ stbtt_MakeGlyphBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, 0.0f,0.0f, glyph);
+}
+
+STBTT_DEF unsigned char *stbtt_GetCodepointBitmapSubpixel(const stbtt_fontinfo *info, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint, int *width, int *height, int *xoff, int *yoff)
+{
+ return stbtt_GetGlyphBitmapSubpixel(info, scale_x, scale_y,shift_x,shift_y, stbtt_FindGlyphIndex(info,codepoint), width,height,xoff,yoff);
+}
+
+STBTT_DEF void stbtt_MakeCodepointBitmapSubpixel(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, float shift_x, float shift_y, int codepoint)
+{
+ stbtt_MakeGlyphBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, shift_x, shift_y, stbtt_FindGlyphIndex(info,codepoint));
+}
+
+STBTT_DEF unsigned char *stbtt_GetCodepointBitmap(const stbtt_fontinfo *info, float scale_x, float scale_y, int codepoint, int *width, int *height, int *xoff, int *yoff)
+{
+ return stbtt_GetCodepointBitmapSubpixel(info, scale_x, scale_y, 0.0f,0.0f, codepoint, width,height,xoff,yoff);
+}
+
+STBTT_DEF void stbtt_MakeCodepointBitmap(const stbtt_fontinfo *info, unsigned char *output, int out_w, int out_h, int out_stride, float scale_x, float scale_y, int codepoint)
+{
+ stbtt_MakeCodepointBitmapSubpixel(info, output, out_w, out_h, out_stride, scale_x, scale_y, 0.0f,0.0f, codepoint);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// bitmap baking
+//
+// This is SUPER-CRAPPY packing to keep source code small
+
+STBTT_DEF int stbtt_BakeFontBitmap(const unsigned char *data, int offset, // font location (use offset=0 for plain .ttf)
+ float pixel_height, // height of font in pixels
+ unsigned char *pixels, int pw, int ph, // bitmap to be filled in
+ int first_char, int num_chars, // characters to bake
+ stbtt_bakedchar *chardata)
+{
+ float scale;
+ int x,y,bottom_y, i;
+ stbtt_fontinfo f;
+ if (!stbtt_InitFont(&f, data, offset))
+ return -1;
+ STBTT_memset(pixels, 0, pw*ph); // background of 0 around pixels
+ x=y=1;
+ bottom_y = 1;
+
+ scale = stbtt_ScaleForPixelHeight(&f, pixel_height);
+
+ for (i=0; i < num_chars; ++i) {
+ int advance, lsb, x0,y0,x1,y1,gw,gh;
+ int g = stbtt_FindGlyphIndex(&f, first_char + i);
+ stbtt_GetGlyphHMetrics(&f, g, &advance, &lsb);
+ stbtt_GetGlyphBitmapBox(&f, g, scale,scale, &x0,&y0,&x1,&y1);
+ gw = x1-x0;
+ gh = y1-y0;
+ if (x + gw + 1 >= pw)
+ y = bottom_y, x = 1; // advance to next row
+ if (y + gh + 1 >= ph) // check if it fits vertically AFTER potentially moving to next row
+ return -i;
+ STBTT_assert(x+gw < pw);
+ STBTT_assert(y+gh < ph);
+ stbtt_MakeGlyphBitmap(&f, pixels+x+y*pw, gw,gh,pw, scale,scale, g);
+ chardata[i].x0 = (stbtt_int16) x;
+ chardata[i].y0 = (stbtt_int16) y;
+ chardata[i].x1 = (stbtt_int16) (x + gw);
+ chardata[i].y1 = (stbtt_int16) (y + gh);
+ chardata[i].xadvance = scale * advance;
+ chardata[i].xoff = (float) x0;
+ chardata[i].yoff = (float) y0;
+ x = x + gw + 1;
+ if (y+gh+1 > bottom_y)
+ bottom_y = y+gh+1;
+ }
+ return bottom_y;
+}
+
+STBTT_DEF void stbtt_GetBakedQuad(stbtt_bakedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int opengl_fillrule)
+{
+ float d3d_bias = opengl_fillrule ? 0 : -0.5f;
+ float ipw = 1.0f / pw, iph = 1.0f / ph;
+ stbtt_bakedchar *b = chardata + char_index;
+ int round_x = STBTT_ifloor((*xpos + b->xoff) + 0.5f);
+ int round_y = STBTT_ifloor((*ypos + b->yoff) + 0.5f);
+
+ q->x0 = round_x + d3d_bias;
+ q->y0 = round_y + d3d_bias;
+ q->x1 = round_x + b->x1 - b->x0 + d3d_bias;
+ q->y1 = round_y + b->y1 - b->y0 + d3d_bias;
+
+ q->s0 = b->x0 * ipw;
+ q->t0 = b->y0 * iph;
+ q->s1 = b->x1 * ipw;
+ q->t1 = b->y1 * iph;
+
+ *xpos += b->xadvance;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// rectangle packing replacement routines if you don't have stb_rect_pack.h
+//
+
+#ifndef STB_RECT_PACK_VERSION
+#ifdef _MSC_VER
+#define STBTT__NOTUSED(v) (void)(v)
+#else
+#define STBTT__NOTUSED(v) (void)sizeof(v)
+#endif
+
+typedef int stbrp_coord;
+
+////////////////////////////////////////////////////////////////////////////////////
+// //
+// //
+// COMPILER WARNING ?!?!? //
+// //
+// //
+// if you get a compile warning due to these symbols being defined more than //
+// once, move #include "stb_rect_pack.h" before #include "stb_truetype.h" //
+// //
+////////////////////////////////////////////////////////////////////////////////////
+
+typedef struct
+{
+ int width,height;
+ int x,y,bottom_y;
+} stbrp_context;
+
+typedef struct
+{
+ unsigned char x;
+} stbrp_node;
+
+struct stbrp_rect
+{
+ stbrp_coord x,y;
+ int id,w,h,was_packed;
+};
+
+static void stbrp_init_target(stbrp_context *con, int pw, int ph, stbrp_node *nodes, int num_nodes)
+{
+ con->width = pw;
+ con->height = ph;
+ con->x = 0;
+ con->y = 0;
+ con->bottom_y = 0;
+ STBTT__NOTUSED(nodes);
+ STBTT__NOTUSED(num_nodes);
+}
+
+static void stbrp_pack_rects(stbrp_context *con, stbrp_rect *rects, int num_rects)
+{
+ int i;
+ for (i=0; i < num_rects; ++i) {
+ if (con->x + rects[i].w > con->width) {
+ con->x = 0;
+ con->y = con->bottom_y;
+ }
+ if (con->y + rects[i].h > con->height)
+ break;
+ rects[i].x = con->x;
+ rects[i].y = con->y;
+ rects[i].was_packed = 1;
+ con->x += rects[i].w;
+ if (con->y + rects[i].h > con->bottom_y)
+ con->bottom_y = con->y + rects[i].h;
+ }
+ for ( ; i < num_rects; ++i)
+ rects[i].was_packed = 0;
+}
+#endif
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// bitmap baking
+//
+// This is SUPER-AWESOME (tm Ryan Gordon) packing using stb_rect_pack.h. If
+// stb_rect_pack.h isn't available, it uses the BakeFontBitmap strategy.
+
+STBTT_DEF int stbtt_PackBegin(stbtt_pack_context *spc, unsigned char *pixels, int pw, int ph, int stride_in_bytes, int padding, void *alloc_context)
+{
+ stbrp_context *context = (stbrp_context *) STBTT_malloc(sizeof(*context) ,alloc_context);
+ int num_nodes = pw - padding;
+ stbrp_node *nodes = (stbrp_node *) STBTT_malloc(sizeof(*nodes ) * num_nodes,alloc_context);
+
+ if (context == NULL || nodes == NULL) {
+ if (context != NULL) STBTT_free(context, alloc_context);
+ if (nodes != NULL) STBTT_free(nodes , alloc_context);
+ return 0;
+ }
+
+ spc->user_allocator_context = alloc_context;
+ spc->width = pw;
+ spc->height = ph;
+ spc->pixels = pixels;
+ spc->pack_info = context;
+ spc->nodes = nodes;
+ spc->padding = padding;
+ spc->stride_in_bytes = stride_in_bytes != 0 ? stride_in_bytes : pw;
+ spc->h_oversample = 1;
+ spc->v_oversample = 1;
+
+ stbrp_init_target(context, pw-padding, ph-padding, nodes, num_nodes);
+
+ if (pixels)
+ STBTT_memset(pixels, 0, pw*ph); // background of 0 around pixels
+
+ return 1;
+}
+
+STBTT_DEF void stbtt_PackEnd (stbtt_pack_context *spc)
+{
+ STBTT_free(spc->nodes , spc->user_allocator_context);
+ STBTT_free(spc->pack_info, spc->user_allocator_context);
+}
+
+STBTT_DEF void stbtt_PackSetOversampling(stbtt_pack_context *spc, unsigned int h_oversample, unsigned int v_oversample)
+{
+ STBTT_assert(h_oversample <= STBTT_MAX_OVERSAMPLE);
+ STBTT_assert(v_oversample <= STBTT_MAX_OVERSAMPLE);
+ if (h_oversample <= STBTT_MAX_OVERSAMPLE)
+ spc->h_oversample = h_oversample;
+ if (v_oversample <= STBTT_MAX_OVERSAMPLE)
+ spc->v_oversample = v_oversample;
+}
+
+#define STBTT__OVER_MASK (STBTT_MAX_OVERSAMPLE-1)
+
+static void stbtt__h_prefilter(unsigned char *pixels, int w, int h, int stride_in_bytes, unsigned int kernel_width)
+{
+ unsigned char buffer[STBTT_MAX_OVERSAMPLE];
+ int safe_w = w - kernel_width;
+ int j;
+ for (j=0; j < h; ++j) {
+ int i;
+ unsigned int total;
+ STBTT_memset(buffer, 0, kernel_width);
+
+ total = 0;
+
+ // make kernel_width a constant in common cases so compiler can optimize out the divide
+ switch (kernel_width) {
+ case 2:
+ for (i=0; i <= safe_w; ++i) {
+ total += pixels[i] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i];
+ pixels[i] = (unsigned char) (total / 2);
+ }
+ break;
+ case 3:
+ for (i=0; i <= safe_w; ++i) {
+ total += pixels[i] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i];
+ pixels[i] = (unsigned char) (total / 3);
+ }
+ break;
+ case 4:
+ for (i=0; i <= safe_w; ++i) {
+ total += pixels[i] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i];
+ pixels[i] = (unsigned char) (total / 4);
+ }
+ break;
+ case 5:
+ for (i=0; i <= safe_w; ++i) {
+ total += pixels[i] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i];
+ pixels[i] = (unsigned char) (total / 5);
+ }
+ break;
+ default:
+ for (i=0; i <= safe_w; ++i) {
+ total += pixels[i] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i];
+ pixels[i] = (unsigned char) (total / kernel_width);
+ }
+ break;
+ }
+
+ for (; i < w; ++i) {
+ STBTT_assert(pixels[i] == 0);
+ total -= buffer[i & STBTT__OVER_MASK];
+ pixels[i] = (unsigned char) (total / kernel_width);
+ }
+
+ pixels += stride_in_bytes;
+ }
+}
+
+static void stbtt__v_prefilter(unsigned char *pixels, int w, int h, int stride_in_bytes, unsigned int kernel_width)
+{
+ unsigned char buffer[STBTT_MAX_OVERSAMPLE];
+ int safe_h = h - kernel_width;
+ int j;
+ for (j=0; j < w; ++j) {
+ int i;
+ unsigned int total;
+ STBTT_memset(buffer, 0, kernel_width);
+
+ total = 0;
+
+ // make kernel_width a constant in common cases so compiler can optimize out the divide
+ switch (kernel_width) {
+ case 2:
+ for (i=0; i <= safe_h; ++i) {
+ total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / 2);
+ }
+ break;
+ case 3:
+ for (i=0; i <= safe_h; ++i) {
+ total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / 3);
+ }
+ break;
+ case 4:
+ for (i=0; i <= safe_h; ++i) {
+ total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / 4);
+ }
+ break;
+ case 5:
+ for (i=0; i <= safe_h; ++i) {
+ total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / 5);
+ }
+ break;
+ default:
+ for (i=0; i <= safe_h; ++i) {
+ total += pixels[i*stride_in_bytes] - buffer[i & STBTT__OVER_MASK];
+ buffer[(i+kernel_width) & STBTT__OVER_MASK] = pixels[i*stride_in_bytes];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / kernel_width);
+ }
+ break;
+ }
+
+ for (; i < h; ++i) {
+ STBTT_assert(pixels[i*stride_in_bytes] == 0);
+ total -= buffer[i & STBTT__OVER_MASK];
+ pixels[i*stride_in_bytes] = (unsigned char) (total / kernel_width);
+ }
+
+ pixels += 1;
+ }
+}
+
+static float stbtt__oversample_shift(int oversample)
+{
+ if (!oversample)
+ return 0.0f;
+
+ // The prefilter is a box filter of width "oversample",
+ // which shifts phase by (oversample - 1)/2 pixels in
+ // oversampled space. We want to shift in the opposite
+ // direction to counter this.
+ return (float)-(oversample - 1) / (2.0f * (float)oversample);
+}
+
+// rects array must be big enough to accommodate all characters in the given ranges
+STBTT_DEF int stbtt_PackFontRangesGatherRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects)
+{
+ int i,j,k;
+
+ k=0;
+ for (i=0; i < num_ranges; ++i) {
+ float fh = ranges[i].font_size;
+ float scale = fh > 0 ? stbtt_ScaleForPixelHeight(info, fh) : stbtt_ScaleForMappingEmToPixels(info, -fh);
+ ranges[i].h_oversample = (unsigned char) spc->h_oversample;
+ ranges[i].v_oversample = (unsigned char) spc->v_oversample;
+ for (j=0; j < ranges[i].num_chars; ++j) {
+ int x0,y0,x1,y1;
+ int codepoint = ranges[i].first_unicode_codepoint_in_range ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j];
+ int glyph = stbtt_FindGlyphIndex(info, codepoint);
+ stbtt_GetGlyphBitmapBoxSubpixel(info,glyph,
+ scale * spc->h_oversample,
+ scale * spc->v_oversample,
+ 0,0,
+ &x0,&y0,&x1,&y1);
+ rects[k].w = (stbrp_coord) (x1-x0 + spc->padding + spc->h_oversample-1);
+ rects[k].h = (stbrp_coord) (y1-y0 + spc->padding + spc->v_oversample-1);
+ ++k;
+ }
+ }
+
+ return k;
+}
+
+// rects array must be big enough to accommodate all characters in the given ranges
+STBTT_DEF int stbtt_PackFontRangesRenderIntoRects(stbtt_pack_context *spc, stbtt_fontinfo *info, stbtt_pack_range *ranges, int num_ranges, stbrp_rect *rects)
+{
+ int i,j,k, return_value = 1;
+
+ // save current values
+ int old_h_over = spc->h_oversample;
+ int old_v_over = spc->v_oversample;
+
+ k = 0;
+ for (i=0; i < num_ranges; ++i) {
+ float fh = ranges[i].font_size;
+ float scale = fh > 0 ? stbtt_ScaleForPixelHeight(info, fh) : stbtt_ScaleForMappingEmToPixels(info, -fh);
+ float recip_h,recip_v,sub_x,sub_y;
+ spc->h_oversample = ranges[i].h_oversample;
+ spc->v_oversample = ranges[i].v_oversample;
+ recip_h = 1.0f / spc->h_oversample;
+ recip_v = 1.0f / spc->v_oversample;
+ sub_x = stbtt__oversample_shift(spc->h_oversample);
+ sub_y = stbtt__oversample_shift(spc->v_oversample);
+ for (j=0; j < ranges[i].num_chars; ++j) {
+ stbrp_rect *r = &rects[k];
+ if (r->was_packed) {
+ stbtt_packedchar *bc = &ranges[i].chardata_for_range[j];
+ int advance, lsb, x0,y0,x1,y1;
+ int codepoint = ranges[i].first_unicode_codepoint_in_range ? ranges[i].first_unicode_codepoint_in_range + j : ranges[i].array_of_unicode_codepoints[j];
+ int glyph = stbtt_FindGlyphIndex(info, codepoint);
+ stbrp_coord pad = (stbrp_coord) spc->padding;
+
+ // pad on left and top
+ r->x += pad;
+ r->y += pad;
+ r->w -= pad;
+ r->h -= pad;
+ stbtt_GetGlyphHMetrics(info, glyph, &advance, &lsb);
+ stbtt_GetGlyphBitmapBox(info, glyph,
+ scale * spc->h_oversample,
+ scale * spc->v_oversample,
+ &x0,&y0,&x1,&y1);
+ stbtt_MakeGlyphBitmapSubpixel(info,
+ spc->pixels + r->x + r->y*spc->stride_in_bytes,
+ r->w - spc->h_oversample+1,
+ r->h - spc->v_oversample+1,
+ spc->stride_in_bytes,
+ scale * spc->h_oversample,
+ scale * spc->v_oversample,
+ 0,0,
+ glyph);
+
+ if (spc->h_oversample > 1)
+ stbtt__h_prefilter(spc->pixels + r->x + r->y*spc->stride_in_bytes,
+ r->w, r->h, spc->stride_in_bytes,
+ spc->h_oversample);
+
+ if (spc->v_oversample > 1)
+ stbtt__v_prefilter(spc->pixels + r->x + r->y*spc->stride_in_bytes,
+ r->w, r->h, spc->stride_in_bytes,
+ spc->v_oversample);
+
+ bc->x0 = (stbtt_int16) r->x;
+ bc->y0 = (stbtt_int16) r->y;
+ bc->x1 = (stbtt_int16) (r->x + r->w);
+ bc->y1 = (stbtt_int16) (r->y + r->h);
+ bc->xadvance = scale * advance;
+ bc->xoff = (float) x0 * recip_h + sub_x;
+ bc->yoff = (float) y0 * recip_v + sub_y;
+ bc->xoff2 = (x0 + r->w) * recip_h + sub_x;
+ bc->yoff2 = (y0 + r->h) * recip_v + sub_y;
+ } else {
+ return_value = 0; // if any fail, report failure
+ }
+
+ ++k;
+ }
+ }
+
+ // restore original values
+ spc->h_oversample = old_h_over;
+ spc->v_oversample = old_v_over;
+
+ return return_value;
+}
+
+STBTT_DEF void stbtt_PackFontRangesPackRects(stbtt_pack_context *spc, stbrp_rect *rects, int num_rects)
+{
+ stbrp_pack_rects((stbrp_context *) spc->pack_info, rects, num_rects);
+}
+
+STBTT_DEF int stbtt_PackFontRanges(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, stbtt_pack_range *ranges, int num_ranges)
+{
+ stbtt_fontinfo info;
+ int i,j,n, return_value = 1;
+ //stbrp_context *context = (stbrp_context *) spc->pack_info;
+ stbrp_rect *rects;
+
+ // flag all characters as NOT packed
+ for (i=0; i < num_ranges; ++i)
+ for (j=0; j < ranges[i].num_chars; ++j)
+ ranges[i].chardata_for_range[j].x0 =
+ ranges[i].chardata_for_range[j].y0 =
+ ranges[i].chardata_for_range[j].x1 =
+ ranges[i].chardata_for_range[j].y1 = 0;
+
+ n = 0;
+ for (i=0; i < num_ranges; ++i)
+ n += ranges[i].num_chars;
+
+ rects = (stbrp_rect *) STBTT_malloc(sizeof(*rects) * n, spc->user_allocator_context);
+ if (rects == NULL)
+ return 0;
+
+ stbtt_InitFont(&info, fontdata, stbtt_GetFontOffsetForIndex(fontdata,font_index));
+
+ n = stbtt_PackFontRangesGatherRects(spc, &info, ranges, num_ranges, rects);
+
+ stbtt_PackFontRangesPackRects(spc, rects, n);
+
+ return_value = stbtt_PackFontRangesRenderIntoRects(spc, &info, ranges, num_ranges, rects);
+
+ STBTT_free(rects, spc->user_allocator_context);
+ return return_value;
+}
+
+STBTT_DEF int stbtt_PackFontRange(stbtt_pack_context *spc, unsigned char *fontdata, int font_index, float font_size,
+ int first_unicode_codepoint_in_range, int num_chars_in_range, stbtt_packedchar *chardata_for_range)
+{
+ stbtt_pack_range range;
+ range.first_unicode_codepoint_in_range = first_unicode_codepoint_in_range;
+ range.array_of_unicode_codepoints = NULL;
+ range.num_chars = num_chars_in_range;
+ range.chardata_for_range = chardata_for_range;
+ range.font_size = font_size;
+ return stbtt_PackFontRanges(spc, fontdata, font_index, &range, 1);
+}
+
+STBTT_DEF void stbtt_GetPackedQuad(stbtt_packedchar *chardata, int pw, int ph, int char_index, float *xpos, float *ypos, stbtt_aligned_quad *q, int align_to_integer)
+{
+ float ipw = 1.0f / pw, iph = 1.0f / ph;
+ stbtt_packedchar *b = chardata + char_index;
+
+ if (align_to_integer) {
+ float x = (float) STBTT_ifloor((*xpos + b->xoff) + 0.5f);
+ float y = (float) STBTT_ifloor((*ypos + b->yoff) + 0.5f);
+ q->x0 = x;
+ q->y0 = y;
+ q->x1 = x + b->xoff2 - b->xoff;
+ q->y1 = y + b->yoff2 - b->yoff;
+ } else {
+ q->x0 = *xpos + b->xoff;
+ q->y0 = *ypos + b->yoff;
+ q->x1 = *xpos + b->xoff2;
+ q->y1 = *ypos + b->yoff2;
+ }
+
+ q->s0 = b->x0 * ipw;
+ q->t0 = b->y0 * iph;
+ q->s1 = b->x1 * ipw;
+ q->t1 = b->y1 * iph;
+
+ *xpos += b->xadvance;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+//
+// font name matching -- recommended not to use this
+//
+
+// check if a utf8 string contains a prefix which is the utf16 string; if so return length of matching utf8 string
+static stbtt_int32 stbtt__CompareUTF8toUTF16_bigendian_prefix(const stbtt_uint8 *s1, stbtt_int32 len1, const stbtt_uint8 *s2, stbtt_int32 len2)
+{
+ stbtt_int32 i=0;
+
+ // convert utf16 to utf8 and compare the results while converting
+ while (len2) {
+ stbtt_uint16 ch = s2[0]*256 + s2[1];
+ if (ch < 0x80) {
+ if (i >= len1) return -1;
+ if (s1[i++] != ch) return -1;
+ } else if (ch < 0x800) {
+ if (i+1 >= len1) return -1;
+ if (s1[i++] != 0xc0 + (ch >> 6)) return -1;
+ if (s1[i++] != 0x80 + (ch & 0x3f)) return -1;
+ } else if (ch >= 0xd800 && ch < 0xdc00) {
+ stbtt_uint32 c;
+ stbtt_uint16 ch2 = s2[2]*256 + s2[3];
+ if (i+3 >= len1) return -1;
+ c = ((ch - 0xd800) << 10) + (ch2 - 0xdc00) + 0x10000;
+ if (s1[i++] != 0xf0 + (c >> 18)) return -1;
+ if (s1[i++] != 0x80 + ((c >> 12) & 0x3f)) return -1;
+ if (s1[i++] != 0x80 + ((c >> 6) & 0x3f)) return -1;
+ if (s1[i++] != 0x80 + ((c ) & 0x3f)) return -1;
+ s2 += 2; // plus another 2 below
+ len2 -= 2;
+ } else if (ch >= 0xdc00 && ch < 0xe000) {
+ return -1;
+ } else {
+ if (i+2 >= len1) return -1;
+ if (s1[i++] != 0xe0 + (ch >> 12)) return -1;
+ if (s1[i++] != 0x80 + ((ch >> 6) & 0x3f)) return -1;
+ if (s1[i++] != 0x80 + ((ch ) & 0x3f)) return -1;
+ }
+ s2 += 2;
+ len2 -= 2;
+ }
+ return i;
+}
+
+STBTT_DEF int stbtt_CompareUTF8toUTF16_bigendian(const char *s1, int len1, const char *s2, int len2)
+{
+ return len1 == stbtt__CompareUTF8toUTF16_bigendian_prefix((const stbtt_uint8*) s1, len1, (const stbtt_uint8*) s2, len2);
+}
+
+// returns results in whatever encoding you request... but note that 2-byte encodings
+// will be BIG-ENDIAN... use stbtt_CompareUTF8toUTF16_bigendian() to compare
+STBTT_DEF const char *stbtt_GetFontNameString(const stbtt_fontinfo *font, int *length, int platformID, int encodingID, int languageID, int nameID)
+{
+ stbtt_int32 i,count,stringOffset;
+ stbtt_uint8 *fc = font->data;
+ stbtt_uint32 offset = font->fontstart;
+ stbtt_uint32 nm = stbtt__find_table(fc, offset, "name");
+ if (!nm) return NULL;
+
+ count = ttUSHORT(fc+nm+2);
+ stringOffset = nm + ttUSHORT(fc+nm+4);
+ for (i=0; i < count; ++i) {
+ stbtt_uint32 loc = nm + 6 + 12 * i;
+ if (platformID == ttUSHORT(fc+loc+0) && encodingID == ttUSHORT(fc+loc+2)
+ && languageID == ttUSHORT(fc+loc+4) && nameID == ttUSHORT(fc+loc+6)) {
+ *length = ttUSHORT(fc+loc+8);
+ return (const char *) (fc+stringOffset+ttUSHORT(fc+loc+10));
+ }
+ }
+ return NULL;
+}
+
+static int stbtt__matchpair(stbtt_uint8 *fc, stbtt_uint32 nm, stbtt_uint8 *name, stbtt_int32 nlen, stbtt_int32 target_id, stbtt_int32 next_id)
+{
+ stbtt_int32 i;
+ stbtt_int32 count = ttUSHORT(fc+nm+2);
+ stbtt_int32 stringOffset = nm + ttUSHORT(fc+nm+4);
+
+ for (i=0; i < count; ++i) {
+ stbtt_uint32 loc = nm + 6 + 12 * i;
+ stbtt_int32 id = ttUSHORT(fc+loc+6);
+ if (id == target_id) {
+ // find the encoding
+ stbtt_int32 platform = ttUSHORT(fc+loc+0), encoding = ttUSHORT(fc+loc+2), language = ttUSHORT(fc+loc+4);
+
+ // is this a Unicode encoding?
+ if (platform == 0 || (platform == 3 && encoding == 1) || (platform == 3 && encoding == 10)) {
+ stbtt_int32 slen = ttUSHORT(fc+loc+8);
+ stbtt_int32 off = ttUSHORT(fc+loc+10);
+
+ // check if there's a prefix match
+ stbtt_int32 matchlen = stbtt__CompareUTF8toUTF16_bigendian_prefix(name, nlen, fc+stringOffset+off,slen);
+ if (matchlen >= 0) {
+ // check for target_id+1 immediately following, with same encoding & language
+ if (i+1 < count && ttUSHORT(fc+loc+12+6) == next_id && ttUSHORT(fc+loc+12) == platform && ttUSHORT(fc+loc+12+2) == encoding && ttUSHORT(fc+loc+12+4) == language) {
+ slen = ttUSHORT(fc+loc+12+8);
+ off = ttUSHORT(fc+loc+12+10);
+ if (slen == 0) {
+ if (matchlen == nlen)
+ return 1;
+ } else if (matchlen < nlen && name[matchlen] == ' ') {
+ ++matchlen;
+ if (stbtt_CompareUTF8toUTF16_bigendian((char*) (name+matchlen), nlen-matchlen, (char*)(fc+stringOffset+off),slen))
+ return 1;
+ }
+ } else {
+ // if nothing immediately following
+ if (matchlen == nlen)
+ return 1;
+ }
+ }
+ }
+
+ // @TODO handle other encodings
+ }
+ }
+ return 0;
+}
+
+static int stbtt__matches(stbtt_uint8 *fc, stbtt_uint32 offset, stbtt_uint8 *name, stbtt_int32 flags)
+{
+ stbtt_int32 nlen = (stbtt_int32) STBTT_strlen((char *) name);
+ stbtt_uint32 nm,hd;
+ if (!stbtt__isfont(fc+offset)) return 0;
+
+ // check italics/bold/underline flags in macStyle...
+ if (flags) {
+ hd = stbtt__find_table(fc, offset, "head");
+ if ((ttUSHORT(fc+hd+44) & 7) != (flags & 7)) return 0;
+ }
+
+ nm = stbtt__find_table(fc, offset, "name");
+ if (!nm) return 0;
+
+ if (flags) {
+ // if we checked the macStyle flags, then just check the family and ignore the subfamily
+ if (stbtt__matchpair(fc, nm, name, nlen, 16, -1)) return 1;
+ if (stbtt__matchpair(fc, nm, name, nlen, 1, -1)) return 1;
+ if (stbtt__matchpair(fc, nm, name, nlen, 3, -1)) return 1;
+ } else {
+ if (stbtt__matchpair(fc, nm, name, nlen, 16, 17)) return 1;
+ if (stbtt__matchpair(fc, nm, name, nlen, 1, 2)) return 1;
+ if (stbtt__matchpair(fc, nm, name, nlen, 3, -1)) return 1;
+ }
+
+ return 0;
+}
+
+STBTT_DEF int stbtt_FindMatchingFont(const unsigned char *font_collection, const char *name_utf8, stbtt_int32 flags)
+{
+ stbtt_int32 i;
+ for (i=0;;++i) {
+ stbtt_int32 off = stbtt_GetFontOffsetForIndex(font_collection, i);
+ if (off < 0) return off;
+ if (stbtt__matches((stbtt_uint8 *) font_collection, off, (stbtt_uint8*) name_utf8, flags))
+ return off;
+ }
+}
+
+#endif // STB_TRUETYPE_IMPLEMENTATION
+
+
+// FULL VERSION HISTORY
+//
+// 1.07 (2015-08-01) allow PackFontRanges to accept arrays of sparse codepoints;
+// allow PackFontRanges to pack and render in separate phases;
+// fix stbtt_GetFontOFfsetForIndex (never worked for non-0 input?);
+// fixed an assert() bug in the new rasterizer
+// replace assert() with STBTT_assert() in new rasterizer
+// 1.06 (2015-07-14) performance improvements (~35% faster on x86 and x64 on test machine)
+// also more precise AA rasterizer, except if shapes overlap
+// remove need for STBTT_sort
+// 1.05 (2015-04-15) fix misplaced definitions for STBTT_STATIC
+// 1.04 (2015-04-15) typo in example
+// 1.03 (2015-04-12) STBTT_STATIC, fix memory leak in new packing, various fixes
+// 1.02 (2014-12-10) fix various warnings & compile issues w/ stb_rect_pack, C++
+// 1.01 (2014-12-08) fix subpixel position when oversampling to exactly match
+// non-oversampled; STBTT_POINT_SIZE for packed case only
+// 1.00 (2014-12-06) add new PackBegin etc. API, w/ support for oversampling
+// 0.99 (2014-09-18) fix multiple bugs with subpixel rendering (ryg)
+// 0.9 (2014-08-07) support certain mac/iOS fonts without an MS platformID
+// 0.8b (2014-07-07) fix a warning
+// 0.8 (2014-05-25) fix a few more warnings
+// 0.7 (2013-09-25) bugfix: subpixel glyph bug fixed in 0.5 had come back
+// 0.6c (2012-07-24) improve documentation
+// 0.6b (2012-07-20) fix a few more warnings
+// 0.6 (2012-07-17) fix warnings; added stbtt_ScaleForMappingEmToPixels,
+// stbtt_GetFontBoundingBox, stbtt_IsGlyphEmpty
+// 0.5 (2011-12-09) bugfixes:
+// subpixel glyph renderer computed wrong bounding box
+// first vertex of shape can be off-curve (FreeSans)
+// 0.4b (2011-12-03) fixed an error in the font baking example
+// 0.4 (2011-12-01) kerning, subpixel rendering (tor)
+// bugfixes for:
+// codepoint-to-glyph conversion using table fmt=12
+// codepoint-to-glyph conversion using table fmt=4
+// stbtt_GetBakedQuad with non-square texture (Zer)
+// updated Hello World! sample to use kerning and subpixel
+// fixed some warnings
+// 0.3 (2009-06-24) cmap fmt=12, compound shapes (MM)
+// userdata, malloc-from-userdata, non-zero fill (stb)
+// 0.2 (2009-03-11) Fix unsigned/signed char warnings
+// 0.1 (2009-03-09) First public release
+//
diff --git a/externals/Pangolin/src/handler/handler.cpp b/externals/Pangolin/src/handler/handler.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..11054a4449bb39c2cd969fffa3d9f0055a4f4840
--- /dev/null
+++ b/externals/Pangolin/src/handler/handler.cpp
@@ -0,0 +1,401 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/handler/handler.h>
+#include <pangolin/display/display_internal.h>
+#include <pangolin/display/view.h>
+
+namespace pangolin
+{
+
+// Pointer to context defined in display.cpp
+extern __thread PangolinGl* context;
+
+void Handler::Keyboard(View& d, unsigned char key, int x, int y, bool pressed)
+{
+ View* child = d.FindChild(x,y);
+ if( child)
+ {
+ context->activeDisplay = child;
+ if( child->handler)
+ child->handler->Keyboard(*child,key,x,y,pressed);
+ }
+}
+
+void Handler::Mouse(View& d, MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ View* child = d.FindChild(x,y);
+ if( child )
+ {
+ context->activeDisplay = child;
+ if( child->handler)
+ child->handler->Mouse(*child,button,x,y,pressed,button_state);
+ }
+}
+
+void Handler::MouseMotion(View& d, int x, int y, int button_state)
+{
+ View* child = d.FindChild(x,y);
+ if( child )
+ {
+ context->activeDisplay = child;
+ if( child->handler)
+ child->handler->MouseMotion(*child,x,y,button_state);
+ }
+}
+
+void Handler::PassiveMouseMotion(View& d, int x, int y, int button_state)
+{
+ View* child = d.FindChild(x,y);
+ if( child )
+ {
+ if( child->handler)
+ child->handler->PassiveMouseMotion(*child,x,y,button_state);
+ }
+}
+
+void Handler::Special(View& d, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state)
+{
+ View* child = d.FindChild( (int)x, (int)y);
+ if( child )
+ {
+ context->activeDisplay = child;
+ if( child->handler)
+ child->handler->Special(*child,inType, x,y, p1, p2, p3, p4, button_state);
+ }
+}
+
+void HandlerScroll::Mouse(View& d, MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ if( pressed && (button == MouseWheelUp || button == MouseWheelDown) )
+ {
+ if( button == MouseWheelUp) d.scroll_offset -= 1;
+ if( button == MouseWheelDown) d.scroll_offset += 1;
+ d.scroll_offset = std::max(0, std::min(d.scroll_offset, (int)d.views.size()) );
+ d.ResizeChildren();
+ }else{
+ Handler::Mouse(d,button,x,y,pressed,button_state);
+ }
+}
+
+void HandlerScroll::Special(View& d, InputSpecial inType, float x, float y, float p1, float p2, float p3, float p4, int button_state)
+{
+ if( inType == InputSpecialScroll )
+ {
+ d.scroll_offset -= (int)(p2 / fabs(p2));
+ d.scroll_offset = std::max(0, std::min(d.scroll_offset, (int)d.views.size()) );
+ d.ResizeChildren();
+ }else{
+ Handler::Special(d,inType,x,y,p1,p2,p3,p4,button_state);
+ }
+}
+
+Handler3D::Handler3D(OpenGlRenderState& cam_state, AxisDirection enforce_up, float trans_scale, float zoom_fraction)
+ : cam_state(&cam_state), enforce_up(enforce_up), tf(trans_scale), zf(zoom_fraction), cameraspec(CameraSpecOpenGl), last_z(0.8)
+{
+ SetZero<3,1>(rot_center);
+}
+
+void Handler3D::Keyboard(View&, unsigned char /*key*/, int /*x*/, int /*y*/, bool /*pressed*/)
+{
+ // TODO: hooks for reset / changing mode (perspective / ortho etc)
+}
+
+bool Handler3D::ValidWinDepth(GLprecision depth)
+{
+ return depth != 1;
+}
+
+void Handler3D::PixelUnproject( View& view, GLprecision winx, GLprecision winy, GLprecision winz, GLprecision Pc[3])
+{
+ const GLint viewport[4] = {view.v.l,view.v.b,view.v.w,view.v.h};
+ const pangolin::OpenGlMatrix proj = cam_state->GetProjectionMatrix();
+ glUnProject(winx, winy, winz, Identity4d, proj.m, viewport, &Pc[0], &Pc[1], &Pc[2]);
+}
+
+void Handler3D::GetPosNormal(pangolin::View& view, int winx, int winy, GLprecision p[3], GLprecision Pw[3], GLprecision Pc[3], GLprecision nw[3], GLprecision default_z)
+{
+ // TODO: Get to work on android
+
+ const int zl = (hwin*2+1);
+ const int zsize = zl*zl;
+ GLfloat zs[zsize];
+
+#ifndef HAVE_GLES
+ glReadBuffer(GL_FRONT);
+ glReadPixels(winx-hwin,winy-hwin,zl,zl,GL_DEPTH_COMPONENT,GL_FLOAT,zs);
+#else
+ std::fill(zs,zs+zsize, 1);
+#endif
+ GLfloat mindepth = *(std::min_element(zs,zs+zsize));
+
+ if(mindepth == 1) mindepth = (GLfloat)default_z;
+
+ p[0] = winx; p[1] = winy; p[2] = mindepth;
+ PixelUnproject(view, winx, winy, mindepth, Pc);
+
+ const pangolin::OpenGlMatrix mv = cam_state->GetModelViewMatrix();
+
+ GLprecision T_wc[3*4];
+ LieSE3from4x4(T_wc, mv.Inverse().m );
+ LieApplySE3vec(Pw, T_wc, Pc);
+
+ // Neighboring points in camera coordinates
+ GLprecision Pl[3]; GLprecision Pr[3]; GLprecision Pb[3]; GLprecision Pt[3];
+ PixelUnproject(view, winx-hwin, winy, zs[hwin*zl + 0], Pl );
+ PixelUnproject(view, winx+hwin, winy, zs[hwin*zl + zl-1], Pr );
+ PixelUnproject(view, winx, winy-hwin, zs[hwin+1], Pb );
+ PixelUnproject(view, winx, winy+hwin, zs[zsize-(hwin+1)], Pt );
+
+ // n = ((Pr-Pl).cross(Pt-Pb)).normalized();
+ GLprecision PrmPl[3]; GLprecision PtmPb[3];
+ MatSub<3,1>(PrmPl,Pr,Pl);
+ MatSub<3,1>(PtmPb,Pt,Pb);
+
+ GLprecision nc[3];
+ CrossProduct(nc, PrmPl, PtmPb);
+ Normalise<3>(nc);
+
+ // T_wc is col major, so the rotation component is first.
+ LieApplySO3(nw,T_wc,nc);
+}
+
+void Handler3D::Mouse(View& display, MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ // mouse down
+ last_pos[0] = (float)x;
+ last_pos[1] = (float)y;
+
+ GLprecision T_nc[3*4];
+ LieSetIdentity(T_nc);
+
+ funcKeyState = 0;
+ if( pressed )
+ {
+ GetPosNormal(display,x,y,p,Pw,Pc,n,last_z);
+ if( ValidWinDepth(p[2]) )
+ {
+ last_z = p[2];
+ std::copy(Pc,Pc+3,rot_center);
+ }
+
+ if( button == MouseWheelUp || button == MouseWheelDown)
+ {
+ LieSetIdentity(T_nc);
+ const GLprecision t[3] = { 0,0,(button==MouseWheelUp?1:-1)*100*tf};
+ LieSetTranslation<>(T_nc,t);
+ if( !(button_state & MouseButtonRight) && !(rot_center[0]==0 && rot_center[1]==0 && rot_center[2]==0) )
+ {
+ LieSetTranslation<>(T_nc,rot_center);
+ const GLprecision s = (button==MouseWheelUp?-1.0:1.0) * zf;
+ MatMul<3,1>(T_nc+(3*3), s);
+ }
+ OpenGlMatrix& spec = cam_state->GetModelViewMatrix();
+ LieMul4x4bySE3<>(spec.m,T_nc,spec.m);
+ }
+
+ funcKeyState = button_state;
+ }
+}
+
+void Handler3D::MouseMotion(View& display, int x, int y, int button_state)
+{
+ const GLprecision rf = 0.01;
+ const float delta[2] = { (float)x - last_pos[0], (float)y - last_pos[1] };
+ const float mag = delta[0]*delta[0] + delta[1]*delta[1];
+
+ if((button_state & KeyModifierCtrl) && (button_state & KeyModifierShift))
+ {
+ GLprecision T_nc[3 * 4];
+ LieSetIdentity(T_nc);
+
+ GetPosNormal(display, x, y, p, Pw, Pc, n, last_z);
+ if(ValidWinDepth(p[2]))
+ {
+ last_z = p[2];
+ std::copy(Pc, Pc + 3, rot_center);
+ }
+
+ funcKeyState = button_state;
+ }
+ else
+ {
+ funcKeyState = 0;
+ }
+
+ // TODO: convert delta to degrees based of fov
+ // TODO: make transformation with respect to cam spec
+ if( mag < 50.0f*50.0f )
+ {
+ OpenGlMatrix& mv = cam_state->GetModelViewMatrix();
+ const GLprecision* up = AxisDirectionVector[enforce_up];
+ GLprecision T_nc[3*4];
+ LieSetIdentity(T_nc);
+ bool rotation_changed = false;
+
+ if( button_state == MouseButtonMiddle )
+ {
+ // Middle Drag: Rotate around view
+
+ // Try to correct for different coordinate conventions.
+ GLprecision aboutx = -rf * delta[1];
+ GLprecision abouty = rf * delta[0];
+ OpenGlMatrix& pm = cam_state->GetProjectionMatrix();
+ abouty *= -pm.m[2 * 4 + 3];
+
+ Rotation<>(T_nc, aboutx, abouty, (GLprecision)0.0);
+ }else if( button_state == MouseButtonLeft )
+ {
+ // Left Drag: in plane translate
+ if( ValidWinDepth(last_z) )
+ {
+ GLprecision np[3];
+ PixelUnproject(display, x, y, last_z, np);
+ const GLprecision t[] = { np[0] - rot_center[0], np[1] - rot_center[1], 0};
+ LieSetTranslation<>(T_nc,t);
+ std::copy(np,np+3,rot_center);
+ }else{
+ const GLprecision t[] = { -10*delta[0]*tf, 10*delta[1]*tf, 0};
+ LieSetTranslation<>(T_nc,t);
+ }
+ }else if( button_state == (MouseButtonLeft | MouseButtonRight) )
+ {
+ // Left and Right Drag: in plane rotate about object
+ // Rotation<>(T_nc,0.0,0.0, delta[0]*0.01);
+
+ GLprecision T_2c[3*4];
+ Rotation<>(T_2c, (GLprecision)0.0, (GLprecision)0.0, delta[0]*rf);
+ GLprecision mrotc[3];
+ MatMul<3,1>(mrotc, rot_center, (GLprecision)-1.0);
+ LieApplySO3<>(T_2c+(3*3),T_2c,mrotc);
+ GLprecision T_n2[3*4];
+ LieSetIdentity<>(T_n2);
+ LieSetTranslation<>(T_n2,rot_center);
+ LieMulSE3(T_nc, T_n2, T_2c );
+ rotation_changed = true;
+ }else if( button_state == MouseButtonRight)
+ {
+ GLprecision aboutx = -rf * delta[1];
+ GLprecision abouty = -rf * delta[0];
+
+ // Try to correct for different coordinate conventions.
+ if(cam_state->GetProjectionMatrix().m[2*4+3] <= 0) {
+ abouty *= -1;
+ }
+
+ if(enforce_up) {
+ // Special case if view direction is parallel to up vector
+ const GLprecision updotz = mv.m[2]*up[0] + mv.m[6]*up[1] + mv.m[10]*up[2];
+ if(updotz > 0.98) aboutx = std::min(aboutx, (GLprecision)0.0);
+ if(updotz <-0.98) aboutx = std::max(aboutx, (GLprecision)0.0);
+ // Module rotation around y so we don't spin too fast!
+ abouty *= (1-0.6*fabs(updotz));
+ }
+
+ // Right Drag: object centric rotation
+ GLprecision T_2c[3*4];
+ Rotation<>(T_2c, aboutx, abouty, (GLprecision)0.0);
+ GLprecision mrotc[3];
+ MatMul<3,1>(mrotc, rot_center, (GLprecision)-1.0);
+ LieApplySO3<>(T_2c+(3*3),T_2c,mrotc);
+ GLprecision T_n2[3*4];
+ LieSetIdentity<>(T_n2);
+ LieSetTranslation<>(T_n2,rot_center);
+ LieMulSE3(T_nc, T_n2, T_2c );
+ rotation_changed = true;
+ }
+
+ LieMul4x4bySE3<>(mv.m,T_nc,mv.m);
+
+ if(enforce_up != AxisNone && rotation_changed) {
+ EnforceUpT_cw(mv.m, up);
+ }
+ }
+
+ last_pos[0] = (float)x;
+ last_pos[1] = (float)y;
+}
+
+void Handler3D::Special(View& display, InputSpecial inType, float x, float y, float p1, float p2, float /*p3*/, float /*p4*/, int button_state)
+{
+ if( !(inType == InputSpecialScroll || inType == InputSpecialRotate) )
+ return;
+
+ // mouse down
+ last_pos[0] = x;
+ last_pos[1] = y;
+
+ GLprecision T_nc[3*4];
+ LieSetIdentity(T_nc);
+
+ GetPosNormal(display, (int)x, (int)y, p, Pw, Pc, n, last_z);
+ if(p[2] < 1.0) {
+ last_z = p[2];
+ std::copy(Pc,Pc+3,rot_center);
+ }
+
+ if( inType == InputSpecialScroll ) {
+ if(button_state & KeyModifierCmd) {
+ const GLprecision rx = -p2 / 1000;
+ const GLprecision ry = -p1 / 1000;
+
+ Rotation<>(T_nc,rx, ry, (GLprecision)0.0);
+ OpenGlMatrix& spec = cam_state->GetModelViewMatrix();
+ LieMul4x4bySE3<>(spec.m,T_nc,spec.m);
+ }else{
+ const GLprecision scrolly = p2/10;
+
+ LieSetIdentity(T_nc);
+ const GLprecision t[] = { 0,0, -scrolly*100*tf};
+ LieSetTranslation<>(T_nc,t);
+ if( !(button_state & MouseButtonRight) && !(rot_center[0]==0 && rot_center[1]==0 && rot_center[2]==0) )
+ {
+ LieSetTranslation<>(T_nc,rot_center);
+ MatMul<3,1>(T_nc+(3*3), -scrolly * zf);
+ }
+ OpenGlMatrix& spec = cam_state->GetModelViewMatrix();
+ LieMul4x4bySE3<>(spec.m,T_nc,spec.m);
+ }
+ }else if(inType == InputSpecialRotate) {
+ const GLprecision r = p1 / 20;
+
+ GLprecision T_2c[3*4];
+ Rotation<>(T_2c, (GLprecision)0.0, (GLprecision)0.0, r);
+ GLprecision mrotc[3];
+ MatMul<3,1>(mrotc, rot_center, (GLprecision)-1.0);
+ LieApplySO3<>(T_2c+(3*3),T_2c,mrotc);
+ GLprecision T_n2[3*4];
+ LieSetIdentity<>(T_n2);
+ LieSetTranslation<>(T_n2,rot_center);
+ LieMulSE3(T_nc, T_n2, T_2c );
+ OpenGlMatrix& spec = cam_state->GetModelViewMatrix();
+ LieMul4x4bySE3<>(spec.m,T_nc,spec.m);
+ }
+
+}
+
+}
diff --git a/externals/Pangolin/src/handler/handler_glbuffer.cpp b/externals/Pangolin/src/handler/handler_glbuffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bf7872e018df50428630bfed9b0024d329058fc4
--- /dev/null
+++ b/externals/Pangolin/src/handler/handler_glbuffer.cpp
@@ -0,0 +1,45 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/handler/handler_glbuffer.h>
+#include <pangolin/display/view.h>
+
+namespace pangolin {
+
+Handler3DFramebuffer::Handler3DFramebuffer(GlFramebuffer& fb, pangolin::OpenGlRenderState& cam_state, pangolin::AxisDirection enforce_up, float trans_scale)
+ : pangolin::Handler3D(cam_state,enforce_up, trans_scale), fb(fb)
+{
+}
+
+void Handler3DFramebuffer::GetPosNormal(pangolin::View& view, int x, int y, GLprecision p[3], GLprecision Pw[3], GLprecision Pc[3], GLprecision n[3], GLprecision default_z)
+{
+ fb.Bind();
+ Handler3D::GetPosNormal(view,x,y,p,Pw,Pc,n,default_z);
+ fb.Unbind();
+}
+
+}
diff --git a/externals/Pangolin/src/handler/handler_image.cpp b/externals/Pangolin/src/handler/handler_image.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8ca18fd5fe7d52b982d9d177d7740849b21ee405
--- /dev/null
+++ b/externals/Pangolin/src/handler/handler_image.cpp
@@ -0,0 +1,462 @@
+#include <pangolin/handler/handler_image.h>
+#include <pangolin/gl/glfont.h>
+
+namespace pangolin
+{
+
+ImageViewHandler::ImageViewHandler()
+ : linked_view_handler(0),
+ use_nn(false), flipTextureX(false), flipTextureY(false)
+{
+ SetDimensions(1, 1);
+}
+
+ImageViewHandler::ImageViewHandler(size_t w, size_t h)
+ : linked_view_handler(0),
+ use_nn(false), flipTextureX(false), flipTextureY(false)
+{
+ SetDimensions(w,h);
+}
+
+void ImageViewHandler::SetDimensions(size_t w, size_t h)
+{
+ rview_default = pangolin::XYRangef(-0.5f, w-0.5f, -0.5f, h-0.5f),
+ rview_max = pangolin::XYRangef(-0.5f, w-0.5f, -0.5f, h-0.5f),
+ rview = rview_default;
+ target = rview;
+}
+
+void ImageViewHandler::UpdateView()
+{
+ // TODO: Base this on current framerate.
+ const float animate_factor = 1.0f / 5.0f;
+
+ if( linked_view_handler ) {
+ // Synchronise rview and target with linked plotter
+ rview = linked_view_handler->rview;
+ target = linked_view_handler->target;
+ selection = linked_view_handler->selection;
+ }else{
+ // Clamp target to image dimensions.
+ AdjustScale();
+ AdjustTranslation();
+
+ // Animate view window toward target
+ pangolin::XYRangef d = target - rview;
+ rview += d * animate_factor;
+ }
+}
+
+void ImageViewHandler::glSetViewOrtho()
+{
+ const pangolin::XYRangef& xy = GetViewToRender();
+
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(xy.x.min, xy.x.max, xy.y.max, xy.y.min, -1, 1);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+}
+
+void ImageViewHandler::glRenderTexture(pangolin::GlTexture& tex)
+{
+ glRenderTexture(tex.tid, tex.width, tex.height);
+}
+
+void ImageViewHandler::glRenderTexture(GLuint tex, GLint width, GLint height)
+{
+ if(tex != 0) {
+ const pangolin::XYRangef& xy = GetViewToRender();
+ const float w = (float)width;
+ const float h = (float)height;
+
+ // discrete coords, (-0.5, -0.5) - (w-0.5, h-0.5)
+ const GLfloat l = xy.x.min;
+ const GLfloat r = xy.x.max;
+ const GLfloat b = xy.y.max;
+ const GLfloat t = xy.y.min;
+
+ // continuous coords, (0,0) - (1,1)
+ GLfloat ln = (l + 0.5f) / w;
+ GLfloat rn = (r + 0.5f) / w;
+ GLfloat bn = (b + 0.5f) / h;
+ GLfloat tn = (t + 0.5f) / h;
+
+ if(flipTextureX) {
+ ln = 1-ln;
+ rn = 1-rn;
+ }
+
+ if(flipTextureY) {
+ bn = 1-bn;
+ tn = 1-tn;
+ }
+
+ const GLfloat sq_vert[] = { l,t, r,t, r,b, l,b };
+ const GLfloat sq_tex[] = { ln,tn, rn,tn, rn,bn, ln,bn };
+
+ glBindTexture(GL_TEXTURE_2D, tex);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, UseNN() ? GL_NEAREST : GL_LINEAR);
+ glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, UseNN() ? GL_NEAREST : GL_LINEAR);
+
+ glEnable(GL_TEXTURE_2D);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glEnableClientState(GL_TEXTURE_COORD_ARRAY);
+ glTexCoordPointer(2, GL_FLOAT, 0, sq_tex);
+ glVertexPointer(2, GL_FLOAT, 0, sq_vert);
+ glDrawArrays(GL_TRIANGLE_FAN, 0, 4);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glDisable(GL_TEXTURE_2D);
+ glBindTexture(GL_TEXTURE_2D, 0);
+ }
+}
+
+void ImageViewHandler::glRenderOverlay()
+{
+ const pangolin::XYRangef& selxy = GetSelection();
+ const GLfloat sq_select[] = {
+ selxy.x.min, selxy.y.min,
+ selxy.x.max, selxy.y.min,
+ selxy.x.max, selxy.y.max,
+ selxy.x.min, selxy.y.max
+ };
+ glColor4f(1.0,0.0,0.0,1.0);
+ glEnableClientState(GL_VERTEX_ARRAY);
+ glVertexPointer(2, GL_FLOAT, 0, sq_select);
+ glDrawArrays(GL_LINE_LOOP, 0, 4);
+ glDisableClientState(GL_TEXTURE_COORD_ARRAY);
+ glColor4f(1.0,1.0,1.0,1.0);
+
+ if( std::abs(selxy.Area()) > 0) {
+ // Render text
+ pangolin::Viewport v;
+ glGetIntegerv( GL_VIEWPORT, &v.l );
+ float xpix, ypix;
+ ImageToScreen(v, selxy.x.max, selxy.y.max, xpix, ypix);
+
+ // Save previous value
+ GLboolean gl_blend_enabled;
+ glGetBooleanv(GL_BLEND, &gl_blend_enabled);
+
+ // Ensure that blending is enabled for rendering text.
+ glEnable(GL_BLEND);
+ glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ pangolin::GlFont::I().Text(
+ "%.2f x %.2f",
+ selxy.x.Size(), selxy.y.Size()
+ ).DrawWindow(xpix,ypix);
+
+ pangolin::GlFont::I().Text(
+ "(%.1f,%.1f)->(%.1f,%.1f)",
+ selxy.x.min, selxy.y.min,
+ selxy.x.max, selxy.y.max
+ ).DrawWindow(xpix, ypix - 1.0f * pangolin::GlFont::I().Height());
+
+ // Restore previous value
+ if(!gl_blend_enabled) glDisable(GL_BLEND);
+ }
+}
+
+void ImageViewHandler::ScreenToImage(Viewport& v, float xpix, float ypix, float& ximg, float& yimg)
+{
+ ximg = rview.x.min + rview.x.Size() * (xpix - v.l) / (float)v.w;
+ yimg = rview.y.min + rview.y.Size() * ( 1.0f - (ypix - v.b) / (float)v.h);
+}
+
+void ImageViewHandler::ImageToScreen(Viewport& v, float ximg, float yimg, float& xpix, float& ypix)
+{
+ xpix = (ximg -rview.x.min) * (float)v.w / rview.x.Size() + v.l;
+ ypix = v.b - (float)v.h * ((yimg - rview.y.min) / rview.y.Size() - 1.0f);
+}
+
+bool ImageViewHandler::UseNN() const
+{
+ return use_nn;
+}
+
+bool& ImageViewHandler::UseNN()
+{
+ return use_nn;
+}
+
+bool& ImageViewHandler::FlipTextureX()
+{
+ return flipTextureX;
+}
+
+bool& ImageViewHandler::FlipTextureY()
+{
+ return flipTextureY;
+}
+
+pangolin::XYRangef& ImageViewHandler::GetViewToRender()
+{
+ return rview;
+}
+
+float ImageViewHandler::GetViewScale()
+{
+ return rview_max.x.Size() / rview.x.Size();
+}
+
+pangolin::XYRangef& ImageViewHandler::GetView()
+{
+ return target;
+}
+
+pangolin::XYRangef& ImageViewHandler::GetDefaultView()
+{
+ return rview_default;
+}
+
+pangolin::XYRangef& ImageViewHandler::GetSelection()
+{
+ return selection;
+}
+
+void ImageViewHandler::GetHover(float& x, float& y)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ x = tv.hover_img[0];
+ y = tv.hover_img[1];
+}
+
+void ImageViewHandler::SetView(const pangolin::XYRangef& range)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ tv.rview = range;
+ tv.target = range;
+}
+
+void ImageViewHandler::SetViewSmooth(const pangolin::XYRangef& range)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ tv.target = range;
+}
+
+void ImageViewHandler::ScrollView(float x, float y)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ ScrollViewSmooth(x,y);
+ tv.rview.x += x;
+ tv.rview.y += y;
+}
+
+void ImageViewHandler::ScrollViewSmooth(float x, float y)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ tv.target.x += x;
+ tv.target.y += y;
+}
+
+void ImageViewHandler::ScaleView(float x, float y, float cx, float cy)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ ScaleViewSmooth(x,y,cx,cy);
+ tv.rview.x.Scale(x,cx);
+ tv.rview.y.Scale(y,cy);
+}
+
+void ImageViewHandler::ScaleViewSmooth(float x, float y, float cx, float cy)
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ tv.target.x.Scale(x,cx);
+ tv.target.y.Scale(y,cy);
+}
+
+void ImageViewHandler::ResetView()
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ tv.target = tv.rview_default;
+}
+
+///////////////////////////////////////////////////////
+/// pangolin::Handler
+///////////////////////////////////////////////////////
+
+void ImageViewHandler::Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed)
+{
+ XYRangef& sel = linked_view_handler ? linked_view_handler->selection : selection;
+ const float mvfactor = 1.0f / 10.0f;
+ const float c[2] = { rview.x.Mid(), rview.y.Mid() };
+
+
+ if(pressed) {
+ if(key == '\r') {
+ if( sel.Area() != 0.0f && std::isfinite(sel.Area()) ) {
+ // Set view to equal selection
+ SetViewSmooth(sel);
+
+ // Reset selection
+ sel.x.max = sel.x.min;
+ sel.y.max = sel.y.min;
+ }
+ }else if(key == 'n') {
+ use_nn = !use_nn;
+ }else if(key == 'l') {
+ if(to_link) {
+ linked_view_handler = to_link;
+ to_link = 0;
+ }else{
+ to_link = this;
+ }
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_LEFT) {
+ const float w = target.x.Size();
+ const float dx = mvfactor*w;
+ ScrollViewSmooth(-dx, 0);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_RIGHT) {
+ const float w = target.x.Size();
+ const float dx = mvfactor*w;
+ ScrollViewSmooth(+dx, 0);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_DOWN) {
+ const float h = target.y.Size();
+ const float dy = mvfactor*h;
+ ScrollViewSmooth(0, -dy);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_UP) {
+ const float h = target.y.Size();
+ const float dy = mvfactor*h;
+ ScrollViewSmooth(0, +dy);
+ }else if(key == '=') {
+ ScaleViewSmooth(0.5, 0.5, c[0], c[1]);
+ }else if(key == '-') {
+ ScaleViewSmooth(2.0, 2.0, c[0], c[1]);
+ }else if(key == '#') {
+ ResetView();
+ }else if(key == 1) {
+ // ctrl-a: select all.
+ sel = rview;
+ }else{
+ pango_print_debug("Unhandled ImageViewHandler::Keyboard. Key: %u\n", (unsigned int)key);
+ }
+ }
+}
+
+void ImageViewHandler::Mouse(View& view, pangolin::MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ XYRangef& sel = linked_view_handler ? linked_view_handler->selection : selection;
+ ScreenToImage(view.v, (float)x, (float)y, hover_img[0], hover_img[1]);
+
+ const float scinc = 1.05f;
+ const float scdec = 1.0f/scinc;
+
+ if(button_state & KeyModifierCtrl) {
+ const float mvfactor = 1.0f/20.0f;
+
+ if(button == MouseWheelUp) {
+ ScrollViewSmooth(0.0f, +mvfactor*rview.y.Size() );
+ }else if(button == MouseWheelDown) {
+ ScrollViewSmooth(0.0f, -mvfactor*rview.y.Size() );
+ }else if(button == MouseWheelLeft) {
+ ScrollViewSmooth(+mvfactor*rview.x.Size(), 0.0f );
+ }else if(button == MouseWheelRight) {
+ ScrollViewSmooth(-mvfactor*rview.x.Size(), 0.0f );
+ }
+ }else{
+ if(button == MouseButtonLeft) {
+ // Update selected range
+ if(pressed) {
+ sel.x.min = hover_img[0];
+ sel.y.min = hover_img[1];
+ }
+ sel.x.max = hover_img[0];
+ sel.y.max = hover_img[1];
+ }else if(button == MouseWheelUp) {
+ ScaleViewSmooth(scdec, scdec, hover_img[0], hover_img[1]);
+ }else if(button == MouseWheelDown) {
+ ScaleViewSmooth(scinc, scinc, hover_img[0], hover_img[1]);
+ }
+ }
+
+ FixSelection(sel);
+ last_mouse_pos[0] = x;
+ last_mouse_pos[1] = y;
+
+ if(OnSelectionCallback) {
+ OnSelectionCallback( OnSelectionEventData(view,*this,pressed) );
+ }
+}
+
+void ImageViewHandler::MouseMotion(View& view, int x, int y, int button_state)
+{
+ XYRangef& sel = linked_view_handler ? linked_view_handler->selection : selection;
+ const int d[2] = {x-last_mouse_pos[0], y-last_mouse_pos[1]};
+
+ // Update hover status (after potential resizing)
+ ScreenToImage(view.v, (float)x, (float)y, hover_img[0], hover_img[1]);
+
+ if( button_state == MouseButtonLeft )
+ {
+ // Update selected range
+ sel.x.max = hover_img[0];
+ sel.y.max = hover_img[1];
+ }else if(button_state == MouseButtonRight )
+ {
+ Special(view, InputSpecialScroll, (float)x, (float)y, (float)d[0], (float)d[1], 0.0f, 0.0f, button_state);
+ }
+
+ last_mouse_pos[0] = x;
+ last_mouse_pos[1] = y;
+
+ if( button_state == MouseButtonLeft && OnSelectionCallback) {
+ OnSelectionCallback( OnSelectionEventData(view,*this,true) );
+ }
+}
+
+void ImageViewHandler::PassiveMouseMotion(View&, int /*x*/, int /*y*/, int /*button_state*/)
+{
+}
+
+void ImageViewHandler::Special(View& view, pangolin::InputSpecial inType, float x, float y, float p1, float p2, float /*p3*/, float /*p4*/, int /*button_state*/)
+{
+ ScreenToImage(view.v, x, y, hover_img[0], hover_img[1]);
+
+ if(inType == InputSpecialScroll) {
+ const float d[2] = {p1,p2};
+ const float is[2] = {rview.x.Size(),rview.y.Size() };
+ const float df[2] = {is[0]*d[0]/(float)view.v.w, is[1]*d[1]/(float)view.v.h};
+ ScrollView(-df[0], df[1]);
+ } else if(inType == InputSpecialZoom) {
+ float scale = 1.0f - p1;
+ ScaleView(scale, scale, hover_img[0], hover_img[1]);
+ }
+
+ // Update hover status (after potential resizing)
+ ScreenToImage( view.v, x, y, hover_img[0], hover_img[1]);
+}
+
+void ImageViewHandler::FixSelection(XYRangef& sel)
+{
+ // Make sure selection matches sign of current viewport
+ if( (sel.x.min<sel.x.max) != (rview.x.min<rview.x.max) ) {
+ std::swap(sel.x.min, sel.x.max);
+ }
+ if( (sel.y.min<sel.y.max) != (rview.y.min<rview.y.max) ) {
+ std::swap(sel.y.min, sel.y.max);
+ }
+}
+
+void ImageViewHandler::AdjustScale()
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ if(tv.target.x.AbsSize() > tv.rview_max.x.AbsSize())
+ tv.target.x.Scale(tv.rview_max.x.AbsSize() / tv.target.x.AbsSize(), tv.target.x.Mid());
+ if(tv.target.y.AbsSize() > tv.rview_max.y.AbsSize())
+ tv.target.y.Scale(tv.rview_max.y.AbsSize() / tv.target.y.AbsSize(), tv.target.y.Mid());
+}
+
+void ImageViewHandler::AdjustTranslation()
+{
+ ImageViewHandler& tv = linked_view_handler ? *linked_view_handler : *this;
+ if( tv.target.x.max > tv.rview_max.x.max) tv.target.x -= tv.target.x.max - tv.rview_max.x.max;
+ if( tv.target.x.min < tv.rview_max.x.min) tv.target.x -= tv.target.x.min - tv.rview_max.x.min;
+
+ if( tv.target.y.max > tv.rview_max.y.max) tv.target.y -= tv.target.y.max - tv.rview_max.y.max;
+ if( tv.target.y.min < tv.rview_max.y.min) tv.target.y -= tv.target.y.min - tv.rview_max.y.min;
+}
+
+float ImageViewHandler::animate_factor = 1.0f / 2.0f;
+ImageViewHandler* ImageViewHandler::to_link = 0;
+
+}
diff --git a/externals/Pangolin/src/image/image_io.cpp b/externals/Pangolin/src/image/image_io.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..854a7d2edddf98774dc54e7f05e372a8e8a7f837
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io.cpp
@@ -0,0 +1,177 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/image/image_io.h>
+
+#include <fstream>
+
+namespace pangolin {
+
+// PNG
+TypedImage LoadPng(std::istream& in);
+void SavePng(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, bool top_line_first, int zlib_compression_level );
+
+// JPG
+TypedImage LoadJpg(std::istream& in);
+void SaveJpg(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, float quality);
+
+// PPM
+TypedImage LoadPpm(std::istream& in);
+void SavePpm(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, bool top_line_first);
+
+// TGA
+TypedImage LoadTga(std::istream& in);
+
+// Pango
+TypedImage LoadPango(const std::string& filename);
+void SavePango(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, bool top_line_first);
+
+// EXR
+TypedImage LoadExr(std::istream& source);
+void SaveExr(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, bool top_line_first);
+
+// ZSTD (https://github.com/facebook/zstd)
+TypedImage LoadZstd(std::istream& in);
+void SaveZstd(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int compression_level);
+
+// https://github.com/lz4/lz4
+TypedImage LoadLz4(std::istream& in);
+void SaveLz4(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int compression_level);
+
+// packed 12 bit image (obtained from unpacked 16bit)
+TypedImage LoadPacked12bit(std::istream& in);
+void SavePacked12bit(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int compression_level);
+
+TypedImage LoadImage(std::istream& in, ImageFileType file_type)
+{
+ switch (file_type) {
+ case ImageFileTypePng:
+ return LoadPng(in);
+ case ImageFileTypeJpg:
+ return LoadJpg(in);
+ case ImageFileTypePpm:
+ return LoadPpm(in);
+ case ImageFileTypeTga:
+ return LoadTga(in);
+ case ImageFileTypeZstd:
+ return LoadZstd(in);
+ case ImageFileTypeLz4:
+ return LoadLz4(in);
+ case ImageFileTypeP12b:
+ return LoadPacked12bit(in);
+ case ImageFileTypeExr:
+ return LoadExr(in);
+ default:
+ throw std::runtime_error("Unable to load image file-type through std::istream");
+ }
+}
+
+TypedImage LoadImage(const std::string& filename, ImageFileType file_type)
+{
+ switch (file_type) {
+ case ImageFileTypePng:
+ case ImageFileTypeJpg:
+ case ImageFileTypePpm:
+ case ImageFileTypeTga:
+ case ImageFileTypeZstd:
+ case ImageFileTypeLz4:
+ case ImageFileTypeP12b:
+ case ImageFileTypeExr:
+ {
+ std::ifstream ifs(filename, std::ios_base::in|std::ios_base::binary);
+ return LoadImage(ifs, file_type);
+ }
+ case ImageFileTypePango:
+ return LoadPango(filename);
+ default:
+ throw std::runtime_error("Unsupported image file type, '" + filename + "'");
+ }
+}
+
+TypedImage LoadImage(const std::string& filename)
+{
+ ImageFileType file_type = FileType(filename);
+ return LoadImage( filename, file_type );
+}
+
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, ImageFileType file_type, bool top_line_first, float quality)
+{
+ switch (file_type) {
+ case ImageFileTypePng:
+ // map quality [0..100] to PNG compression levels [0..9]
+ return SavePng(image, fmt, out, top_line_first, int(quality*0.09));
+ case ImageFileTypeJpg:
+ return SaveJpg(image, fmt, out, quality);
+ case ImageFileTypePpm:
+ return SavePpm(image,fmt,out,top_line_first);
+ case ImageFileTypeZstd:
+ return SaveZstd(image,fmt,out, quality);
+ case ImageFileTypeLz4:
+ return SaveLz4(image,fmt,out, quality);
+ case ImageFileTypeP12b:
+ return SavePacked12bit(image,fmt,out, quality);
+ default:
+ throw std::runtime_error("Unable to save image file-type through std::istream");
+ }
+}
+
+
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, ImageFileType file_type, bool top_line_first, float quality)
+{
+ switch (file_type) {
+ case ImageFileTypePng:
+ case ImageFileTypeJpg:
+ case ImageFileTypePpm:
+ case ImageFileTypeZstd:
+ case ImageFileTypeLz4:
+ case ImageFileTypeP12b:
+ {
+ std::ofstream ofs(filename, std::ios_base::binary);
+ return SaveImage(image, fmt, ofs, file_type, top_line_first, quality);
+ }
+ case ImageFileTypeExr:
+ return SaveExr(image, fmt, filename, top_line_first);
+ case ImageFileTypePango:
+ return SavePango(image, fmt, filename, top_line_first);
+ default:
+ throw std::runtime_error("Unsupported image file type, '" + filename + "'");
+ }
+}
+
+void SaveImage(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& filename, bool top_line_first, float quality)
+{
+ const std::string ext = FileLowercaseExtention(filename);
+ const ImageFileType file_type = FileTypeExtension(ext);
+ SaveImage(image, fmt, filename,file_type, top_line_first, quality);
+}
+
+void SaveImage(const TypedImage& image, const std::string& filename, bool top_line_first, float quality)
+{
+ SaveImage(image, image.fmt, filename, top_line_first, quality);
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_exr.cpp b/externals/Pangolin/src/image/image_io_exr.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..52c00788b7a502faef7b72cafdd9ac350686819f
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_exr.cpp
@@ -0,0 +1,188 @@
+#include <pangolin/platform.h>
+
+#include <fstream>
+#include <pangolin/image/typed_image.h>
+
+#ifdef HAVE_OPENEXR
+#include <ImfChannelList.h>
+#include <ImfInputFile.h>
+#include <ImfOutputFile.h>
+#include <ImfIO.h>
+#endif // HAVE_OPENEXR
+
+namespace pangolin {
+
+#ifdef HAVE_OPENEXR
+Imf::PixelType OpenEXRPixelType(int channel_bits)
+{
+ if( channel_bits == 16 ) {
+ return Imf::PixelType::HALF;
+ }else if( channel_bits == 32 ) {
+ return Imf::PixelType::FLOAT;
+ }else{
+ throw std::runtime_error("Unsupported OpenEXR Pixel Type.");
+ }
+}
+
+void SetOpenEXRChannels(Imf::ChannelList& ch, const pangolin::PixelFormat& fmt)
+{
+ const char* CHANNEL_NAMES[] = {"R","G","B","A"};
+ for(size_t c=0; c < fmt.channels; ++c) {
+ ch.insert( CHANNEL_NAMES[c], Imf::Channel(OpenEXRPixelType(fmt.channel_bits[c])) );
+ }
+}
+
+class StdIStream: public Imf::IStream
+{
+ public:
+ StdIStream (std::istream &is):
+ Imf::IStream ("stream"),
+ _is (&is)
+ {
+ }
+
+ virtual bool read (char c[/*n*/], int n)
+ {
+ if (!*_is)
+ throw std::runtime_error("Unexpected end of file.");
+ _is->read (c, n);
+ if (_is->gcount() < n)
+ {
+ throw std::runtime_error("Early end of file");
+ return false;
+ }
+ return true;
+ }
+
+ virtual Imf::Int64 tellg ()
+ {
+ return std::streamoff (_is->tellg());
+ }
+
+ virtual void seekg (Imf::Int64 pos)
+ {
+ _is->seekg (pos);
+ }
+
+ virtual void clear ()
+ {
+ _is->clear();
+ }
+
+ private:
+ std::istream * _is;
+};
+
+PixelFormat GetPixelFormat(const Imf::Header& header)
+{
+ const Imf::ChannelList &channels = header.channels();
+ size_t count = 0;
+ for (Imf::ChannelList::ConstIterator i = channels.begin(); i != channels.end(); ++i){
+ const Imf::Channel& channel = i.channel();
+ if (channel.type != Imf::FLOAT){
+ throw std::invalid_argument("Currently, only 32-bit float OpenEXR files are supported.");
+ }
+ count += 1;
+ }
+
+ switch (count) {
+ case 1: return PixelFormatFromString("GRAY32F");
+ case 3: return PixelFormatFromString("RGB96F");
+ case 4: return PixelFormatFromString("RGBA128F");
+ default: throw std::invalid_argument("Currently, only 1, 3 or 4-channel OpenEXR files are supported.");
+ }
+}
+
+#endif //HAVE_OPENEXR
+
+TypedImage LoadExr(std::istream& source)
+{
+#ifdef HAVE_OPENEXR
+ StdIStream istream(source);
+ Imf::InputFile file(istream);
+ PANGO_ENSURE(file.isComplete());
+
+ Imath::Box2i dw = file.header().dataWindow();
+ int width = dw.max.x - dw.min.x + 1;
+ int height = dw.max.y - dw.min.y + 1;
+
+ PixelFormat format = GetPixelFormat(file.header());
+ TypedImage img(width, height, format);
+
+ char *imgBase = (char *) img.ptr - (dw.min.x + dw.min.y * width) * sizeof(float) * format.channels;
+ Imf::FrameBuffer fb;
+
+ const Imf::ChannelList &channels = file.header().channels();
+ size_t c = 0;
+ for (Imf::ChannelList::ConstIterator i = channels.begin(); i != channels.end(); ++i){
+ fb.insert(i.name(), Imf::Slice(
+ Imf::FLOAT, imgBase + sizeof(float) * c++,
+ sizeof(float) * format.channels,
+ sizeof(float) * format.channels * size_t(width),
+ 1, 1,
+ 0.0));
+ }
+
+ file.setFrameBuffer(fb);
+ file.readPixels(dw.min.y, dw.max.y);
+
+ return img;
+#else
+ PANGOLIN_UNUSED(source);
+ throw std::runtime_error("Rebuild Pangolin for EXR support.");
+#endif //HAVE_OPENEXR
+}
+
+void SaveExr(const Image<unsigned char>& image_in, const pangolin::PixelFormat& fmt, const std::string& filename, bool top_line_first)
+{
+#ifdef HAVE_OPENEXR
+ ManagedImage<unsigned char> flip_image;
+ Image<unsigned char> image;
+
+ if(top_line_first) {
+ image = image_in;
+ }else{
+ flip_image.Reinitialise(image_in.pitch,image_in.h);
+ for(size_t y=0; y<image_in.h; ++y) {
+ std::memcpy(flip_image.RowPtr(y), image_in.RowPtr(y), image_in.pitch);
+ }
+ image = flip_image;
+ }
+
+
+ Imf::Header header (image.w, image.h);
+ SetOpenEXRChannels(header.channels(), fmt);
+
+ Imf::OutputFile file (filename.c_str(), header);
+ Imf::FrameBuffer frameBuffer;
+
+ int ch=0;
+ size_t ch_bits = 0;
+ for(Imf::ChannelList::Iterator it = header.channels().begin(); it != header.channels().end(); ++it)
+ {
+ frameBuffer.insert(
+ it.name(),
+ Imf::Slice(
+ it.channel().type,
+ (char*)image.ptr + ch_bits/8,
+ fmt.channel_bits[ch]/8,
+ image.pitch
+ )
+ );
+
+ ch_bits += fmt.channel_bits[ch++];
+ }
+
+ file.setFrameBuffer(frameBuffer);
+ file.writePixels(image.h);
+
+#else
+ PANGOLIN_UNUSED(image_in);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(filename);
+ PANGOLIN_UNUSED(top_line_first);
+ throw std::runtime_error("EXR Support not enabled. Please rebuild Pangolin.");
+#endif // HAVE_OPENEXR
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_jpg.cpp b/externals/Pangolin/src/image/image_io_jpg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b8ecbbc51450c50524c34294da553b932c57954d
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_jpg.cpp
@@ -0,0 +1,256 @@
+#include <algorithm>
+#include <fstream>
+
+
+#include <pangolin/platform.h>
+
+#include <pangolin/image/typed_image.h>
+
+#ifdef HAVE_JPEG
+# include <jpeglib.h>
+# ifdef _WIN_
+// Undef windows Macro polution from jpeglib.h
+# undef LoadImage
+# endif
+#endif // HAVE_JPEG
+
+
+// Inspired by https://cs.stanford.edu/~acoates/jpegAndIOS.txt
+
+namespace pangolin {
+
+#ifdef HAVE_JPEG
+
+const static size_t PANGO_JPEG_BUF_SIZE = 16384;
+
+struct pango_jpeg_source_mgr {
+ struct jpeg_source_mgr pub;
+ std::istream* is;
+ JOCTET* buffer;
+};
+
+static void pango_jpeg_init_source(j_decompress_ptr /*cinfo*/) {
+}
+
+static boolean pango_jpeg_fill_input_buffer(j_decompress_ptr cinfo) {
+ pango_jpeg_source_mgr* src = (pango_jpeg_source_mgr*)cinfo->src;
+
+ src->is->read((char*)src->buffer, PANGO_JPEG_BUF_SIZE);
+ size_t bytes = src->is->gcount();
+ if (bytes == 0) {
+ /* Insert a fake EOI marker */
+ src->buffer[0] = (JOCTET) 0xFF;
+ src->buffer[1] = (JOCTET) JPEG_EOI;
+ bytes = 2;
+ }
+ src->pub.next_input_byte = src->buffer;
+ src->pub.bytes_in_buffer = bytes;
+ return TRUE;
+}
+static void pango_jpeg_skip_input_data(j_decompress_ptr cinfo, long num_bytes) {
+ pango_jpeg_source_mgr* src = (pango_jpeg_source_mgr*)cinfo->src;
+ if (num_bytes > 0) {
+ while (num_bytes > (long)src->pub.bytes_in_buffer) {
+ num_bytes -= (long)src->pub.bytes_in_buffer;
+ pango_jpeg_fill_input_buffer(cinfo);
+ }
+ src->pub.next_input_byte += num_bytes;
+ src->pub.bytes_in_buffer -= num_bytes;
+ }
+}
+static void pango_jpeg_term_source(j_decompress_ptr cinfo) {
+ // must seek backward so that future reads will start at correct place.
+ pango_jpeg_source_mgr* src = (pango_jpeg_source_mgr*)cinfo->src;
+ src->is->clear();
+ src->is->seekg( src->is->tellg() - (std::streampos)src->pub.bytes_in_buffer );
+}
+
+static void pango_jpeg_set_source_mgr(j_decompress_ptr cinfo, std::istream& is) {
+ pango_jpeg_source_mgr* src = nullptr;
+
+ if (cinfo->src == 0)
+ {
+ cinfo->src = (struct jpeg_source_mgr *)(*cinfo->mem->alloc_small)
+ ((j_common_ptr) cinfo, JPOOL_PERMANENT, sizeof(pango_jpeg_source_mgr));
+
+ src = (pango_jpeg_source_mgr*) cinfo->src;
+ src->buffer = (JOCTET *)(*cinfo->mem->alloc_small)
+ ((j_common_ptr) cinfo, JPOOL_PERMANENT, PANGO_JPEG_BUF_SIZE*sizeof(JOCTET));
+ }else{
+ src = (pango_jpeg_source_mgr*) cinfo->src;
+ }
+
+ src->is = &is;
+ src->pub.init_source = pango_jpeg_init_source;
+ src->pub.fill_input_buffer = pango_jpeg_fill_input_buffer;
+ src->pub.skip_input_data = pango_jpeg_skip_input_data;
+ src->pub.resync_to_restart = jpeg_resync_to_restart; /* use default method */
+ src->pub.term_source = pango_jpeg_term_source;
+ src->pub.bytes_in_buffer = 0;
+ src->pub.next_input_byte = 0;
+}
+
+struct pango_jpeg_destination_mgr {
+ struct jpeg_destination_mgr pub; /* public fields */
+ std::ostream* os; /* target stream */
+ JOCTET * buffer; /* start of buffer */
+};
+
+void pango_jpeg_init_destination (j_compress_ptr cinfo) {
+ pango_jpeg_destination_mgr* dest = (pango_jpeg_destination_mgr*) cinfo->dest;
+ dest->buffer = (JOCTET *)(*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_IMAGE,
+ PANGO_JPEG_BUF_SIZE * sizeof(JOCTET));
+ dest->pub.next_output_byte = dest->buffer;
+ dest->pub.free_in_buffer = PANGO_JPEG_BUF_SIZE;
+}
+
+boolean pango_jpeg_empty_output_buffer(j_compress_ptr cinfo) {
+ pango_jpeg_destination_mgr* dest = (pango_jpeg_destination_mgr*)cinfo->dest;
+
+ dest->os->write((const char*)dest->buffer, PANGO_JPEG_BUF_SIZE);
+
+ if (dest->os->fail()) {
+ throw std::runtime_error("Couldn't write entire jpeg buffer to stream.");
+ }
+
+ dest->pub.next_output_byte = dest->buffer;
+ dest->pub.free_in_buffer = PANGO_JPEG_BUF_SIZE;
+ return TRUE;
+}
+
+void pango_jpeg_term_destination (j_compress_ptr cinfo) {
+ pango_jpeg_destination_mgr* dest = (pango_jpeg_destination_mgr*) cinfo->dest;
+ size_t datacount = PANGO_JPEG_BUF_SIZE - dest->pub.free_in_buffer;
+
+ /* Write any data remaining in the buffer */
+ if (datacount > 0) {
+ dest->os->write((const char*)dest->buffer, datacount);
+ if (dest->os->fail()) {
+ throw std::runtime_error("Couldn't write remaining jpeg data to stream.");
+ }
+ }
+ dest->os->flush();
+}
+
+void pango_jpeg_set_dest_mgr(j_compress_ptr cinfo, std::ostream& os) {
+ pango_jpeg_destination_mgr* dest;
+
+ if (cinfo->dest == NULL) { /* first time for this JPEG object? */
+ cinfo->dest = (struct jpeg_destination_mgr *)
+ (*cinfo->mem->alloc_small) ((j_common_ptr) cinfo, JPOOL_PERMANENT,
+ sizeof(pango_jpeg_destination_mgr));
+ }
+
+ dest = (pango_jpeg_destination_mgr*)cinfo->dest;
+ dest->pub.init_destination = pango_jpeg_init_destination;
+ dest->pub.empty_output_buffer = pango_jpeg_empty_output_buffer;
+ dest->pub.term_destination = pango_jpeg_term_destination;
+ dest->os = &os;
+}
+
+#endif // HAVE_JPEG
+
+TypedImage LoadJpg(std::istream& is) {
+#ifdef HAVE_JPEG
+ TypedImage image;
+
+ struct jpeg_decompress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ // Setup decompression structure
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_decompress(&cinfo);
+ pango_jpeg_set_source_mgr(&cinfo, is);
+
+ // read info from header.
+ int r = jpeg_read_header(&cinfo, TRUE);
+ if (r != JPEG_HEADER_OK) {
+ throw std::runtime_error("Failed to read JPEG header.");
+ } else if (cinfo.num_components != 3 && cinfo.num_components != 1) {
+ throw std::runtime_error("Unsupported number of color components");
+ } else {
+ jpeg_start_decompress(&cinfo);
+ // resize storage if necessary
+ PixelFormat fmt = PixelFormatFromString(cinfo.output_components == 3 ? "RGB24" : "GRAY8");
+ image.Reinitialise(cinfo.output_width, cinfo.output_height, fmt);
+ JSAMPARRAY imageBuffer = (*cinfo.mem->alloc_sarray)((j_common_ptr)&cinfo, JPOOL_IMAGE,
+ cinfo.output_width*cinfo.output_components, 1);
+ for (size_t y = 0; y < cinfo.output_height; y++) {
+ jpeg_read_scanlines(&cinfo, imageBuffer, 1);
+ uint8_t* dstRow = (uint8_t*)image.RowPtr(y);
+ memcpy(dstRow, imageBuffer[0], cinfo.output_width*cinfo.output_components);
+ }
+ jpeg_finish_decompress(&cinfo);
+ }
+
+ // clean up.
+ jpeg_destroy_decompress(&cinfo);
+
+ return image;
+#else
+ PANGOLIN_UNUSED(is);
+ throw std::runtime_error("Rebuild Pangolin for JPEG support.");
+#endif // HAVE_JPEG
+
+}
+
+TypedImage LoadJpg(const std::string& filename) {
+ std::ifstream f(filename);
+ return LoadJpg(f);
+}
+
+void SaveJpg(const Image<unsigned char>& img, const PixelFormat& fmt, std::ostream& os, float quality) {
+#ifdef HAVE_JPEG
+ const int iquality = (int)std::max(std::min(quality, 100.0f),0.0f);
+
+ struct jpeg_compress_struct cinfo;
+ struct jpeg_error_mgr jerr;
+
+ if (fmt.channels != 1 && fmt.channels != 3) {
+ throw std::runtime_error("Unsupported number of image channels.");
+ }
+ if (fmt.bpp != 8 && fmt.bpp != 24) {
+ throw std::runtime_error("Unsupported image depth.");
+ }
+
+ // set up compression structure
+ cinfo.err = jpeg_std_error(&jerr);
+ jpeg_create_compress(&cinfo);
+ pango_jpeg_set_dest_mgr(&cinfo, os);
+
+ cinfo.image_width = img.w;
+ cinfo.image_height = img.h;
+ cinfo.input_components = fmt.channels;
+ if (fmt.channels == 3) {
+ cinfo.in_color_space = JCS_RGB;
+ } else {
+ cinfo.in_color_space = JCS_GRAYSCALE;
+ }
+
+ jpeg_set_defaults(&cinfo);
+ jpeg_set_quality(&cinfo, iquality, (boolean)true);
+ jpeg_start_compress(&cinfo, (boolean)true);
+
+ JSAMPROW row;
+ while (cinfo.next_scanline < cinfo.image_height) {
+ row = (JSAMPROW)((char*)img.RowPtr(cinfo.next_scanline));
+ jpeg_write_scanlines(&cinfo, &row, 1);
+ }
+
+ jpeg_finish_compress(&cinfo);
+ jpeg_destroy_compress(&cinfo);
+#else
+ PANGOLIN_UNUSED(img);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(os);
+ PANGOLIN_UNUSED(quality);
+ throw std::runtime_error("Rebuild Pangolin for JPEG support.");
+#endif // HAVE_JPEG
+}
+
+void SaveJpg(const Image<unsigned char>& img, const PixelFormat& fmt, const std::string& filename, float quality) {
+ std::ofstream f(filename);
+ SaveJpg(img, fmt, f, quality);
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_lz4.cpp b/externals/Pangolin/src/image/image_io_lz4.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..93ca60458f447515ecc67cfc5226db7c7dde5376
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_lz4.cpp
@@ -0,0 +1,87 @@
+#include <fstream>
+#include <memory>
+
+#include <pangolin/image/typed_image.h>
+
+#ifdef HAVE_LZ4
+# include <lz4.h>
+#endif
+
+namespace pangolin {
+
+#pragma pack(push, 1)
+struct lz4_image_header
+{
+ char magic[3];
+ char fmt[16];
+ size_t w, h;
+ int64_t compressed_size;
+};
+#pragma pack(pop)
+
+void SaveLz4(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int compression_level)
+{
+#ifdef HAVE_LZ4
+
+ const int64_t src_size = image.SizeBytes();
+ const int64_t max_dst_size = LZ4_compressBound(src_size);
+ std::unique_ptr<char[]> output_buffer(new char[max_dst_size]);
+
+ // Same as LZ4_compress_default(), but allows to select an "acceleration" factor.
+ // The larger the acceleration value, the faster the algorithm, but also the lesser the compression.
+ // It's a trade-off. It can be fine tuned, with each successive value providing roughly +~3% to speed.
+ // An acceleration value of "1" is the same as regular LZ4_compress_default()
+ // Values <= 0 will be replaced by ACCELERATION_DEFAULT (see lz4.c), which is 1.
+ const int64_t compressed_data_size = LZ4_compress_fast((char*)image.ptr, output_buffer.get(), src_size, max_dst_size, compression_level);
+
+ if (compressed_data_size < 0)
+ throw std::runtime_error("A negative result from LZ4_compress_default indicates a failure trying to compress the data.");
+ if (compressed_data_size == 0)
+ throw std::runtime_error("A result of 0 for LZ4 means compression worked, but was stopped because the destination buffer couldn't hold all the information.");
+
+ lz4_image_header header;
+ strncpy(header.magic,"LZ4",3);
+ strncpy(header.fmt, fmt.format.c_str(), sizeof(header.fmt));
+ header.w = image.w;
+ header.h = image.h;
+ header.compressed_size = compressed_data_size;
+ out.write((char*)&header, sizeof(header));
+
+ out.write(output_buffer.get(), compressed_data_size);
+
+#else
+ PANGOLIN_UNUSED(image);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(out);
+ PANGOLIN_UNUSED(compression_level);
+ throw std::runtime_error("Rebuild Pangolin for LZ4 support.");
+#endif // HAVE_LZ4
+}
+
+TypedImage LoadLz4(std::istream& in)
+{
+#ifdef HAVE_LZ4
+ // Read in header, uncompressed
+ lz4_image_header header;
+ in.read( (char*)&header, sizeof(header));
+
+ TypedImage img(header.w, header.h, PixelFormatFromString(header.fmt));
+ std::unique_ptr<char[]> input_buffer(new char[header.compressed_size]);
+
+ in.read(input_buffer.get(), header.compressed_size);
+ const int decompressed_size = LZ4_decompress_safe(input_buffer.get(), (char*)img.ptr, header.compressed_size, img.SizeBytes());
+ if (decompressed_size < 0)
+ throw std::runtime_error(FormatString("A negative result from LZ4_decompress_safe indicates a failure trying to decompress the data. See exit code (%) for value returned.", decompressed_size));
+ if (decompressed_size == 0)
+ throw std::runtime_error("I'm not sure this function can ever return 0. Documentation in lz4.h doesn't indicate so.");
+ if (decompressed_size != (int)img.SizeBytes())
+ throw std::runtime_error(FormatString("decompressed size % is not equal to predicted size %", decompressed_size, img.SizeBytes()));
+
+ return img;
+#else
+ PANGOLIN_UNUSED(in);
+ throw std::runtime_error("Rebuild Pangolin for LZ4 support.");
+#endif // HAVE_LZ4
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_packed12bit.cpp b/externals/Pangolin/src/image/image_io_packed12bit.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b280388d5487175df8c85ef830b10137d721268c
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_packed12bit.cpp
@@ -0,0 +1,85 @@
+#include <fstream>
+#include <memory>
+
+#include <pangolin/image/typed_image.h>
+
+namespace pangolin {
+
+#pragma pack(push, 1)
+struct packed12bit_image_header
+{
+ char magic[4];
+ char fmt[16];
+ size_t w, h;
+};
+#pragma pack(pop)
+
+void SavePacked12bit(const Image<uint8_t>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int /*compression_level*/)
+{
+
+ if (fmt.bpp != 16) {
+ throw std::runtime_error("packed12bit currently only supported with 16bit input image");
+ }
+
+ const size_t dest_pitch = (image.w*12)/ 8 + ((image.w*12) % 8 > 0? 1 : 0);
+ const size_t dest_size = image.h*dest_pitch;
+ std::unique_ptr<uint8_t[]> output_buffer(new uint8_t[dest_size]);
+
+ for(size_t r=0; r<image.h; ++r) {
+ uint8_t* pout = output_buffer.get() + r*dest_pitch;
+ uint16_t* pin = (uint16_t*)(image.ptr + r*image.pitch);
+ const uint16_t* pin_end = (uint16_t*)(image.ptr + (r+1)*image.pitch);
+ while(pin < pin_end) {
+ uint32_t val = (*(pin++) & 0x00000FFF);
+ val |= uint32_t(*(pin++) & 0x00000FFF) << 12;
+ *(pout++) = uint8_t( val & 0x000000FF);
+ *(pout++) = uint8_t((val & 0x0000FF00) >> 8);
+ *(pout++) = uint8_t((val & 0x00FF0000) >> 16);
+ }
+ }
+
+ packed12bit_image_header header;
+ strncpy(header.magic,"P12B",4);
+ strncpy(header.fmt, fmt.format.c_str(), sizeof(header.fmt));
+ header.w = image.w;
+ header.h = image.h;
+ out.write((char*)&header, sizeof(header));
+ out.write((char*)output_buffer.get(), dest_size);
+
+}
+
+TypedImage LoadPacked12bit(std::istream& in)
+{
+ // Read in header, uncompressed
+ packed12bit_image_header header;
+ in.read((char*)&header, sizeof(header));
+
+ TypedImage img(header.w, header.h, PixelFormatFromString(header.fmt));
+
+ if (img.fmt.bpp != 16) {
+ throw std::runtime_error("packed12bit currently only supported with 16bit input image");
+ }
+
+ const size_t input_pitch = (img.w*12)/ 8 + ((img.w*12) % 8 > 0? 1 : 0);
+ const size_t input_size = img.h*input_pitch;
+ std::unique_ptr<uint8_t[]> input_buffer(new uint8_t[input_size]);
+
+ in.read((char*)input_buffer.get(), input_size);
+
+ for(size_t r=0; r<img.h; ++r) {
+ uint16_t* pout = (uint16_t*)(img.ptr + r*img.pitch);
+ uint8_t* pin = input_buffer.get() + r*input_pitch;
+ const uint8_t* pin_end = input_buffer.get() + (r+1)*input_pitch;
+ while(pin < pin_end) {
+ uint32_t val = *(pin++);
+ val |= uint32_t(*(pin++)) << 8;
+ val |= uint32_t(*(pin++)) << 16;
+ *(pout++) = uint16_t( val & 0x000FFF);
+ *(pout++) = uint16_t((val & 0xFFF000) >> 12);
+ }
+ }
+
+ return img;
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_pango.cpp b/externals/Pangolin/src/image/image_io_pango.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..469599cc03872a13104a6bf68ba3a59cf25d28f5
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_pango.cpp
@@ -0,0 +1,62 @@
+#include <pangolin/platform.h>
+
+#include <pangolin/image/typed_image.h>
+
+#ifdef BUILD_PANGOLIN_VIDEO
+# include <pangolin/video/drivers/pango.h>
+# include <pangolin/video/drivers/pango_video_output.h>
+#endif
+
+namespace pangolin {
+
+TypedImage LoadPango(const std::string& uri)
+{
+ PANGOLIN_UNUSED(uri);
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ std::unique_ptr<VideoInterface> video = OpenVideo(uri);
+ if(!video || video->Streams().size() != 1) {
+ throw pangolin::VideoException("Wrong number of streams: exactly one expected.");
+ }
+
+ std::unique_ptr<uint8_t[]> buffer( new uint8_t[video->SizeBytes()] );
+ const StreamInfo& stream_info = video->Streams()[0];
+
+ // Grab first image from video
+ if(!video->GrabNext(buffer.get(), true)) {
+ throw pangolin::VideoException("Failed to grab image from stream");
+ }
+
+ // Allocate storage for user image to return
+ TypedImage image(stream_info.Width(), stream_info.Height(), stream_info.PixFormat());
+
+ // Copy image data into user buffer.
+ const Image<unsigned char> img = stream_info.StreamImage(buffer.get());
+ PANGO_ENSURE(image.pitch <= img.pitch);
+ for(size_t y=0; y < image.h; ++y) {
+ std::memcpy(image.RowPtr(y), img.RowPtr(y), image.pitch);
+ }
+
+ return image;
+#else
+ throw std::runtime_error("Video Support not enabled. Please rebuild Pangolin.");
+#endif
+}
+
+void SavePango(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, const std::string& uri, bool /*top_line_first*/)
+{
+ PANGOLIN_UNUSED(image);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(uri);
+
+#ifdef BUILD_PANGOLIN_VIDEO
+ std::unique_ptr<VideoOutputInterface> video = OpenVideoOutput(uri);
+ StreamInfo stream(fmt, image.w, image.h, image.pitch);
+ video->SetStreams({stream});
+ video->WriteStreams(image.ptr);
+#else
+ throw std::runtime_error("Video Support not enabled. Please rebuild Pangolin.");
+#endif
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_png.cpp b/externals/Pangolin/src/image/image_io_png.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..edaf3ec1cfe75c7b16ba9c36929c67e92628b143
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_png.cpp
@@ -0,0 +1,233 @@
+#include <pangolin/platform.h>
+
+#include <fstream>
+#include <pangolin/image/image_io.h>
+#include <vector>
+
+#ifdef HAVE_PNG
+# include <png.h>
+#endif // HAVE_PNG
+
+namespace pangolin {
+
+#ifdef HAVE_PNG
+
+PixelFormat PngFormat(png_structp png_ptr, png_infop info_ptr )
+{
+ const png_byte colour = png_get_color_type(png_ptr, info_ptr);
+ const png_byte depth = png_get_bit_depth(png_ptr, info_ptr);
+
+ if( depth == 8 ) {
+ if( colour == PNG_COLOR_MASK_COLOR ) {
+ return PixelFormatFromString("RGB24");
+ } else if( colour == (PNG_COLOR_MASK_COLOR | PNG_COLOR_MASK_ALPHA) ) {
+ return PixelFormatFromString("RGBA32");
+ } else if( colour == PNG_COLOR_MASK_ALPHA ) {
+ return PixelFormatFromString("Y400A");
+ } else {
+ return PixelFormatFromString("GRAY8");
+ }
+ }else if( depth == 16 ) {
+ if( colour == 0 ) {
+ return PixelFormatFromString("GRAY16LE");
+ }
+ }
+
+ throw std::runtime_error("Unsupported PNG format");
+}
+
+void PNGAPI PngWarningsCallback(png_structp /*png_ptr*/, png_const_charp /*warning_message*/)
+{
+ // Override default behaviour - don't do anything.
+}
+
+#define PNGSIGSIZE 8
+bool pango_png_validate(std::istream& source)
+{
+ png_byte pngsig[PNGSIGSIZE];
+ source.read((char*)pngsig, PNGSIGSIZE);
+ return source.good() && png_sig_cmp(pngsig, 0, PNGSIGSIZE) == 0;
+}
+
+void pango_png_stream_read(png_structp pngPtr, png_bytep data, png_size_t length) {
+ std::istream* s = (std::istream*)png_get_io_ptr(pngPtr);
+ PANGO_ASSERT(s);
+ s->read((char*)data, length);
+}
+
+void pango_png_stream_write(png_structp pngPtr, png_bytep data, png_size_t length) {
+ std::ostream* s = (std::ostream*)png_get_io_ptr(pngPtr);
+ PANGO_ASSERT(s);
+ s->write((char*)data, length);
+}
+
+void pango_png_stream_write_flush(png_structp pngPtr)
+{
+ std::ostream* s = (std::ostream*)png_get_io_ptr(pngPtr);
+ PANGO_ASSERT(s);
+ s->flush();
+}
+
+#endif // HAVE_PNG
+
+
+TypedImage LoadPng(std::istream& source)
+{
+#ifdef HAVE_PNG
+ //so First, we validate our stream with the validate function I just mentioned
+ if (!pango_png_validate(source)) {
+ throw std::runtime_error("Not valid PNG header");
+ }
+
+ //set up initial png structs
+ png_structp png_ptr = png_create_read_struct( PNG_LIBPNG_VER_STRING, (png_voidp)NULL, NULL, &PngWarningsCallback);
+ if (!png_ptr) {
+ throw std::runtime_error( "PNG Init error 1" );
+ }
+
+ png_infop info_ptr = png_create_info_struct(png_ptr);
+ if (!info_ptr) {
+ png_destroy_read_struct(&png_ptr, (png_infopp)NULL, (png_infopp)NULL);
+ throw std::runtime_error( "PNG Init error 2" );
+ }
+
+ png_infop end_info = png_create_info_struct(png_ptr);
+ if (!end_info) {
+ png_destroy_read_struct(&png_ptr, &info_ptr, (png_infopp)NULL);
+ throw std::runtime_error( "PNG Init error 3" );
+ }
+
+ png_set_read_fn(png_ptr,(png_voidp)&source, pango_png_stream_read);
+
+ png_set_sig_bytes(png_ptr, PNGSIGSIZE);
+
+ // Setup transformation options
+ if( png_get_bit_depth(png_ptr, info_ptr) == 1) {
+ //Unpack bools to bytes to ease loading.
+ png_set_packing(png_ptr);
+ } else if( png_get_bit_depth(png_ptr, info_ptr) < 8) {
+ //Expand nonbool colour depths up to 8bpp
+ png_set_expand_gray_1_2_4_to_8(png_ptr);
+ }
+
+ //Get rid of palette, by transforming it to RGB
+ if(png_get_color_type(png_ptr, info_ptr) == PNG_COLOR_TYPE_PALETTE) {
+ png_set_palette_to_rgb(png_ptr);
+ }
+
+ //read the file
+ png_read_png(png_ptr, info_ptr, PNG_TRANSFORM_SWAP_ENDIAN, NULL);
+
+ if( png_get_interlace_type(png_ptr,info_ptr) != PNG_INTERLACE_NONE) {
+ throw std::runtime_error( "Interlace not yet supported" );
+ }
+
+ const size_t w = png_get_image_width(png_ptr,info_ptr);
+ const size_t h = png_get_image_height(png_ptr,info_ptr);
+ const size_t pitch = png_get_rowbytes(png_ptr, info_ptr);
+
+ TypedImage img(w, h, PngFormat(png_ptr, info_ptr), pitch);
+
+ png_bytepp rows = png_get_rows(png_ptr, info_ptr);
+ for( unsigned int r = 0; r < h; r++) {
+ memcpy( img.ptr + pitch*r, rows[r], pitch );
+ }
+ png_destroy_read_struct(&png_ptr, &info_ptr, &end_info);
+
+ return img;
+#else
+ PANGOLIN_UNUSED(source);
+ throw std::runtime_error("Rebuild Pangolin for PNG support.");
+#endif // HAVE_PNG
+}
+
+TypedImage LoadPng(const std::string& filename)
+{
+ std::ifstream f(filename);
+ return LoadPng(f);
+}
+
+void SavePng(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& stream, bool top_line_first, int zlib_compression_level)
+{
+#ifdef HAVE_PNG
+ // Check image has supported bit depth
+ for(unsigned int i=1; i < fmt.channels; ++i) {
+ if( fmt.channel_bits[i] != fmt.channel_bits[0] ) {
+ throw std::runtime_error("PNG Saving only supported for images where each channel has the same bit depth.");
+ }
+ }
+
+ png_structp png_ptr;
+ png_infop info_ptr;
+
+ // Initialize write structure
+ png_ptr = png_create_write_struct(PNG_LIBPNG_VER_STRING, NULL, NULL, NULL);
+ if (png_ptr == NULL) {
+ throw std::runtime_error( "PNG Error: Could not allocate write struct." );
+ }
+
+ // Initialize info structure
+ info_ptr = png_create_info_struct(png_ptr);
+ if (info_ptr == NULL) {
+ png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
+ throw std::runtime_error( "PNG Error: Could not allocate info struct." );
+ }
+
+ // Setup Exception handling
+ if (setjmp(png_jmpbuf(png_ptr))) {
+ png_free_data(png_ptr, info_ptr, PNG_FREE_ALL, -1);
+ png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
+ throw std::runtime_error( "PNG Error: Error during png creation." );
+ }
+
+ png_set_compression_level(png_ptr, zlib_compression_level);
+
+ png_set_write_fn(png_ptr,(png_voidp)&stream, pango_png_stream_write, pango_png_stream_write_flush);
+
+ const int bit_depth = fmt.channel_bits[0];
+
+ int colour_type;
+ switch (fmt.channels) {
+ case 1: colour_type = PNG_COLOR_TYPE_GRAY; break;
+ case 2: colour_type = PNG_COLOR_TYPE_GRAY_ALPHA; break;
+ case 3: colour_type = PNG_COLOR_TYPE_RGB; break;
+ case 4: colour_type = PNG_COLOR_TYPE_RGBA; break;
+ default:
+ throw std::runtime_error( "PNG Error: unexpected image channel number");
+ }
+
+ // Write header
+ png_set_IHDR(
+ png_ptr, info_ptr, (png_uint_32)image.w, (png_uint_32)image.h, bit_depth, colour_type,
+ PNG_INTERLACE_NONE, PNG_COMPRESSION_TYPE_DEFAULT, PNG_FILTER_TYPE_DEFAULT
+ );
+
+ // Setup rows to write:
+ std::vector<png_bytep> rows(image.h);
+ if(top_line_first) {
+ for (unsigned int y = 0; y< image.h; y++) {
+ rows[y] = image.ptr + y*image.pitch;
+ }
+ }else{
+ for (unsigned int y = 0; y< image.h; y++) {
+ rows[y] = image.ptr + (image.h-1-y)*image.pitch;
+ }
+ }
+ png_set_rows(png_ptr,info_ptr, &rows[0]);
+
+ // Write image data: switch to little-endian byte order, to match host.
+ png_write_png(png_ptr,info_ptr, PNG_TRANSFORM_SWAP_ENDIAN, 0);
+
+ // Free resources
+ png_free_data(png_ptr, info_ptr, PNG_FREE_ALL, -1);
+ png_destroy_write_struct(&png_ptr, (png_infopp)NULL);
+#else
+ PANGOLIN_UNUSED(image);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(stream);
+ PANGOLIN_UNUSED(top_line_first);
+ throw std::runtime_error("Rebuild Pangolin for PNG support.");
+#endif // HAVE_PNG
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_ppm.cpp b/externals/Pangolin/src/image/image_io_ppm.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..359a09de1f3ad4a34f199fdb8666150d29518f34
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_ppm.cpp
@@ -0,0 +1,104 @@
+#include <fstream>
+#include <pangolin/image/typed_image.h>
+
+namespace pangolin {
+
+PixelFormat PpmFormat(const std::string& strType, int num_colours)
+{
+ if(strType == "P5") {
+ if(num_colours < 256) {
+ return PixelFormatFromString("GRAY8");
+ } else {
+ return PixelFormatFromString("GRAY16LE");
+ }
+ }else if(strType == "P6") {
+ return PixelFormatFromString("RGB24");
+ }else{
+ throw std::runtime_error("Unsupported PPM/PGM format");
+ }
+}
+
+void PpmConsumeWhitespaceAndComments(std::istream& in)
+{
+ // TODO: Make a little more general / more efficient
+ while( in.peek() == ' ' ) in.get();
+ while( in.peek() == '\n' ) in.get();
+ while( in.peek() == '#' ) in.ignore(4096, '\n');
+}
+
+TypedImage LoadPpm(std::istream& in)
+{
+ // Parse header
+ std::string ppm_type = "";
+ int num_colors = 0;
+ int w = 0;
+ int h = 0;
+
+ in >> ppm_type;
+ PpmConsumeWhitespaceAndComments(in);
+ in >> w;
+ PpmConsumeWhitespaceAndComments(in);
+ in >> h;
+ PpmConsumeWhitespaceAndComments(in);
+ in >> num_colors;
+ in.ignore(1,'\n');
+
+ if(!in.fail() && w > 0 && h > 0) {
+ TypedImage img(w, h, PpmFormat(ppm_type, num_colors) );
+
+ // Read in data
+ for(size_t r=0; r<img.h; ++r) {
+ in.read( (char*)img.ptr + r*img.pitch, img.pitch );
+ }
+ if(!in.fail()) {
+ return img;
+ }
+ }
+
+ throw std::runtime_error("Unable to load PPM file.");
+}
+
+void SavePpm(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, bool top_line_first)
+{
+ // Setup header variables
+ std::string ppm_type = "";
+ int num_colors = 0;
+ int w = (int)image.w;
+ int h = (int)image.h;
+
+ if(fmt.format == "GRAY8") {
+ ppm_type = "P5";
+ num_colors = 255;
+ }else if(fmt.format == "GRAY16LE") {
+ ppm_type = "P5";
+ num_colors = 65535;
+ }else if(fmt.format == "RGB24") {
+ ppm_type = "P6";
+ num_colors = 255;
+ }else{
+ throw std::runtime_error("Unsupported pixel format");
+ }
+
+ // Write header
+ out << ppm_type;
+ out << " ";
+ out << w;
+ out << " ";
+ out << h;
+ out << " ";
+ out << num_colors;
+ out << "\n";
+
+ // Write out data
+ if(top_line_first) {
+ for(size_t r=0; r<image.h; ++r) {
+ out.write( (char*)image.ptr + r*image.pitch, image.pitch );
+ }
+ }else{
+ for(size_t r=0; r<image.h; ++r) {
+ out.write( (char*)image.ptr + (image.h-1-r)*image.pitch, image.pitch );
+ }
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_raw.cpp b/externals/Pangolin/src/image/image_io_raw.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9198760dcbe892bf6b24e92f6cebd749ec2a4d8e
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_raw.cpp
@@ -0,0 +1,25 @@
+#include <fstream>
+#include <pangolin/image/typed_image.h>
+
+namespace pangolin {
+
+TypedImage LoadImage(
+ const std::string& filename,
+ const PixelFormat& raw_fmt,
+ size_t raw_width, size_t raw_height, size_t raw_pitch
+) {
+ TypedImage img(raw_width, raw_height, raw_fmt, raw_pitch);
+
+ // Read from file, row at a time.
+ std::ifstream bFile( filename.c_str(), std::ios::in | std::ios::binary );
+ for(size_t r=0; r<img.h; ++r) {
+ bFile.read( (char*)img.ptr + r*img.pitch, img.pitch );
+ if(bFile.fail()) {
+ pango_print_warn("Unable to read raw image file to completion.");
+ break;
+ }
+ }
+ return img;
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_tga.cpp b/externals/Pangolin/src/image/image_io_tga.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc9c395ce0da6e563329c57fac2890116a308b8d
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_tga.cpp
@@ -0,0 +1,53 @@
+#include <pangolin/platform.h>
+
+#include <pangolin/image/image_io.h>
+
+namespace pangolin {
+
+PixelFormat TgaFormat(int depth, int color_type, int color_map)
+{
+ if(color_map == 0) {
+ if(color_type == 2) {
+ // Colour
+ if(depth == 24) {
+ return PixelFormatFromString("RGB24");
+ }else if(depth == 32) {
+ return PixelFormatFromString("RGBA32");
+ }
+ }else if(color_type == 3){
+ // Greyscale
+ if(depth == 8) {
+ return PixelFormatFromString("GRAY8");
+ }else if(depth == 16) {
+ return PixelFormatFromString("Y400A");
+ }
+ }
+ }
+ throw std::runtime_error("Unsupported TGA format");
+}
+
+TypedImage LoadTga(std::istream& in)
+{
+ unsigned char type[4];
+ unsigned char info[6];
+
+ in.read((char*)type, 3*sizeof(char));
+ in.seekg(12);
+ in.read((char*)info,6*sizeof(char));
+
+ const int width = info[0] + (info[1] * 256);
+ const int height = info[2] + (info[3] * 256);
+
+ if(in.good()) {
+ TypedImage img(width, height, TgaFormat(info[4], type[2], type[1]) );
+
+ //read in image data
+ const size_t data_size = img.h * img.pitch;
+ in.read((char*)img.ptr, sizeof(char)*data_size);
+ return img;
+ }
+
+ throw std::runtime_error("Unable to load TGA file");
+}
+
+}
diff --git a/externals/Pangolin/src/image/image_io_zstd.cpp b/externals/Pangolin/src/image/image_io_zstd.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cfb25c9410f86641c53959e43ded51857e11120b
--- /dev/null
+++ b/externals/Pangolin/src/image/image_io_zstd.cpp
@@ -0,0 +1,125 @@
+
+#include <fstream>
+#include <memory>
+
+#include <pangolin/image/typed_image.h>
+
+#ifdef HAVE_ZSTD
+# include <zstd.h>
+#endif
+
+namespace pangolin {
+
+#pragma pack(push, 1)
+struct zstd_image_header
+{
+ char magic[4];
+ char fmt[16];
+ size_t w, h;
+};
+#pragma pack(pop)
+
+void SaveZstd(const Image<unsigned char>& image, const pangolin::PixelFormat& fmt, std::ostream& out, int compression_level)
+{
+#ifdef HAVE_ZSTD
+ // Write out header, uncompressed
+ zstd_image_header header;
+ strncpy(header.magic,"ZSTD",4);
+ strncpy(header.fmt, fmt.format.c_str(), sizeof(header.fmt));
+ header.w = image.w;
+ header.h = image.h;
+ out.write((char*)&header, sizeof(header));
+
+ // Write out image data
+ const size_t output_buffer_size = ZSTD_CStreamOutSize();
+ std::unique_ptr<char[]> output_buffer(new char[output_buffer_size]);
+
+ ZSTD_CStream* const cstream = ZSTD_createCStream();
+ if (cstream==nullptr) {
+ throw std::runtime_error("ZSTD_createCStream() error");
+ }
+
+ size_t const initResult = ZSTD_initCStream(cstream, compression_level);
+ if (ZSTD_isError(initResult)) {
+ throw std::runtime_error(FormatString("ZSTD_initCStream() error : %", ZSTD_getErrorName(initResult)));
+ }
+
+ const size_t row_size_bytes = (fmt.bpp * image.w)/8;
+
+ for(size_t y=0; y < image.h; ++y) {
+ ZSTD_inBuffer input = { image.RowPtr(y), row_size_bytes, 0 };
+
+ while (input.pos < input.size) {
+ ZSTD_outBuffer output = { output_buffer.get(), output_buffer_size, 0 };
+ size_t left_to_read = ZSTD_compressStream(cstream, &output , &input);
+ if (ZSTD_isError(left_to_read)) {
+ throw std::runtime_error(FormatString("ZSTD_compressStream() error : %", ZSTD_getErrorName(left_to_read)));
+ }
+ out.write(output_buffer.get(), output.pos);
+ }
+ }
+
+ ZSTD_outBuffer output = { output_buffer.get(), output_buffer_size, 0 };
+ size_t const remainingToFlush = ZSTD_endStream(cstream, &output); /* close frame */
+ if (remainingToFlush) {
+ throw std::runtime_error("not fully flushed");
+ }
+ out.write(output_buffer.get(), output.pos);
+
+ ZSTD_freeCStream(cstream);
+#else
+ PANGOLIN_UNUSED(image);
+ PANGOLIN_UNUSED(fmt);
+ PANGOLIN_UNUSED(out);
+ PANGOLIN_UNUSED(compression_level);
+ throw std::runtime_error("Rebuild Pangolin for ZSTD support.");
+#endif // HAVE_ZSTD
+}
+
+TypedImage LoadZstd(std::istream& in)
+{
+#ifdef HAVE_ZSTD
+ // Read in header, uncompressed
+ zstd_image_header header;
+ in.read( (char*)&header, sizeof(header));
+
+ TypedImage img(header.w, header.h, PixelFormatFromString(header.fmt));
+
+ const size_t input_buffer_size = ZSTD_DStreamInSize();
+ std::unique_ptr<char[]> input_buffer(new char[input_buffer_size]);
+
+ ZSTD_DStream* dstream = ZSTD_createDStream();
+ if(!dstream) {
+ throw std::runtime_error("ZSTD_createDStream() error");
+ }
+
+ size_t read_size_hint = ZSTD_initDStream(dstream);
+ if (ZSTD_isError(read_size_hint)) {
+ throw std::runtime_error(FormatString("ZSTD_initDStream() error : % \n", ZSTD_getErrorName(read_size_hint)));
+ }
+
+ // Image represents our fixed buffer.
+ ZSTD_outBuffer output = { img.ptr, img.SizeBytes(), 0 };
+
+ while(read_size_hint)
+ {
+ const size_t read = in.readsome(input_buffer.get(), read_size_hint);
+ ZSTD_inBuffer input = { input_buffer.get(), read, 0 };
+ while (input.pos < input.size) {
+ read_size_hint = ZSTD_decompressStream(dstream, &output , &input);
+ if (ZSTD_isError(read_size_hint)) {
+ throw std::runtime_error(FormatString("ZSTD_decompressStream() error : %", ZSTD_getErrorName(read_size_hint)));
+ }
+ }
+ }
+
+ ZSTD_freeDStream(dstream);
+
+ return img;
+#else
+ PANGOLIN_UNUSED(in);
+ throw std::runtime_error("Rebuild Pangolin for ZSTD support.");
+#endif // HAVE_ZSTD
+}
+
+}
diff --git a/externals/Pangolin/src/image/pixel_format.cpp b/externals/Pangolin/src/image/pixel_format.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..25164093eb61ad466681256d16ad290913c3832d
--- /dev/null
+++ b/externals/Pangolin/src/image/pixel_format.cpp
@@ -0,0 +1,70 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/image/pixel_format.h>
+
+#include <stdexcept>
+#include <vector>
+
+namespace pangolin
+{
+
+// Not to exceed 8 byte Format code.
+const PixelFormat SupportedPixelFormats[] =
+{
+ {"GRAY8", 1, {8}, 8, 8, false},
+ {"GRAY10", 1, {10}, 10, 10, false},
+ {"GRAY12", 1, {12}, 12, 12, false},
+ {"GRAY16LE", 1, {16}, 16, 16, false},
+ {"GRAY32", 1, {32}, 32, 32, false},
+ {"Y400A", 2, {8,8}, 16, 8, false},
+ {"RGB24", 3, {8,8,8}, 24, 8, false},
+ {"BGR24", 3, {8,8,8}, 24, 8, false},
+ {"RGB48", 3, {16,16,16}, 48, 16, false},
+ {"BGR48", 3, {16,16,16}, 48, 16, false},
+ {"YUYV422", 3, {4,2,2}, 16, 8, false},
+ {"UYVY422", 3, {4,2,2}, 16, 8, false},
+ {"RGBA32", 4, {8,8,8,8}, 32, 8, false},
+ {"BGRA32", 4, {8,8,8,8}, 32, 8, false},
+ {"RGBA64", 4, {16,16,16,16}, 64, 16, false},
+ {"BGRA64", 4, {16,16,16,16}, 64, 16, false},
+ {"GRAY32F", 1, {32}, 32, 32, false},
+ {"GRAY64F", 1, {64}, 64, 64, false},
+ {"RGB96F", 3, {32,32,32}, 96, 32, false},
+ {"RGBA128F", 4, {32,32,32,32}, 128, 32, false},
+ {"",0,{0,0,0,0},0,0,0}
+};
+
+PixelFormat PixelFormatFromString(const std::string& format)
+{
+ for(int i=0; !SupportedPixelFormats[i].format.empty(); ++i)
+ if(!format.compare(SupportedPixelFormats[i].format))
+ return SupportedPixelFormats[i];
+ throw std::runtime_error( std::string("Unknown Format: ") + format);
+}
+
+}
diff --git a/externals/Pangolin/src/ios/PangolinAppDelegate.mm b/externals/Pangolin/src/ios/PangolinAppDelegate.mm
new file mode 100644
index 0000000000000000000000000000000000000000..b9f386c72b1d62245661f039f0802ab788f80082
--- /dev/null
+++ b/externals/Pangolin/src/ios/PangolinAppDelegate.mm
@@ -0,0 +1,49 @@
+//
+// GlTestAppDelegate.m
+// gltest
+//
+// Created by Steven Lovegrove on 30/01/2014.
+// Copyright (c) 2014 Steven Lovegrove. All rights reserved.
+//
+
+#import <pangolin/ios/PangolinAppDelegate.h>
+#import <pangolin/ios/PangolinUIView.h>
+
+@implementation PangolinAppDelegate
+
+@synthesize window=_window;
+
+
+- (BOOL)application:(UIApplication *)application didFinishLaunchingWithOptions:(NSDictionary *)launchOptions
+{
+ return YES;
+}
+
+- (void)applicationWillResignActive:(UIApplication *)application
+{
+ // Sent when the application is about to move from active to inactive state. This can occur for certain types of temporary interruptions (such as an incoming phone call or SMS message) or when the user quits the application and it begins the transition to the background state.
+ // Use this method to pause ongoing tasks, disable timers, and throttle down OpenGL ES frame rates. Games should use this method to pause the game.
+}
+
+- (void)applicationDidEnterBackground:(UIApplication *)application
+{
+ // Use this method to release shared resources, save user data, invalidate timers, and store enough application state information to restore your application to its current state in case it is terminated later.
+ // If your application supports background execution, this method is called instead of applicationWillTerminate: when the user quits.
+}
+
+- (void)applicationWillEnterForeground:(UIApplication *)application
+{
+ // Called as part of the transition from the background to the inactive state; here you can undo many of the changes made on entering the background.
+}
+
+- (void)applicationDidBecomeActive:(UIApplication *)application
+{
+ // Restart any tasks that were paused (or not yet started) while the application was inactive. If the application was previously in the background, optionally refresh the user interface.
+}
+
+- (void)applicationWillTerminate:(UIApplication *)application
+{
+ // Called when the application is about to terminate. Save data if appropriate. See also applicationDidEnterBackground:.
+}
+
+@end
diff --git a/externals/Pangolin/src/ios/PangolinUIView.mm b/externals/Pangolin/src/ios/PangolinUIView.mm
new file mode 100644
index 0000000000000000000000000000000000000000..2dcdf62633a77fc2b9fa4f55b4a33f6d27e38772
--- /dev/null
+++ b/externals/Pangolin/src/ios/PangolinUIView.mm
@@ -0,0 +1,202 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#import <QuartzCore/QuartzCore.h>
+
+#import <pangolin/ios/PangolinUIView.h>
+#import <pangolin/ios/PangolinAppDelegate.h>
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/pangolin.h>
+#include <pangolin/platform.h>
+#include <pangolin/gl/glinclude.h>
+#include <pangolin/display/display.h>
+#include <pangolin/display/display_internal.h>
+
+namespace pangolin
+{
+ extern __thread PangolinGl* context;
+}
+
+
+@implementation PangolinUIView
+
++ (Class)layerClass {
+ return [CAEAGLLayer class];
+}
+
+- (void)setupLayer {
+ _eaglLayer = (CAEAGLLayer*) self.layer;
+ _eaglLayer.opaque = YES;
+}
+
+- (void)setupContext {
+ EAGLRenderingAPI api = kEAGLRenderingAPIOpenGLES2;
+ _context = [[EAGLContext alloc] initWithAPI:api];
+ if (!_context) {
+ NSLog(@"Failed to initialize OpenGLES 2.0 context");
+ exit(1);
+ }
+
+ if (![EAGLContext setCurrentContext:_context]) {
+ NSLog(@"Failed to set current OpenGL context");
+ exit(1);
+ }
+}
+
+- (void)setupRenderBuffer {
+ glGenRenderbuffers(1, &_colorRenderBuffer);
+ glBindRenderbuffer(GL_RENDERBUFFER, _colorRenderBuffer);
+ [_context renderbufferStorage:GL_RENDERBUFFER fromDrawable:_eaglLayer];
+}
+
+- (void)setupDepthBuffer {
+ glGenRenderbuffers(1, &_depthRenderBuffer);
+ glBindRenderbuffer(GL_RENDERBUFFER, _depthRenderBuffer);
+ const int w = self.frame.size.width * self.contentScaleFactor;
+ const int h = self.frame.size.height * self.contentScaleFactor;
+ glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
+}
+
+- (void)setupFrameBuffer {
+ GLuint framebuffer;
+ glGenFramebuffers(1, &framebuffer);
+ glBindFramebuffer(GL_FRAMEBUFFER, framebuffer);
+ glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_RENDERBUFFER, _colorRenderBuffer);
+ glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, _depthRenderBuffer);
+}
+
+- (void)render:(CADisplayLink*)displayLink {
+ pangolin::glEngine().prog_fixed.Bind();
+
+ if(pangolin::context->user_app) {
+ pangolin::context->user_app->Render();
+ }
+
+ pangolin::RenderViews();
+ pangolin::PostRender();
+
+ [_context presentRenderbuffer:GL_RENDERBUFFER];
+}
+
+- (void)setupDisplayLink {
+ CADisplayLink* displayLink = [CADisplayLink displayLinkWithTarget:self selector:@selector(render:)];
+ [displayLink addToRunLoop:[NSRunLoop currentRunLoop] forMode:NSDefaultRunLoopMode];
+}
+
+- (void)setup
+{
+ self.contentScaleFactor = 2;
+
+ [self setupLayer];
+ [self setupContext];
+ [self setupDepthBuffer];
+ [self setupRenderBuffer];
+ [self setupFrameBuffer];
+ [self setupDisplayLink];
+
+ pangolin::PangolinCommonInit();
+
+ const int w = self.frame.size.width * self.contentScaleFactor;
+ const int h = self.frame.size.height * self.contentScaleFactor;
+ pangolin::process::Resize(w,h);
+
+ if(pangolin::context->user_app) {
+ pangolin::context->user_app->Init();
+ }
+}
+
+- (id)initWithFrame:(CGRect)frame
+{
+ self = [super initWithFrame:frame];
+
+ if (self) {
+ [self setup];
+ }
+ return self;
+}
+
+- (void)awakeFromNib
+{
+ [self setup];
+}
+
+- (void)dealloc
+{
+ _context = nil;
+}
+
+@end
+
+
+namespace pangolin
+{
+ int LaunchUserApp(UserApp& app)
+ {
+ // Create new context
+ BindToContext("UserApp");
+
+ // Reference user application
+ context->user_app = &app;
+
+ // Start IOS Window containing GL Context from which we'll receive events
+ // These events will be communicated to pangolin::process::... and the UserApp app.
+ int argc = 1;
+ char* argv[] = { (char*)"dummy" };
+ @autoreleasepool {
+ return UIApplicationMain(argc, argv, nil, NSStringFromClass([PangolinAppDelegate class]));
+ }
+ }
+
+ // Implement platform agnostic version
+ void CreateIosWindowAndBind(std::string window_title, int w, int h )
+ {
+ throw std::runtime_error("pangolin::CreateWindowAndBind(...) Not supported on this platform");
+ }
+
+ // Implement platform agnostic version
+ void FinishFrame()
+ {
+ throw std::runtime_error("pangolin::FinishFrame() Not supported on this platform");
+ }
+
+ PANGOLIN_REGISTER_FACTORY(IosWindow)
+ {
+ struct IosWindowFactory : public FactoryInterface<WindowInterface> {
+ std::unique_ptr<WindowInterface> Open(const Uri& uri) override {
+
+ const std::string window_title = uri.Get<std::string>("window_title", "window");
+ CreateIosWindowAndBind(window_title, 0, 0);
+ return NULL;
+ }
+ };
+
+ auto factory = std::make_shared<IosWindowFactory>();
+ FactoryRegistry<WindowInterface>::I().RegisterFactory(factory, 10, "ioswindow");
+ }
+}
+
diff --git a/externals/Pangolin/src/log/packet.cpp b/externals/Pangolin/src/log/packet.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..34e40338c7ffc2286c07045d938ff39b430a6ef3
--- /dev/null
+++ b/externals/Pangolin/src/log/packet.cpp
@@ -0,0 +1,79 @@
+#include <pangolin/log/packet.h>
+
+namespace pangolin {
+
+
+Packet::Packet(PacketStream& s, std::unique_lock<std::recursive_mutex>&& lock, std::vector<PacketStreamSource>& srcs)
+ : _stream(s), lock(std::move(lock))
+{
+ ParsePacketHeader(s, srcs);
+}
+
+Packet::Packet(Packet&& o)
+ : src(o.src), time(o.time), size(o.size), sequence_num(o.sequence_num),
+ meta(std::move(o.meta)), frame_streampos(o.frame_streampos), _stream(o._stream),
+ lock(std::move(o.lock)), data_streampos(o.data_streampos), _data_len(o._data_len)
+{
+ o._data_len = 0;
+}
+
+Packet::~Packet()
+{
+ ReadRemaining();
+}
+
+size_t Packet::BytesRead() const
+{
+ return _stream.tellg() - data_streampos;
+}
+
+int Packet::BytesRemaining() const
+{
+ if(_data_len) {
+ return (int)_data_len - (int)BytesRead();
+ }else{
+ return 0;
+ }
+}
+
+void Packet::ParsePacketHeader(PacketStream& s, std::vector<PacketStreamSource>& srcs)
+{
+ size_t json_src = -1;
+
+ frame_streampos = s.tellg();
+ if (s.peekTag() == TAG_SRC_JSON)
+ {
+ s.readTag(TAG_SRC_JSON);
+ json_src = s.readUINT();
+ picojson::parse(meta, s);
+ }
+
+ s.readTag(TAG_SRC_PACKET);
+ time = s.readTimestamp();
+
+ src = s.readUINT();
+ PANGO_ENSURE(json_src == size_t(-1) || json_src == src, "Frame preceded by metadata for a mismatched source. Stream may be corrupt.");
+
+ PacketStreamSource& src_packet = srcs[src];
+
+ size = src_packet.data_size_bytes;
+ if (!size) {
+ size = s.readUINT();
+ }
+ sequence_num = src_packet.next_packet_id++;
+
+ _data_len = size;
+ data_streampos = s.tellg();
+}
+
+void Packet::ReadRemaining()
+{
+ int bytes_left = BytesRemaining();
+
+ while(bytes_left > 0 && Stream().good()) {
+ Stream().skip(bytes_left);
+ bytes_left = BytesRemaining();
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/log/packetstream.cpp b/externals/Pangolin/src/log/packetstream.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7b97f864292d532f496428c410ddba4feb1d46f1
--- /dev/null
+++ b/externals/Pangolin/src/log/packetstream.cpp
@@ -0,0 +1,146 @@
+#include <pangolin/log/packetstream.h>
+#include <stdexcept>
+
+namespace pangolin {
+
+size_t PacketStream::readUINT()
+{
+ size_t n = 0;
+ size_t v = get();
+ uint32_t shift = 0;
+ while (good() && (v & 0x80))
+ {
+ n |= (v & 0x7F) << shift;
+ shift += 7;
+ v = get();
+ }
+ if (!good())
+ return static_cast<size_t>(-1);
+ return n | (v & 0x7F) << shift;
+}
+
+int64_t PacketStream::readTimestamp()
+{
+ int64_t time_us;
+ read(reinterpret_cast<char*>(&time_us), sizeof(int64_t));
+ return time_us;
+}
+
+pangoTagType PacketStream::readTag()
+{
+ auto r = peekTag();
+ _tag = 0;
+ return r;
+}
+
+pangoTagType PacketStream::readTag(pangoTagType x)
+{
+ auto r = readTag();
+ if (r != x)
+ throw std::runtime_error(("Tag mismatch error: expected tag '" + tagName(r) + "' does not match found tag '" + tagName(x) + "'").c_str());
+ return r;
+}
+
+pangoTagType PacketStream::peekTag()
+{
+ if (!_tag)
+ {
+ _tag = 0;
+ Base::read(reinterpret_cast<char*>(&_tag), TAG_LENGTH);
+ if (!good())
+ _tag = TAG_END;
+ }
+ return _tag;
+}
+
+char PacketStream::get()
+{
+ _tag = 0;
+ return Base::get();
+}
+
+size_t PacketStream::read(char* target, size_t len)
+{
+ _tag = 0;
+ Base::read(target, len);
+ return gcount();
+}
+
+size_t PacketStream::skip(size_t len)
+{
+ if (seekable()) {
+ Base::seekg(len, std::ios_base::cur);
+ } else {
+ Base::ignore(len);
+ }
+ cclear();
+ return len;
+}
+
+std::streampos PacketStream::tellg()
+{
+ if (_tag) {
+ return Base::tellg() - std::streamoff(TAG_LENGTH);
+ }else{
+ return Base::tellg();
+ }
+}
+
+void PacketStream::seekg(std::streampos target)
+{
+ if (seekable()) {
+ cclear();
+ Base::seekg(target);
+ }
+}
+
+void PacketStream::seekg(std::streamoff off, std::ios_base::seekdir way)
+{
+ if (seekable()) {
+ cclear();
+ Base::seekg(off, way);
+ }
+}
+
+static bool valid(pangoTagType t)
+{
+ switch (t)
+ {
+ case TAG_PANGO_SYNC:
+ case TAG_ADD_SOURCE:
+ case TAG_SRC_JSON:
+ case TAG_SRC_PACKET:
+ case TAG_PANGO_STATS:
+ case TAG_PANGO_FOOTER:
+ case TAG_END:
+ case TAG_PANGO_HDR:
+ case TAG_PANGO_MAGIC:
+ return true;
+ default:
+ return false;
+ }
+}
+
+pangoTagType PacketStream::syncToTag() //scan through chars one by one until the last three look like a tag
+{
+ peekTag();
+ char * buffer = reinterpret_cast<char*>(&_tag);
+
+ buffer[3] = 0;
+
+ do
+ {
+ buffer[0] = buffer[1];
+ buffer[1] = buffer[2];
+ buffer[2] = get();
+ }
+ while (good() && !valid(_tag));
+
+ if (!good())
+ _tag = TAG_END;
+
+ return _tag;
+
+}
+
+}
diff --git a/externals/Pangolin/src/log/packetstream_reader.cpp b/externals/Pangolin/src/log/packetstream_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..791be3efb72889ed07b9f85aca6208bc429e232c
--- /dev/null
+++ b/externals/Pangolin/src/log/packetstream_reader.cpp
@@ -0,0 +1,425 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#include <pangolin/log/packetstream_reader.h>
+#include <pangolin/log/packetstream_writer.h>
+
+using std::string;
+using std::istream;
+using std::ios;
+using std::lock_guard;
+using std::runtime_error;
+using std::ios_base;
+using std::streampos;
+using std::streamoff;
+
+#include <thread>
+
+#ifndef _WIN_
+# include <unistd.h>
+#endif
+
+namespace pangolin
+{
+
+PacketStreamReader::PacketStreamReader()
+ : _pipe_fd(-1)
+{
+}
+
+PacketStreamReader::PacketStreamReader(const std::string& filename)
+ : _pipe_fd(-1)
+{
+ Open(filename);
+}
+
+PacketStreamReader::~PacketStreamReader()
+{
+ Close();
+}
+
+void PacketStreamReader::Open(const std::string& filename)
+{
+ std::lock_guard<std::recursive_mutex> lg(_mutex);
+
+ Close();
+
+ _filename = filename;
+ _is_pipe = IsPipe(filename);
+ _stream.open(filename);
+
+ if (!_stream.is_open())
+ throw runtime_error("Cannot open stream.");
+
+ for (auto i : PANGO_MAGIC)
+ {
+ if (_stream.get() != i)
+ throw runtime_error("Unrecognised file header.");
+ if (!_stream.good())
+ throw runtime_error("Bad stream");
+ }
+
+
+ ParseHeader();
+
+ while (_stream.peekTag() == TAG_ADD_SOURCE) {
+ ParseNewSource();
+ }
+
+ if(!SetupIndex()) {
+ FixFileIndex();
+ }
+}
+
+void PacketStreamReader::Close() {
+ std::lock_guard<std::recursive_mutex> lg(_mutex);
+
+ _stream.close();
+ _sources.clear();
+
+#ifndef _WIN_
+ if (_pipe_fd != -1) {
+ close(_pipe_fd);
+ }
+#endif
+}
+
+void PacketStreamReader::ParseHeader()
+{
+ _stream.readTag(TAG_PANGO_HDR);
+
+ picojson::value json_header;
+ picojson::parse(json_header, _stream);
+
+ // File timestamp
+ const int64_t start_us = json_header["time_us"].get<int64_t>();
+ packet_stream_start = SyncTime::TimePoint() + std::chrono::microseconds(start_us);
+
+ _stream.get(); // consume newline
+}
+
+void PacketStreamReader::ParseNewSource()
+{
+ _stream.readTag(TAG_ADD_SOURCE);
+ picojson::value json;
+ picojson::parse(json, _stream);
+ _stream.get(); // consume newline
+
+
+ const size_t src_id = json[pss_src_id].get<int64_t>();
+
+ if(_sources.size() <= src_id) {
+ _sources.resize(src_id+1);
+ }
+
+ PacketStreamSource& pss = _sources[src_id];
+ pss.id = src_id;
+ pss.driver = json[pss_src_driver].get<string>();
+ pss.uri = json[pss_src_uri].get<string>();
+ pss.info = json[pss_src_info];
+ pss.version = json[pss_src_version].get<int64_t>();
+ pss.data_alignment_bytes = json[pss_src_packet][pss_pkt_alignment_bytes].get<int64_t>();
+ pss.data_definitions = json[pss_src_packet][pss_pkt_definitions].get<string>();
+ pss.data_size_bytes = json[pss_src_packet][pss_pkt_size_bytes].get<int64_t>();
+}
+
+bool PacketStreamReader::SetupIndex()
+{
+ bool index_good = false;
+
+ if (_stream.seekable())
+ {
+ // Save current position
+ std::streampos pos = _stream.tellg();
+
+ // Look for footer at end of file (TAG_PANGO_FOOTER + index position).
+ _stream.seekg(-(static_cast<istream::off_type>(sizeof(uint64_t)) + TAG_LENGTH), ios_base::end);
+ if (_stream.peekTag() == TAG_PANGO_FOOTER)
+ {
+ //parsing the footer returns the index position
+ _stream.seekg(ParseFooter());
+ if (_stream.peekTag() == TAG_PANGO_STATS) {
+ // Read the pre-build index from the file
+ index_good = ParseIndex();
+ }
+ }
+
+ // Restore previous location
+ _stream.clear();
+ _stream.seekg(pos);
+ }
+
+ return index_good;
+}
+
+streampos PacketStreamReader::ParseFooter() //returns position of index.
+{
+ _stream.readTag(TAG_PANGO_FOOTER);
+ uint64_t index=0;
+ size_t bytes_read = _stream.read(reinterpret_cast<char*>(&index), sizeof(index));
+ PANGO_ENSURE(bytes_read == sizeof(index));
+ return index;
+}
+
+bool PacketStreamReader::ParseIndex()
+{
+ _stream.readTag(TAG_PANGO_STATS);
+ picojson::value json;
+ picojson::parse(json, _stream);
+
+ const bool index_good = json.contains("src_packet_index") && json.contains("src_packet_times");
+
+ if (index_good)
+ {
+ // This is a two-dimensional serialized array, [source id][sequence number] ---> packet position in stream
+ const auto& json_index = json["src_packet_index"].get<picojson::array>();
+ const auto& json_times = json["src_packet_times"].get<picojson::array>();
+
+ // We shouldn't have seen more sources than exist in the index
+ PANGO_ENSURE(_sources.size() <= json_index.size());
+ PANGO_ENSURE(json_index.size() == json_times.size());
+
+ _sources.resize(json_index.size());
+
+ // Populate index
+ for(size_t i=0; i < _sources.size(); ++i) {
+ PANGO_ENSURE(json_index[i].size() == json_times[i].size());
+ _sources[i].index.resize(json_index[i].size());
+ for(size_t f=0; f < json_index[i].size(); ++f) {
+ _sources[i].index[f].pos = json_index[i][f].get<int64_t>();
+ _sources[i].index[f].capture_time = json_times[i][f].get<int64_t>();
+ }
+ }
+ }
+
+ return index_good;
+}
+
+bool PacketStreamReader::GoodToRead()
+{
+ if(!_stream.good()) {
+#ifndef _WIN_
+ if (_is_pipe)
+ {
+ if (_pipe_fd == -1) {
+ _pipe_fd = ReadablePipeFileDescriptor(_filename);
+ }
+
+ if (_pipe_fd != -1) {
+ // Test whether the pipe has data to be read. If so, open the
+ // file stream and start reading. After this point, the file
+ // descriptor is owned by the reader.
+ if (PipeHasDataToRead(_pipe_fd))
+ {
+ close(_pipe_fd);
+ _pipe_fd = -1;
+ Open(_filename);
+ return _stream.good();
+ }
+ }
+ }
+#endif
+
+ return false;
+ }
+
+ return true;
+
+}
+
+Packet PacketStreamReader::NextFrame()
+{
+ std::unique_lock<std::recursive_mutex> lock(_mutex);
+
+ while (GoodToRead())
+ {
+ const pangoTagType t = _stream.peekTag();
+
+ switch (t)
+ {
+ case TAG_PANGO_SYNC:
+ SkipSync();
+ break;
+ case TAG_ADD_SOURCE:
+ ParseNewSource();
+ break;
+ case TAG_SRC_JSON: //frames are sometimes preceded by metadata, but metadata must ALWAYS be followed by a frame from the same source.
+ case TAG_SRC_PACKET:
+ return Packet(_stream, std::move(lock), _sources);
+ case TAG_PANGO_STATS:
+ ParseIndex();
+ break;
+ case TAG_PANGO_FOOTER: //end of frames
+ case TAG_END:
+ throw std::runtime_error("PacketStreamReader: end of stream");
+ case TAG_PANGO_HDR: //shoudln't encounter this
+ ParseHeader();
+ break;
+ case TAG_PANGO_MAGIC: //or this
+ SkipSync();
+ break;
+ default: //or anything else
+ pango_print_warn("Unexpected packet type: \"%s\". Resyncing()\n", tagName(t).c_str());
+ ReSync();
+ break;
+ }
+ }
+
+ // No frame
+ throw std::runtime_error("PacketStreamReader: no frame");
+}
+
+Packet PacketStreamReader::NextFrame(PacketStreamSourceId src)
+{
+ while (1)
+ {
+ // This will throw if nothing is left.
+ auto fi = NextFrame();
+ if (fi.src == src) {
+ return fi;
+ }
+ }
+}
+
+void PacketStreamReader::RebuildIndex()
+{
+ lock_guard<decltype(_mutex)> lg(_mutex);
+
+ if(_stream.seekable()) {
+ pango_print_warn("Index for '%s' bad / outdated. Rebuilding.\n", _filename.c_str());
+
+ // Save current position
+ std::streampos pos = _stream.tellg();
+
+ // Clear existing index
+ for(PacketStreamSource& s : _sources) {
+ s.index.clear();
+ s.next_packet_id = 0;
+ }
+
+ // Read through entire file, updating index
+ try{
+ while (1)
+ {
+ // This will throw if we've run out of frames
+ auto fi = NextFrame();
+ PacketStreamSource& s = _sources[fi.src];
+ PANGO_ENSURE(s.index.size() == fi.sequence_num);
+ s.index.push_back({fi.frame_streampos, fi.time});
+ }
+ }catch(...){
+ }
+
+ // Reset Packet id's
+ for(PacketStreamSource& s : _sources) {
+ s.next_packet_id = 0;
+ }
+
+ // Restore previous location
+ _stream.clear();
+ _stream.seekg(pos);
+ }
+}
+
+void PacketStreamReader::AppendIndex()
+{
+ lock_guard<decltype(_mutex)> lg(_mutex);
+
+ if(_stream.seekable()) {
+ // Open file again for append
+ std::ofstream of(_filename, std::ios::app | std::ios::binary);
+ if(of.is_open()) {
+ pango_print_warn("Appending new index to '%s'.\n", _filename.c_str());
+ uint64_t indexpos = (uint64_t)of.tellp();
+ writeTag(of, TAG_PANGO_STATS);
+ SourceStats(_sources).serialize(std::ostream_iterator<char>(of), false);
+ writeTag(of, TAG_PANGO_FOOTER);
+ of.write(reinterpret_cast<char*>(&indexpos), sizeof(uint64_t));
+ }
+ }
+}
+
+void PacketStreamReader::FixFileIndex()
+{
+ if(_stream.seekable())
+ {
+ RebuildIndex();
+ AppendIndex();
+ }
+}
+
+size_t PacketStreamReader::Seek(PacketStreamSourceId src, size_t framenum)
+{
+ lock_guard<decltype(_mutex)> lg(_mutex);
+
+ PANGO_ASSERT(_stream.seekable());
+ PANGO_ASSERT(src < _sources.size());
+ PacketStreamSource& source = _sources[src];
+ PANGO_ASSERT(framenum < source.index.size());
+
+ if(source.index[framenum].pos > 0) {
+ _stream.clear();
+ _stream.seekg(source.index[framenum].pos);
+ source.next_packet_id = framenum;
+ }
+ return source.next_packet_id;
+}
+
+// Jumps to the first packet with time >= time
+size_t PacketStreamReader::Seek(PacketStreamSourceId src, SyncTime::TimePoint time)
+{
+ PacketStreamSource& source = _sources[src];
+
+ PacketStreamSource::PacketInfo v = {
+ 0, std::chrono::duration_cast<std::chrono::microseconds>(time.time_since_epoch()).count()
+ };
+
+ // Find time in indextime
+ auto lb = std::lower_bound(
+ source.index.begin(), source.index.end(), v,
+ [](const PacketStreamSource::PacketInfo& a, const PacketStreamSource::PacketInfo& b){
+ return a.capture_time < b.capture_time;
+ }
+ );
+
+ if(lb != source.index.end()) {
+ const size_t frame_num = lb - source.index.begin();
+ return Seek(src, frame_num);
+ }else{
+ return source.next_packet_id;
+ }
+}
+
+void PacketStreamReader::SkipSync()
+{
+ //Assume we have just read PAN, read GO
+ if (_stream.get() != 'G' && _stream.get() != 'O')
+ throw std::runtime_error("Unknown packet type.");
+
+ while (_stream.peekTag() != TAG_SRC_PACKET && _stream.peekTag() != TAG_END)
+ _stream.readTag();
+}
+
+}
diff --git a/externals/Pangolin/src/log/packetstream_writer.cpp b/externals/Pangolin/src/log/packetstream_writer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ab1102244c9b058f07a3198a35e80a6c57f9781f
--- /dev/null
+++ b/externals/Pangolin/src/log/packetstream_writer.cpp
@@ -0,0 +1,156 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * 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.
+ */
+
+#include <pangolin/log/packetstream_writer.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/timer.h>
+
+using std::ios;
+using std::lock_guard;
+
+#define SCOPED_LOCK lock_guard<decltype(_lock)> lg(_lock)
+
+//#define SCOPED_LOCK
+
+namespace pangolin
+{
+
+static inline const std::string CurrentTimeStr()
+{
+ time_t time_now = time(0);
+ struct tm time_struct = *localtime(&time_now);
+ char buffer[80];
+ strftime(buffer, sizeof(buffer), "%Y-%m-%d %X", &time_struct);
+ return buffer;
+}
+
+void PacketStreamWriter::WriteHeader()
+{
+ SCOPED_LOCK;
+ _stream.write(PANGO_MAGIC.c_str(), PANGO_MAGIC.size());
+ picojson::value pango;
+ pango["pangolin_version"] = PANGOLIN_VERSION_STRING;
+ pango["time_us"] = Time_us(TimeNow());
+ pango["date_created"] = CurrentTimeStr();
+ pango["endian"] = "little_endian";
+
+ writeTag(_stream, TAG_PANGO_HDR);
+ pango.serialize(std::ostream_iterator<char>(_stream), true);
+
+ for (const auto& source : _sources)
+ Write(source);
+}
+
+void PacketStreamWriter::Write(const PacketStreamSource& source)
+{
+ SCOPED_LOCK;
+ picojson::value serialize;
+ serialize[pss_src_driver] = source.driver;
+ serialize[pss_src_id] = source.id;
+ serialize[pss_src_uri] = source.uri;
+ serialize[pss_src_info] = source.info;
+ serialize[pss_src_version] = source.version;
+ serialize[pss_src_packet][pss_pkt_alignment_bytes] = source.data_alignment_bytes;
+ serialize[pss_src_packet][pss_pkt_definitions] = source.data_definitions;
+ serialize[pss_src_packet][pss_pkt_size_bytes] = source.data_size_bytes;
+
+ writeTag(_stream, TAG_ADD_SOURCE);
+ serialize.serialize(std::ostream_iterator<char>(_stream), true);
+}
+
+
+PacketStreamSourceId PacketStreamWriter::AddSource(PacketStreamSource& source)
+{
+ SCOPED_LOCK;
+ source.id = AddSource(const_cast<const PacketStreamSource&>(source));
+ return source.id;
+}
+
+PacketStreamSourceId PacketStreamWriter::AddSource(const PacketStreamSource& source)
+{
+ SCOPED_LOCK;
+ PacketStreamSourceId r = _sources.size(); //source id is by vector position, so we must reassign.
+ _sources.push_back(source);
+ _sources.back().id = r;
+
+ if (_open) //we might be a pipe, in which case we may not be open
+ Write(_sources.back());
+
+ return _sources.back().id;
+}
+
+void PacketStreamWriter::WriteMeta(PacketStreamSourceId src, const picojson::value& data)
+{
+ SCOPED_LOCK;
+ writeTag(_stream, TAG_SRC_JSON);
+ writeCompressedUnsignedInt(_stream, src);
+ data.serialize(std::ostream_iterator<char>(_stream), false);
+}
+
+void PacketStreamWriter::WriteSourcePacket(PacketStreamSourceId src, const char* source, const int64_t receive_time_us, size_t sourcelen, const picojson::value& meta)
+{
+
+ SCOPED_LOCK;
+ _sources[src].index.push_back({_stream.tellp(), receive_time_us});
+
+ if (!meta.is<picojson::null>())
+ WriteMeta(src, meta);
+
+ writeTag(_stream, TAG_SRC_PACKET);
+ writeTimestamp(_stream, receive_time_us);
+ writeCompressedUnsignedInt(_stream, src);
+
+ if (_sources[src].data_size_bytes) {
+ if (sourcelen != static_cast<size_t>(_sources[src].data_size_bytes))
+ throw std::runtime_error("oPacketStream::writePacket --> Tried to write a fixed-size packet with bad size.");
+ } else {
+ writeCompressedUnsignedInt(_stream, sourcelen);
+ }
+
+ _stream.write(source, sourcelen);
+ _bytes_written += sourcelen;
+}
+
+void PacketStreamWriter::WriteSync()
+{
+ SCOPED_LOCK;
+ for (unsigned i = 0; i < 10; ++i)
+ writeTag(_stream, TAG_PANGO_SYNC);
+}
+
+void PacketStreamWriter::WriteEnd()
+{
+ SCOPED_LOCK;
+ if (!_indexable)
+ return;
+
+ auto indexpos = _stream.tellp();
+ writeTag(_stream, TAG_PANGO_STATS);
+ SourceStats(_sources).serialize(std::ostream_iterator<char>(_stream), false);
+ writeTag(_stream, TAG_PANGO_FOOTER);
+ _stream.write(reinterpret_cast<char*>(&indexpos), sizeof(uint64_t));
+}
+
+}
diff --git a/externals/Pangolin/src/log/playback_session.cpp b/externals/Pangolin/src/log/playback_session.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..97e10bf7d0bf1f481706784bf08eed4583049a00
--- /dev/null
+++ b/externals/Pangolin/src/log/playback_session.cpp
@@ -0,0 +1,26 @@
+#include <pangolin/log/playback_session.h>
+#include <pangolin/utils/params.h>
+
+namespace pangolin {
+
+std::shared_ptr<PlaybackSession> PlaybackSession::Default()
+{
+ static std::shared_ptr<PlaybackSession> instance = std::make_shared<PlaybackSession>();
+ return instance;
+}
+
+std::shared_ptr<PlaybackSession> PlaybackSession::ChooseFromParams(const Params& params)
+{
+ bool use_ordered_playback = params.Get<bool>("OrderedPlayback", false);
+ std::shared_ptr<pangolin::PlaybackSession> playback_session;
+ if(use_ordered_playback)
+ {
+ return Default();
+ }
+ else
+ {
+ return std::make_shared<PlaybackSession>();
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/plot/datalog.cpp b/externals/Pangolin/src/plot/datalog.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..813967d3d0fd17fba7ca47ce99aeb00ff0e5a416
--- /dev/null
+++ b/externals/Pangolin/src/plot/datalog.cpp
@@ -0,0 +1,255 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/plot/datalog.h>
+
+#include <algorithm>
+#include <fstream>
+#include <iomanip>
+#include <iostream>
+#include <limits>
+#include <stdexcept>
+
+namespace pangolin
+{
+
+void DataLogBlock::AddSamples(size_t num_samples, size_t dimensions, const float* data_dim_major )
+{
+ if(nextBlock) {
+ // If next block exists, add to it instead
+ nextBlock->AddSamples(num_samples, dimensions, data_dim_major);
+ }else{
+ if(dimensions > dim) {
+ // If dimensions is too high for this block, start a new bigger one
+ nextBlock = std::unique_ptr<DataLogBlock>(new DataLogBlock(dimensions, max_samples, start_id + samples));
+ nextBlock->AddSamples(num_samples,dimensions,data_dim_major);
+ }else{
+ // Try to copy samples to this block
+ const size_t samples_to_copy = std::min(num_samples, SampleSpaceLeft());
+
+ if(dimensions == dim) {
+ // Copy entire block all together
+ std::copy(data_dim_major, data_dim_major + samples_to_copy*dim, sample_buffer.get()+samples*dim);
+ samples += samples_to_copy;
+ data_dim_major += samples_to_copy*dim;
+ }else{
+ // Copy sample at a time, filling with NaN's where needed.
+ float* dst = sample_buffer.get();
+ for(size_t i=0; i< samples_to_copy; ++i) {
+ std::copy(data_dim_major, data_dim_major + dimensions, dst);
+ for(size_t ii = dimensions; ii < dim; ++ii) {
+ dst[ii] = std::numeric_limits<float>::quiet_NaN();
+ }
+ dst += dimensions;
+ data_dim_major += dimensions;
+ }
+ samples += samples_to_copy;
+ }
+
+// // Update Stats
+// for(size_t s=0; s < samples_to_copy; ++s) {
+// for(size_t d = 0; d < dimensions; ++d) {
+// stats[d].Add(data_dim_major[s*dim + d]);
+// }
+// }
+
+ // Copy remaining data to next block (this one is full)
+ if(samples_to_copy < num_samples) {
+ nextBlock = std::unique_ptr<DataLogBlock>(new DataLogBlock(dim, max_samples, start_id + Samples()));
+ nextBlock->AddSamples(num_samples-samples_to_copy, dimensions, data_dim_major);
+ }
+ }
+ }
+}
+
+DataLog::DataLog(unsigned int buffer_size)
+ : block_samples_alloc(buffer_size), block0(nullptr), blockn(nullptr), record_stats(true)
+{
+}
+
+DataLog::~DataLog()
+{
+ Clear();
+}
+
+void DataLog::SetLabels(const std::vector<std::string> & new_labels)
+{
+ std::lock_guard<std::mutex> l(access_mutex);
+
+ // Create new labels if needed
+ for( size_t i= labels.size(); i < new_labels.size(); ++i )
+ labels.push_back( std::string() );
+
+ // Add data to existing plots
+ for( unsigned int i=0; i<labels.size(); ++i )
+ labels[i] = new_labels[i];
+}
+
+const std::vector<std::string>& DataLog::Labels() const
+{
+ return labels;
+}
+
+void DataLog::Log(size_t dimension, const float* vals, unsigned int samples )
+{
+ if(!block0) {
+ // Create first block
+ block0 = std::unique_ptr<DataLogBlock>(new DataLogBlock(dimension, block_samples_alloc, 0));
+ blockn = block0.get();
+ }
+
+ if(record_stats) {
+ while(stats.size() < dimension) {
+ stats.push_back( DimensionStats() );
+ }
+ for(unsigned int d=0; d<dimension; ++d) {
+ DimensionStats& ds = stats[d];
+ for(unsigned int s=0; s<samples; ++s) {
+ ds.Add(vals[s*dimension+d]);
+ }
+ }
+ }
+
+ blockn->AddSamples(samples,dimension,vals);
+
+ // Update pointer to most recent block.
+ while(blockn->NextBlock()) {
+ blockn = blockn->NextBlock();
+ }
+}
+
+void DataLog::Log(float v)
+{
+ const float vs[] = {v};
+ Log(1,vs);
+}
+
+void DataLog::Log(float v1, float v2)
+{
+ const float vs[] = {v1,v2};
+ Log(2,vs);
+}
+
+void DataLog::Log(float v1, float v2, float v3)
+{
+ const float vs[] = {v1,v2,v3};
+ Log(3,vs);
+}
+void DataLog::Log(float v1, float v2, float v3, float v4)
+{
+ const float vs[] = {v1,v2,v3,v4};
+ Log(4,vs);
+}
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5)
+{
+ const float vs[] = {v1,v2,v3,v4,v5};
+ Log(5,vs);
+}
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5, float v6)
+{
+ const float vs[] = {v1,v2,v3,v4,v5,v6};
+ Log(6,vs);
+}
+
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7)
+{
+ const float vs[] = {v1,v2,v3,v4,v5,v6,v7};
+ Log(7,vs);
+}
+
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8)
+{
+ const float vs[] = {v1,v2,v3,v4,v5,v6,v7,v8};
+ Log(8,vs);
+}
+
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8, float v9)
+{
+ const float vs[] = {v1,v2,v3,v4,v5,v6,v7,v8,v9};
+ Log(9,vs);
+}
+
+void DataLog::Log(float v1, float v2, float v3, float v4, float v5, float v6, float v7, float v8, float v9, float v10)
+{
+ const float vs[] = {v1,v2,v3,v4,v5,v6,v7,v8,v9,v10};
+ Log(10,vs);
+}
+
+void DataLog::Log(const std::vector<float> & vals)
+{
+ Log(vals.size(), &vals[0]);
+}
+
+void DataLog::Clear()
+{
+ std::lock_guard<std::mutex> l(access_mutex);
+
+ blockn = nullptr;
+ block0 = nullptr;
+
+ stats.clear();
+}
+
+void DataLog::Save(std::string /*filename*/)
+{
+ // TODO: Implement
+ throw std::runtime_error("Method not implemented");
+}
+
+const DataLogBlock* DataLog::FirstBlock() const
+{
+ return block0.get();
+}
+
+const DataLogBlock* DataLog::LastBlock() const
+{
+ return blockn;
+}
+
+const DimensionStats& DataLog::Stats(size_t dim) const
+{
+ return stats[dim];
+}
+
+size_t DataLog::Samples() const
+{
+ if(blockn) {
+ return blockn->StartId() + blockn->Samples();
+ }
+ return 0;
+}
+
+const float* DataLog::Sample(int n) const
+{
+ if(block0) {
+ return block0->Sample(n);
+ }else{
+ return 0;
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/plot/plotter.cpp b/externals/Pangolin/src/plot/plotter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..abadd6ac7afb6393f98a9ebf7987e81441e296ac
--- /dev/null
+++ b/externals/Pangolin/src/plot/plotter.cpp
@@ -0,0 +1,1165 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/gl/gldraw.h>
+#include <pangolin/plot/plotter.h>
+
+#include <cctype>
+#include <iomanip>
+
+namespace pangolin
+{
+
+std::string ReplaceChar(const std::string& str, char from, char to)
+{
+ std::string ret = str;
+ for(size_t i=0; i<ret.length(); ++i) {
+ if(ret[i] == from) ret[i] = to;
+ }
+ return ret;
+}
+
+std::set<int> ConvertSequences(const std::string& str, char seq_char='$', char id_char='i')
+{
+ std::set<int> sequences;
+
+ for(size_t i=0; i<str.length(); ++i) {
+ if(str[i] == seq_char) {
+ if(i+1 < str.length() && str[i+1] == id_char) {
+ sequences.insert(-1);
+ }else{
+ int v = 0;
+ for(size_t j=i+1; std::isdigit(str[j]) && j< str.length(); ++j) {
+ v = v*10 + (str[j] - '0');
+ }
+ sequences.insert(v);
+ }
+ }
+ }
+
+ return sequences;
+}
+
+Plotter::PlotSeries::PlotSeries()
+ : log(nullptr), drawing_mode(GL_LINE_STRIP)
+{
+
+}
+
+// X-Y Plot given C-Code style (GLSL) expressions x and y.
+void Plotter::PlotSeries::CreatePlot(const std::string &x, const std::string &y, Colour colour, std::string title)
+{
+ static const std::string vs_header =
+ "uniform float u_id_offset;\n"
+ "uniform vec4 u_color;\n"
+ "uniform vec2 u_scale;\n"
+ "uniform vec2 u_offset;\n"
+ "varying vec4 v_color;\n"
+ "void main() {\n";
+
+ static const std::string vs_footer =
+ " vec2 pos = vec2(x, y);\n"
+ " gl_Position = vec4(u_scale * (pos + u_offset),0,1);\n"
+ " v_color = u_color;\n"
+ "}\n";
+
+ static const std::string fs =
+ #ifdef HAVE_GLES_2
+ "precision mediump float;\n"
+ #endif // HAVE_GLES_2
+ "varying vec4 v_color;\n"
+ "void main() {\n"
+ " gl_FragColor = v_color;\n"
+ "}\n";
+
+ attribs.clear();
+
+ this->colour = colour;
+ this->title = GlFont::I().Text(title.c_str());
+ const std::set<int> ax = ConvertSequences(x);
+ const std::set<int> ay = ConvertSequences(y);
+ std::set<int> as;
+ as.insert(ax.begin(), ax.end());
+ as.insert(ay.begin(), ay.end());
+ contains_id = ( as.find(-1) != as.end() );
+
+ std::ostringstream oss_prog;
+
+ for(std::set<int>::const_iterator i=as.begin(); i != as.end(); ++i) {
+ std::ostringstream oss;
+ oss << "s" << *i;
+ const std::string name = *i >= 0 ? oss.str() : "sn";
+ attribs.push_back( PlotAttrib(name, *i) );
+ oss_prog << "attribute float " + name + ";\n";
+ }
+
+ oss_prog << vs_header;
+ if(contains_id) {
+ oss_prog << "float si = sn + u_id_offset;\n";
+ }
+ oss_prog << "float x = " + ReplaceChar(x,'$','s') + ";\n";
+ oss_prog << "float y = " + ReplaceChar(y,'$','s') + ";\n";
+ oss_prog << vs_footer;
+
+ prog.AddShader( GlSlVertexShader, oss_prog.str() );
+ prog.AddShader( GlSlFragmentShader, fs );
+ prog.Link();
+
+ // Lookup attribute locations in compiled shader
+ prog.SaveBind();
+ for(size_t i=0; i<attribs.size(); ++i) {
+ attribs[i].location = prog.GetAttributeHandle( attribs[i].name );
+ }
+ prog.Unbind();
+}
+
+void Plotter::PlotImplicit::CreatePlot(const std::string& code)
+{
+ static const std::string vs =
+ "attribute vec2 a_position;\n"
+ "uniform vec2 u_scale;\n"
+ "uniform vec2 u_offset;\n"
+ "varying float x;\n"
+ "varying float y;\n"
+ "void main() {\n"
+ " gl_Position = vec4(u_scale * (a_position + u_offset),0,1);\n"
+ " x = a_position.x;"
+ " y = a_position.y;"
+ "}\n";
+
+ static const std::string fs1 =
+ #ifdef HAVE_GLES_2
+ "precision mediump float;\n"
+ #endif // HAVE_GLES_2
+ "varying float x;\n"
+ "varying float y;\n"
+ "void main() {\n";
+ static const std::string fs2 =
+ " gl_FragColor = z;\n"
+ "}\n";
+
+ prog.AddShader( GlSlVertexShader, vs );
+ prog.AddShader( GlSlFragmentShader, fs1 + code + fs2 );
+ prog.BindPangolinDefaultAttribLocationsAndLink();
+}
+
+
+void Plotter::PlotImplicit::CreateColouredPlot(const std::string& code)
+{
+ CreatePlot(
+ " float r=1.0;\n"
+ " float g=1.0;\n"
+ " float b=1.0;\n"
+ " float a=0.5;\n" +
+ code +
+ " z = vec4(r,g,b,a);\n"
+ );
+}
+
+void Plotter::PlotImplicit::CreateInequality(const std::string& ie, Colour c)
+{
+ std::ostringstream oss;
+ oss << std::fixed << std::setprecision(1);
+ oss << "if( !(" << ie << ") ) discard;\n";
+ oss << "z = vec4(" << c.r << "," << c.g << "," << c.b << "," << c.a << ");\n";
+
+ CreatePlot( oss.str() );
+}
+
+void Plotter::PlotImplicit::CreateDistancePlot(const std::string& /*dist*/)
+{
+
+}
+
+Plotter::Plotter(
+ DataLog* log,
+ float left, float right, float bottom, float top,
+ float tickx, float ticky,
+ Plotter* linked_plotter_x,
+ Plotter* linked_plotter_y
+) : default_log(log),
+ colour_wheel(0.6f),
+ rview_default(left,right,bottom,top), rview(rview_default), target(rview),
+ selection(0,0,0,0),
+ track(false), track_x("$i"), track_y(""),
+ trigger_edge(0), trigger("$0"),
+ linked_plotter_x(linked_plotter_x),
+ linked_plotter_y(linked_plotter_y)
+{
+ // Prevent links to ourselves - this could cause infinite recursion.
+ if(linked_plotter_x == this) this->linked_plotter_x = 0;
+ if(linked_plotter_y == this) this->linked_plotter_y = 0;
+
+ // Handle our own mouse / keyboard events
+ this->handler = this;
+ hover[0] = 0;
+ hover[1] = 0;
+
+ // Default colour scheme
+ colour_bg = Colour(0.0f, 0.0f, 0.0f);
+ colour_tk = Colour(0.2f, 0.2f, 0.2f);
+ colour_ax = Colour(0.5f, 0.5f, 0.5f);
+
+ SetTicks(tickx, ticky);
+
+ // Create shader for drawing simple primitives
+ prog_lines.AddShader( GlSlVertexShader,
+ "attribute vec2 a_position;\n"
+ "uniform vec4 u_color;\n"
+ "uniform vec2 u_scale;\n"
+ "uniform vec2 u_offset;\n"
+ "varying vec4 v_color;\n"
+ "void main() {\n"
+ " gl_Position = vec4(u_scale * (a_position + u_offset),0,1);\n"
+ " v_color = u_color;\n"
+ "}\n"
+ );
+ prog_lines.AddShader( GlSlFragmentShader,
+ #ifdef HAVE_GLES_2
+ "precision mediump float;\n"
+ #endif // HAVE_GLES_2
+ "varying vec4 v_color;\n"
+ "void main() {\n"
+ " gl_FragColor = v_color;\n"
+ "}\n"
+ );
+ prog_lines.BindPangolinDefaultAttribLocationsAndLink();
+
+ prog_text.AddShader( GlSlVertexShader,
+ "attribute vec2 a_position;\n"
+ "attribute vec2 a_texcoord;\n"
+ "uniform vec4 u_color;\n"
+ "uniform vec2 u_scale;\n"
+ "uniform vec2 u_offset;\n"
+ "varying vec4 v_color;\n"
+ "varying vec2 v_texcoord;\n"
+ "void main() {\n"
+ " gl_Position = vec4(u_scale * (a_position + u_offset),0,1);\n"
+ " v_color = u_color;\n"
+ " v_texcoord = a_texcoord;\n"
+ "}\n"
+ );
+ prog_text.AddShader( GlSlFragmentShader,
+ #ifdef HAVE_GLES_2
+ "precision mediump float;\n"
+ #endif // HAVE_GLES_2
+ "varying vec4 v_color;\n"
+ "varying vec2 v_texcoord;\n"
+ "uniform sampler2D u_texture;\n"
+ "void main() {\n"
+ " gl_FragColor = v_color;\n"
+ " gl_FragColor.a *= texture2D(u_texture, v_texcoord).a;\n"
+ "}\n"
+ );
+ prog_text.BindPangolinDefaultAttribLocationsAndLink();
+
+ const size_t RESERVED_SIZE = 100;
+
+ // Setup default PlotSeries
+ plotseries.reserve(RESERVED_SIZE);
+ for(unsigned int i=0; i< 10; ++i) {
+ std::ostringstream ss;
+ ss << "$" << i;
+ if(log) AddSeries("$i", ss.str());
+ }
+
+ // Setup test PlotMarkers
+ plotmarkers.reserve(RESERVED_SIZE);
+// plotmarkers.push_back( Marker( Marker::Vertical, 10, Marker::GreaterThan, Colour(1,0,0,0.2)) );
+// plotmarkers.push_back( Marker( Marker::Horizontal, 1, Marker::LessThan, Colour(0,1,0,0.2)) );
+
+ // Setup test implicit plots.
+ plotimplicits.reserve(RESERVED_SIZE);
+// plotimplicits.push_back( PlotImplicit() );
+// plotimplicits.back().CreateInequality("x+y <= 150.0", colour_wheel.GetUniqueColour().WithAlpha(0.2) );
+// plotimplicits.push_back( PlotImplicit() );
+// plotimplicits.back().CreateInequality("x+2.0*y <= 170.0", colour_wheel.GetUniqueColour().WithAlpha(0.2) );
+// plotimplicits.push_back( PlotImplicit() );
+// plotimplicits.back().CreateInequality("3.0*y <= 180.0", colour_wheel.GetUniqueColour().WithAlpha(0.2) );
+
+ // Setup texture spectogram style plots
+ // ...
+
+}
+
+Plotter::~Plotter()
+{
+
+}
+
+template <typename T> int data_sgn(T val) {
+ return (T(0) < val) - (val < T(0));
+}
+
+void Plotter::ComputeTrackValue( float track_val[2] )
+{
+ if(trigger_edge) {
+ // Track last edge transition matching trigger_edge
+ const DataLogBlock* block = default_log->LastBlock();
+ if(block) {
+ int s = (int)block->StartId() + (int)block->Samples() - 1;
+ const size_t dim = block->Dimensions();
+ const float* data = block->Sample(s);
+ int last_sgn = 0;
+ for(; s >= 0; --s, data -= dim )
+ {
+ const float val = data[0] - trigger_value;
+ const int sgn = data_sgn(val);
+ if(last_sgn * sgn == -1 && last_sgn == trigger_edge) {
+ track_val[0] = (float)s;
+ track_val[1] = 0.0f;
+ return;
+ }
+ last_sgn = sgn;
+ }
+ }
+ // Fall back to simple last value tracking
+ }
+
+ track_val[0] = (float)default_log->Samples();
+ track_val[1] = 0.0f;
+}
+
+XYRangef Plotter::ComputeAutoSelection()
+{
+ XYRangef range;
+ range.x = target.x;
+
+ const DataLogBlock* block = default_log->FirstBlock();
+
+ if(block) {
+ for(size_t i=0; i < plotseries.size(); ++i)
+ {
+ if( plotseries[i].attribs.size() == 2 && plotseries[i].attribs[0].plot_id == -1) {
+ const int id = plotseries[i].attribs[1].plot_id;
+ if( 0<= id && id < (int)block->Dimensions()) {
+ range.y.Insert(default_log->Stats(id).min);
+ range.y.Insert(default_log->Stats(id).max);
+ }
+ }
+
+ }
+ }
+
+ return range;
+}
+
+void Plotter::Render()
+{
+ // Animate scroll / zooming
+ UpdateView();
+
+#ifndef HAVE_GLES
+ glPushAttrib(GL_ENABLE_BIT | GL_COLOR_BUFFER_BIT | GL_LINE_BIT);
+#endif
+
+ glClearColor(colour_bg.r, colour_bg.g, colour_bg.b, colour_bg.a);
+ ActivateAndScissor();
+
+ if(std::isfinite(colour_bg.a)) {
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+ }
+
+ if(extern_draw_function && show) {
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ glOrtho(rview.x.min, rview.x.max, rview.y.max, rview.y.min, -1, 1);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+
+ extern_draw_function(*this);
+ }
+
+ // Try to create smooth lines
+ glDisable(GL_MULTISAMPLE);
+ glLineWidth(1.5);
+ glEnable(GL_LINE_SMOOTH);
+ glHint( GL_LINE_SMOOTH_HINT, GL_NICEST );
+ glEnable (GL_BLEND);
+ glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+ glDisable(GL_LIGHTING);
+ glDisable( GL_DEPTH_TEST );
+
+ const float w = rview.x.AbsSize();
+ const float h = rview.y.AbsSize();
+ const float ox = -rview.x.Mid();
+ const float oy = -rview.y.Mid();
+ const float sx = 2.0f / w;
+ const float sy = 2.0f / h;
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw ticks
+ prog_lines.SaveBind();
+ prog_lines.SetUniform("u_scale", sx, sy);
+ prog_lines.SetUniform("u_offset", ox, oy);
+ prog_lines.SetUniform("u_color", colour_tk );
+
+ const float min_space = 80.0;
+ float ta[2] = {1,1};
+
+ while(true)
+ {
+ const float interval = v.w * tick[0].val / w;
+
+ if(ta[0] * interval < min_space)
+ ta[0] *=2;
+ if(ta[0] * interval < min_space)
+ ta[0] =(5*ta[0])/2;
+ if(ta[0] * interval < min_space)
+ ta[0] *=2;
+ else
+ break;
+ }
+ while(true)
+ {
+ const float interval = v.h * tick[1].val / h;
+
+ if(ta[1] * interval < min_space)
+ ta[1] *=2;
+ if(ta[1] * interval < min_space)
+ ta[1] =(5*ta[1])/2;
+ if(ta[1] * interval < min_space)
+ ta[1] *=2;
+ else
+ break;
+ }
+
+ const float tdelta[2] = {
+ tick[0].val * ta[0],
+ tick[1].val * ta[1]
+ };
+
+ const int tx[2] = {
+ (int)ceil(rview.x.min / tdelta[0]),
+ (int)ceil(rview.x.max / tdelta[0])
+ };
+
+ const int ty[2] = {
+ (int)ceil(rview.y.min / tdelta[1]),
+ (int)ceil(rview.y.max / tdelta[1])
+ };
+
+ for( int i=tx[0]; i<tx[1]; ++i ) {
+ glDrawLine((i)*tdelta[0], rview.y.min, (i)*tdelta[0], rview.y.max);
+ }
+
+ for( int i=ty[0]; i<ty[1]; ++i ) {
+ glDrawLine(rview.x.min, (i)*tdelta[1], rview.x.max, (i)*tdelta[1]);
+ }
+ prog_lines.Unbind();
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw axis
+
+ prog_lines.SaveBind();
+ prog_lines.SetUniform("u_color", colour_ax );
+ glDrawLine(0, rview.y.min, 0, rview.y.max );
+ glDrawLine(rview.x.min,0, rview.x.max,0 );
+ prog_lines.Unbind();
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw Implicits
+
+ for(size_t i=0; i < plotimplicits.size(); ++i) {
+ PlotImplicit& im = plotimplicits[i];
+ im.prog.SaveBind();
+
+ im.prog.SetUniform("u_scale", sx, sy);
+ im.prog.SetUniform("u_offset", ox, oy);
+
+ glDrawRect(rview.x.min,rview.y.min,rview.x.max,rview.y.max);
+
+ im.prog.Unbind();
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw series
+
+ static size_t id_size = 0;
+ static float* id_array = 0;
+
+ for(size_t i=0; i < plotseries.size(); ++i)
+ {
+ PlotSeries& ps = plotseries[i];
+
+ if(ps.drawing_mode != pangolin::DrawingModeNone)
+ {
+ GlSlProgram& prog = ps.prog;
+ ps.used = false;
+
+ prog.SaveBind();
+ prog.SetUniform("u_scale", sx, sy);
+ prog.SetUniform("u_offset", ox, oy);
+ prog.SetUniform("u_color", ps.colour );
+
+ // TODO: Try to skip drawing of blocks which aren't in view.
+ DataLog* log = ps.log ? ps.log : default_log;
+ std::lock_guard<std::mutex> l(log->access_mutex);
+
+ const DataLogBlock* block = log->FirstBlock();
+ while(block) {
+ if(ps.contains_id ) {
+ if(id_size < block->Samples() ) {
+ // Create index array that we can bind
+ delete[] id_array;
+ id_size = block->MaxSamples();
+ id_array = new float[id_size];
+ for(size_t k=0; k < id_size; ++k) {
+ id_array[k] = (float)k;
+ }
+ }
+ prog.SetUniform("u_id_offset", (float)block->StartId() );
+ }
+
+ // Enable appropriate attributes
+ bool shouldRender = true;
+ for(size_t i=0; i< ps.attribs.size(); ++i) {
+ if(0 <= ps.attribs[i].plot_id && ps.attribs[i].plot_id < (int)block->Dimensions() ) {
+ glVertexAttribPointer(ps.attribs[i].location, 1, GL_FLOAT, GL_FALSE, (GLsizei)(block->Dimensions()*sizeof(float)), block->DimData(ps.attribs[i].plot_id) );
+ glEnableVertexAttribArray(ps.attribs[i].location);
+ }else if( ps.attribs[i].plot_id == -1 ){
+ glVertexAttribPointer(ps.attribs[i].location, 1, GL_FLOAT, GL_FALSE, 0, id_array );
+ glEnableVertexAttribArray(ps.attribs[i].location);
+ }else{
+ // bad id: don't render
+ shouldRender = false;
+ break;
+ }
+ }
+
+ if(shouldRender) {
+ // Draw geometry
+ glDrawArrays(ps.drawing_mode, 0, (GLsizei)block->Samples());
+ ps.used = true;
+ }
+
+ // Disable enabled attributes
+ for(size_t i=0; i< ps.attribs.size(); ++i) {
+ glDisableVertexAttribArray(ps.attribs[i].location);
+ }
+
+ block = block->NextBlock();
+ }
+ prog.Unbind();
+ }
+ }
+
+ prog_lines.SaveBind();
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw markers
+ glLineWidth(2.5f);
+
+ for( size_t i=0; i < plotmarkers.size(); ++i) {
+ const Marker& m = plotmarkers[i];
+
+ XYRangef draw_range = m.range;
+ draw_range.Clamp(rview);
+
+ prog_lines.SetUniform("u_color", m.colour );
+ if(draw_range.x.Size() == 0.0 || draw_range.y.Size() == 0.0) {
+ // Horizontal or Vertical line
+ glDrawLine(draw_range.x.min, draw_range.y.min, draw_range.x.max, draw_range.y.max );
+ }else{
+ // Region
+ glDrawRect(draw_range.x.min, draw_range.y.min, draw_range.x.max, draw_range.y.max );
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw hover / selection
+ glLineWidth(1.5f);
+
+ // hover over
+ prog_lines.SetUniform("u_color", colour_ax.WithAlpha(0.3f) );
+ glDrawLine(hover[0], rview.y.min, hover[0], rview.y.max );
+ glDrawLine(rview.x.min, hover[1], rview.x.max, hover[1] );
+
+ // range
+ prog_lines.SetUniform("u_color", colour_ax.WithAlpha(0.5) );
+ glDrawLine(selection.x.min, rview.y.min, selection.x.min, rview.y.max );
+ glDrawLine(selection.x.max, rview.y.min, selection.x.max, rview.y.max );
+ glDrawLine(rview.x.min, selection.y.min, rview.x.max, selection.y.min );
+ glDrawLine(rview.x.min, selection.y.max, rview.x.max, selection.y.max );
+ glDrawRect(selection.x.min, selection.y.min, selection.x.max, selection.y.max);
+
+ prog_lines.Unbind();
+
+ prog_text.SaveBind();
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw Key
+
+ prog_text.SetUniform("u_scale", 2.0f / v.w, 2.0f / v.h);
+
+ int keyid = 0;
+ for(size_t i=0; i < plotseries.size(); ++i)
+ {
+ PlotSeries& ps = plotseries[i];
+ if(ps.used && ps.drawing_mode != pangolin::DrawingModeNone) {
+ prog_text.SetUniform("u_color", ps.colour );
+ prog_text.SetUniform("u_offset",
+ v.w-5-ps.title.Width() -(v.w/2.0f),
+ v.h-1.5*ps.title.Height()*(++keyid) -(v.h/2.0f)
+ );
+ ps.title.DrawGlSl();
+ }
+ }
+
+ //////////////////////////////////////////////////////////////////////////
+ // Draw axis text
+
+ prog_text.SetUniform("u_scale", 2.0f / v.w, 2.0f / v.h);
+ prog_text.SetUniform("u_color", colour_ax );
+
+ for( int i=tx[0]; i<tx[1]; ++i ) {
+ std::ostringstream oss;
+ oss << i*tdelta[0]*tick[0].factor << tick[0].symbol;
+ GlText txt = GlFont::I().Text(oss.str().c_str());
+ float sx = v.w*((i)*tdelta[0]-rview.x.Mid())/w - txt.Width()/2.0f;
+ prog_text.SetUniform("u_offset", sx, 15 -v.h/2.0f );
+ txt.DrawGlSl();
+ }
+
+ for( int i=ty[0]; i<ty[1]; ++i ) {
+ std::ostringstream oss;
+ oss << i*tdelta[1]*tick[1].factor << tick[1].symbol;
+ GlText txt = GlFont::I().Text(oss.str().c_str());
+ float sy = v.h*((i)*tdelta[1]-rview.y.Mid())/h - txt.Height()/2.0f;
+ prog_text.SetUniform("u_offset", 15 -v.w/2.0f, sy );
+ txt.DrawGlSl();
+ }
+
+ prog_text.Unbind();
+
+
+ glLineWidth(1.0f);
+
+#ifndef HAVE_GLES
+ glPopAttrib();
+#endif
+
+}
+
+Plotter::Tick Plotter::FindTickFactor(float tick)
+{
+ Plotter::Tick ret;
+ ret.val = tick;
+
+ const float eps = 1E-6f;
+
+ if( std::abs(tick/M_PI - floor(tick/M_PI)) < eps ) {
+ ret.factor = 1.0f / (float)M_PI;
+ ret.symbol = "pi";
+ }else if( std::abs(tick/M_PI_2 - floor(tick/M_PI_2)) < eps ) {
+ ret.factor = 1.0f / (float)M_PI;
+ ret.symbol = "pi";
+ }else if( std::abs(tick/M_PI_4 - floor(tick/M_PI_4)) < eps ) {
+ ret.factor = 1.0f / (float)M_PI;
+ ret.symbol = "pi";
+ }else if( std::abs(tick/M_SQRT2 - floor(tick/M_SQRT2)) < eps ) {
+ ret.factor = 1.0f / (float)M_SQRT2;
+ ret.symbol = "\251 2";
+ }else if( std::abs(tick/M_E - floor(tick/M_E)) < eps ) {
+ ret.factor = 1.0f / (float)M_E;
+ ret.symbol = "e";
+ }else{
+ ret.factor = 1.0f;
+ ret.symbol = "";
+ }
+ return ret;
+}
+
+void Plotter::SetTicks(float tickx, float ticky)
+{
+ // Compute tick_factor, tick_label
+ tick[0] = FindTickFactor(tickx);
+ tick[1] = FindTickFactor(ticky);
+}
+
+void Plotter::Track(const std::string& x, const std::string& y)
+{
+ Plotter& p = linked_plotter_x ? *linked_plotter_x :
+ (linked_plotter_y ? *linked_plotter_y : *this);
+
+ if( x != "$i" || y != "") {
+ throw std::runtime_error("Track option not fully implemented");
+ }
+
+ p.track_x = x;
+ p.track_y = y;
+ p.track = !p.track_x.empty() || !p.track_y.empty();
+ p.ComputeTrackValue(p.last_track_val);
+}
+
+void Plotter::ToggleTracking()
+{
+ Plotter& p = linked_plotter_x ? *linked_plotter_x :
+ (linked_plotter_y ? *linked_plotter_y : *this);
+
+ p.track = !p.track;
+ p.ComputeTrackValue(p.last_track_val);
+}
+
+void Plotter::Trigger(const std::string& x, int edge, float value)
+{
+ if( x != "$0") {
+ throw std::runtime_error("Trigger option not fully implemented");
+ }
+
+ trigger = x;
+ trigger_edge = edge;
+ trigger_value = value;
+ ComputeTrackValue(last_track_val);
+}
+
+void Plotter::ToggleTrigger()
+{
+ trigger_edge = trigger_edge ? 0 : -1;
+ ComputeTrackValue(last_track_val);
+}
+
+void Plotter::SetBackgroundColour(const Colour& col)
+{
+ colour_bg = col;
+}
+
+void Plotter::SetAxisColour(const Colour& col)
+{
+ colour_ax = col;
+}
+
+void Plotter::SetTickColour(const Colour& col)
+{
+ colour_tk = col;
+}
+
+XYRangef& Plotter::GetView()
+{
+ return target;
+}
+
+XYRangef& Plotter::GetDefaultView()
+{
+ return rview_default;
+}
+
+XYRangef& Plotter::GetSelection()
+{
+ return selection;
+}
+
+void Plotter::FixSelection()
+{
+ // Make sure selection matches sign of current viewport
+ if( (selection.x.min<selection.x.max) != (rview.x.min<rview.x.max) ) {
+ std::swap(selection.x.min, selection.x.max);
+ }
+ if( (selection.y.min<selection.y.max) != (rview.y.min<rview.y.max) ) {
+ std::swap(selection.y.min, selection.y.max);
+ }
+}
+
+void Plotter::UpdateView()
+{
+ // Track value based on last log sample
+ if( (track || trigger_edge) && !(linked_plotter_x || linked_plotter_y) ) {
+ float newTrackVal[2];
+ ComputeTrackValue(newTrackVal);
+ if(target.x.max <= newTrackVal[0] ) {
+ ScrollView(newTrackVal[0]-last_track_val[0], newTrackVal[1]-last_track_val[1] );
+ }
+ last_track_val[0] = newTrackVal[0];
+ last_track_val[1] = newTrackVal[1];
+ }
+
+ const float sf = 1.0f / 20.0f;
+// XYRange d = target - rview;
+// rview += d * sf;
+
+ if( linked_plotter_x ) {
+ // Synchronise rview and target with linked plotter
+ rview.x = linked_plotter_x->rview.x;
+ target.x = linked_plotter_x->target.x;
+ }else{
+ // Animate view window toward target
+ Rangef d = target.x - rview.x;
+ rview.x += d * sf;
+ }
+
+ if( linked_plotter_y ) {
+ // Synchronise rview and target with linked plotter
+ rview.y = linked_plotter_y->rview.y;
+ target.y = linked_plotter_y->target.y;
+ }else{
+ // Animate view window toward target
+ Rangef d = target.y - rview.y;
+ rview.y += d * sf;
+ }
+}
+
+void Plotter::SetView(const XYRangef& range)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.rview.x = range.x;
+ py.rview.y = range.y;
+}
+
+void Plotter::SetViewSmooth(const XYRangef &range)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x = range.x;
+ py.target.y = range.y;
+}
+
+void Plotter::SetDefaultView(const XYRangef &range)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.rview_default.x = range.x;
+ py.rview_default.y = range.y;
+}
+
+void Plotter::ScrollView(float x, float y)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x += x;
+ py.target.y += y;
+ px.rview.x += x;
+ py.rview.y += y;
+}
+
+void Plotter::ScrollViewSmooth(float x, float y)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x += x;
+ py.target.y += y;
+}
+
+void Plotter::ScaleView(float x, float y, float cx, float cy)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x.Scale(x, cx);
+ py.target.y.Scale(y, cy);
+ px.rview.x.Scale(x, cx);
+ py.rview.y.Scale(y, cy);
+}
+
+void Plotter::ScaleViewSmooth(float x, float y, float cx, float cy)
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x.Scale(x,cx);
+ py.target.y.Scale(y,cy);
+}
+
+void Plotter::ResetView()
+{
+ Plotter& px = linked_plotter_x ? *linked_plotter_x : *this;
+ Plotter& py = linked_plotter_y ? *linked_plotter_y : *this;
+
+ px.target.x = px.rview_default.x;
+ py.target.y = py.rview_default.y;
+}
+
+void Plotter::Keyboard(View&, unsigned char key, int /*x*/, int /*y*/, bool pressed)
+{
+ const float mvfactor = 1.0f / 10.0f;
+
+ const float c[2] = {
+ track || trigger_edge ? target.x.max : rview.x.Mid(),
+ rview.y.Mid()
+ };
+
+ if(pressed) {
+ if(key == ' ') {
+ if( selection.Area() <= 0.0f) {
+ // Work out Auto zoom selection bounds
+ selection = ComputeAutoSelection();
+ }
+
+ if( selection.Area() > 0.0f) {
+ // Set view to equal selection
+ SetViewSmooth(selection);
+
+ // Reset selection
+ selection.x.max = selection.x.min;
+ selection.y.max = selection.y.min;
+ }
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_LEFT) {
+ const float w = rview.x.Size();
+ const float dx = mvfactor*w;
+ ScrollViewSmooth(-dx, 0);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_RIGHT) {
+ const float w = rview.x.Size();
+ const float dx = mvfactor*w;
+ ScrollViewSmooth(+dx, 0);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_DOWN) {
+ const float h = target.y.Size();
+ const float dy = mvfactor*h;
+ ScrollViewSmooth(0, -dy);
+ }else if(key == PANGO_SPECIAL + PANGO_KEY_UP) {
+ const float h = target.y.Size();
+ const float dy = mvfactor*h;
+ ScrollViewSmooth(0, +dy);
+ }else if(key == '=') {
+ ScaleViewSmooth(0.5, 0.5, c[0], c[1]);
+ }else if(key == '-') {
+ ScaleViewSmooth(2.0, 2.0, c[0], c[1]);
+ }else if(key == 'r') {
+ ResetView();
+ }else if(key == 't') {
+ ToggleTracking();
+ }else if(key == 'e') {
+ ToggleTrigger();
+ }else if('1' <= key && key <= '9') {
+ const size_t id = key - '1';
+ if(id < plotseries.size()) {
+ PlotSeries& s = plotseries[id];
+ if(s.drawing_mode == DrawingModeNone) {
+ s.drawing_mode = DrawingModePoints;
+ }else{
+ ++s.drawing_mode;
+ if(s.drawing_mode == GL_LINE_LOOP) {
+ ++s.drawing_mode;
+ }
+ }
+ }
+ }
+ }
+}
+
+void Plotter::ScreenToPlot(int xpix, int ypix, float& xplot, float& yplot)
+{
+ xplot = rview.x.min + rview.x.Size() * (xpix - v.l) / (float)v.w;
+ yplot = rview.y.min + rview.y.Size() * (ypix - v.b) / (float)v.h;
+}
+
+void Plotter::Mouse(View& /*view*/, MouseButton button, int x, int y, bool pressed, int button_state)
+{
+ // Update hover status (after potential resizing)
+ ScreenToPlot(x, y, hover[0], hover[1]);
+
+ const float scinc = 1.05f;
+ const float scdec = 1.0f/scinc;
+
+ const float c[2] = {
+ track || trigger_edge ? last_track_val[0] : hover[0],
+ hover[1]
+ };
+
+ if(button_state & KeyModifierShift) {
+ if(button == MouseWheelUp) {
+ ScaleViewSmooth(1.0f, scinc, c[0], c[1]);
+ }else if(button == MouseWheelDown) {
+ ScaleViewSmooth(1.0f, scdec, c[0], c[1]);
+ }else if(button == MouseWheelLeft) {
+ ScaleViewSmooth(scinc, 1.0f, c[0], c[1]);
+ }else if(button == MouseWheelRight) {
+ ScaleViewSmooth(scdec, 1.0f, c[0], c[1]);
+ }
+ }else if(button_state & KeyModifierCtrl) {
+ if(button == MouseWheelUp) {
+ ScaleViewSmooth(scinc, 1.0f, c[0], c[1]);
+ }else if(button == MouseWheelDown) {
+ ScaleViewSmooth(scdec, 1.0f, c[0], c[1]);
+ }
+ }else{
+ const float mvfactor = 1.0f/20.0f;
+
+ if(button == MouseButtonLeft) {
+ // Update selected range
+ if(pressed) {
+ selection.x.min = hover[0];
+ selection.y.min = hover[1];
+ trigger_value = selection.y.min;
+ }
+ selection.x.max = hover[0];
+ selection.y.max = hover[1];
+ }else if(button == MouseWheelUp) {
+ ScrollViewSmooth(0.0f, +mvfactor*rview.y.Size() );
+ }else if(button == MouseWheelDown) {
+ ScrollViewSmooth(0.0f, -mvfactor*rview.y.Size() );
+ }else if(button == MouseWheelLeft) {
+ ScrollViewSmooth(+mvfactor*rview.x.Size(), 0.0f );
+ }else if(button == MouseWheelRight) {
+ ScrollViewSmooth(-mvfactor*rview.x.Size(), 0.0f );
+ }
+ }
+
+ FixSelection();
+
+ last_mouse_pos[0] = x;
+ last_mouse_pos[1] = y;
+}
+
+void Plotter::MouseMotion(View& view, int x, int y, int button_state)
+{
+ const int d[2] = {x-last_mouse_pos[0], y-last_mouse_pos[1]};
+ const float is[2] = {rview.x.Size(), rview.y.Size() };
+ const float df[2] = {is[0]*d[0]/(float)v.w, is[1]*d[1]/(float)v.h};
+
+ // Update hover status (after potential resizing)
+ ScreenToPlot(x, y, hover[0], hover[1]);
+
+ if( button_state == MouseButtonLeft )
+ {
+ // Update selected range
+ selection.x.max = hover[0];
+ selection.y.max = hover[1];
+ }else if(button_state == MouseButtonMiddle )
+ {
+ Special(view, InputSpecialScroll, df[0], df[1], 0.0f, 0.0f, 0.0f, 0.0f, button_state);
+ }else if(button_state == MouseButtonRight )
+ {
+ const float c[2] = {
+ track || trigger_edge ? last_track_val[0] : hover[0],
+ hover[1]
+ };
+
+ const float scale[2] = {
+ 1.0f + (float)d[0] / (float)v.w,
+ 1.0f - (float)d[1] / (float)v.h,
+ };
+ ScaleView(scale[0], scale[1], c[0], c[1]);
+ }
+
+ last_mouse_pos[0] = x;
+ last_mouse_pos[1] = y;
+}
+
+void Plotter::PassiveMouseMotion(View&, int x, int y, int /*button_state*/)
+{
+ ScreenToPlot(x, y, hover[0], hover[1]);
+}
+
+void Plotter::Special(View&, InputSpecial inType, float x, float y, float p1, float p2, float /*p3*/, float /*p4*/, int button_state)
+{
+ if(inType == InputSpecialScroll) {
+ const float d[2] = {p1,-p2};
+ const float is[2] = {rview.x.Size(),rview.y.Size() };
+ const float df[2] = {is[0]*d[0]/(float)v.w, is[1]*d[1]/(float)v.h};
+
+ ScrollView(-df[0], -df[1]);
+ } else if(inType == InputSpecialZoom) {
+ float scalex = 1.0;
+ float scaley = 1.0;
+
+#ifdef _OSX_
+ if (button_state & KeyModifierCmd) {
+#else
+ if (button_state & KeyModifierCtrl) {
+#endif
+ scalex = 1-p1;
+ }else{
+ scaley = 1-p1;
+ }
+
+ const float c[2] = {
+ track || trigger_edge ? last_track_val[0] : hover[0],
+ hover[1]
+ };
+
+ ScaleView(scalex, scaley, c[0], c[1]);
+ }
+
+ // Update hover status (after potential resizing)
+ ScreenToPlot( (int)x, (int)y, hover[0], hover[1]);
+}
+
+void Plotter::AddSeries(const std::string& x_expr, const std::string& y_expr,
+ DrawingMode drawing_mode, Colour colour,
+ const std::string& title, DataLog *log)
+{
+ if( !std::isfinite(colour.r) ) {
+ colour = colour_wheel.GetUniqueColour();
+ }
+ plotseries.push_back( PlotSeries() );
+ plotseries.back().CreatePlot(x_expr, y_expr, colour, (title == "$y") ? PlotTitleFromExpr(y_expr) : title);
+ plotseries.back().log = log;
+ plotseries.back().drawing_mode = (GLenum)drawing_mode;
+}
+
+std::string Plotter::PlotTitleFromExpr(const std::string& expr) const
+{
+ const std::vector<std::string>& labels = default_log->Labels();
+
+ std::stringstream exp_in(expr);
+ std::stringstream exp_out;
+ while(exp_in) {
+ int c = exp_in.get();
+ if(c=='$') {
+ size_t id = -1;
+ exp_in >> id;
+ if(id < labels.size()) {
+ exp_out << '\'' << labels[id] << '\'';
+ }else{
+ exp_out << '$' << id;
+ }
+ }else{
+ exp_out << (char)c;
+ }
+ }
+
+ return exp_out.str();
+}
+
+void Plotter::ClearSeries()
+{
+ plotseries.clear();
+}
+
+Marker& Plotter::AddMarker(Marker::Direction d, float value, Marker::Equality leg, Colour c )
+{
+ return AddMarker(Marker(d,value,leg,c));
+}
+
+Marker& Plotter::AddMarker( const Marker& marker )
+{
+ plotmarkers.push_back( marker );
+ return plotmarkers.back();
+}
+
+void Plotter::ClearMarkers()
+{
+ plotmarkers.clear();
+}
+
+
+}
diff --git a/externals/Pangolin/src/tools/video_viewer.cpp b/externals/Pangolin/src/tools/video_viewer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..17ed0bdf6407621757fc2ed825dad6031791fee0
--- /dev/null
+++ b/externals/Pangolin/src/tools/video_viewer.cpp
@@ -0,0 +1,449 @@
+#include <pangolin/tools/video_viewer.h>
+
+#include <pangolin/display/image_view.h>
+#include <pangolin/gl/glpixformat.h>
+#include <pangolin/gl/gltexturecache.h>
+#include <pangolin/handler/handler_image.h>
+#include <pangolin/pangolin.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/sigstate.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/video/video_input.h>
+
+
+namespace pangolin
+{
+
+void videoviewer_signal_quit(int) {
+ pango_print_info("Caught signal. Program will exit after any IO is complete.\n");
+ pangolin::QuitAll();
+}
+
+VideoViewer::VideoViewer(const std::string& window_name, const std::string& input_uri, const std::string& output_uri)
+ : window_name(window_name),
+ video_playback(nullptr),
+ video_interface(nullptr),
+ output_uri(output_uri),
+ current_frame(-1),
+ grab_until(std::numeric_limits<int>::max()),
+ record_nth_frame(1),
+ draw_nth_frame(1),
+ video_grab_wait(true),
+ video_grab_newest(false),
+ should_run(true),
+ active_cam(0)
+{
+ pangolin::Var<int>::Attach("ui.frame", current_frame);
+ pangolin::Var<int>::Attach("ui.record_nth_frame", record_nth_frame);
+ pangolin::Var<int>::Attach("ui.draw_nth_frame", draw_nth_frame);
+
+
+ if(!input_uri.empty()) {
+ OpenInput(input_uri);
+ }
+}
+
+VideoViewer::~VideoViewer()
+{
+ QuitAndWait();
+
+}
+
+void VideoViewer::Quit()
+{
+ // Signal any running thread to stop
+ should_run = false;
+}
+
+void VideoViewer::QuitAndWait()
+{
+ Quit();
+
+ if(vv_thread.joinable()) {
+ vv_thread.join();
+ }
+}
+
+void VideoViewer::RunAsync()
+{
+ if(!should_run) {
+ // Make sure any other thread has finished
+ if(vv_thread.joinable()) {
+ vv_thread.join();
+ }
+
+ // Launch in another thread
+ vv_thread = std::thread(&VideoViewer::Run, this);
+ }
+}
+
+void VideoViewer::Run()
+{
+ should_run = true;
+
+ /////////////////////////////////////////////////////////////////////////
+ /// Register pangolin variables
+ /////////////////////////////////////////////////////////////////////////
+
+ std::unique_ptr<unsigned char[]> buffer(new unsigned char[video.SizeBytes()+1]);
+
+ const int slider_size = (TotalFrames() < std::numeric_limits<int>::max() ? 20 : 0);
+
+ // Create OpenGL window - guess sensible dimensions
+ pangolin::CreateWindowAndBind( window_name,
+ (int)(video.Width() * video.Streams().size()),
+ (int)(video.Height() + slider_size)
+ );
+ glEnable (GL_BLEND);
+ glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+
+ pangolin::Var<int> frame("ui.frame");
+ pangolin::Slider frame_slider("frame", frame.Ref() );
+
+ if(video_playback && TotalFrames() < std::numeric_limits<int>::max())
+ {
+ // frame_slider should be added first so that it can be rendered correctly.
+ pangolin::DisplayBase().AddDisplay(frame_slider);
+ frame_slider.SetBounds(0.0, pangolin::Attach::Pix(slider_size), 0.0, 1.0);
+ }
+
+ pangolin::View& container = pangolin::Display("streams");
+ container.SetLayout(pangolin::LayoutEqual)
+ .SetBounds(pangolin::Attach::Pix(slider_size), 1.0, 0.0, 1.0);
+
+ std::vector<ImageView> stream_views(video.Streams().size());
+ for(auto& sv : stream_views) {
+ container.AddDisplay(sv);
+ }
+
+ pangolin::View& record_graphic = pangolin::Display("record_glyth").
+ SetBounds(pangolin::Attach::Pix(-28),1.0f, pangolin::Attach::Pix(-28), 1.0f);
+ record_graphic.extern_draw_function = [&](pangolin::View& v){
+ if(video.IsRecording()) {
+ v.ActivatePixelOrthographic();
+ pangolin::glRecordGraphic(14.0f, 14.0f, 7.0f);
+ }
+ };
+
+ std::vector<pangolin::Image<unsigned char> > images;
+
+ /////////////////////////////////////////////////////////////////////////
+ /// Register key shortcuts
+ /////////////////////////////////////////////////////////////////////////
+
+ RegisterDefaultKeyShortcutsAndPangoVariables();
+
+ const char show_hide_keys[] = {'1','2','3','4','5','6','7','8','9'};
+ const char screenshot_keys[] = {'!','@','#','$','%','^','&','*','('};
+
+ // Show/hide streams
+ for(size_t v=0; v < container.NumChildren() && v < 9; v++) {
+ pangolin::RegisterKeyPressCallback(show_hide_keys[v], [v,&container](){
+ container[v].ToggleShow();
+ } );
+ pangolin::RegisterKeyPressCallback(screenshot_keys[v], [this,v,&images](){
+ if(v < images.size() && images[v].ptr) {
+ try{
+ pangolin::SaveImage(
+ images[v], video.Streams()[v].PixFormat(),
+ pangolin::MakeUniqueFilename("capture.png")
+ );
+ }catch(std::exception e){
+ pango_print_error("Unable to save frame: %s\n", e.what());
+ }
+ }
+ } );
+ }
+
+ video.Start();
+
+ // Stream and display video
+ while(should_run && !pangolin::ShouldQuit())
+ {
+ glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+ glColor3f(1.0f, 1.0f, 1.0f);
+
+ {
+ std::lock_guard<std::mutex> lock(control_mutex);
+
+ if(frame.GuiChanged()) {
+ if(video_playback) {
+ frame = video_playback->Seek(frame) -1;
+ }
+ grab_until = frame + 1;
+ }
+
+ if ( frame < grab_until && video.Grab(&buffer[0], images, video_grab_wait, video_grab_newest)) {
+ frame = frame +1;
+
+ if(frame_changed_callback) {
+ frame_changed_callback(buffer.get(), images, GetVideoFrameProperties(video_interface));
+ }
+
+ // Update images
+ if((frame-1) % draw_nth_frame == 0) {
+ for(unsigned int i=0; i<images.size(); ++i)
+ if(stream_views[i].IsShown()) {
+ stream_views[i].SetImage(images[i], pangolin::GlPixFormat(video.Streams()[i].PixFormat() ));
+ }
+ }
+ }
+ }
+
+ // leave in pixel orthographic for slider to render.
+ pangolin::DisplayBase().ActivatePixelOrthographic();
+ pangolin::FinishFrame();
+ }
+
+ pangolin::DestroyWindow(window_name);
+}
+
+void VideoViewer::RegisterDefaultKeyShortcutsAndPangoVariables()
+{
+ pangolin::RegisterKeyPressCallback(' ', [this](){TogglePlay();} );
+ pangolin::RegisterKeyPressCallback('r', [this](){ToggleRecord();});
+ pangolin::RegisterKeyPressCallback('w', [this](){ToggleWaitForFrames();} );
+ pangolin::RegisterKeyPressCallback('d', [this](){ToggleDiscardBufferedFrames();} );
+ pangolin::RegisterKeyPressCallback(',', [this](){Skip(-1);} );
+ pangolin::RegisterKeyPressCallback('.', [this](){Skip(+1);} );
+ pangolin::RegisterKeyPressCallback('<', [this](){Skip(-FRAME_SKIP);} );
+ pangolin::RegisterKeyPressCallback('>', [this](){Skip(+FRAME_SKIP);} );
+ pangolin::RegisterKeyPressCallback('0', [this](){RecordOneFrame();} );
+ pangolin::RegisterKeyPressCallback('E', [this](){ChangeExposure(1000);} );
+ pangolin::RegisterKeyPressCallback('e', [this](){ChangeExposure(-1000);} );
+ pangolin::RegisterKeyPressCallback('G', [this](){ChangeGain(1);} );
+ pangolin::RegisterKeyPressCallback('g', [this](){ChangeGain(-1);} );
+ pangolin::RegisterKeyPressCallback('c', [this](){SetActiveCamera(+1);} );
+}
+
+void VideoViewer::OpenInput(const std::string& input_uri)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video.Open(input_uri, output_uri);
+
+ // Output details of video stream
+ for(size_t s = 0; s < video.Streams().size(); ++s) {
+ const pangolin::StreamInfo& si = video.Streams()[s];
+ std::cout << FormatString(
+ "Stream %: % x % % (pitch: % bytes)",
+ s, si.Width(), si.Height(), si.PixFormat().format, si.Pitch()
+ ) << std::endl;
+ }
+
+ if(video.Streams().size() == 0) {
+ pango_print_error("No video streams from device.\n");
+ return;
+ }
+
+ video_playback = pangolin::FindFirstMatchingVideoInterface<pangolin::VideoPlaybackInterface>(video);
+ video_interface = pangolin::FindFirstMatchingVideoInterface<pangolin::VideoInterface>(video);
+
+ if(TotalFrames() < std::numeric_limits<int>::max() ) {
+ std::cout << "Video length: " << TotalFrames() << " frames" << std::endl;
+ grab_until = 0;
+ }
+
+ pangolin::Var<int> frame("ui.frame");
+ frame.Meta().range[0] = 0;
+ frame.Meta().range[1] = TotalFrames()-1;
+}
+
+void VideoViewer::CloseInput()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video.Close();
+}
+
+void VideoViewer::Record()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video.Record();
+}
+
+void VideoViewer::RecordOneFrame()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video.RecordOneFrame();
+}
+
+void VideoViewer::StopRecording()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ if(video.IsRecording()) {
+ video.Stop();
+ }
+}
+
+void VideoViewer::TogglePlay()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ grab_until = (current_frame < grab_until) ? current_frame: std::numeric_limits<int>::max();
+}
+
+void VideoViewer::ToggleRecord()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ if(!video.IsRecording()) {
+ video.SetTimelapse( static_cast<size_t>(record_nth_frame) );
+ video.Record();
+ pango_print_info("Started Recording.\n");
+ }else{
+ video.Stop();
+ pango_print_info("Finished recording.\n");
+ }
+ fflush(stdout);
+}
+
+void VideoViewer::ToggleDiscardBufferedFrames()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video_grab_newest = !video_grab_newest;
+ if(video_grab_newest) {
+ pango_print_info("Discarding old frames.\n");
+ }else{
+ pango_print_info("Not discarding old frames.\n");
+ }
+}
+
+void VideoViewer::ToggleWaitForFrames()
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video_grab_wait = !video_grab_wait;
+ if(video_grab_wait) {
+ pango_print_info("Gui wait's for video frame.\n");
+ }else{
+ pango_print_info("Gui doesn't wait for video frame.\n");
+ }
+}
+
+void VideoViewer::SetDiscardBufferedFrames(bool new_state)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video_grab_newest = new_state;
+ if(video_grab_newest) {
+ pango_print_info("Discarding old frames.\n");
+ }else{
+ pango_print_info("Not discarding old frames.\n");
+ }
+}
+
+
+void VideoViewer::DrawEveryNFrames(int n)
+{
+ if(n <= 0) {
+ pango_print_warn("Cannot draw every %d frames. Ignoring request.\n",n);
+ return;
+ }
+
+ if(n != draw_nth_frame && n == 1)
+ pango_print_info("Drawing every frame.\n");
+ if(n != draw_nth_frame && n > 1)
+ pango_print_info("Drawing one in every %d frames.\n",n);
+
+ draw_nth_frame=n;
+}
+
+
+
+void VideoViewer::SetWaitForFrames(bool new_state)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ video_grab_wait = new_state;
+ if(video_grab_wait) {
+ pango_print_info("Gui wait's for video frame.\n");
+ }else{
+ pango_print_info("Gui doesn't wait for video frame.\n");
+ }
+}
+
+void VideoViewer::Skip(int frames)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+
+ if(video_playback) {
+ const int next_frame = current_frame + frames;
+ if (next_frame >= 0) {
+ current_frame = video_playback->Seek(next_frame) -1;
+ grab_until = current_frame + 1;
+ }
+ }else{
+ if(frames >= 0) {
+ grab_until = current_frame + frames;
+ }else{
+ pango_print_warn("Unable to skip backward.");
+ }
+ }
+
+}
+
+bool VideoViewer::ChangeExposure(int delta_us)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+
+ std::vector<pangolin::GenicamVideoInterface*> ifs = FindMatchingVideoInterfaces<pangolin::GenicamVideoInterface>(video);
+ std::string exposure_time;
+ if (ifs[active_cam]->GetParameter("ExposureTime",exposure_time))
+ {
+ return false;
+ }
+
+ int exp = atoi(exposure_time.c_str());
+
+ return ifs[active_cam]->SetParameter("ExposureTime", std::to_string(exp+delta_us));
+}
+
+bool VideoViewer::ChangeGain(float delta)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+
+ std::vector<pangolin::GenicamVideoInterface*> ifs = FindMatchingVideoInterfaces<pangolin::GenicamVideoInterface>(video);
+
+ std::string gain_string;
+ if (!ifs[active_cam]->GetParameter("Gain", gain_string))
+ {
+ return false;
+ }
+ double gain = atoi(gain_string.c_str());
+
+ return ifs[active_cam]->SetParameter("Gain", std::to_string(gain+delta));
+}
+
+
+void VideoViewer::SetActiveCamera(int delta)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+
+ std::vector<pangolin::GenicamVideoInterface*> ifs = FindMatchingVideoInterfaces<pangolin::GenicamVideoInterface>(video);
+
+ active_cam += delta;
+
+ if(active_cam >= ifs.size()) {active_cam = 0;}
+}
+
+
+void VideoViewer::SetFrameChangedCallback(FrameChangedCallbackFn cb)
+{
+ std::lock_guard<std::mutex> lock(control_mutex);
+ frame_changed_callback=cb;
+}
+
+
+
+void VideoViewer::WaitUntilExit()
+{
+ if(vv_thread.joinable()) {
+ vv_thread.join();
+ }
+}
+
+
+void RunVideoViewerUI(const std::string& input_uri, const std::string& output_uri)
+{
+ RegisterNewSigCallback(videoviewer_signal_quit, nullptr, SIGINT);
+ RegisterNewSigCallback(videoviewer_signal_quit, nullptr, SIGTERM);
+
+ VideoViewer vv("VideoViewer", input_uri, output_uri);
+ vv.Run();
+}
+
+}
diff --git a/externals/Pangolin/src/utils/file_extension.cpp b/externals/Pangolin/src/utils/file_extension.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1acc2d20f64a480586c68b82d25e6bce728fc6ba
--- /dev/null
+++ b/externals/Pangolin/src/utils/file_extension.cpp
@@ -0,0 +1,223 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/utils/file_extension.h>
+
+#include <algorithm>
+#include <fstream>
+#include <string.h>
+
+namespace pangolin
+{
+
+std::string ImageFileTypeToName(ImageFileType t)
+{
+ switch(t)
+ {
+ case ImageFileTypePpm:
+ return "ppm";
+ case ImageFileTypeTga:
+ return "tga";
+ case ImageFileTypePng:
+ return "png";
+ case ImageFileTypeJpg:
+ return "jpg";
+ case ImageFileTypeTiff:
+ return "tiff";
+ case ImageFileTypeGif:
+ return "gif";
+ case ImageFileTypeExr:
+ return "exr";
+ case ImageFileTypePango:
+ return "pango";
+ case ImageFileTypePvn:
+ return "pvn";
+ case ImageFileTypePly:
+ return "ply";
+ case ImageFileTypeObj:
+ return "obj";
+ case ImageFileTypeUnknown:
+ default:
+ return "unknown";
+ }
+}
+
+ImageFileType NameToImageFileType(const std::string& name)
+{
+ if ("ppm" == name)
+ return ImageFileTypePpm;
+ else if ("pgm" == name)
+ return ImageFileTypePpm;
+ else if ("tga" == name)
+ return ImageFileTypeTga;
+ else if ("png" == name)
+ return ImageFileTypePng;
+ else if ("jpg" == name)
+ return ImageFileTypeJpg;
+ else if ("jpeg" == name)
+ return ImageFileTypeJpg;
+ else if ("tiff" == name)
+ return ImageFileTypeTiff;
+ else if ("gif" == name)
+ return ImageFileTypeGif;
+ else if ("exr" == name)
+ return ImageFileTypeExr;
+ else if ("pango" == name)
+ return ImageFileTypePango;
+ else if ("pvn" == name)
+ return ImageFileTypePvn;
+ else if ("zstd" == name)
+ return ImageFileTypeZstd;
+ else if ("lzf" == name)
+ return ImageFileTypeLz4;
+ else if ("p12b" == name)
+ return ImageFileTypeP12b;
+ else if ("ply" == name)
+ return ImageFileTypePly;
+ else if ("obj" == name)
+ return ImageFileTypeObj;
+
+ return ImageFileTypeUnknown;
+}
+
+ImageFileType FileTypeExtension(const std::string& ext)
+{
+ if( ext == ".png" ) {
+ return ImageFileTypePng;
+ } else if( ext == ".tga" || ext == ".targa") {
+ return ImageFileTypeTga;
+ } else if( ext == ".jpg" || ext == ".jpeg" ) {
+ return ImageFileTypeJpg;
+ } else if( ext == ".gif" ) {
+ return ImageFileTypeGif;
+ } else if( ext == ".tif" || ext == ".tiff" ) {
+ return ImageFileTypeTiff;
+ } else if( ext == ".exr" ) {
+ return ImageFileTypeExr;
+ } else if( ext == ".ppm" || ext == ".pgm" || ext == ".pbm" || ext == ".pxm" || ext == ".pdm" ) {
+ return ImageFileTypePpm;
+ } else if( ext == ".pvn" ) {
+ return ImageFileTypePvn;
+ } else if( ext == ".pango" ) {
+ return ImageFileTypePango;
+ } else if( ext == ".zstd" ) {
+ return ImageFileTypeZstd;
+ } else if( ext == ".lzf" ) {
+ return ImageFileTypeLz4;
+ } else if( ext == ".p12b" ) {
+ return ImageFileTypeP12b;
+ } else if( ext == ".ply" ) {
+ return ImageFileTypePly;
+ } else if( ext == ".obj" ) {
+ return ImageFileTypeObj;
+ } else {
+ return ImageFileTypeUnknown;
+ }
+}
+
+std::string FileLowercaseExtention(const std::string& filename)
+{
+ size_t pos = filename.find_last_of('.');
+ if(pos != std::string::npos) {
+ std::string ext = filename.substr(pos);
+ std::transform( ext.begin(), ext.end(), ext.begin(), tolower );
+ return ext;
+ }else{
+ return "";
+ }
+}
+
+ImageFileType FileTypeMagic(const unsigned char data[], size_t bytes)
+{
+ // Check we wont go over bounds when comparing.
+ if(bytes >= 8) {
+ const unsigned char magic_png[] = "\211PNG\r\n\032\n";
+ const unsigned char magic_jpg1[] = "\xFF\xD8";
+ const unsigned char magic_jpg2[] = "\xFF\xD9";
+ const unsigned char magic_gif1[] = "GIF87a";
+ const unsigned char magic_gif2[] = "GIF89a";
+ const unsigned char magic_tiff1[] = "\x49\x49\x2A\x00";
+ const unsigned char magic_tiff2[] = "\x4D\x4D\x00\x2A";
+ const unsigned char magic_exr[] = "\x76\x2F\x31\x01";
+ const unsigned char magic_pango[] = "PANGO";
+ const unsigned char magic_pango_zstd[] = "ZSTD";
+ const unsigned char magic_pango_lz4[] = "LZ4";
+ const unsigned char magic_pango_p12b[] = "P12B";
+ const unsigned char magic_ply[] = "ply";
+
+ if( !strncmp((char*)data, (char*)magic_png, 8) ) {
+ return ImageFileTypePng;
+ }else if( !strncmp( (char*)data, (char*)magic_jpg1, 2)
+ || !strncmp( (char*)data, (char*)magic_jpg2, 2) ) {
+ return ImageFileTypeJpg;
+ }else if( !strncmp((char*)data, (char*)magic_gif1,6)
+ || !strncmp((char*)data, (char*)magic_gif2,6) ) {
+ return ImageFileTypeGif;
+ }else if( !strncmp((char*)data, (char*)magic_tiff1,4)
+ || !strncmp((char*)data, (char*)magic_tiff2,4) ) {
+ return ImageFileTypeTiff;
+ }else if( !strncmp((char*)data, (char*)magic_exr,4) ) {
+ return ImageFileTypeExr;
+ }else if( !strncmp((char*)data, (char*)magic_pango,5) ) {
+ return ImageFileTypePango;
+ }else if( !strncmp((char*)data, (char*)magic_pango_zstd,4) ) {
+ return ImageFileTypeZstd;
+ }else if( !strncmp((char*)data, (char*)magic_pango_lz4,3) ) {
+ return ImageFileTypeLz4;
+ }else if( !strncmp((char*)data, (char*)magic_pango_p12b,4) ) {
+ return ImageFileTypeP12b;
+ }else if( !strncmp((char*)data, (char*)magic_ply, 3) ) {
+ return ImageFileTypePly;
+ }else if( data[0] == 'P' && '0' < data[1] && data[1] < '9') {
+ return ImageFileTypePpm;
+ }
+ }
+ return ImageFileTypeUnknown;
+}
+
+ImageFileType FileType(const std::string& filename)
+{
+ // Check magic number of file...
+ std::ifstream f(filename.c_str(), std::ios::binary );
+ if(f.is_open()) {
+ const size_t magic_bytes = 8;
+ unsigned char magic[magic_bytes];
+ f.read((char*)magic, magic_bytes);
+ if(f.good()) {
+ ImageFileType magic_type = FileTypeMagic(magic, magic_bytes);
+ if(magic_type != ImageFileTypeUnknown) {
+ return magic_type;
+ }
+ }
+ }
+
+ // Fallback on using extension...
+ const std::string ext = FileLowercaseExtention(filename);
+ return FileTypeExtension(ext);
+}
+
+}
diff --git a/externals/Pangolin/src/utils/file_utils.cpp b/externals/Pangolin/src/utils/file_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d1c873752d585a6a2e6962d3d06ae1dc46ca0399
--- /dev/null
+++ b/externals/Pangolin/src/utils/file_utils.cpp
@@ -0,0 +1,493 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/platform.h>
+
+#include <pangolin/utils/file_utils.h>
+
+#ifdef _WIN_
+# ifndef WIN32_LEAN_AND_MEAN
+# define WIN32_LEAN_AND_MEAN
+# endif
+# include <Windows.h>
+# include <Shlobj.h>
+#else
+# include <dirent.h>
+# include <sys/types.h>
+# include <sys/stat.h>
+# include <sys/signal.h>
+# include <stdio.h>
+# include <string.h>
+# include <unistd.h>
+# include <fcntl.h>
+# include <errno.h>
+# include <poll.h>
+#endif // _WIN_
+
+#include <algorithm>
+#include <sstream>
+#include <list>
+
+namespace pangolin
+{
+
+std::vector<std::string>& Split(const std::string& s, char delim, std::vector<std::string>& elements) {
+ std::stringstream ss(s);
+ std::string item;
+ while (std::getline(ss, item, delim)) {
+ elements.push_back(item);
+ }
+ return elements;
+}
+
+std::vector<std::string> Split(const std::string &s, char delim) {
+ std::vector<std::string> elems;
+ return Split(s, delim, elems);
+}
+
+std::vector<std::string> Expand(const std::string &s, char open, char close, char delim)
+{
+ const size_t no = s.find_first_of(open);
+ if(no != std::string::npos) {
+ const size_t nc = s.find_first_of(close, no);
+ if(no != std::string::npos) {
+ const std::string pre = s.substr(0, no);
+ const std::string mid = s.substr(no+1, nc-no-1);
+ const std::string post = s.substr(nc+1, std::string::npos);
+ const std::vector<std::string> options = Split(mid, delim);
+ std::vector<std::string> expansion;
+
+ for(std::vector<std::string>::const_iterator iop = options.begin(); iop != options.end(); ++iop)
+ {
+ std::string full = pre + *iop + post;
+ expansion.push_back(full);
+ }
+ return expansion;
+ }
+ // Open but no close is unusual. Leave it for caller to see if valid
+ }
+
+ std::vector<std::string> ret;
+ ret.push_back(s);
+ return ret;
+}
+
+// Make path seperator consistent for OS.
+void PathOsNormaliseInplace(std::string& path)
+{
+#ifdef _WIN_
+ std::replace(path.begin(), path.end(), '/', '\\');
+#else
+ std::replace(path.begin(), path.end(), '\\', '/');
+#endif
+}
+
+std::string SanitizePath(const std::string& path)
+{
+ std::string path_copy(path.length(), '\0');
+
+ int p_slash1 = -1;
+ int p_slash0 = -1;
+ int n_dots = 0;
+
+ int dst = 0;
+ for(int src=0; src < (int)path.length(); ++src) {
+ if(path[src] == '/') {
+ if(n_dots==1 && p_slash0 >=0) {
+ dst = p_slash0;
+ for(p_slash1=p_slash0-1; p_slash1>=0 && path_copy[p_slash1] != '/'; --p_slash1);
+ }else if(n_dots==2) {
+ if(p_slash1 >=0) {
+ dst = p_slash1;
+ p_slash0 = dst;
+ for(p_slash1=p_slash0-1; p_slash1>=0 && path_copy[p_slash1] != '/'; --p_slash1) {
+ if( path_copy[p_slash1] == '.' ) {
+ p_slash1=-1;
+ break;
+ }
+ }
+ }else{
+ p_slash1 = -1;
+ p_slash0 = dst;
+ }
+ }else{
+ p_slash1 = p_slash0;
+ p_slash0 = dst;
+ }
+ n_dots = 0;
+ }else if(path[src] == '.' ){
+ if((dst-p_slash0) == n_dots+1) {
+ ++n_dots;
+ }
+ }else{
+ n_dots = 0;
+ }
+ path_copy[dst++] = path[src];
+ }
+
+ return path_copy.substr(0,dst);
+}
+
+// Return path 'levels' directories above 'path'
+std::string PathParent(const std::string& path, int levels)
+{
+ std::string res = path;
+
+ while (levels > 0) {
+ if (res.length() == 0) {
+ res = std::string();
+ for (int l = 0; l < levels; ++l) {
+#ifdef _WIN_
+ res += std::string("..\\");
+#else
+ res += std::string("../");
+#endif
+ }
+ return res;
+ }else{
+ const size_t nLastSlash = res.find_last_of("/\\");
+
+ if (nLastSlash != std::string::npos) {
+ res = path.substr(0, nLastSlash);
+ } else{
+ res = std::string();
+ }
+
+ --levels;
+ }
+ }
+
+ return res;
+}
+
+std::string FindPath(const std::string& child_path, const std::string& signature_path)
+{
+ std::string path = PathExpand(child_path);
+#ifdef _UNIX_
+ char abs_path[PATH_MAX];
+ if (realpath(path.c_str(), abs_path)) {
+ path = abs_path;
+ }
+#endif
+ std::string signature = signature_path;
+ PathOsNormaliseInplace(path);
+ PathOsNormaliseInplace(signature);
+
+ while(!FileExists(path + signature)) {
+ if (path.empty()) {
+ return std::string();
+ } else {
+ path = PathParent(path);
+ }
+ }
+
+ return path + signature;
+}
+
+std::string PathExpand(const std::string& sPath)
+{
+ if(sPath.length() >0 && sPath[0] == '~') {
+#ifdef _WIN_
+ std::string sHomeDir;
+ WCHAR path[MAX_PATH];
+ if (SUCCEEDED(SHGetFolderPathW(NULL, CSIDL_PROFILE, NULL, 0, path))) {
+ std::wstring ws(path);
+ sHomeDir = std::string(ws.begin(), ws.end());
+ }
+#else
+ std::string sHomeDir = std::string(getenv("HOME"));
+#endif
+ return sHomeDir + sPath.substr(1,std::string::npos);
+ }else{
+ return sPath;
+ }
+}
+
+// Based on http://www.codeproject.com/Articles/188256/A-Simple-Wildcard-Matching-Function
+bool MatchesWildcard(const std::string& str, const std::string& wildcard)
+{
+ const char* psQuery = str.c_str();
+ const char* psWildcard = wildcard.c_str();
+
+ while(*psWildcard)
+ {
+ if(*psWildcard=='?')
+ {
+ if(!*psQuery)
+ return false;
+
+ ++psQuery;
+ ++psWildcard;
+ }
+ else if(*psWildcard=='*')
+ {
+ if(MatchesWildcard(psQuery,psWildcard+1))
+ return true;
+
+ if(*psQuery && MatchesWildcard(psQuery+1,psWildcard))
+ return true;
+
+ return false;
+ }
+ else
+ {
+ if(*psQuery++ != *psWildcard++ )
+ return false;
+ }
+ }
+
+ return !*psQuery && !*psWildcard;
+}
+
+std::string MakeUniqueFilename(const std::string& filename)
+{
+ if( FileExists(filename) ) {
+ const size_t dot = filename.find_last_of('.');
+
+ std::string fn;
+ std::string ext;
+
+ if(dot == filename.npos) {
+ fn = filename;
+ ext = "";
+ }else{
+ fn = filename.substr(0, dot);
+ ext = filename.substr(dot);
+ }
+
+ int id = 1;
+ std::string new_file;
+ do {
+ id++;
+ std::stringstream ss;
+ ss << fn << "_" << id << ext;
+ new_file = ss.str();
+ }while( FileExists(new_file) );
+
+ return new_file;
+ }else{
+ return filename;
+ }
+}
+
+bool IsPipe(const std::string& file)
+{
+#ifdef _WIN_
+ return false;
+#else
+ struct stat st;
+ int err = stat(file.c_str(), &st);
+ return (err == 0) && ((st.st_mode & S_IFMT) == S_IFIFO);
+#endif // _WIN_
+}
+
+bool IsPipe(int fd)
+{
+#ifdef _WIN_
+ return false;
+#else
+ struct stat st;
+ int err = fstat(fd, &st);
+ return (err == 0) && ((st.st_mode & S_IFMT) == S_IFIFO);
+#endif
+}
+
+int WritablePipeFileDescriptor(const std::string& file)
+{
+#ifdef _WIN_
+ return false;
+#else
+ // open(2) will return ENXIO when there is no reader on the other
+ // side of the pipe, but only if O_WRONLY|O_NONBLOCK is specified.
+ // The file descriptor can be adjusted to be a blocking file
+ // descriptor if it is valid.
+ return open(file.c_str(), O_WRONLY | O_NONBLOCK);
+#endif // _WIN_
+}
+
+int ReadablePipeFileDescriptor(const std::string& file)
+{
+#ifdef _WIN_
+ return -1;
+#else
+ return open(file.c_str(), O_RDONLY | O_NONBLOCK);
+#endif
+}
+
+bool PipeHasDataToRead(int fd)
+{
+#ifdef _WIN_
+ return false;
+#else
+ struct pollfd pfd;
+ memset(&pfd, 0, sizeof(pfd));
+ pfd.fd = fd;
+ pfd.events = POLLIN;
+
+ int err = poll(&pfd, 1, 0);
+
+ // If err is 0, the file has no data. If err is negative, an error
+ // occurred.
+ return err == 1 && pfd.revents & POLLIN;
+#endif
+}
+
+void FlushPipe(const std::string& file)
+{
+#ifndef _WIN_
+ int fd = open(file.c_str(), O_RDONLY | O_NONBLOCK);
+ char buf[65535];
+ int n = 0;
+ do
+ {
+ n = read(fd, buf, sizeof(buf));
+ } while(n > 0);
+ close(fd);
+#endif
+}
+
+#ifdef _WIN_
+
+#ifdef UNICODE
+ typedef std::codecvt_utf8<wchar_t> WinStringConvert;
+ typedef std::wstring WinString;
+
+ WinString s2ws(const std::string& str) {
+ std::wstring_convert<WinStringConvert, wchar_t> converter;
+ return converter.from_bytes(str);
+ }
+ std::string ws2s(const WinString& wstr) {
+ std::wstring_convert<WinStringConvert, wchar_t> converter;
+ return converter.to_bytes(wstr);
+ }
+#else
+ typedef std::string WinString;
+ // No conversions necessary
+# define s2ws(X) (X)
+# define ws2s(X) (X)
+#endif // UNICODE
+
+bool FilesMatchingWildcard(const std::string& wildcard, std::vector<std::string>& file_vec)
+{
+ size_t nLastSlash = wildcard.find_last_of("/\\");
+
+ std::string sPath;
+ std::string sFileWc;
+
+ if(nLastSlash != std::string::npos) {
+ sPath = wildcard.substr(0, nLastSlash);
+ sFileWc = wildcard.substr(nLastSlash+1, std::string::npos);
+ }else{
+ sPath = ".";
+ sFileWc = wildcard;
+ }
+
+ sPath = PathExpand(sPath);
+
+ WIN32_FIND_DATA wfd;
+ HANDLE fh = FindFirstFile( s2ws(sPath + "\\" + sFileWc).c_str(), &wfd);
+
+ std::vector<std::string> files;
+
+ if (fh != INVALID_HANDLE_VALUE) {
+ do {
+ if (!(wfd.dwFileAttributes & FILE_ATTRIBUTE_DIRECTORY)) {
+ files.push_back( sPath + "\\" + ws2s(wfd.cFileName) );
+ }
+ } while (FindNextFile (fh, &wfd));
+ FindClose(fh);
+ }
+
+ // TOOD: Use different comparison to achieve better ordering.
+ std::sort(files.begin(), files.end() );
+
+ // Put file list at end of file_vec
+ file_vec.insert(file_vec.end(), files.begin(), files.end() );
+
+ return files.size() > 0;
+}
+
+bool FileExists(const std::string& filename)
+{
+ std::string search_filename = filename;
+ if(filename.length() > 0 && filename[filename.length()-1] == '\\') {
+ search_filename.resize(filename.length()-1);
+ }
+ WIN32_FIND_DATA wfd;
+ HANDLE fh = FindFirstFile( s2ws(search_filename).c_str(), &wfd);
+ const bool exists = fh != INVALID_HANDLE_VALUE;
+ FindClose(fh);
+ return exists;
+}
+
+#else // _WIN_
+
+bool FilesMatchingWildcard(const std::string& in_wildcard, std::vector<std::string>& file_vec)
+{
+ const std::string wildcard = PathExpand(in_wildcard);
+ const size_t first_wildcard = wildcard.find_first_of("?*");
+ if(first_wildcard != std::string::npos) {
+ const std::string root = PathParent(wildcard.substr(0,first_wildcard));
+ struct dirent **namelist;
+ int n = scandir(root.c_str(), &namelist, 0, alphasort );
+ if (n >= 0) {
+ const size_t next_slash = wildcard.find_first_of("/\\",first_wildcard+1);
+ std::string dir_wildcard, rest;
+ if(next_slash != std::string::npos) {
+ dir_wildcard = wildcard.substr(root.size()+1, next_slash-root.size()-1);
+ rest = wildcard.substr(next_slash);
+ }else{
+ dir_wildcard = wildcard.substr(root.size()+1);
+ }
+
+ while(n--) {
+ const std::string file_name(namelist[n]->d_name);
+ if( file_name != "." && file_name != ".." && MatchesWildcard(file_name, dir_wildcard) ) {
+ const std::string sub_wildcard = root + "/" + file_name + rest;
+ FilesMatchingWildcard(sub_wildcard, file_vec);
+ if(dir_wildcard == "**") {
+ const std::string sub_wildcard2 = root + "/" + file_name + "/**" + rest;
+ FilesMatchingWildcard(sub_wildcard2, file_vec);
+ }
+ }
+ }
+ }
+ }else if(FileExists(wildcard)) {
+ file_vec.push_back(wildcard);
+ }
+ return file_vec.size() > 0;
+}
+
+bool FileExists(const std::string& filename)
+{
+ struct stat buf;
+ return stat(filename.c_str(), &buf) != -1;
+}
+
+#endif //_WIN_
+
+}
diff --git a/externals/Pangolin/src/utils/posix/condition_variable.cpp b/externals/Pangolin/src/utils/posix/condition_variable.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9c6560065741c8cfe02f3a64e4c31d89007632f6
--- /dev/null
+++ b/externals/Pangolin/src/utils/posix/condition_variable.cpp
@@ -0,0 +1,89 @@
+#include <pangolin/utils/posix/condition_variable.h>
+
+#include <pangolin/utils/posix/shared_memory_buffer.h>
+
+#include <pthread.h>
+
+using namespace std;
+
+namespace pangolin {
+
+struct PThreadSharedData {
+ pthread_mutex_t lock;
+ pthread_cond_t cond;
+};
+
+class PThreadConditionVariable : public ConditionVariableInterface {
+public:
+ PThreadConditionVariable(std::shared_ptr<SharedMemoryBufferInterface> &shmem)
+ : _shmem(shmem),
+ _pthread_data(reinterpret_cast<PThreadSharedData *>(_shmem->ptr())) {}
+
+ ~PThreadConditionVariable() {}
+
+ void wait() {
+ _lock();
+ pthread_cond_wait(&_pthread_data->cond, &_pthread_data->lock);
+ _unlock();
+ }
+
+ bool wait(timespec abstime) {
+ _lock();
+ int err = pthread_cond_timedwait(&_pthread_data->cond, &_pthread_data->lock,
+ &abstime);
+ _unlock();
+
+ return 0 == err;
+ }
+
+ void signal() { pthread_cond_signal(&_pthread_data->cond); }
+
+ void broadcast() { pthread_cond_broadcast(&_pthread_data->cond); }
+
+private:
+ void _lock() { pthread_mutex_lock(&_pthread_data->lock); }
+
+ void _unlock() { pthread_mutex_unlock(&_pthread_data->lock); }
+
+ std::shared_ptr<SharedMemoryBufferInterface> _shmem;
+ PThreadSharedData *_pthread_data;
+};
+
+std::shared_ptr<ConditionVariableInterface>
+create_named_condition_variable(const string &name) {
+ std::shared_ptr<SharedMemoryBufferInterface> shmem =
+ create_named_shared_memory_buffer(name, sizeof(PThreadSharedData));
+ std::shared_ptr<ConditionVariableInterface> ptr;
+
+ PThreadSharedData *pthread_data =
+ reinterpret_cast<PThreadSharedData *>(shmem->ptr());
+
+ pthread_mutexattr_t mattr;
+ pthread_mutexattr_init(&mattr);
+ pthread_mutexattr_setpshared(&mattr, PTHREAD_PROCESS_SHARED);
+
+ pthread_condattr_t cattr;
+ pthread_condattr_init(&cattr);
+ pthread_condattr_setpshared(&cattr, PTHREAD_PROCESS_SHARED);
+
+ pthread_mutex_init(&pthread_data->lock, &mattr);
+ pthread_cond_init(&pthread_data->cond, &cattr);
+
+ ptr.reset(static_cast<ConditionVariableInterface *>(
+ new PThreadConditionVariable(shmem)));
+ return ptr;
+}
+
+std::shared_ptr<ConditionVariableInterface>
+open_named_condition_variable(const string &name) {
+ std::shared_ptr<SharedMemoryBufferInterface> shmem =
+ open_named_shared_memory_buffer(name, true);
+ std::shared_ptr<ConditionVariableInterface> ptr;
+
+ if(shmem) {
+ ptr.reset(new PThreadConditionVariable(shmem));
+ }
+ return ptr;
+}
+
+}
diff --git a/externals/Pangolin/src/utils/posix/semaphore.cpp b/externals/Pangolin/src/utils/posix/semaphore.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8dcb9059054fe20d2449fdade4619c2af815ace6
--- /dev/null
+++ b/externals/Pangolin/src/utils/posix/semaphore.cpp
@@ -0,0 +1,83 @@
+#include <pangolin/utils/posix/semaphore.h>
+
+#include <fcntl.h>
+#include <semaphore.h>
+#include <sys/stat.h>
+
+#include <string>
+
+using namespace std;
+
+namespace pangolin
+{
+
+// TODO(shaheen) register a signal handler for SIGTERM and unlink shared
+// semaphores.
+class PosixSemaphore : public SemaphoreInterface
+{
+public:
+ PosixSemaphore(sem_t *semaphore, bool ownership, const string& name) :
+ _semaphore(semaphore),
+ _ownership(ownership),
+ _name(name)
+ {
+ }
+
+ ~PosixSemaphore()
+ {
+ if (_ownership) {
+ sem_unlink(_name.c_str());
+ } else {
+ sem_close(_semaphore);
+ }
+ }
+
+ bool tryAcquire()
+ {
+ int err = sem_trywait(_semaphore);
+ return err == 0;
+ }
+
+ void acquire()
+ {
+ sem_wait(_semaphore);
+ }
+
+ void release()
+ {
+ sem_post(_semaphore);
+ }
+
+private:
+ sem_t *_semaphore;
+ bool _ownership;
+ string _name;
+};
+
+std::shared_ptr<SemaphoreInterface> create_named_semaphore(const string& name, unsigned int value)
+{
+ std::shared_ptr<SemaphoreInterface> ptr;
+ sem_t *semaphore = sem_open(name.c_str(), O_CREAT | O_EXCL,
+ S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP, value);
+ if (NULL == semaphore) {
+ return ptr;
+ }
+
+ ptr.reset(new PosixSemaphore(semaphore, true, name));
+ return ptr;
+}
+
+std::shared_ptr<SemaphoreInterface> open_named_semaphore(const string& name)
+{
+ std::shared_ptr<SemaphoreInterface> ptr;
+ sem_t *semaphore = sem_open(name.c_str(), 0);
+
+ if (NULL == semaphore) {
+ return ptr;
+ }
+
+ ptr.reset(new PosixSemaphore(semaphore, false, name));
+ return ptr;
+}
+
+}
diff --git a/externals/Pangolin/src/utils/posix/shared_memory_buffer.cpp b/externals/Pangolin/src/utils/posix/shared_memory_buffer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e8e5724025989039f4af95b6c6037dd01695668
--- /dev/null
+++ b/externals/Pangolin/src/utils/posix/shared_memory_buffer.cpp
@@ -0,0 +1,134 @@
+#include <pangolin/utils/posix/shared_memory_buffer.h>
+
+#include <fcntl.h>
+#include <limits.h>
+#include <sys/file.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+using namespace std;
+
+namespace pangolin
+{
+
+// TODO(shaheen) register a signal handler for SIGTERM and unlink shared memory
+// objects.
+class PosixSharedMemoryBuffer : public SharedMemoryBufferInterface
+{
+public:
+ PosixSharedMemoryBuffer(int fd, unsigned char *ptr, size_t size, bool ownership, const std::string& name) :
+ _fd(fd),
+ _ptr(ptr),
+ _size(size),
+ _ownership(ownership),
+ _name(name),
+ _lockCount(0)
+ {
+ }
+
+ ~PosixSharedMemoryBuffer()
+ {
+ close(_fd);
+ munmap(_ptr, _size);
+
+ if (_ownership) {
+ shm_unlink(_name.c_str());
+ }
+ }
+
+ bool tryLock()
+ {
+ if (_lockCount == 0) {
+ int err = flock(_fd, LOCK_EX|LOCK_NB);
+ if (0 == err) {
+ _lockCount++;
+ }
+ }
+ return _lockCount != 0;
+ }
+
+ void lock()
+ {
+ if (_lockCount == 0) {
+ flock(_fd, LOCK_EX);
+ }
+ _lockCount++;
+ }
+
+ void unlock()
+ {
+ if (_lockCount != 0) {
+ flock(_fd, LOCK_UN);
+ }
+ _lockCount--;
+ }
+
+ unsigned char *ptr()
+ {
+ return _ptr;
+ }
+
+ std::string name()
+ {
+ return _name;
+ }
+
+private:
+ int _fd;
+ unsigned char *_ptr;
+ size_t _size;
+ bool _ownership;
+ string _name;
+ unsigned int _lockCount;
+};
+
+std::shared_ptr<SharedMemoryBufferInterface> create_named_shared_memory_buffer(const
+ string& name, size_t size)
+{
+ std::shared_ptr<SharedMemoryBufferInterface> ptr;
+
+ int fd = shm_open(name.c_str(), O_RDWR | O_RDONLY | O_CREAT,
+ S_IRUSR|S_IWUSR|S_IRGRP|S_IWGRP);
+ if (-1 == fd) {
+ return ptr;
+ }
+
+ int err = ftruncate(fd, size);
+ if (-1 == err) {
+ shm_unlink(name.c_str());
+ return ptr;
+ }
+
+ unsigned char *buffer = reinterpret_cast<unsigned char *>(mmap(NULL, size,
+ PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0));
+
+ ptr.reset(new PosixSharedMemoryBuffer(fd, buffer, size, true, name));
+ return ptr;
+}
+
+std::shared_ptr<SharedMemoryBufferInterface> open_named_shared_memory_buffer(const
+ string& name, bool readwrite)
+{
+ std::shared_ptr<SharedMemoryBufferInterface> ptr;
+
+ int fd = shm_open(name.c_str(), readwrite ? O_RDWR | O_RDONLY : O_RDONLY, 0);
+ if (-1 == fd) {
+ return ptr;
+ }
+
+ struct stat sbuf;
+ int err = fstat(fd, &sbuf);
+ if (-1 == err) {
+ return ptr;
+ }
+
+ size_t size = sbuf.st_size;
+ unsigned char *buffer = reinterpret_cast<unsigned char *>(mmap(NULL, size,
+ PROT_READ|PROT_WRITE, MAP_SHARED, fd, 0));
+
+ ptr.reset(new PosixSharedMemoryBuffer(fd, buffer, size, false, name));
+ return ptr;
+}
+
+}
diff --git a/externals/Pangolin/src/utils/sigstate.cpp b/externals/Pangolin/src/utils/sigstate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cc9fd58741e4efd0eb45b4d3ad80ccfe112b37ac
--- /dev/null
+++ b/externals/Pangolin/src/utils/sigstate.cpp
@@ -0,0 +1,60 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/utils/sigstate.h>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+using namespace std;
+
+namespace pangolin
+{
+
+SigState& SigState::I() {
+ static SigState singleton;
+ return singleton;
+}
+
+SigState::SigState()
+{
+}
+
+SigState::~SigState() {
+ Clear();
+}
+
+void SigState::Clear() {
+ sig_callbacks.clear();
+}
+
+void RegisterNewSigCallback(SigCallbackFn callback, void* data, const int signal)
+{
+ SigState::I().sig_callbacks.insert(std::pair<int, SigCallback>(signal, SigCallback(signal, callback, data)));
+}
+
+}
diff --git a/externals/Pangolin/src/utils/threadedfilebuf.cpp b/externals/Pangolin/src/utils/threadedfilebuf.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..cb48b87cae77401309ed8fa16a6f2990a95cf258
--- /dev/null
+++ b/externals/Pangolin/src/utils/threadedfilebuf.cpp
@@ -0,0 +1,286 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/utils/threadedfilebuf.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/sigstate.h>
+
+#include <cstring>
+#include <stdexcept>
+
+#ifdef USE_POSIX_FILE_IO
+#include <unistd.h>
+#include <fcntl.h>
+#endif
+
+using namespace std;
+
+namespace pangolin
+{
+
+threadedfilebuf::threadedfilebuf()
+ : mem_buffer(0), mem_size(0), mem_max_size(0), mem_start(0), mem_end(0), should_run(false), is_pipe(false)
+{
+}
+
+threadedfilebuf::threadedfilebuf(const std::string& filename, size_t buffer_size_bytes )
+ : mem_buffer(0), mem_size(0), mem_max_size(0), mem_start(0), mem_end(0), should_run(false), is_pipe(pangolin::IsPipe(filename))
+{
+ open(filename, buffer_size_bytes);
+}
+
+void threadedfilebuf::open(const std::string& filename, size_t buffer_size_bytes)
+{
+ is_pipe = pangolin::IsPipe(filename);
+
+#ifdef USE_POSIX_FILE_IO
+ if (filenum != -1) {
+#else
+ if (file.is_open()) {
+#endif
+ close();
+ }
+
+#ifdef USE_POSIX_FILE_IO
+ filenum = ::open(filename.c_str(), O_CREAT | O_TRUNC | O_WRONLY | O_SYNC, S_IRWXU);
+#else
+ file.open(filename.c_str(), ios::out | ios::binary);
+#endif
+
+#ifdef USE_POSIX_FILE_IO
+ if (filenum == -1) {
+#else
+ if(!file.is_open()) {
+#endif
+ throw std::runtime_error("Unable to open '" + filename + "' for writing.");
+ }
+
+ mem_buffer = 0;
+ mem_size = 0;
+ mem_start = 0;
+ mem_end = 0;
+ mem_max_size = static_cast<std::streamsize>(buffer_size_bytes);
+ mem_buffer = new char[static_cast<size_t>(mem_max_size)];
+
+ should_run = true;
+ write_thread = std::thread(std::ref(*this));
+}
+
+void threadedfilebuf::close()
+{
+ should_run = false;
+
+ cond_queued.notify_all();
+
+ if(write_thread.joinable())
+ {
+ write_thread.join();
+ }
+
+ if(mem_buffer)
+ {
+ delete mem_buffer;
+ mem_buffer = 0;
+ }
+
+#ifdef USE_POSIX_FILE_IO
+ ::close(filenum);
+ filenum = -1;
+#else
+ file.close();
+#endif
+}
+
+void threadedfilebuf::soft_close()
+{
+ // Forces sputn to write no bytes and exit early, results in lost data
+ mem_size = 0;
+}
+
+void threadedfilebuf::force_close()
+{
+ soft_close();
+ close();
+}
+
+threadedfilebuf::~threadedfilebuf()
+{
+ close();
+}
+
+std::streamsize threadedfilebuf::xsputn(const char* data, std::streamsize num_bytes)
+{
+ if( num_bytes > mem_max_size ) {
+ std::unique_lock<std::mutex> lock(update_mutex);
+ // Wait until queue is empty
+ while( mem_size > 0 ) {
+ cond_dequeued.wait(lock);
+ }
+
+ // Allocate bigger buffer
+ delete mem_buffer;
+ mem_start = 0;
+ mem_end = 0;
+ mem_max_size = num_bytes * 4;
+ mem_buffer = new char[static_cast<size_t>(mem_max_size)];
+ }
+
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ // wait until there is space to write into buffer
+ while( mem_size + num_bytes > mem_max_size ) {
+ cond_dequeued.wait(lock);
+ }
+
+ // add image to end of mem_buffer
+ const std::streamsize array_a_size =
+ (mem_start <= mem_end) ? (mem_max_size - mem_end) : (mem_start - mem_end);
+
+ if( num_bytes <= array_a_size )
+ {
+ // copy in one
+ memcpy(mem_buffer + mem_end, data, static_cast<size_t>(num_bytes));
+ mem_end += num_bytes;
+ mem_size += num_bytes;
+ }else{
+ const std::streamsize array_b_size = num_bytes - array_a_size;
+ memcpy(mem_buffer + mem_end, data, (size_t)array_a_size);
+ memcpy(mem_buffer, data+array_a_size, (size_t)array_b_size);
+ mem_end = array_b_size;
+ mem_size += num_bytes;
+ }
+
+ if(mem_end == mem_max_size)
+ mem_end = 0;
+ }
+
+ cond_queued.notify_one();
+
+ input_pos += num_bytes;
+ return num_bytes;
+}
+
+int threadedfilebuf::overflow(int c)
+{
+ const std::streamsize num_bytes = 1;
+
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ // wait until there is space to write into buffer
+ while( mem_size + num_bytes > mem_max_size ) {
+ cond_dequeued.wait(lock);
+ }
+
+ // add image to end of mem_buffer
+ mem_buffer[mem_end] = c;
+ mem_end += num_bytes;
+ mem_size += num_bytes;
+
+ if(mem_end == mem_max_size)
+ mem_end = 0;
+ }
+
+ cond_queued.notify_one();
+
+ input_pos += num_bytes;
+ return num_bytes;
+}
+
+std::streampos threadedfilebuf::seekoff(
+ std::streamoff off, std::ios_base::seekdir way,
+ std::ios_base::openmode /*which*/
+) {
+ if(off == 0 && way == ios_base::cur) {
+ return input_pos;
+ }else{
+ return -1;
+ }
+}
+
+void threadedfilebuf::operator()()
+{
+ std::streamsize data_to_write = 0;
+
+ while(true)
+ {
+ if(is_pipe)
+ {
+ try
+ {
+ if(SigState::I().sig_callbacks.at(SIGPIPE).value)
+ {
+ soft_close();
+ return;
+ }
+ } catch(std::out_of_range&)
+ {
+// std::cout << "Please register a SIGPIPE handler for your writer" << std::endl;
+ }
+ }
+
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ while( mem_size == 0 ) {
+ if(!should_run) return;
+ cond_queued.wait(lock);
+ }
+
+ data_to_write =
+ (mem_start < mem_end) ?
+ mem_end - mem_start :
+ mem_max_size - mem_start;
+ }
+
+#ifdef USE_POSIX_FILE_IO
+ int bytes_written = ::write(filenum, mem_buffer + mem_start, data_to_write);
+ if(bytes_written == -1)
+ {
+ throw std::runtime_error("Unable to write data.");
+ }
+#else
+ std::streamsize bytes_written =
+ file.sputn(mem_buffer + mem_start, data_to_write );
+#endif
+
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ mem_size -= bytes_written;
+ mem_start += bytes_written;
+
+ if(mem_start == mem_max_size)
+ mem_start = 0;
+ }
+
+ cond_dequeued.notify_all();
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/utils/timer.cpp b/externals/Pangolin/src/utils/timer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391
diff --git a/externals/Pangolin/src/utils/uri.cpp b/externals/Pangolin/src/utils/uri.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6624caaa101e8da7ddeda357e7feb61e57e2b1f2
--- /dev/null
+++ b/externals/Pangolin/src/utils/uri.cpp
@@ -0,0 +1,104 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/uri.h>
+
+#include <stdexcept>
+#include <vector>
+
+namespace pangolin
+{
+
+Uri ParseUri(const std::string &str_uri)
+{
+ Uri uri;
+ uri.full_uri = str_uri;
+
+ // Find Scheme delimiter
+ size_t npos = 0;
+ const size_t ns = str_uri.find(':', npos);
+ if( ns != std::string::npos )
+ {
+ uri.scheme = str_uri.substr(0,ns);
+ npos = ns+1;
+ }else{
+ uri.scheme = "file";
+ uri.url = str_uri;
+ return uri;
+ }
+
+ // Find Options delimiters
+ if( str_uri.size() > npos && str_uri[npos] == '[' )
+ {
+ const size_t nob = npos;
+ const size_t ncb = str_uri.find(']', nob+1);
+ if(ncb != std::string::npos)
+ {
+ const std::string queries = str_uri.substr(nob+1, ncb - (ns+2) );
+ std::vector<std::string> params;
+ Split(queries, ',', params);
+ for(size_t i=0; i< params.size(); ++i)
+ {
+ std::vector<std::string> args;
+ Split(params[i], '=', args );
+ std::string key = Trim(args[0]);
+ std::string val = args.size() > 1 ? Trim(args[1]) : "";
+ uri.Set(key,val);
+ }
+ }else{
+ throw std::runtime_error("Unable to parse URI: '" + str_uri + "'");
+ }
+ npos = ncb + 1;
+ }
+
+ // Find url delimiter
+ size_t nurl = str_uri.find("//", npos);
+ if(nurl != std::string::npos)
+ {
+ uri.url = str_uri.substr(nurl+2);
+ }
+
+ return uri;
+}
+
+std::ostream& operator<< (std::ostream &out, Uri &uri)
+{
+ out << "scheme: " << uri.scheme << std::endl;
+ out << "url: " << uri.url << std::endl;
+ out << "params:" << std::endl;
+
+ for( Uri::ParamMap::const_iterator ip = uri.params.begin();
+ ip != uri.params.end(); ++ip)
+ {
+ out << "\t" << ip->first << " = " << ip->second << std::endl;
+ }
+
+ return out;
+}
+
+}
diff --git a/externals/Pangolin/src/var/input_record_repeat.cpp b/externals/Pangolin/src/var/input_record_repeat.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e932f47e411656633a844dd66cdabc3ace45fb70
--- /dev/null
+++ b/externals/Pangolin/src/var/input_record_repeat.cpp
@@ -0,0 +1,183 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/var/input_record_repeat.h>
+
+#include <limits>
+
+using namespace std;
+
+namespace pangolin
+{
+
+InputRecordRepeat::InputRecordRepeat(const std::string& var_record_prefix)
+ : record(false), play(false), index(-1)
+{
+ RegisterGuiVarChangedCallback(&InputRecordRepeat::GuiVarChanged,(void*)this,var_record_prefix);
+}
+
+InputRecordRepeat::~InputRecordRepeat()
+{
+
+}
+
+void InputRecordRepeat::SetIndex(int id)
+{
+// if( id < index )
+// Clear();
+
+ index = id;
+
+ while( !play_queue.empty() && play_queue.front().index < index )
+ {
+ // 'Play' Frameinput
+ FrameInput in = play_queue.front();
+ play_queue.pop_front();
+ Var<std::string> var(in.var);
+ var = in.val;
+ }
+}
+
+void InputRecordRepeat::Record()
+{
+ ClearBuffer();
+ play = false;
+ record = true;
+}
+
+void InputRecordRepeat::Stop()
+{
+ record = false;
+ play = false;
+}
+
+void InputRecordRepeat::ClearBuffer()
+{
+ index = -1;
+ record_queue.clear();
+ play_queue.clear();
+}
+
+ostream& operator<<(ostream& os, const FrameInput& fi )
+{
+ os << fi.index << endl << fi.var << endl << fi.val << endl;
+ return os;
+}
+
+istream& operator>>(istream& is, FrameInput& fi)
+{
+ is >> fi.index;
+ is.ignore(std::numeric_limits<streamsize>::max(),'\n');
+ getline(is,fi.var);
+ getline(is,fi.val);
+ return is;
+}
+
+void InputRecordRepeat::SaveBuffer(const std::string& filename)
+{
+ ofstream f(filename.c_str());
+
+ for( std::list<FrameInput>::const_iterator i = record_queue.begin(); i!=record_queue.end(); ++i )
+ {
+ f << *i;
+ }
+}
+
+void InputRecordRepeat::LoadBuffer(const std::string& filename)
+{
+ record_queue.clear();
+
+ ifstream f(filename.c_str());
+ while(f.good())
+ {
+ FrameInput fi;
+ f >> fi;
+ if( f.good() )
+ record_queue.push_back(fi);
+ }
+}
+
+void InputRecordRepeat::PlayBuffer()
+{
+ play_queue = record_queue;
+
+ record = false;
+ play = true;
+}
+
+void InputRecordRepeat::PlayBuffer(size_t start, size_t end)
+{
+ std::list<FrameInput>::iterator s = record_queue.begin();
+ std::list<FrameInput>::iterator e = record_queue.begin();
+
+ for(size_t i=0; i<start; i++) s++;
+ for(size_t i=0; i<end; i++) e++;
+
+ play_queue.clear();
+ play_queue.insert(play_queue.begin(),s,e);
+
+ record = false;
+ play = true;
+}
+
+size_t InputRecordRepeat::Size()
+{
+ return record_queue.size();
+}
+
+void InputRecordRepeat::UpdateVariable(const std::string& name )
+{
+ Var<std::string> var(name);
+
+ if( record )
+ {
+ FrameInput input;
+ input.index = index;
+ input.var = name;
+ input.val = var.Get();
+ record_queue.push_back(input);
+ }
+}
+
+void InputRecordRepeat::GuiVarChanged(void* data, const std::string& name, VarValueGeneric &_var)
+{
+ InputRecordRepeat* thisptr = (InputRecordRepeat*)data;
+
+ if( thisptr->record )
+ {
+ Var<std::string> var(_var);
+
+ FrameInput input;
+ input.index = thisptr->index;
+ input.var = name;
+ input.val = var.Get();
+
+ thisptr->record_queue.push_back(input);
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/var/vars.cpp b/externals/Pangolin/src/var/vars.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..42100a35a42d628fcec8d7d5993e844d92c548a1
--- /dev/null
+++ b/externals/Pangolin/src/var/vars.cpp
@@ -0,0 +1,260 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/var/varextra.h>
+#include <pangolin/var/varstate.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/picojson.h>
+#include <pangolin/utils/transform.h>
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+using namespace std;
+
+namespace pangolin
+{
+
+VarState& VarState::I() {
+ static VarState singleton;
+ return singleton;
+}
+
+VarState::VarState()
+ : varHasChanged(false)
+{
+}
+
+VarState::~VarState() {
+ Clear();
+}
+
+void VarState::Clear() {
+ for(VarStoreContainer::iterator i = vars.begin(); i != vars.end(); ++i) {
+ delete i->second;
+ }
+ vars.clear();
+ var_adds.clear();
+}
+
+void ProcessHistoricCallbacks(NewVarCallbackFn callback, void* data, const std::string& filter)
+{
+ for (VarState::VarStoreAdditions::const_iterator i = VarState::I().var_adds.begin(); i != VarState::I().var_adds.end(); ++i)
+ {
+ const std::string& name = *i;
+ if (StartsWith(name, filter)) {
+ callback(data, name, *VarState::I()[name], false);
+ }
+ }
+}
+
+void RegisterNewVarCallback(NewVarCallbackFn callback, void* data, const std::string& filter)
+{
+ VarState::I().new_var_callbacks.push_back(NewVarCallback(filter,callback,data));
+}
+
+void RegisterGuiVarChangedCallback(GuiVarChangedCallbackFn callback, void* data, const std::string& filter)
+{
+ VarState::I().gui_var_changed_callbacks.push_back(GuiVarChangedCallback(filter,callback,data));
+}
+
+// Recursively expand val
+string ProcessVal(const string& val )
+{
+ return Transform(val, [](const std::string& k) -> std::string {
+ if( VarState::I().Exists(k) ) {
+ return VarState::I()[k]->str->Get();
+ }else{
+ return std::string("#");
+ }
+ });
+}
+
+void AddVar(const std::string& name, const string& val )
+{
+ const std::string full = ProcessVal(val);
+
+ VarValueGeneric*& v = VarState::I()[name];
+ if(!v) {
+ VarValue<std::string>* nv = new VarValue<std::string>(val);
+ InitialiseNewVarMetaGeneric<std::string>(*nv, name);
+ v = nv;
+ }
+ v->str->Set(full);
+}
+
+#ifdef ALIAS
+void AddAlias(const string& alias, const string& name)
+{
+ std::map<std::string,_Var*>::iterator vi = vars.find(name);
+
+ if( vi != vars.end() )
+ {
+ _Var * v = vi->second;
+ vars[alias].create(v->val,v->val_default,v->type_name);
+ vars[alias].Meta().friendly = alias;
+ v->generic = false;
+ vars[alias].generic = false;
+ }else{
+ pango_print_error("Variable %s does not exist to alias.\n", name);
+ }
+}
+#endif
+
+void ParseVarsFile(const string& filename)
+{
+ ifstream f(filename.c_str());
+
+ if( f.is_open() )
+ {
+ while( !f.bad() && !f.eof())
+ {
+ const int c = f.peek();
+
+ if( isspace(c) )
+ {
+ // ignore leading whitespace
+ f.get();
+ }else{
+ if( c == '#' || c == '%' )
+ {
+ // ignore lines starting # or %
+ string comment;
+ getline(f,comment);
+ }else{
+ // Otherwise, find name and value, seperated by '=' and ';'
+ string name;
+ string val;
+ getline(f,name,'=');
+ getline(f,val,';');
+ name = Trim(name, " \t\n\r");
+ val = Trim(val, " \t\n\r");
+
+ if( name.size() >0 && val.size() > 0 )
+ {
+ if( !val.substr(0,1).compare("@") )
+ {
+#ifdef ALIAS
+ AddAlias(name,val.substr(1));
+#endif
+ }else{
+ AddVar(name,val);
+ }
+ }
+ }
+ }
+ }
+ f.close();
+ }else{
+ cerr << "Unable to open '" << filename << "' for configuration data" << endl;
+ }
+}
+
+PANGOLIN_EXPORT
+void LoadJsonFile(const std::string& filename, const string &prefix)
+{
+ bool some_change = false;
+
+ picojson::value file_json(picojson::object_type,true);
+ std::ifstream f(filename);
+ if(f.is_open()) {
+ const std::string err = picojson::parse(file_json,f);
+ if(err.empty()) {
+ if(file_json.contains("vars") ) {
+ picojson::value vars = file_json["vars"];
+ if(vars.is<picojson::object>()) {
+ for(picojson::object::iterator
+ i = vars.get<picojson::object>().begin();
+ i!= vars.get<picojson::object>().end();
+ ++i)
+ {
+ const std::string& name = i->first;
+ if(pangolin::StartsWith(name, prefix)) {
+ const std::string& val = i->second.get<std::string>();
+
+ VarValueGeneric*& v = VarState::I()[name];
+ if(!v) {
+ VarValue<std::string>* nv = new VarValue<std::string>(val);
+ InitialiseNewVarMetaGeneric<std::string>(*nv, name);
+ v = nv;
+ }else{
+ v->str->Set(val);
+ v->Meta().gui_changed = true;
+ }
+ some_change = true;
+ }
+ }
+ }
+ }
+ }else{
+ pango_print_error("%s\n", err.c_str());
+ }
+ }else{
+ pango_print_error("Unable to load vars from %s\n", filename.c_str());
+ }
+
+ if(some_change) {
+ FlagVarChanged();
+ }
+}
+
+PANGOLIN_EXPORT
+void SaveJsonFile(const std::string& filename, const string &prefix)
+{
+ picojson::value vars(picojson::object_type,true);
+
+ for(VarState::VarStoreAdditions::const_iterator
+ i = VarState::I().var_adds.begin();
+ i != VarState::I().var_adds.end();
+ ++i)
+ {
+ const std::string& name = *i;
+ if(StartsWith(name,prefix)) {
+ try{
+ const std::string val = VarState::I()[name]->str->Get();
+ vars[name] = val;
+ }catch(BadInputException)
+ {
+ // Ignore things we can't serialise
+ }
+ }
+ }
+
+ picojson::value file_json(picojson::object_type,true);
+ file_json["pangolin_version"] = PANGOLIN_VERSION_STRING;
+ file_json["vars"] = vars;
+
+ std::ofstream f(filename);
+ if(f.is_open()) {
+ f << file_json.serialize(true);
+ }else{
+ pango_print_error("Unable to serialise to %s\n", filename.c_str());
+ }
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/debayer.cpp b/externals/Pangolin/src/video/drivers/debayer.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7d01d24e872cea5ba73e75be8c9232c42eba6a56
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/debayer.cpp
@@ -0,0 +1,362 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/debayer.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+#ifdef HAVE_DC1394
+# include <dc1394/conversions.h>
+ const bool have_dc1394 = true;
+#else
+ const bool have_dc1394 = false;
+#endif
+
+namespace pangolin
+{
+
+pangolin::StreamInfo BayerOutputFormat( const StreamInfo& stream_in, bayer_method_t method, size_t start_offset)
+{
+ const bool downsample = (method == BAYER_METHOD_DOWNSAMPLE) || (method == BAYER_METHOD_DOWNSAMPLE_MONO);
+
+ const size_t w = downsample ? stream_in.Width() / 2 : stream_in.Width();
+ const size_t h = downsample ? stream_in.Height() / 2 : stream_in.Height();
+
+ pangolin::PixelFormat fmt =
+ (method == BAYER_METHOD_NONE) ?
+ stream_in.PixFormat() :
+ pangolin::PixelFormatFromString(
+ (stream_in.PixFormat().bpp == 16) ?
+ (method == BAYER_METHOD_DOWNSAMPLE_MONO ? "GRAY16LE" : "RGB48") :
+ (method == BAYER_METHOD_DOWNSAMPLE_MONO ? "GRAY8" : "RGB24")
+ );
+
+ fmt.channel_bit_depth = stream_in.PixFormat().channel_bit_depth;
+
+ return pangolin::StreamInfo( fmt, w, h, w*fmt.bpp / 8, (unsigned char*)0 + start_offset );
+}
+
+DebayerVideo::DebayerVideo(std::unique_ptr<VideoInterface> &src_, const std::vector<bayer_method_t>& bayer_method, color_filter_t tile)
+ : src(std::move(src_)), size_bytes(0), methods(bayer_method), tile(tile)
+{
+ if(!src.get()) {
+ throw VideoException("DebayerVideo: VideoInterface in must not be null");
+ }
+ videoin.push_back(src.get());
+
+ while(methods.size() < src->Streams().size()) {
+ methods.push_back(BAYER_METHOD_NONE);
+ }
+
+ for(size_t s=0; s< src->Streams().size(); ++s) {
+ if( (methods[s] < BAYER_METHOD_NONE) && (!have_dc1394 || src->Streams()[0].IsPitched()) ) {
+ pango_print_warn("debayer: Switching to simple downsampling method because No DC1394 or image is pitched.\n");
+ methods[s] = BAYER_METHOD_DOWNSAMPLE;
+ }
+
+ const StreamInfo& stin = src->Streams()[s];
+ streams.push_back(BayerOutputFormat(stin, methods[s], size_bytes));
+ size_bytes += streams.back().SizeBytes();
+ }
+
+ buffer = std::unique_ptr<unsigned char[]>(new unsigned char[src->SizeBytes()]);
+}
+
+DebayerVideo::~DebayerVideo()
+{
+}
+
+//! Implement VideoInput::Start()
+void DebayerVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+//! Implement VideoInput::Stop()
+void DebayerVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t DebayerVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& DebayerVideo::Streams() const
+{
+ return streams;
+}
+
+
+unsigned int DebayerVideo::AvailableFrames() const
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Debayer: child interface is not buffer aware.");
+ return 0;
+ }
+ else
+ {
+ return vpi->AvailableFrames();
+ }
+}
+
+bool DebayerVideo::DropNFrames(uint32_t n)
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Debayer: child interface is not buffer aware.");
+ return false;
+ }
+ else
+ {
+ return vpi->DropNFrames(n);
+ }
+}
+
+template<typename Tup, typename Tout, typename Tin>
+void DownsampleToMono(Image<Tout>& out, const Image<Tin>& in)
+{
+ for(int y=0; y< (int)out.h; ++y) {
+ Tout* pixout = out.RowPtr(y);
+ const Tin* irow0 = in.RowPtr(2*y);
+ const Tin* irow1 = in.RowPtr(2*y+1);
+ for(size_t x=0; x<out.w; ++x) {
+ Tup val = ((Tup)irow0[0] + (Tup)irow0[1] + (Tup)irow1[0] + (Tup)irow1[1]) / 4;
+ *(pixout++) = (Tout)std::min(std::max(static_cast<Tup>(0), val), static_cast<Tup>(std::numeric_limits<Tout>::max()));
+ irow0 += 2;
+ irow1 += 2;
+ }
+ }
+}
+
+template<typename Tout, typename Tin>
+void DownsampleDebayer(Image<Tout>& out, const Image<Tin>& in, color_filter_t tile)
+{
+ switch(tile) {
+ case DC1394_COLOR_FILTER_RGGB:
+ for(int y=0; y< (int)out.h; ++y) {
+ Tout* pixout = out.RowPtr(y);
+ const Tin* irow0 = in.RowPtr(2*y);
+ const Tin* irow1 = in.RowPtr(2*y+1);
+ for(size_t x=0; x<out.w; ++x) {
+ *(pixout++) = irow0[2*x];
+ *(pixout++) = (irow0[2*x+1] + irow1[2*x]) >> 1;
+ *(pixout++) = irow1[2*x+1];
+ }
+ }
+ break;
+ case DC1394_COLOR_FILTER_GBRG:
+ for(int y=0; y< (int)out.h; ++y) {
+ Tout* pixout = out.RowPtr(y);
+ const Tin* irow0 = in.RowPtr(2*y);
+ const Tin* irow1 = in.RowPtr(2*y+1);
+ for(size_t x=0; x<out.w; ++x) {
+ *(pixout++) = irow1[2*x];
+ *(pixout++) = (irow0[2*x] + irow1[2*x+1]) >> 1;
+ *(pixout++) = irow0[2*x+1];
+ }
+ }
+ break;
+ case DC1394_COLOR_FILTER_GRBG:
+ for(int y=0; y< (int)out.h; ++y) {
+ Tout* pixout = out.RowPtr(y);
+ const Tin* irow0 = in.RowPtr(2*y);
+ const Tin* irow1 = in.RowPtr(2*y+1);
+ for(size_t x=0; x<out.w; ++x) {
+ *(pixout++) = irow0[2*x+1];
+ *(pixout++) = (irow0[2*x] + irow1[2*x+1]) >> 1;
+ *(pixout++) = irow1[2*x];
+ }
+ }
+ break;
+ case DC1394_COLOR_FILTER_BGGR:
+ for(int y=0; y< (int)out.h; ++y) {
+ Tout* pixout = out.RowPtr(y);
+ const Tin* irow0 = in.RowPtr(2*y);
+ const Tin* irow1 = in.RowPtr(2*y+1);
+ for(size_t x=0; x<out.w; ++x) {
+ *(pixout++) = irow1[2*x+1];
+ *(pixout++) = (irow0[2*x+1] + irow1[2*x]) >> 1;
+ *(pixout++) = irow0[2*x];
+ }
+ }
+ break;
+ }
+}
+
+template<typename T>
+void PitchedImageCopy( Image<T>& img_out, const Image<T>& img_in ) {
+ if( img_out.w != img_in.w || img_out.h != img_in.h || sizeof(T) * img_in.w > img_out.pitch) {
+ throw std::runtime_error("PitchedImageCopy: Incompatible image sizes");
+ }
+
+ for(size_t y=0; y < img_out.h; ++y) {
+ std::memcpy(img_out.RowPtr((int)y), img_in.RowPtr((int)y), sizeof(T) * img_in.w);
+ }
+}
+
+template<typename Tout, typename Tin>
+void ProcessImage(Image<Tout>& img_out, const Image<Tin>& img_in, bayer_method_t method, color_filter_t tile)
+{
+ if(method == BAYER_METHOD_NONE) {
+ PitchedImageCopy(img_out, img_in.template UnsafeReinterpret<Tout>() );
+ }else if(method == BAYER_METHOD_DOWNSAMPLE_MONO) {
+ if( sizeof(Tout) == 1) {
+ DownsampleToMono<int,Tout, Tin>(img_out, img_in);
+ }else{
+ DownsampleToMono<double,Tout, Tin>(img_out, img_in);
+ }
+ }else if(method == BAYER_METHOD_DOWNSAMPLE) {
+ DownsampleDebayer(img_out, img_in, tile);
+ }else{
+#ifdef HAVE_DC1394
+ if(sizeof(Tout) == 1) {
+ dc1394_bayer_decoding_8bit(
+ (uint8_t*)img_in.ptr, (uint8_t*)img_out.ptr, img_in.w, img_in.h,
+ (dc1394color_filter_t)tile, (dc1394bayer_method_t)method
+ );
+ }else if(sizeof(Tout) == 2) {
+ dc1394_bayer_decoding_16bit(
+ (uint16_t*)img_in.ptr, (uint16_t*)img_out.ptr, img_in.w, img_in.h,
+ (dc1394color_filter_t)tile, (dc1394bayer_method_t)method,
+ 16
+ );
+ }
+#endif
+ }
+}
+
+void DebayerVideo::ProcessStreams(unsigned char* out, const unsigned char *in)
+{
+ for(size_t s=0; s<streams.size(); ++s) {
+ const StreamInfo& stin = videoin[0]->Streams()[s];
+ Image<unsigned char> img_in = stin.StreamImage(in);
+ Image<unsigned char> img_out = Streams()[s].StreamImage(out);
+
+ if(methods[s] == BAYER_METHOD_NONE) {
+ const size_t num_bytes = std::min(img_in.w, img_out.w) * stin.PixFormat().bpp / 8;
+ for(size_t y=0; y < img_out.h; ++y) {
+ std::memcpy(img_out.RowPtr((int)y), img_in.RowPtr((int)y), num_bytes);
+ }
+ }else if(stin.PixFormat().bpp == 8) {
+ ProcessImage(img_out, img_in, methods[s], tile);
+ }else if(stin.PixFormat().bpp == 16){
+ Image<uint16_t> img_in16 = img_in.UnsafeReinterpret<uint16_t>();
+ Image<uint16_t> img_out16 = img_out.UnsafeReinterpret<uint16_t>();
+ ProcessImage(img_out16, img_in16, methods[s], tile);
+ }else {
+ throw std::runtime_error("debayer: unhandled format combination: " + stin.PixFormat().format );
+ }
+ }
+}
+
+//! Implement VideoInput::GrabNext()
+bool DebayerVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNext(buffer.get(),wait)) {
+ ProcessStreams(image, buffer.get());
+ return true;
+ }else{
+ return false;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool DebayerVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNewest(buffer.get(),wait)) {
+ ProcessStreams(image, buffer.get());
+ return true;
+ }else{
+ return false;
+ }
+}
+
+std::vector<VideoInterface*>& DebayerVideo::InputStreams()
+{
+ return videoin;
+}
+
+color_filter_t DebayerVideo::ColorFilterFromString(std::string str)
+{
+ if(!str.compare("rggb") || !str.compare("RGGB")) return DC1394_COLOR_FILTER_RGGB;
+ else if(!str.compare("gbrg") || !str.compare("GBRG")) return DC1394_COLOR_FILTER_GBRG;
+ else if(!str.compare("grbg") || !str.compare("GRBG")) return DC1394_COLOR_FILTER_GRBG;
+ else if(!str.compare("bggr") || !str.compare("BGGR")) return DC1394_COLOR_FILTER_BGGR;
+ else {
+ pango_print_error("Debayer error, %s is not a valid tile type using RGGB\n", str.c_str());
+ return DC1394_COLOR_FILTER_RGGB;
+ }
+}
+
+bayer_method_t DebayerVideo::BayerMethodFromString(std::string str)
+{
+ if(!str.compare("nearest")) return BAYER_METHOD_NEAREST;
+ else if(!str.compare("simple")) return BAYER_METHOD_SIMPLE;
+ else if(!str.compare("bilinear")) return BAYER_METHOD_BILINEAR;
+ else if(!str.compare("hqlinear")) return BAYER_METHOD_HQLINEAR;
+ else if(!str.compare("downsample")) return BAYER_METHOD_DOWNSAMPLE;
+ else if(!str.compare("edgesense")) return BAYER_METHOD_EDGESENSE;
+ else if(!str.compare("vng")) return BAYER_METHOD_VNG;
+ else if(!str.compare("ahd")) return BAYER_METHOD_AHD;
+ else if(!str.compare("mono")) return BAYER_METHOD_DOWNSAMPLE_MONO;
+ else if(!str.compare("none")) return BAYER_METHOD_NONE;
+ else {
+ pango_print_error("Debayer error, %s is not a valid debayer method using downsample\n", str.c_str());
+ return BAYER_METHOD_DOWNSAMPLE;
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(DebayerVideo)
+{
+ struct DebayerVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ const std::string tile_string = uri.Get<std::string>("tile","rggb");
+ const std::string method = uri.Get<std::string>("method","none");
+ const color_filter_t tile = DebayerVideo::ColorFilterFromString(tile_string);
+
+ std::vector<bayer_method_t> methods;
+ for(size_t s=0; s < subvid->Streams().size(); ++s) {
+ const std::string key = std::string("method") + ToString(s+1);
+ std::string method_s = uri.Get<std::string>(key, method);
+ methods.push_back(DebayerVideo::BayerMethodFromString(method_s));
+ }
+ return std::unique_ptr<VideoInterface>( new DebayerVideo(subvid, methods, tile) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<DebayerVideoFactory>(), 10, "debayer");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/deinterlace.cpp b/externals/Pangolin/src/video/drivers/deinterlace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..177858741fd9958b701dd92c2cd0140558049826
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/deinterlace.cpp
@@ -0,0 +1,107 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/deinterlace.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <dc1394/conversions.h>
+
+namespace pangolin
+{
+
+DeinterlaceVideo::DeinterlaceVideo(std::unique_ptr<VideoInterface> &videoin_)
+ : videoin(std::move(videoin_)), buffer(0)
+{
+ if(videoin->Streams().size() != 1)
+ throw VideoException("FirewireDeinterlace input must have exactly one stream");
+
+ const StreamInfo& stmin = videoin->Streams()[0];
+
+ StreamInfo stm1(PixelFormatFromString("GRAY8"), stmin.Width(), stmin.Height(), stmin.Width(), 0);
+ StreamInfo stm2(PixelFormatFromString("GRAY8"), stmin.Width(), stmin.Height(), stmin.Width(), (unsigned char*)0 + stmin.Width()*stmin.Height());
+ streams.push_back(stm1);
+ streams.push_back(stm2);
+
+ buffer = new unsigned char[videoin->SizeBytes()];
+
+ std::cout << videoin->Streams()[0].Width() << ", " << videoin->Streams()[0].Height() << std::endl;
+}
+
+DeinterlaceVideo::~DeinterlaceVideo()
+{
+ delete[] buffer;
+}
+
+size_t DeinterlaceVideo::SizeBytes() const
+{
+ return videoin->SizeBytes();
+}
+
+const std::vector<StreamInfo>& DeinterlaceVideo::Streams() const
+{
+ return streams;
+}
+
+void DeinterlaceVideo::Start()
+{
+ videoin->Start();
+}
+
+void DeinterlaceVideo::Stop()
+{
+ videoin->Stop();
+}
+
+bool DeinterlaceVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin->GrabNext(buffer, wait)) {
+ return ( dc1394_deinterlace_stereo(buffer,image, videoin->Streams()[0].Width(), 2*videoin->Streams()[0].Height() ) == DC1394_SUCCESS );
+ }
+ return false;
+}
+
+bool DeinterlaceVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin->GrabNewest(buffer, wait)) {
+ return ( dc1394_deinterlace_stereo(buffer,image, videoin->Streams()[0].Width(), 2*videoin->Streams()[0].Height() ) == DC1394_SUCCESS );
+ }
+ return false;
+}
+
+PANGOLIN_REGISTER_FACTORY(DeinterlaceVideo)
+{
+ struct DeinterlaceVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ return std::unique_ptr<VideoInterface>( new DeinterlaceVideo(subvid) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<DeinterlaceVideoFactory>(), 10, "deinterlace");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/depthsense.cpp b/externals/Pangolin/src/video/drivers/depthsense.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5098beb08f3e588d53775d62750b8485045723cb
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/depthsense.cpp
@@ -0,0 +1,656 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/depthsense.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <iomanip>
+
+namespace pangolin
+{
+
+const size_t ROGUE_ADDR = 0x01;
+const double MAX_DELTA_TIME = 20000.0; //u_s
+
+DepthSenseContext& DepthSenseContext::I()
+{
+ static DepthSenseContext s;
+ return s;
+}
+
+DepthSense::Context& DepthSenseContext::Context()
+{
+ return g_context;
+}
+
+void DepthSenseContext::NewDeviceRunning()
+{
+ running_devices++;
+ if(running_devices == 1) {
+ StartNodes();
+ }
+}
+
+void DepthSenseContext::DeviceClosing()
+{
+ running_devices--;
+ if(running_devices == 0) {
+ StopNodes();
+
+ // Force destruction of current context
+ g_context = DepthSense::Context();
+ }
+}
+
+DepthSenseVideo* DepthSenseContext::GetDepthSenseVideo(size_t device_num, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri)
+{
+ if(running_devices == 0) {
+ // Initialise SDK
+ g_context = DepthSense::Context::create("localhost");
+ }
+
+ // Get the list of currently connected devices
+ std::vector<DepthSense::Device> da = g_context.getDevices();
+
+ if( da.size() > device_num )
+ {
+ return new DepthSenseVideo(da[device_num], s1, s2, dim1, dim2, fps1, fps2, uri);
+ }
+
+ throw VideoException("DepthSense device not connected.");
+}
+
+DepthSenseContext::DepthSenseContext()
+ : is_running(false), running_devices(0)
+{
+}
+
+DepthSenseContext::~DepthSenseContext()
+{
+ StopNodes();
+}
+
+
+void DepthSenseContext::StartNodes()
+{
+ if(!is_running) {
+ // Launch EventLoop thread
+ event_thread = std::thread(&DepthSenseContext::EventLoop, this );
+ }
+}
+
+void DepthSenseContext::StopNodes()
+{
+ if(is_running && event_thread.joinable()) {
+ g_context.quit();
+ event_thread.join();
+ }
+}
+
+void DepthSenseContext::EventLoop()
+{
+ is_running = true;
+ g_context.startNodes();
+ g_context.run();
+ g_context.stopNodes();
+ is_running = false;
+}
+
+DepthSenseVideo::DepthSenseVideo(DepthSense::Device device, DepthSenseSensorType s1, DepthSenseSensorType s2, ImageDim dim1, ImageDim dim2, unsigned int fps1, unsigned int fps2, const Uri& uri)
+ : device(device), fill_image(0), depthmap_stream(-1), rgb_stream(-1), gotDepth(0), gotColor(0),
+ enableDepth(false), enableColor(false), depthTs(0.0), colorTs(0.0), size_bytes(0)
+{
+ streams_properties = &frame_properties["streams"];
+ *streams_properties = picojson::value(picojson::array_type, false);
+ streams_properties->get<picojson::array>().resize(2);
+
+ sensorConfig[0] = {s1, dim1, fps1};
+ sensorConfig[1] = {s2, dim2, fps2};
+ ConfigureNodes(uri);
+
+ DepthSenseContext::I().NewDeviceRunning();
+}
+
+DepthSenseVideo::~DepthSenseVideo()
+{
+ if (g_cnode.isSet()) DepthSenseContext::I().Context().unregisterNode(g_cnode);
+ if (g_dnode.isSet()) DepthSenseContext::I().Context().unregisterNode(g_dnode);
+
+ fill_image = (unsigned char*)ROGUE_ADDR;
+ cond_image_requested.notify_all();
+
+ DepthSenseContext::I().DeviceClosing();
+}
+
+picojson::value Json(DepthSense::IntrinsicParameters& p)
+{
+ picojson::value js;
+ js["model"] = "polynomial";
+ js["width"] = p.width;
+ js["height"] = p.height;
+ js["RDF"] = "[1,0,0; 0,1,0; 0,0,1]";
+
+ js["fx"] = p.fx;
+ js["fy"] = p.fy;
+ js["u0"] = p.cx;
+ js["v0"] = p.cy;
+ js["k1"] = p.k1;
+ js["k2"] = p.k2;
+ js["k3"] = p.k3;
+ js["p1"] = p.p1;
+ js["p2"] = p.p2;
+
+ return js;
+}
+
+picojson::value Json(DepthSense::ExtrinsicParameters& p)
+{
+ picojson::value js;
+ js["rows"] = "3";
+ js["cols"] = "4";
+
+ std::ostringstream oss;
+ oss << std::setprecision(17);
+ oss << "[" << p.r11 << "," << p.r12 << "," << p.r13 << "," << p.t1 << ";";
+ oss << p.r21 << "," << p.r22 << "," << p.r23 << "," << p.t2 << ";";
+ oss << p.r31 << "," << p.r32 << "," << p.r33 << "," << p.t3 << "]";
+
+ js["data"] = oss.str();
+ return js;
+}
+
+void DepthSenseVideo::ConfigureNodes(const Uri& uri)
+{
+ std::vector<DepthSense::Node> nodes = device.getNodes();
+
+ for (int i = 0; i<2; ++i)
+ {
+ switch (sensorConfig[i].type)
+ {
+ case DepthSenseDepth:
+ {
+ for (int n = 0; n < (int)nodes.size(); n++)
+ {
+ DepthSense::Node node = nodes[n];
+ if ((node.is<DepthSense::DepthNode>()) && (!g_dnode.isSet()))
+ {
+ depthmap_stream = i;
+ g_dnode = node.as<DepthSense::DepthNode>();
+ ConfigureDepthNode(sensorConfig[i], uri);
+ DepthSenseContext::I().Context().registerNode(node);
+ }
+ }
+ break;
+ }
+ case DepthSenseRgb:
+ {
+ for (int n = 0; n < (int)nodes.size(); n++)
+ {
+ DepthSense::Node node = nodes[n];
+ if ((node.is<DepthSense::ColorNode>()) && (!g_cnode.isSet()))
+ {
+ rgb_stream = i;
+ g_cnode = node.as<DepthSense::ColorNode>();
+ ConfigureColorNode(sensorConfig[i], uri);
+ DepthSenseContext::I().Context().registerNode(node);
+ }
+ }
+ break;
+ }
+ default:
+ continue;
+ }
+ }
+
+ DepthSense::StereoCameraParameters scp = device.getStereoCameraParameters();
+
+ //Set json device properties for intrinsics and extrinsics
+ picojson::value& jsintrinsics = device_properties["intrinsics"];
+ if (jsintrinsics.is<picojson::null>()) {
+ jsintrinsics = picojson::value(picojson::array_type, false);
+ jsintrinsics.get<picojson::array>().resize(streams.size());
+ if (depthmap_stream >= 0) jsintrinsics[depthmap_stream] = Json(scp.depthIntrinsics);
+ if (rgb_stream >= 0) jsintrinsics[rgb_stream] = Json(scp.colorIntrinsics);
+ }
+
+ picojson::value& jsextrinsics = device_properties["extrinsics"];
+ if(jsextrinsics.is<picojson::null>()){
+ jsextrinsics = Json(scp.extrinsics);
+ }
+}
+
+inline DepthSense::FrameFormat ImageDim2FrameFormat(const ImageDim& dim)
+{
+ DepthSense::FrameFormat retVal = DepthSense::FRAME_FORMAT_UNKNOWN;
+ if(dim.x == 160 && dim.y == 120)
+ {
+ retVal = DepthSense::FRAME_FORMAT_QQVGA;
+ }
+ else if(dim.x == 176 && dim.y == 144)
+ {
+ retVal = DepthSense::FRAME_FORMAT_QCIF;
+ }
+ else if(dim.x == 240 && dim.y == 160)
+ {
+ retVal = DepthSense::FRAME_FORMAT_HQVGA;
+ }
+ else if(dim.x == 320 && dim.y == 240)
+ {
+ retVal = DepthSense::FRAME_FORMAT_QVGA;
+ }
+ else if(dim.x == 352 && dim.y == 288)
+ {
+ retVal = DepthSense::FRAME_FORMAT_CIF;
+ }
+ else if(dim.x == 480 && dim.y == 320)
+ {
+ retVal = DepthSense::FRAME_FORMAT_HVGA;
+ }
+ else if(dim.x == 640 && dim.y == 480)
+ {
+ retVal = DepthSense::FRAME_FORMAT_VGA;
+ }
+ else if(dim.x == 1280 && dim.y == 720)
+ {
+ retVal = DepthSense::FRAME_FORMAT_WXGA_H;
+ }
+ else if(dim.x == 320 && dim.y == 120)
+ {
+ retVal = DepthSense::FRAME_FORMAT_DS311;
+ }
+ else if(dim.x == 1024 && dim.y == 768)
+ {
+ retVal = DepthSense::FRAME_FORMAT_XGA;
+ }
+ else if(dim.x == 800 && dim.y == 600)
+ {
+ retVal = DepthSense::FRAME_FORMAT_SVGA;
+ }
+ else if(dim.x == 636 && dim.y == 480)
+ {
+ retVal = DepthSense::FRAME_FORMAT_OVVGA;
+ }
+ else if(dim.x == 640 && dim.y == 240)
+ {
+ retVal = DepthSense::FRAME_FORMAT_WHVGA;
+ }
+ else if(dim.x == 640 && dim.y == 360)
+ {
+ retVal = DepthSense::FRAME_FORMAT_NHD;
+ }
+ return retVal;
+}
+
+void DepthSenseVideo::UpdateParameters(const DepthSense::Node& node, const Uri& uri)
+{
+ DepthSense::Type type = node.getType();
+ picojson::value& jsnode = device_properties[type.name()];
+
+ std::vector<DepthSense::PropertyBase> properties = type.getProperties();
+ for(std::vector<DepthSense::PropertyBase>::const_iterator it = properties.begin(); it != properties.end(); ++it) {
+ const DepthSense::PropertyBase& prop = *it;
+
+ if (prop.is<DepthSense::Property<int32_t> >()) {
+ DepthSense::Property<int32_t> tprop = prop.as<DepthSense::Property<int32_t> >();
+ if (uri.Contains(prop.name())) {
+ if (!prop.isReadOnly()) {
+ tprop.setValue(node, uri.Get<int32_t>(prop.name(), tprop.getValue(node)));
+ } else {
+ pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+ }
+ }
+ jsnode[prop.name()] = tprop.getValue(node);
+ } else if (prop.is<DepthSense::Property<float> >()) {
+ DepthSense::Property<float> tprop = prop.as<DepthSense::Property<float> >();
+ if (uri.Contains(prop.name())) {
+ if (!prop.isReadOnly()) {
+ tprop.setValue(node, uri.Get<float>(prop.name(), tprop.getValue(node)));
+ } else {
+ pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+ }
+ }
+ jsnode[prop.name()] = tprop.getValue(node);
+ } else if (prop.is<DepthSense::Property<bool> >()) {
+ DepthSense::Property<bool> tprop = prop.as<DepthSense::Property<bool> >();
+ if (uri.Contains(prop.name())) {
+ if (!prop.isReadOnly()) {
+ tprop.setValue(node, uri.Get<bool>(prop.name(), tprop.getValue(node)));
+ } else {
+ pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+ }
+ }
+ jsnode[prop.name()] = tprop.getValue(node);
+ } else if (prop.is<DepthSense::Property<std::string> >()){
+ DepthSense::Property<std::string> tprop = prop.as<DepthSense::Property<std::string> >();
+ if (uri.Contains(prop.name())) {
+ if (!prop.isReadOnly()) {
+ tprop.setValue(node, uri.Get<std::string>(prop.name(), tprop.getValue(node)).c_str() );
+ } else {
+ pango_print_warn("DepthSense property '%s' is read-only\n", prop.name().c_str() );
+ }
+ }
+ jsnode[prop.name()] = tprop.getValue(node);
+ }
+ }
+}
+
+void DepthSenseVideo::ConfigureDepthNode(const SensorConfig& sensorConfig, const Uri& uri)
+{
+ g_dnode.newSampleReceivedEvent().connect(this, &DepthSenseVideo::onNewDepthSample);
+
+ DepthSense::DepthNode::Configuration config = g_dnode.getConfiguration();
+
+ config.frameFormat = ImageDim2FrameFormat(sensorConfig.dim);
+ config.framerate = sensorConfig.fps;
+ config.mode = DepthSense::DepthNode::CAMERA_MODE_CLOSE_MODE;
+ config.saturation = true;
+
+ try {
+ DepthSenseContext::I().Context().requestControl(g_dnode, 0);
+ g_dnode.setConfiguration(config);
+ g_dnode.setEnableDepthMap(true);
+ } catch (DepthSense::Exception& e) {
+ throw pangolin::VideoException("DepthSense exception whilst configuring node", e.what());
+ }
+
+ //Set pangolin stream for this channel
+ const int w = sensorConfig.dim.x;
+ const int h = sensorConfig.dim.y;
+
+ const PixelFormat pfmt = PixelFormatFromString("GRAY16LE");
+
+ const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp) / 8, (unsigned char*)0);
+ streams.push_back(stream_info);
+
+ size_bytes += stream_info.SizeBytes();
+
+ enableDepth = true;
+
+ UpdateParameters(g_dnode, uri);
+}
+
+void DepthSenseVideo::ConfigureColorNode(const SensorConfig& sensorConfig, const Uri& uri)
+{
+ // connect new color sample handler
+ g_cnode.newSampleReceivedEvent().connect(this, &DepthSenseVideo::onNewColorSample);
+
+ DepthSense::ColorNode::Configuration config = g_cnode.getConfiguration();
+ config.frameFormat = ImageDim2FrameFormat(sensorConfig.dim);
+ config.compression = DepthSense::COMPRESSION_TYPE_MJPEG;
+ config.powerLineFrequency = DepthSense::POWER_LINE_FREQUENCY_50HZ;
+ config.framerate = sensorConfig.fps;
+
+ try {
+ DepthSenseContext::I().Context().requestControl(g_cnode,0);
+ g_cnode.setConfiguration(config);
+ g_cnode.setEnableColorMap(true);
+ UpdateParameters(g_cnode, uri);
+ } catch (DepthSense::Exception& e) {
+ throw pangolin::VideoException("DepthSense exception whilst configuring node", e.what());
+ }
+
+ //Set pangolin stream for this channel
+ const int w = sensorConfig.dim.x;
+ const int h = sensorConfig.dim.y;
+
+ const PixelFormat pfmt = PixelFormatFromString("BGR24");
+
+ const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp) / 8, (unsigned char*)0 + size_bytes);
+ streams.push_back(stream_info);
+
+ size_bytes += stream_info.SizeBytes();
+
+ enableColor = true;
+}
+
+void DepthSenseVideo::onNewColorSample(DepthSense::ColorNode node, DepthSense::ColorNode::NewSampleReceivedData data)
+{
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ // Wait for fill request
+ while (!fill_image) {
+ cond_image_requested.wait(lock);
+ }
+
+ // Update per-frame parameters
+ //printf("Color delta: %.1f\n", fabs(colorTs - data.timeOfCapture));
+ colorTs = data.timeOfCapture;
+ picojson::value& jsstream = frame_properties["streams"][rgb_stream];
+ jsstream["time_us"] = data.timeOfCapture;
+
+ if (fill_image != (unsigned char*)ROGUE_ADDR) {
+ // Fill with data
+ unsigned char* imagePtr = fill_image;
+ bool copied = false;
+ for (int i = 0; !copied && i < 2; ++i)
+ {
+ switch (sensorConfig[i].type)
+ {
+ case DepthSenseDepth:
+ {
+ imagePtr += streams[i].SizeBytes();
+ break;
+ }
+ case DepthSenseRgb:
+ {
+ // Leave as BGR
+ std::memcpy(imagePtr, data.colorMap, streams[i].SizeBytes());
+ copied = true;
+ break;
+ }
+ default:
+ continue;
+ }
+ }
+ gotColor++;
+ }
+
+ //printf("Got color at: %.1f\n", colorTs);
+
+ if(gotDepth)
+ {
+ double delta = fabs(GetDeltaTime());
+ if(delta > MAX_DELTA_TIME)
+ {
+ //printf("**** Waiting for another depth, delta: %.1f ****\n", delta);
+ gotDepth = 0;
+ return;
+ }
+ }
+ }
+
+ cond_image_filled.notify_one();
+}
+
+void DepthSenseVideo::onNewDepthSample(DepthSense::DepthNode node, DepthSense::DepthNode::NewSampleReceivedData data)
+{
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+
+ // Wait for fill request
+ while(!fill_image) {
+ cond_image_requested.wait(lock);
+ }
+
+ // Update per-frame parameters
+ //printf("Depth delta: %.1f\n", fabs(depthTs - data.timeOfCapture));
+ depthTs = data.timeOfCapture;
+
+ picojson::value& jsstream = frame_properties["streams"][depthmap_stream];
+ jsstream["time_us"] = depthTs;
+
+ if(fill_image != (unsigned char*)ROGUE_ADDR) {
+ // Fill with data
+ unsigned char* imagePtr = fill_image;
+ bool copied = false;
+ for (int i = 0; i < 2; ++i)
+ {
+ switch (sensorConfig[i].type)
+ {
+ case DepthSenseDepth:
+ {
+ memcpy(imagePtr, data.depthMap, streams[i].SizeBytes());
+ copied = true;
+ break;
+ }
+ case DepthSenseRgb:
+ {
+ imagePtr += streams[i].SizeBytes();
+ break;
+ }
+ default:
+ continue;
+ }
+ if(copied)
+ {
+ break;
+ }
+ }
+ gotDepth++;
+ }
+
+ //printf("Got depth at: %.1f\n", depthTs);
+
+ if(gotColor)
+ {
+ double delta = fabs(GetDeltaTime());
+ if(delta > MAX_DELTA_TIME)
+ {
+ //printf("**** Waiting for another color, delta: %.1f ****\n", delta);
+ gotColor = 0;
+ return;
+ }
+ }
+ }
+
+ cond_image_filled.notify_one();
+}
+
+void DepthSenseVideo::Start()
+{
+}
+
+void DepthSenseVideo::Stop()
+{
+}
+
+size_t DepthSenseVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+const std::vector<StreamInfo>& DepthSenseVideo::Streams() const
+{
+ return streams;
+}
+
+bool DepthSenseVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ if(fill_image) {
+ throw std::runtime_error("GrabNext Cannot be called concurrently");
+ }
+
+ //printf("#### Grab Next ####\n");
+
+ // Request that image is filled with data
+ fill_image = image;
+ cond_image_requested.notify_one();
+
+ // Wait until it has been filled successfully.
+ {
+ std::unique_lock<std::mutex> lock(update_mutex);
+ while ((enableDepth && !gotDepth) || (enableColor && !gotColor))
+ {
+ cond_image_filled.wait(lock);
+ }
+
+ if (gotDepth)
+ {
+ gotDepth = 0;
+ }
+ if (gotColor)
+ {
+ gotColor = 0;
+ }
+ fill_image = 0;
+ }
+
+ //printf("Delta time: %.1f\n", fabs(GetDeltaTime()));
+
+ return true;
+}
+
+bool DepthSenseVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+double DepthSenseVideo::GetDeltaTime() const
+{
+ return depthTs - colorTs;
+}
+
+DepthSenseSensorType depthsense_sensor(const std::string& str)
+{
+ if (!str.compare("rgb")) {
+ return DepthSenseRgb;
+ }
+ else if (!str.compare("depth")) {
+ return DepthSenseDepth;
+ }
+ else if (str.empty()) {
+ return DepthSenseUnassigned;
+ }
+ else{
+ throw pangolin::VideoException("Unknown DepthSense sensor", str);
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(DepthSenseVideo)
+{
+ struct DepthSenseVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ DepthSenseSensorType img1 = depthsense_sensor(uri.Get<std::string>("img1", "depth"));
+ DepthSenseSensorType img2 = depthsense_sensor(uri.Get<std::string>("img2", ""));
+
+ const ImageDim dim1 = uri.Get<ImageDim>("size1", img1 == DepthSenseDepth ? ImageDim(320, 240) : ImageDim(640, 480) );
+ const ImageDim dim2 = uri.Get<ImageDim>("size2", img2 == DepthSenseDepth ? ImageDim(320, 240) : ImageDim(640, 480) );
+
+ const unsigned int fps1 = uri.Get<unsigned int>("fps1", 30);
+ const unsigned int fps2 = uri.Get<unsigned int>("fps2", 30);
+
+ return std::unique_ptr<VideoInterface>(
+ DepthSenseContext::I().GetDepthSenseVideo(0, img1, img2, dim1, dim2, fps1, fps2, uri)
+ );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<DepthSenseVideoFactory>(), 10, "depthsense");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/ffmpeg.cpp b/externals/Pangolin/src/video/drivers/ffmpeg.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..67b2dc9baa5473fe22687d8d1315e6f3792214b8
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/ffmpeg.cpp
@@ -0,0 +1,952 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <array>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/video/drivers/ffmpeg.h>
+
+// Some versions of FFMPEG define this horrid macro in global scope.
+#undef PixelFormat
+
+// It is impossible to keep up with ffmpeg deprecations, so ignore these warnings.
+#if defined(_GCC_) || defined(_CLANG_)
+# pragma GCC diagnostic ignored "-Wdeprecated"
+#endif
+
+
+extern "C"
+{
+#include <libavformat/avio.h>
+#include <libavutil/mathematics.h>
+#include <libavdevice/avdevice.h>
+}
+
+#define CODEC_FLAG_GLOBAL_HEADER AV_CODEC_FLAG_GLOBAL_HEADER
+
+namespace pangolin
+{
+
+#ifdef HAVE_FFMPEG_AVPIXELFORMAT
+# define TEST_PIX_FMT_RETURN(fmt) case AV_PIX_FMT_##fmt: return #fmt;
+#else
+# define AV_PIX_FMT_NONE PIX_FMT_NONE
+# define AV_PIX_FMT_GRAY8 PIX_FMT_GRAY8
+# define TEST_PIX_FMT_RETURN(fmt) case PIX_FMT_##fmt: return #fmt;
+#endif
+
+AVPixelFormat FfmpegFmtFromString(const std::string fmt)
+{
+ const std::string lfmt = ToLowerCopy(fmt);
+ if(!lfmt.compare("gray8") || !lfmt.compare("grey8") || !lfmt.compare("grey")) {
+ return AV_PIX_FMT_GRAY8;
+ }
+ return av_get_pix_fmt(lfmt.c_str());
+}
+
+
+std::string FfmpegFmtToString(const AVPixelFormat fmt)
+{
+ switch( fmt )
+ {
+ TEST_PIX_FMT_RETURN(YUV420P);
+ TEST_PIX_FMT_RETURN(YUYV422);
+ TEST_PIX_FMT_RETURN(RGB24);
+ TEST_PIX_FMT_RETURN(BGR24);
+ TEST_PIX_FMT_RETURN(YUV422P);
+ TEST_PIX_FMT_RETURN(YUV444P);
+ TEST_PIX_FMT_RETURN(YUV410P);
+ TEST_PIX_FMT_RETURN(YUV411P);
+ TEST_PIX_FMT_RETURN(GRAY8);
+ TEST_PIX_FMT_RETURN(MONOWHITE);
+ TEST_PIX_FMT_RETURN(MONOBLACK);
+ TEST_PIX_FMT_RETURN(PAL8);
+ TEST_PIX_FMT_RETURN(YUVJ420P);
+ TEST_PIX_FMT_RETURN(YUVJ422P);
+ TEST_PIX_FMT_RETURN(YUVJ444P);
+#ifdef FF_API_XVMC
+ TEST_PIX_FMT_RETURN(XVMC_MPEG2_MC);
+ TEST_PIX_FMT_RETURN(XVMC_MPEG2_IDCT);
+#endif
+ TEST_PIX_FMT_RETURN(UYVY422);
+ TEST_PIX_FMT_RETURN(UYYVYY411);
+ TEST_PIX_FMT_RETURN(BGR8);
+ TEST_PIX_FMT_RETURN(BGR4);
+ TEST_PIX_FMT_RETURN(BGR4_BYTE);
+ TEST_PIX_FMT_RETURN(RGB8);
+ TEST_PIX_FMT_RETURN(RGB4);
+ TEST_PIX_FMT_RETURN(RGB4_BYTE);
+ TEST_PIX_FMT_RETURN(NV12);
+ TEST_PIX_FMT_RETURN(NV21);
+ TEST_PIX_FMT_RETURN(ARGB);
+ TEST_PIX_FMT_RETURN(RGBA);
+ TEST_PIX_FMT_RETURN(ABGR);
+ TEST_PIX_FMT_RETURN(BGRA);
+ TEST_PIX_FMT_RETURN(GRAY16BE);
+ TEST_PIX_FMT_RETURN(GRAY16LE);
+ TEST_PIX_FMT_RETURN(YUV440P);
+ TEST_PIX_FMT_RETURN(YUVJ440P);
+ TEST_PIX_FMT_RETURN(YUVA420P);
+#ifdef FF_API_VDPAU
+ TEST_PIX_FMT_RETURN(VDPAU_H264);
+ TEST_PIX_FMT_RETURN(VDPAU_MPEG1);
+ TEST_PIX_FMT_RETURN(VDPAU_MPEG2);
+ TEST_PIX_FMT_RETURN(VDPAU_WMV3);
+ TEST_PIX_FMT_RETURN(VDPAU_VC1);
+#endif
+ TEST_PIX_FMT_RETURN(RGB48BE );
+ TEST_PIX_FMT_RETURN(RGB48LE );
+ TEST_PIX_FMT_RETURN(RGB565BE);
+ TEST_PIX_FMT_RETURN(RGB565LE);
+ TEST_PIX_FMT_RETURN(RGB555BE);
+ TEST_PIX_FMT_RETURN(RGB555LE);
+ TEST_PIX_FMT_RETURN(BGR565BE);
+ TEST_PIX_FMT_RETURN(BGR565LE);
+ TEST_PIX_FMT_RETURN(BGR555BE);
+ TEST_PIX_FMT_RETURN(BGR555LE);
+ TEST_PIX_FMT_RETURN(VAAPI_MOCO);
+ TEST_PIX_FMT_RETURN(VAAPI_IDCT);
+ TEST_PIX_FMT_RETURN(VAAPI_VLD);
+ TEST_PIX_FMT_RETURN(YUV420P16LE);
+ TEST_PIX_FMT_RETURN(YUV420P16BE);
+ TEST_PIX_FMT_RETURN(YUV422P16LE);
+ TEST_PIX_FMT_RETURN(YUV422P16BE);
+ TEST_PIX_FMT_RETURN(YUV444P16LE);
+ TEST_PIX_FMT_RETURN(YUV444P16BE);
+#ifdef FF_API_VDPAU
+ TEST_PIX_FMT_RETURN(VDPAU_MPEG4);
+#endif
+ TEST_PIX_FMT_RETURN(DXVA2_VLD);
+ TEST_PIX_FMT_RETURN(RGB444BE);
+ TEST_PIX_FMT_RETURN(RGB444LE);
+ TEST_PIX_FMT_RETURN(BGR444BE);
+ TEST_PIX_FMT_RETURN(BGR444LE);
+ TEST_PIX_FMT_RETURN(Y400A );
+ TEST_PIX_FMT_RETURN(NB );
+ default: return "";
+ }
+}
+
+#undef TEST_PIX_FMT_RETURN
+
+FfmpegVideo::FfmpegVideo(const std::string filename, const std::string strfmtout, const std::string codec_hint, bool dump_info, int user_video_stream, ImageDim size)
+ :pFormatCtx(0)
+{
+ InitUrl(filename, strfmtout, codec_hint, dump_info, user_video_stream, size);
+}
+
+void FfmpegVideo::InitUrl(const std::string url, const std::string strfmtout, const std::string codec_hint, bool dump_info, int user_video_stream, ImageDim size)
+{
+ if( url.find('*') != url.npos )
+ throw VideoException("Wildcards not supported. Please use ffmpegs printf style formatting for image sequences. e.g. img-000000%04d.ppm");
+
+ // Register all formats and codecs
+ av_register_all();
+ // Register all devices
+ avdevice_register_all();
+
+ AVInputFormat* fmt = NULL;
+
+ if( !codec_hint.empty() ) {
+ fmt = av_find_input_format(codec_hint.c_str());
+ }
+
+#if (LIBAVFORMAT_VERSION_MAJOR >= 53)
+ AVDictionary* options = nullptr;
+ if(size.x != 0 && size.y != 0) {
+ std::string s = std::to_string(size.x) + "x" + std::to_string(size.y);
+ av_dict_set(&options, "video_size", s.c_str(), 0);
+ }
+ if( avformat_open_input(&pFormatCtx, url.c_str(), fmt, &options) )
+#else
+ // Deprecated - can't use with mjpeg
+ if( av_open_input_file(&pFormatCtx, url.c_str(), fmt, 0, NULL) )
+#endif
+ throw VideoException("Couldn't open stream");
+
+ if( !ToLowerCopy(codec_hint).compare("mjpeg") )
+#ifdef HAVE_FFMPEG_MAX_ANALYZE_DURATION2
+ pFormatCtx->max_analyze_duration2 = AV_TIME_BASE * 0.0;
+#else
+ pFormatCtx->max_analyze_duration = AV_TIME_BASE * 0.0;
+#endif
+
+ // Retrieve stream information
+#if (LIBAVFORMAT_VERSION_MAJOR >= 53)
+ if(avformat_find_stream_info(pFormatCtx, 0)<0)
+#else
+ // Deprecated
+ if(av_find_stream_info(pFormatCtx)<0)
+#endif
+ throw VideoException("Couldn't find stream information");
+
+ if(dump_info) {
+ // Dump information about file onto standard error
+#if (LIBAVFORMAT_VERSION_MAJOR >= 53)
+ av_dump_format(pFormatCtx, 0, url.c_str(), false);
+#else
+ // Deprecated
+ dump_format(pFormatCtx, 0, url.c_str(), false);
+#endif
+ }
+
+ // Find the first video stream
+ videoStream=-1;
+ audioStream=-1;
+
+ std::vector<int> videoStreams;
+ std::vector<int> audioStreams;
+
+ for(unsigned i=0; i<pFormatCtx->nb_streams; i++)
+ {
+ if(pFormatCtx->streams[i]->codec->codec_type==AVMEDIA_TYPE_VIDEO)
+ {
+ videoStreams.push_back(i);
+ }else if(pFormatCtx->streams[i]->codec->codec_type==AVMEDIA_TYPE_AUDIO)
+ {
+ audioStreams.push_back(i);
+ }
+ }
+
+ if(videoStreams.size()==0)
+ throw VideoException("Couldn't find a video stream");
+
+ if(0 <= user_video_stream && user_video_stream < (int)videoStreams.size() ) {
+ videoStream = videoStreams[user_video_stream];
+ }else{
+ videoStream = videoStreams[0];
+ }
+
+ // Get a pointer to the codec context for the video stream
+ pVidCodecCtx = pFormatCtx->streams[videoStream]->codec;
+
+ // Find the decoder for the video stream
+ pVidCodec=avcodec_find_decoder(pVidCodecCtx->codec_id);
+ if(pVidCodec==0)
+ throw VideoException("Codec not found");
+
+ // Open video codec
+#if LIBAVCODEC_VERSION_MAJOR > 52
+ if(avcodec_open2(pVidCodecCtx, pVidCodec,0)<0)
+#else
+ if(avcodec_open(pVidCodecCtx, pVidCodec)<0)
+#endif
+ throw VideoException("Could not open codec");
+
+ // Hack to correct wrong frame rates that seem to be generated by some codecs
+ if(pVidCodecCtx->time_base.num>1000 && pVidCodecCtx->time_base.den==1)
+ pVidCodecCtx->time_base.den=1000;
+
+
+ // Allocate video frames
+#if LIBAVUTIL_VERSION_MAJOR >= 54
+ pFrame = av_frame_alloc();
+ pFrameOut = av_frame_alloc();
+#else
+ // deprecated
+ pFrame = avcodec_alloc_frame();
+ pFrameOut = avcodec_alloc_frame();
+#endif
+ if(!pFrame || !pFrameOut)
+ throw VideoException("Couldn't allocate frames");
+
+ fmtout = FfmpegFmtFromString(strfmtout);
+ if(fmtout == AV_PIX_FMT_NONE )
+ throw VideoException("Output format not recognised",strfmtout);
+
+ // Image dimensions
+ const int w = pVidCodecCtx->width;
+ const int h = pVidCodecCtx->height;
+
+ // Determine required buffer size and allocate buffer
+ numBytesOut=avpicture_get_size(fmtout, w, h);
+
+ buffer= new uint8_t[numBytesOut];
+
+ // Assign appropriate parts of buffer to image planes in pFrameRGB
+ avpicture_fill((AVPicture *)pFrameOut, buffer, fmtout, w, h);
+
+ // Allocate SWS for converting pixel formats
+ img_convert_ctx = sws_getContext(w, h,
+ pVidCodecCtx->pix_fmt,
+ w, h, fmtout, FFMPEG_POINT,
+ NULL, NULL, NULL);
+ if(img_convert_ctx == NULL) {
+ throw VideoException("Cannot initialize the conversion context");
+ }
+
+ // Populate stream info for users to query
+ const PixelFormat strm_fmt = PixelFormatFromString(FfmpegFmtToString(fmtout));
+ const StreamInfo stream(strm_fmt, w, h, (w*strm_fmt.bpp)/8, 0);
+ streams.push_back(stream);
+}
+
+FfmpegVideo::~FfmpegVideo()
+{
+ // Free the RGB image
+ delete[] buffer;
+ av_free(pFrameOut);
+
+ // Free the YUV frame
+ av_free(pFrame);
+
+ // Close the codec
+ avcodec_close(pVidCodecCtx);
+
+ // Close the video file
+#if (LIBAVFORMAT_VERSION_MAJOR >= 54 || (LIBAVFORMAT_VERSION_MAJOR >= 53 && LIBAVFORMAT_VERSION_MINOR >= 21) )
+ avformat_close_input(&pFormatCtx);
+#else
+ // Deprecated
+ av_close_input_file(pFormatCtx);
+#endif
+
+ // Free pixel conversion context
+ sws_freeContext(img_convert_ctx);
+}
+
+const std::vector<StreamInfo>& FfmpegVideo::Streams() const
+{
+ return streams;
+}
+
+size_t FfmpegVideo::SizeBytes() const
+{
+ return numBytesOut;
+}
+
+void FfmpegVideo::Start()
+{
+}
+
+void FfmpegVideo::Stop()
+{
+}
+
+bool FfmpegVideo::GrabNext(unsigned char* image, bool /*wait*/)
+{
+ int gotFrame = 0;
+
+ while(!gotFrame && av_read_frame(pFormatCtx, &packet)>=0)
+ {
+ // Is this a packet from the video stream?
+ if(packet.stream_index==videoStream)
+ {
+ // Decode video frame
+ avcodec_decode_video2(pVidCodecCtx, pFrame, &gotFrame, &packet);
+ }
+
+ // Did we get a video frame?
+ if(gotFrame) {
+ sws_scale(img_convert_ctx, pFrame->data, pFrame->linesize, 0, pVidCodecCtx->height, pFrameOut->data, pFrameOut->linesize);
+ memcpy(image,pFrameOut->data[0],numBytesOut);
+ }
+
+ // Free the packet that was allocated by av_read_frame
+ av_free_packet(&packet);
+ }
+
+ return gotFrame;
+}
+
+bool FfmpegVideo::GrabNewest(unsigned char *image, bool wait)
+{
+ return GrabNext(image,wait);
+}
+
+void FfmpegConverter::ConvertContext::convert(const unsigned char* src, unsigned char* dst)
+{
+ // avpicture_fill expects uint8_t* w/o const as the second parameter in earlier versions
+ avpicture_fill((AVPicture*)avsrc, const_cast<unsigned char*>(src + src_buffer_offset), fmtsrc, w, h);
+ avpicture_fill((AVPicture*)avdst, dst + dst_buffer_offset, fmtdst, w, h);
+ sws_scale( img_convert_ctx,
+ avsrc->data, avsrc->linesize, 0, h,
+ avdst->data, avdst->linesize );
+}
+
+FfmpegConverter::FfmpegConverter(std::unique_ptr<VideoInterface> &videoin_, const std::string sfmtdst, FfmpegMethod method )
+ :videoin(std::move(videoin_))
+{
+ if( !videoin )
+ throw VideoException("Source video interface not specified");
+
+ input_buffer = std::unique_ptr<unsigned char[]>(new unsigned char[videoin->SizeBytes()]);
+
+ converters.resize(videoin->Streams().size());
+
+ dst_buffer_size = 0;
+
+ for(size_t i=0; i < videoin->Streams().size(); ++i) {
+ const StreamInfo instrm = videoin->Streams()[i];
+
+ converters[i].w=instrm.Width();
+ converters[i].h=instrm.Height();
+
+ converters[i].fmtdst = FfmpegFmtFromString(sfmtdst);
+ converters[i].fmtsrc = FfmpegFmtFromString(instrm.PixFormat());
+ converters[i].img_convert_ctx = sws_getContext(
+ instrm.Width(), instrm.Height(), converters[i].fmtsrc,
+ instrm.Width(), instrm.Height(), converters[i].fmtdst,
+ method, NULL, NULL, NULL
+ );
+ if(!converters[i].img_convert_ctx)
+ throw VideoException("Could not create SwScale context for pixel conversion");
+
+ converters[i].dst_buffer_offset=dst_buffer_size;
+ converters[i].src_buffer_offset=instrm.Offset() - (unsigned char*)0;
+ //converters[i].src_buffer_offset=src_buffer_size;
+
+ #if LIBAVUTIL_VERSION_MAJOR >= 54
+ converters[i].avsrc = av_frame_alloc();
+ converters[i].avdst = av_frame_alloc();
+ #else
+ // deprecated
+ converters[i].avsrc = avcodec_alloc_frame();
+ converters[i].avdst = avcodec_alloc_frame();
+ #endif
+
+ const PixelFormat pxfmtdst = PixelFormatFromString(sfmtdst);
+ const StreamInfo sdst( pxfmtdst, instrm.Width(), instrm.Height(), (instrm.Width()*pxfmtdst.bpp)/8, (unsigned char*)0 + converters[i].dst_buffer_offset );
+ streams.push_back(sdst);
+
+
+ //src_buffer_size += instrm.SizeBytes();
+ dst_buffer_size += avpicture_get_size(converters[i].fmtdst, instrm.Width(), instrm.Height());
+ }
+
+}
+
+FfmpegConverter::~FfmpegConverter()
+{
+ for(ConvertContext&c:converters)
+ {
+ av_free(c.avsrc);
+ av_free(c.avdst);
+ }
+}
+
+void FfmpegConverter::Start()
+{
+ // No-Op
+}
+
+void FfmpegConverter::Stop()
+{
+ // No-Op
+}
+
+size_t FfmpegConverter::SizeBytes() const
+{
+ return dst_buffer_size;
+}
+
+const std::vector<StreamInfo>& FfmpegConverter::Streams() const
+{
+ return streams;
+}
+
+bool FfmpegConverter::GrabNext( unsigned char* image, bool wait )
+{
+ if( videoin->GrabNext(input_buffer.get(),wait) )
+ {
+ for(ConvertContext&c:converters) {
+ c.convert(input_buffer.get(),image);
+ }
+ return true;
+ }
+ return false;
+}
+
+bool FfmpegConverter::GrabNewest( unsigned char* image, bool wait )
+{
+ if( videoin->GrabNewest(input_buffer.get(),wait) )
+ {
+ for(ConvertContext&c:converters) {
+ c.convert(input_buffer.get(),image);
+ }
+ return true;
+ }
+ return false;
+}
+
+// Based on this example
+// http://cekirdek.pardus.org.tr/~ismail/ffmpeg-docs/output-example_8c-source.html
+static AVStream* CreateStream(AVFormatContext *oc, CodecID codec_id, uint64_t frame_rate, int bit_rate, AVPixelFormat EncoderFormat, int width, int height)
+{
+ AVCodec* codec = avcodec_find_encoder(codec_id);
+ if (!(codec)) throw
+ VideoException("Could not find encoder");
+
+#if (LIBAVFORMAT_VERSION_MAJOR >= 54 || (LIBAVFORMAT_VERSION_MAJOR >= 53 && LIBAVFORMAT_VERSION_MINOR >= 21) )
+ AVStream* stream = avformat_new_stream(oc, codec);
+#else
+ AVStream* stream = av_new_stream(oc, codec_id);
+#endif
+
+ if (!stream) throw VideoException("Could not allocate stream");
+
+ stream->id = oc->nb_streams-1;
+
+ switch (codec->type) {
+// case AVMEDIA_TYPE_AUDIO:
+// stream->id = 1;
+// stream->codec->sample_fmt = AV_SAMPLE_FMT_S16;
+// stream->codec->bit_rate = 64000;
+// stream->codec->sample_rate = 44100;
+// stream->codec->channels = 2;
+// break;
+ case AVMEDIA_TYPE_VIDEO:
+ stream->codec->codec_id = codec_id;
+ stream->codec->bit_rate = bit_rate;
+ stream->codec->width = width;
+ stream->codec->height = height;
+ stream->codec->time_base.num = 1;
+ stream->codec->time_base.den = frame_rate;
+ stream->codec->gop_size = 12;
+ stream->codec->pix_fmt = EncoderFormat;
+ break;
+ default:
+ break;
+ }
+
+ /* Some formats want stream headers to be separate. */
+ if (oc->oformat->flags & AVFMT_GLOBALHEADER)
+ stream->codec->flags |= CODEC_FLAG_GLOBAL_HEADER;
+
+ /* open the codec */
+ int ret = avcodec_open2(stream->codec, codec, NULL);
+ if (ret < 0) throw VideoException("Could not open video codec");
+
+ return stream;
+}
+
+class FfmpegVideoOutputStream
+{
+public:
+ FfmpegVideoOutputStream(FfmpegVideoOutput& recorder, CodecID codec_id, uint64_t frame_rate, int bit_rate, const StreamInfo& input_info );
+ ~FfmpegVideoOutputStream();
+
+ const StreamInfo& GetStreamInfo() const;
+
+ void WriteImage(const uint8_t* img, int w, int h, double time);
+ void Flush();
+
+protected:
+ void WriteAvPacket(AVPacket* pkt);
+ void WriteFrame(AVFrame* frame);
+ double BaseFrameTime();
+
+ FfmpegVideoOutput& recorder;
+
+ StreamInfo input_info;
+ AVPixelFormat input_format;
+ AVPixelFormat output_format;
+
+ AVPicture src_picture;
+ AVPicture dst_picture;
+ int64_t last_pts;
+
+ // These pointers are owned by class
+ AVStream* stream;
+ SwsContext *sws_ctx;
+ AVFrame* frame;
+
+ bool flip;
+};
+
+void FfmpegVideoOutputStream::WriteAvPacket(AVPacket* pkt)
+{
+ if (pkt->size) {
+ pkt->stream_index = stream->index;
+ int64_t pts = pkt->pts;
+ /* convert unit from CODEC's timestamp to stream's one */
+#define C2S(field) \
+ do { \
+ if (pkt->field != (int64_t) AV_NOPTS_VALUE) \
+ pkt->field = av_rescale_q(pkt->field, \
+ stream->codec->time_base, \
+ stream->time_base); \
+ } while (0)
+
+ C2S(pts);
+ C2S(dts);
+ C2S(duration);
+#undef C2S
+ int ret = av_interleaved_write_frame(recorder.oc, pkt);
+ if (ret < 0) throw VideoException("Error writing video frame");
+ if(pkt->pts != (int64_t)AV_NOPTS_VALUE) last_pts = pts;
+ }
+}
+
+void FfmpegVideoOutputStream::WriteFrame(AVFrame* frame)
+{
+ AVPacket pkt;
+ pkt.data = NULL;
+ pkt.size = 0;
+ av_init_packet(&pkt);
+
+ int ret;
+ int got_packet = 1;
+
+#if FF_API_LAVF_FMT_RAWPICTURE
+ // Setup AVPacket
+ if (recorder.oc->oformat->flags & AVFMT_RAWPICTURE) {
+ /* Raw video case - directly store the picture in the packet */
+ pkt.flags |= AV_PKT_FLAG_KEY;
+ pkt.data = frame->data[0];
+ pkt.size = sizeof(AVPicture);
+ pkt.pts = frame->pts;
+ ret = 0;
+ } else {
+#else
+ {
+#endif
+ /* encode the image */
+#if (LIBAVFORMAT_VERSION_MAJOR >= 54)
+ ret = avcodec_encode_video2(stream->codec, &pkt, frame, &got_packet);
+#else
+ // TODO: Why is ffmpeg so fussy about this buffer size?
+ // Making this too big results in garbled output.
+ // Too small and it will fail entirely.
+ pkt.size = 50* FF_MIN_BUFFER_SIZE; //std::max(FF_MIN_BUFFER_SIZE, frame->width * frame->height * 4 );
+ // TODO: Make sure this is being freed by av_free_packet
+ pkt.data = (uint8_t*) malloc(pkt.size);
+ pkt.pts = frame->pts;
+ ret = avcodec_encode_video(stream->codec, pkt.data, pkt.size, frame);
+ got_packet = ret > 0;
+#endif
+ if (ret < 0) throw VideoException("Error encoding video frame");
+ }
+
+ if (ret >= 0 && got_packet) {
+ WriteAvPacket(&pkt);
+ }
+
+ av_free_packet(&pkt);
+}
+
+void FfmpegVideoOutputStream::WriteImage(const uint8_t* img, int w, int h, double time=-1.0)
+{
+ const int64_t pts = (time >= 0) ? time / BaseFrameTime() : ++last_pts;
+
+ recorder.StartStream();
+
+ AVCodecContext *c = stream->codec;
+
+ if(flip) {
+ // Earlier versions of ffmpeg do not annotate img as const, although it is
+ avpicture_fill(&src_picture,const_cast<uint8_t*>(img),input_format,w,h);
+ for(int i=0; i<4; ++i) {
+ src_picture.data[i] += (h-1) * src_picture.linesize[i];
+ src_picture.linesize[i] *= -1;
+ }
+ }else{
+ // Earlier versions of ffmpeg do not annotate img as const, although it is
+ avpicture_fill(&src_picture,const_cast<uint8_t*>(img),input_format,w,h);
+ }
+
+ if (c->pix_fmt != input_format || c->width != w || c->height != h) {
+ if(!sws_ctx) {
+ sws_ctx = sws_getCachedContext( sws_ctx,
+ w, h, input_format,
+ c->width, c->height, c->pix_fmt,
+ SWS_BICUBIC, NULL, NULL, NULL
+ );
+ if (!sws_ctx) throw VideoException("Could not initialize the conversion context");
+ }
+ sws_scale(sws_ctx,
+ src_picture.data, src_picture.linesize, 0, h,
+ dst_picture.data, dst_picture.linesize
+ );
+ *((AVPicture *)frame) = dst_picture;
+ } else {
+ *((AVPicture *)frame) = src_picture;
+ }
+
+ frame->pts = pts;
+ frame->width = w;
+ frame->height = h;
+ WriteFrame(frame);
+}
+
+void FfmpegVideoOutputStream::Flush()
+{
+#if (LIBAVFORMAT_VERSION_MAJOR >= 54)
+ if (stream->codec->codec->capabilities & AV_CODEC_CAP_DELAY) {
+ /* some CODECs like H.264 needs flushing buffered frames by encoding NULL frames. */
+ /* cf. https://www.ffmpeg.org/doxygen/trunk/group__lavc__encoding.html#ga2c08a4729f72f9bdac41b5533c4f2642 */
+
+ AVPacket pkt;
+ pkt.data = NULL;
+ pkt.size = 0;
+ av_init_packet(&pkt);
+
+ int got_packet = 1;
+ while (got_packet) {
+ int ret = avcodec_encode_video2(stream->codec, &pkt, NULL, &got_packet);
+ if (ret < 0) throw VideoException("Error encoding video frame");
+ WriteAvPacket(&pkt);
+ }
+
+ av_free_packet(&pkt);
+ }
+#endif
+}
+
+const StreamInfo& FfmpegVideoOutputStream::GetStreamInfo() const
+{
+ return input_info;
+}
+
+double FfmpegVideoOutputStream::BaseFrameTime()
+{
+ return (double)stream->codec->time_base.num / (double)stream->codec->time_base.den;
+}
+
+FfmpegVideoOutputStream::FfmpegVideoOutputStream(
+ FfmpegVideoOutput& recorder, CodecID codec_id, uint64_t frame_rate,
+ int bit_rate, const StreamInfo& input_info
+)
+ : recorder(recorder), input_info(input_info),
+ input_format(FfmpegFmtFromString(input_info.PixFormat())),
+ output_format( FfmpegFmtFromString("YUV420P") ),
+ last_pts(-1), sws_ctx(NULL), frame(NULL), flip(true)
+{
+ stream = CreateStream(recorder.oc, codec_id, frame_rate, bit_rate, output_format, input_info.Width(), input_info.Height() );
+
+ // Allocate the encoded raw picture.
+ int ret = avpicture_alloc(&dst_picture, stream->codec->pix_fmt, stream->codec->width, stream->codec->height);
+ if (ret < 0) throw VideoException("Could not allocate picture");
+
+ // Allocate frame
+#if LIBAVUTIL_VERSION_MAJOR >= 54
+ frame = av_frame_alloc();
+#else
+ // Deprecated
+ frame = avcodec_alloc_frame();
+#endif
+}
+
+FfmpegVideoOutputStream::~FfmpegVideoOutputStream()
+{
+ Flush();
+
+ if(sws_ctx) {
+ sws_freeContext(sws_ctx);
+ }
+
+ av_free(frame);
+ av_free(dst_picture.data[0]);
+ avcodec_close(stream->codec);
+}
+
+FfmpegVideoOutput::FfmpegVideoOutput(const std::string& filename, int base_frame_rate, int bit_rate)
+ : filename(filename), started(false), oc(NULL),
+ frame_count(0), base_frame_rate(base_frame_rate), bit_rate(bit_rate), is_pipe(pangolin::IsPipe(filename))
+{
+ Initialise(filename);
+}
+
+FfmpegVideoOutput::~FfmpegVideoOutput()
+{
+ Close();
+}
+
+bool FfmpegVideoOutput::IsPipe() const
+{
+ return is_pipe;
+}
+
+void FfmpegVideoOutput::Initialise(std::string filename)
+{
+ av_register_all();
+
+#ifdef HAVE_FFMPEG_AVFORMAT_ALLOC_OUTPUT_CONTEXT2
+ int ret = avformat_alloc_output_context2(&oc, NULL, NULL, filename.c_str());
+#else
+ oc = avformat_alloc_context();
+ oc->oformat = av_guess_format(NULL, filename.c_str(), NULL);
+ int ret = oc->oformat ? 0 : -1;
+#endif
+
+ if (ret < 0 || !oc) {
+ pango_print_error("Could not deduce output format from file extension: using MPEG.\n");
+#ifdef HAVE_FFMPEG_AVFORMAT_ALLOC_OUTPUT_CONTEXT2
+ ret = avformat_alloc_output_context2(&oc, NULL, "mpeg", filename.c_str());
+#else
+ oc->oformat = av_guess_format("mpeg", filename.c_str(), NULL);
+#endif
+ if (ret < 0 || !oc) throw VideoException("Couldn't create AVFormatContext");
+ }
+
+ /* open the output file, if needed */
+ if (!(oc->oformat->flags & AVFMT_NOFILE)) {
+ ret = avio_open(&oc->pb, filename.c_str(), AVIO_FLAG_WRITE);
+ if (ret < 0) throw VideoException("Could not open '%s'\n", filename);
+ }
+}
+
+void FfmpegVideoOutput::StartStream()
+{
+ if(!started) {
+#if (LIBAVFORMAT_VERSION_MAJOR >= 53)
+ av_dump_format(oc, 0, filename.c_str(), 1);
+#else
+ // Deprecated
+ dump_format(oc, 0, filename.c_str(), 1);
+#endif
+
+ /* Write the stream header, if any. */
+ int ret = avformat_write_header(oc, NULL);
+ if (ret < 0) throw VideoException("Error occurred when opening output file");
+
+ started = true;
+ }
+}
+
+void FfmpegVideoOutput::Close()
+{
+ for(std::vector<FfmpegVideoOutputStream*>::iterator i = streams.begin(); i!=streams.end(); ++i)
+ {
+ (*i)->Flush();
+ delete *i;
+ }
+
+ av_write_trailer(oc);
+
+ if (!(oc->oformat->flags & AVFMT_NOFILE)) avio_close(oc->pb);
+
+ avformat_free_context(oc);
+}
+
+const std::vector<StreamInfo>& FfmpegVideoOutput::Streams() const
+{
+ return strs;
+}
+
+void FfmpegVideoOutput::SetStreams(const std::vector<StreamInfo>& str, const std::string& /*uri*/, const picojson::value& properties)
+{
+ strs.insert(strs.end(), str.begin(), str.end());
+
+ for(std::vector<StreamInfo>::const_iterator i = str.begin(); i!= str.end(); ++i)
+ {
+ streams.push_back( new FfmpegVideoOutputStream(
+ *this, oc->oformat->video_codec, base_frame_rate, bit_rate, *i
+ ) );
+ }
+
+ if(!properties.is<picojson::null>()) {
+ pango_print_warn("Ignoring attached video properties.");
+ }
+}
+
+int FfmpegVideoOutput::WriteStreams(const unsigned char* data, const picojson::value& /*frame_properties*/)
+{
+ for(std::vector<FfmpegVideoOutputStream*>::iterator i = streams.begin(); i!= streams.end(); ++i)
+ {
+ FfmpegVideoOutputStream& s = **i;
+ Image<unsigned char> img = s.GetStreamInfo().StreamImage(data);
+ s.WriteImage(img.ptr, img.w, img.h);
+ }
+ return frame_count++;
+}
+
+PANGOLIN_REGISTER_FACTORY(FfmpegVideo)
+{
+ struct FfmpegVideoFactory : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const std::array<std::string,43> ffmpeg_ext = {{
+ ".3g2",".3gp", ".amv", ".asf", ".avi", ".drc", ".flv", ".f4v",
+ ".f4p", ".f4a", ".f4b", ".gif", ".gifv", ".m4v", ".mkv", ".mng", ".mov", ".qt",
+ ".mp4", ".m4p", ".m4v", ".mpg", ".mp2", ".mpeg", ".mpe", ".mpv", ".mpg", ".mpeg",
+ ".m2v", ".mxf", ".nsv", ".ogv", ".ogg", ".rm", ".rmvb", ".roq", ".svi", ".vob",
+ ".webm", ".wmv", ".yuv", ".h264", ".h265"
+ }};
+
+ if(!uri.scheme.compare("ffmpeg") || !uri.scheme.compare("file") || !uri.scheme.compare("files") )
+ {
+ if(!uri.scheme.compare("file") || !uri.scheme.compare("files")) {
+ const std::string ext = FileLowercaseExtention(uri.url);
+ if(std::find(ffmpeg_ext.begin(), ffmpeg_ext.end(), ext) == ffmpeg_ext.end()) {
+ // Don't try to load unknown files without the ffmpeg:// scheme.
+ return std::unique_ptr<VideoInterface>();
+ }
+ }
+ std::string outfmt = uri.Get<std::string>("fmt","RGB24");
+ ToUpper(outfmt);
+ const int video_stream = uri.Get<int>("stream",-1);
+ return std::unique_ptr<VideoInterface>( new FfmpegVideo(uri.url.c_str(), outfmt, "", false, video_stream) );
+ }else if( !uri.scheme.compare("v4lmjpeg")) {
+ const int video_stream = uri.Get<int>("stream",-1);
+ const ImageDim size = uri.Get<ImageDim>("size",ImageDim(0,0));
+ return std::unique_ptr<VideoInterface>( new FfmpegVideo(uri.url.c_str(),"RGB24", "video4linux", false, video_stream, size) );
+ } else if( !uri.scheme.compare("mjpeg")) {
+ return std::unique_ptr<VideoInterface>( new FfmpegVideo(uri.url.c_str(),"RGB24", "MJPEG" ) );
+ }else if( !uri.scheme.compare("convert") ) {
+ std::string outfmt = uri.Get<std::string>("fmt","RGB24");
+ ToUpper(outfmt);
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ return std::unique_ptr<VideoInterface>( new FfmpegConverter(subvid,outfmt,FFMPEG_POINT) );
+ }else{
+ return std::unique_ptr<VideoInterface>();
+ }
+ }
+ };
+
+ auto factory = std::make_shared<FfmpegVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "ffmpeg");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "v4lmjpeg");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "mjpeg");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 20, "convert");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 15, "file");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 15, "files");
+}
+
+PANGOLIN_REGISTER_FACTORY(FfmpegVideoOutput)
+{
+ struct FfmpegVideoFactory final : public FactoryInterface<VideoOutputInterface> {
+ std::unique_ptr<VideoOutputInterface> Open(const Uri& uri) override {
+ int desired_frame_rate = uri.Get("fps", 60);
+ int desired_bit_rate = uri.Get("bps", 20000*1024);
+ std::string filename = uri.url;
+
+ if(uri.Contains("unique_filename")) {
+ filename = MakeUniqueFilename(filename);
+ }
+
+ return std::unique_ptr<VideoOutputInterface>(
+ new FfmpegVideoOutput(filename, desired_frame_rate, desired_bit_rate)
+ );
+ }
+ };
+
+ auto factory = std::make_shared<FfmpegVideoFactory>();
+ FactoryRegistry<VideoOutputInterface>::I().RegisterFactory(factory, 10, "ffmpeg");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/firewire.cpp b/externals/Pangolin/src/video/drivers/firewire.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8e9baa3913cff4be90984e41597e93085aa18659
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/firewire.cpp
@@ -0,0 +1,969 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/firewire.h>
+#include <pangolin/video/drivers/deinterlace.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <inttypes.h>
+
+using namespace std;
+
+namespace pangolin
+{
+
+void FirewireVideo::init_camera(
+ uint64_t guid, int dma_frames,
+ dc1394speed_t iso_speed,
+ dc1394video_mode_t video_mode,
+ dc1394framerate_t framerate
+ ) {
+
+ if(video_mode>=DC1394_VIDEO_MODE_FORMAT7_0)
+ throw VideoException("format7 modes need to be initialized through the constructor that allows for specifying the roi");
+
+ camera = dc1394_camera_new (d, guid);
+ if (!camera)
+ throw VideoException("Failed to initialize camera");
+
+ // Attempt to stop camera if it is already running
+ dc1394switch_t is_iso_on = DC1394_OFF;
+ dc1394_video_get_transmission(camera, &is_iso_on);
+ if (is_iso_on==DC1394_ON) {
+ dc1394_video_set_transmission(camera, DC1394_OFF);
+ }
+
+
+ cout << "Using camera with GUID " << camera->guid << endl;
+
+ //-----------------------------------------------------------------------
+ // setup capture
+ //-----------------------------------------------------------------------
+
+ if( iso_speed >= DC1394_ISO_SPEED_800)
+ {
+ err=dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set DC1394_OPERATION_MODE_1394B");
+ }
+
+ err=dc1394_video_set_iso_speed(camera, iso_speed);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set iso speed");
+
+ err=dc1394_video_set_mode(camera, video_mode);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set video mode");
+
+ err=dc1394_video_set_framerate(camera, framerate);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set framerate");
+
+ err=dc1394_capture_setup(camera,dma_frames, DC1394_CAPTURE_FLAGS_DEFAULT);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not setup camera - check settings");
+
+ //-----------------------------------------------------------------------
+ // initialise width and height from mode
+ //-----------------------------------------------------------------------
+ dc1394_get_image_size_from_video_mode(camera, video_mode, &width, &height);
+
+ init_stream_info();
+ Start();
+}
+
+
+// Note:
+// the following was tested on a IIDC camera over USB therefore might not work as
+// well on a camera over proper firewire transport
+void FirewireVideo::init_format7_camera(
+ uint64_t guid, int dma_frames,
+ dc1394speed_t iso_speed,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top, bool reset_at_boot
+ ) {
+
+ if(video_mode< DC1394_VIDEO_MODE_FORMAT7_0)
+ throw VideoException("roi can be specified only for format7 modes");
+
+ camera = dc1394_camera_new (d, guid);
+ if (!camera)
+ throw VideoException("Failed to initialize camera");
+
+ // Attempt to stop camera if it is already running
+ dc1394switch_t is_iso_on = DC1394_OFF;
+ dc1394_video_get_transmission(camera, &is_iso_on);
+ if (is_iso_on==DC1394_ON) {
+ dc1394_video_set_transmission(camera, DC1394_OFF);
+ }
+
+ cout << "Using camera with GUID " << camera->guid << endl;
+
+ if(reset_at_boot){
+ dc1394_camera_reset(camera);
+ }
+
+ //-----------------------------------------------------------------------
+ // setup mode and roi
+ //-----------------------------------------------------------------------
+
+ if(iso_speed >= DC1394_ISO_SPEED_800)
+ {
+ err=dc1394_video_set_operation_mode(camera, DC1394_OPERATION_MODE_1394B);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set DC1394_OPERATION_MODE_1394B");
+ }
+
+ err=dc1394_video_set_iso_speed(camera, iso_speed);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set iso speed");
+
+ // check that the required mode is actually supported
+ dc1394format7mode_t format7_info;
+
+ err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not get format7 mode info");
+
+ // safely set the video mode
+ err=dc1394_video_set_mode(camera, video_mode);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 video mode");
+
+ // set position to 0,0 so that setting any size within min and max is a valid command
+ err = dc1394_format7_set_image_position(camera, video_mode,0,0);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 image position");
+
+ // work out the desired image size
+ width = nearest_value(width, format7_info.unit_pos_x, 0, format7_info.max_size_x - left);
+ height = nearest_value(height, format7_info.unit_pos_y, 0, format7_info.max_size_y - top);
+
+ // set size
+ err = dc1394_format7_set_image_size(camera,video_mode,width,height);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 size");
+
+ // get the info again since many parameters depend on image size
+ err = dc1394_format7_get_mode_info(camera, video_mode, &format7_info);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not get format7 mode info");
+
+ // work out position of roi
+ left = nearest_value(left, format7_info.unit_size_x, format7_info.unit_size_x, format7_info.max_size_x - width);
+ top = nearest_value(top, format7_info.unit_size_y, format7_info.unit_size_y, format7_info.max_size_y - height);
+
+ // set roi position
+ err = dc1394_format7_set_image_position(camera,video_mode,left,top);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 size");
+
+ this->width = width;
+ this->height = height;
+ this->top = top;
+ this->left = left;
+
+ cout<<"roi: "<<left<<" "<<top<<" "<<width<<" "<<height<<" ";
+
+
+ //-----------------------------------------------------------------------
+ // setup frame rate
+ //-----------------------------------------------------------------------
+
+ err=dc1394_format7_set_packet_size(camera,video_mode, format7_info.max_packet_size);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 packet size");
+
+ if((framerate != MAX_FR) && (framerate != EXT_TRIG)){
+ //set the framerate by using the absolute feature as suggested by the
+ //folks at PointGrey
+ err = dc1394_feature_set_absolute_control(camera,DC1394_FEATURE_FRAME_RATE,DC1394_ON);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not turn on absolute frame rate control");
+
+ err = dc1394_feature_set_mode(camera,DC1394_FEATURE_FRAME_RATE,DC1394_FEATURE_MODE_MANUAL);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not make frame rate manual ");
+
+ err=dc1394_feature_set_absolute_value(camera,DC1394_FEATURE_FRAME_RATE,framerate);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not set format7 framerate ");
+ }
+
+ // ask the camera what is the resulting framerate (this assume that such a rate is actually
+ // allowed by the shutter time)
+ float value;
+ err=dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_FRAME_RATE,&value);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not get framerate");
+
+ cout<<" framerate(shutter permitting):"<<value<<endl;
+
+ //-----------------------------------------------------------------------
+ // setup capture
+ //-----------------------------------------------------------------------
+
+ err=dc1394_capture_setup(camera,dma_frames, DC1394_CAPTURE_FLAGS_DEFAULT);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not setup camera - check settings");
+
+ init_stream_info();
+ Start();
+}
+
+std::string Dc1394ColorCodingToString(dc1394color_coding_t coding)
+{
+ switch(coding)
+ {
+ case DC1394_COLOR_CODING_RGB8 : return "RGB24";
+ case DC1394_COLOR_CODING_MONO8 : return "GRAY8";
+
+ case DC1394_COLOR_CODING_MONO16 : return "GRAY16LE";
+ case DC1394_COLOR_CODING_RGB16 : return "RGB48LE";
+
+ case DC1394_COLOR_CODING_MONO16S : return "GRAY16BE";
+ case DC1394_COLOR_CODING_RGB16S : return "RGB48BE";
+
+ case DC1394_COLOR_CODING_YUV411 : return "YUV411P";
+ case DC1394_COLOR_CODING_YUV422 : return "YUV422P";
+ case DC1394_COLOR_CODING_YUV444 : return "YUV444P";
+
+ case DC1394_COLOR_CODING_RAW8 : return "GRAY8";
+ case DC1394_COLOR_CODING_RAW16 : return "GRAY16LE";
+
+ default:
+ throw VideoException("Unknown colour coding");
+ }
+}
+
+dc1394color_coding_t Dc1394ColorCodingFromString(std::string coding)
+{
+ if( !coding.compare("RGB24")) return DC1394_COLOR_CODING_RGB8;
+ else if(!coding.compare("GRAY8")) return DC1394_COLOR_CODING_MONO8;
+
+ else if(!coding.compare("GRAY16LE")) return DC1394_COLOR_CODING_MONO16;
+ else if(!coding.compare("RGB48LE")) return DC1394_COLOR_CODING_RGB16;
+ else if(!coding.compare("GRAY16BE")) return DC1394_COLOR_CODING_MONO16S;
+ else if(!coding.compare("RGB48BE")) return DC1394_COLOR_CODING_RGB16S;
+
+ else if(!coding.compare("YUV411P")) return DC1394_COLOR_CODING_YUV411;
+ else if(!coding.compare("YUV422P")) return DC1394_COLOR_CODING_YUV422;
+ else if(!coding.compare("YUV444P")) return DC1394_COLOR_CODING_YUV444;
+ // else if(!coding.compare("RAW8")) return DC1394_COLOR_CODING_RAW8;
+ // else if(!coding.compare("RAW16")) return DC1394_COLOR_CODING_RAW16;
+ throw VideoException("Unknown colour coding");
+}
+
+void Dc1394ModeDetails(dc1394video_mode_t mode, unsigned& w, unsigned& h, string& format )
+{
+ switch( mode )
+ {
+ // RGB Modes
+ case DC1394_VIDEO_MODE_1024x768_RGB8:
+ w=1024; h=768; format = "RGB24";
+ break;
+ case DC1394_VIDEO_MODE_640x480_RGB8:
+ w=640; h=480; format = "RGB24";
+ break;
+ case DC1394_VIDEO_MODE_800x600_RGB8:
+ w=800; h=600; format = "RGB24";
+ break;
+ case DC1394_VIDEO_MODE_1280x960_RGB8:
+ w=1280; h=960; format = "RGB24";
+ break;
+ case DC1394_VIDEO_MODE_1600x1200_RGB8:
+ w=1600; h=1200; format = "RGB24";
+ break;
+
+ // Greyscale modes
+ case DC1394_VIDEO_MODE_640x480_MONO8:
+ w=640; h=480; format = "GRAY8";
+ break;
+ case DC1394_VIDEO_MODE_800x600_MONO8:
+ w=800; h=600; format = "GRAY8";
+ break;
+ case DC1394_VIDEO_MODE_1024x768_MONO8:
+ w=1024; h=768; format = "GRAY8";
+ break;
+ case DC1394_VIDEO_MODE_1280x960_MONO8:
+ w=1280; h=960; format = "GRAY8";
+ break;
+ case DC1394_VIDEO_MODE_1600x1200_MONO8:
+ w=1600; h=1200; format = "GRAY8";
+ break;
+ case DC1394_VIDEO_MODE_640x480_MONO16:
+ w=640; h=480; format = "GRAY16";
+ break;
+ case DC1394_VIDEO_MODE_800x600_MONO16:
+ w=800; h=600; format = "GRAY16";
+ break;
+ case DC1394_VIDEO_MODE_1024x768_MONO16:
+ w=1024; h=768; format = "GRAY16";
+ break;
+ case DC1394_VIDEO_MODE_1280x960_MONO16:
+ w=1280; h=960; format = "GRAY16";
+ break;
+ case DC1394_VIDEO_MODE_1600x1200_MONO16:
+ w=1600; h=1200; format = "GRAY16";
+ break;
+
+ // Chrome modes
+ case DC1394_VIDEO_MODE_640x480_YUV411:
+ w=640; h=480; format = "YUV411P";
+ break;
+ case DC1394_VIDEO_MODE_160x120_YUV444:
+ w=160; h=120; format = "YUV444P";
+ break;
+ case DC1394_VIDEO_MODE_320x240_YUV422:
+ w=320; h=240; format = "YUV422P";
+ break;
+ case DC1394_VIDEO_MODE_640x480_YUV422:
+ w=640; h=480; format = "YUV422P";
+ break;
+ case DC1394_VIDEO_MODE_800x600_YUV422:
+ w=800; h=600; format = "YUV422P";
+ break;
+ case DC1394_VIDEO_MODE_1024x768_YUV422:
+ w=1024; h=768; format = "YUV422P";
+ break;
+ case DC1394_VIDEO_MODE_1600x1200_YUV422:
+ w=1600; h=1200; format = "YUV422P";
+ break;
+ case DC1394_VIDEO_MODE_1280x960_YUV422:
+ w=1280; h=960; format = "YUV422P";
+ break;
+ default:
+ throw VideoException("Unknown colour coding");
+ }
+}
+
+void FirewireVideo::init_stream_info()
+{
+ streams.clear();
+
+ dc1394video_mode_t video_mode;
+ dc1394color_coding_t color_coding;
+ dc1394_video_get_mode(camera,&video_mode);
+ dc1394_get_color_coding_from_video_mode(camera,video_mode,&color_coding);
+ const std::string strformat = Dc1394ColorCodingToString(color_coding);
+ const PixelFormat fmt = PixelFormatFromString(strformat);
+
+ StreamInfo stream(fmt, width, height, (width*fmt.bpp)/8, 0 );
+ streams.push_back( stream );
+
+ frame_size_bytes = stream.Pitch() * stream.Height();
+}
+
+const std::vector<StreamInfo>& FirewireVideo::Streams() const
+{
+ return streams;
+}
+
+size_t FirewireVideo::SizeBytes() const
+{
+ return frame_size_bytes;
+}
+
+void FirewireVideo::Start()
+{
+ if( !running )
+ {
+ err=dc1394_video_set_transmission(camera, DC1394_ON);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not start camera iso transmission");
+ running = true;
+ }
+}
+
+void FirewireVideo::Stop()
+{
+ if( running )
+ {
+ // Stop transmission
+ err=dc1394_video_set_transmission(camera,DC1394_OFF);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Could not stop the camera");
+ running = false;
+ }
+}
+
+FirewireVideo::FirewireVideo(
+ Guid guid,
+ dc1394video_mode_t video_mode,
+ dc1394framerate_t framerate,
+ dc1394speed_t iso_speed,
+ int dma_buffers
+ ) :running(false),top(0),left(0)
+{
+ d = dc1394_new ();
+ if (!d)
+ throw VideoException("Failed to get 1394 bus");
+
+ init_camera(guid.guid,dma_buffers,iso_speed,video_mode,framerate);
+}
+
+FirewireVideo::FirewireVideo(
+ Guid guid,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top,
+ dc1394speed_t iso_speed,
+ int dma_buffers, bool reset_at_boot
+ ) :running(false)
+{
+ d = dc1394_new ();
+ if (!d)
+ throw VideoException("Failed to get 1394 bus");
+
+ init_format7_camera(guid.guid,dma_buffers,iso_speed,video_mode,framerate,width,height,left,top, reset_at_boot);
+}
+
+FirewireVideo::FirewireVideo(
+ unsigned deviceid,
+ dc1394video_mode_t video_mode,
+ dc1394framerate_t framerate,
+ dc1394speed_t iso_speed,
+ int dma_buffers
+ ) :running(false),top(0),left(0)
+{
+ d = dc1394_new ();
+ if (!d)
+ throw VideoException("Failed to get 1394 bus");
+
+ err=dc1394_camera_enumerate (d, &list);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to enumerate cameras");
+
+ if (list->num == 0)
+ throw VideoException("No cameras found");
+
+ if( deviceid >= list->num )
+ throw VideoException("Invalid camera index");
+
+ const uint64_t guid = list->ids[deviceid].guid;
+
+ dc1394_camera_free_list (list);
+
+ init_camera(guid,dma_buffers,iso_speed,video_mode,framerate);
+
+}
+
+FirewireVideo::FirewireVideo(
+ unsigned deviceid,
+ dc1394video_mode_t video_mode,
+ float framerate,
+ uint32_t width, uint32_t height,
+ uint32_t left, uint32_t top,
+ dc1394speed_t iso_speed,
+ int dma_buffers, bool reset_at_boot
+ ) :running(false)
+{
+ d = dc1394_new ();
+ if (!d)
+ throw VideoException("Failed to get 1394 bus");
+
+ err=dc1394_camera_enumerate (d, &list);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to enumerate cameras");
+
+ if (list->num == 0)
+ throw VideoException("No cameras found");
+
+ if( deviceid >= list->num )
+ throw VideoException("Invalid camera index");
+
+ const uint64_t guid = list->ids[deviceid].guid;
+
+ dc1394_camera_free_list (list);
+
+ init_format7_camera(guid,dma_buffers,iso_speed,video_mode,framerate,width,height,left,top, reset_at_boot);
+
+}
+
+bool FirewireVideo::GrabNext( unsigned char* image, bool wait )
+{
+ const dc1394capture_policy_t policy =
+ wait ? DC1394_CAPTURE_POLICY_WAIT : DC1394_CAPTURE_POLICY_POLL;
+
+ dc1394video_frame_t *frame;
+ err = dc1394_capture_dequeue(camera, policy, &frame);
+ if( err != DC1394_SUCCESS)
+ throw VideoException("Could not capture frame", dc1394_error_get_string(err) );
+
+ if( frame )
+ {
+ memcpy(image,frame->image,frame->image_bytes);
+ dc1394_capture_enqueue(camera,frame);
+ return true;
+ }
+ return false;
+}
+
+bool FirewireVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ dc1394video_frame_t *f;
+ err = dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_POLL, &f);
+ if( err != DC1394_SUCCESS)
+ throw VideoException("Could not capture frame", dc1394_error_get_string(err) );
+
+ if( f ) {
+ while( true )
+ {
+ dc1394video_frame_t *nf;
+ err = dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_POLL, &nf);
+ if( err != DC1394_SUCCESS)
+ throw VideoException("Could not capture frame", dc1394_error_get_string(err) );
+
+ if( nf )
+ {
+ err=dc1394_capture_enqueue(camera,f);
+ f = nf;
+ }else{
+ break;
+ }
+ }
+ memcpy(image,f->image,f->image_bytes);
+ err=dc1394_capture_enqueue(camera,f);
+ return true;
+ }else if(wait){
+ return GrabNext(image,true);
+ }
+ return false;
+}
+
+FirewireFrame FirewireVideo::GetNext(bool wait)
+{
+ const dc1394capture_policy_t policy =
+ wait ? DC1394_CAPTURE_POLICY_WAIT : DC1394_CAPTURE_POLICY_POLL;
+
+ dc1394video_frame_t *frame;
+ dc1394_capture_dequeue(camera, policy, &frame);
+ return FirewireFrame(frame);
+}
+
+FirewireFrame FirewireVideo::GetNewest(bool wait)
+{
+ dc1394video_frame_t *f;
+ err = dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_POLL, &f);
+ if( err != DC1394_SUCCESS)
+ throw VideoException("Could not capture frame", dc1394_error_get_string(err) );
+
+ if( f ) {
+ while( true )
+ {
+ dc1394video_frame_t *nf;
+ err = dc1394_capture_dequeue(camera, DC1394_CAPTURE_POLICY_POLL, &nf);
+ if( err != DC1394_SUCCESS)
+ throw VideoException("Could not capture frame", dc1394_error_get_string(err) );
+
+ if( nf )
+ {
+ err=dc1394_capture_enqueue(camera,f);
+ f = nf;
+ }else{
+ break;
+ }
+ }
+ return FirewireFrame(f);
+ }else if(wait){
+ return GetNext(true);
+ }
+ return FirewireFrame(0);
+}
+
+void FirewireVideo::PutFrame(FirewireFrame& f)
+{
+ if( f.frame )
+ {
+ dc1394_capture_enqueue(camera,f.frame);
+ f.frame = 0;
+ }
+}
+
+float FirewireVideo::GetGain() const
+{
+ float gain;
+ err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_GAIN,&gain);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to read gain");
+
+ return gain;
+
+}
+
+void FirewireVideo::SetAutoGain()
+{
+
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, DC1394_FEATURE_MODE_AUTO);
+ if (err < 0) {
+ throw VideoException("Could not set auto gain mode");
+ }
+}
+
+void FirewireVideo::SetGain(float val)
+{
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_GAIN, DC1394_FEATURE_MODE_MANUAL);
+ if (err < 0) {
+ throw VideoException("Could not set manual gain mode");
+ }
+
+ err = dc1394_feature_set_absolute_control(camera, DC1394_FEATURE_GAIN, DC1394_ON);
+ if (err < 0) {
+ throw VideoException("Could not set absolute control for gain");
+ }
+
+ err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_GAIN, val);
+ if (err < 0) {
+ throw VideoException("Could not set gain value");
+ }
+}
+
+
+float FirewireVideo::GetBrightness() const
+{
+ float brightness;
+ err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_BRIGHTNESS,&brightness);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to read brightness");
+
+ return brightness;
+
+}
+
+void FirewireVideo::SetAutoBrightness()
+{
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, DC1394_FEATURE_MODE_AUTO);
+ if (err < 0) {
+ throw VideoException("Could not set auto brightness mode");
+ }
+}
+
+void FirewireVideo::SetBrightness(float val)
+{
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_BRIGHTNESS, DC1394_FEATURE_MODE_MANUAL);
+ if (err < 0) {
+ throw VideoException("Could not set manual brightness mode");
+ }
+
+ err = dc1394_feature_set_absolute_control(camera, DC1394_FEATURE_BRIGHTNESS, DC1394_ON);
+ if (err < 0) {
+ throw VideoException("Could not set absolute control for brightness");
+ }
+
+ err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_BRIGHTNESS, val);
+ if (err < 0) {
+ throw VideoException("Could not set brightness value");
+ }
+}
+
+float FirewireVideo::GetShutterTime() const
+{
+ float shutter;
+ err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_SHUTTER,&shutter);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to read shutter");
+
+ return shutter;
+}
+
+void FirewireVideo::SetAutoShutterTime()
+{
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, DC1394_FEATURE_MODE_AUTO);
+ if (err < 0) {
+ throw VideoException("Could not set auto shutter mode");
+ }
+}
+
+void FirewireVideo::SetShutterTime(float val)
+{
+ dc1394error_t err = dc1394_feature_set_mode(camera, DC1394_FEATURE_SHUTTER, DC1394_FEATURE_MODE_MANUAL);
+ if (err < 0) {
+ throw VideoException("Could not set manual shutter mode");
+ }
+
+ err = dc1394_feature_set_absolute_control(camera, DC1394_FEATURE_SHUTTER, DC1394_ON);
+ if (err < 0) {
+ throw VideoException("Could not set absolute control for shutter");
+ }
+
+ err = dc1394_feature_set_absolute_value(camera, DC1394_FEATURE_SHUTTER, val);
+ if (err < 0) {
+ throw VideoException("Could not set shutter value");
+ }
+}
+
+void FirewireVideo::SetShutterTimeQuant(int shutter)
+{
+ // TODO: Set mode as well
+
+ err = dc1394_feature_set_value(camera,DC1394_FEATURE_SHUTTER,shutter);
+
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to set shutter");
+}
+
+float FirewireVideo::GetGamma() const
+{
+ float gamma;
+ err = dc1394_feature_get_absolute_value(camera,DC1394_FEATURE_GAMMA,&gamma);
+ if( err != DC1394_SUCCESS )
+ throw VideoException("Failed to read gamma");
+ return gamma;
+}
+
+void FirewireVideo::SetInternalTrigger()
+{
+ dc1394error_t err = dc1394_external_trigger_set_power(camera, DC1394_OFF);
+ if (err < 0) {
+ throw VideoException("Could not set internal trigger mode");
+ }
+}
+
+void FirewireVideo::SetExternalTrigger(dc1394trigger_mode_t mode, dc1394trigger_polarity_t polarity, dc1394trigger_source_t source)
+{
+ dc1394error_t err = dc1394_external_trigger_set_polarity(camera, polarity);
+ if (err < 0) {
+ throw VideoException("Could not set external trigger polarity");
+ }
+
+ err = dc1394_external_trigger_set_mode(camera, mode);
+ if (err < 0) {
+ throw VideoException("Could not set external trigger mode");
+ }
+
+ err = dc1394_external_trigger_set_source(camera, source);
+ if (err < 0) {
+ throw VideoException("Could not set external trigger source");
+ }
+
+ err = dc1394_external_trigger_set_power(camera, DC1394_ON);
+ if (err < 0) {
+ throw VideoException("Could not set external trigger power");
+ }
+}
+
+
+FirewireVideo::~FirewireVideo()
+{
+ Stop();
+
+ // Close camera
+ dc1394_video_set_transmission(camera, DC1394_OFF);
+ dc1394_capture_stop(camera);
+ dc1394_camera_free(camera);
+ dc1394_free (d);
+}
+
+void FirewireVideo::SetRegister(uint64_t offset, uint32_t value){
+ dc1394error_t err = dc1394_set_register (camera, offset, value);
+ if (err < 0) {
+ throw VideoException("Could not set camera register");
+ }
+}
+
+uint32_t FirewireVideo::GetRegister(uint64_t offset)
+{
+ uint32_t value = 0;
+ dc1394error_t err = dc1394_get_register (camera, offset, &value);
+ if (err < 0) {
+ throw VideoException("Could not get camera register");
+ }
+ return value;
+}
+
+void FirewireVideo::SetControlRegister(uint64_t offset, uint32_t value)
+{
+ dc1394error_t err = dc1394_set_control_register (camera, offset, value);
+ if (err < 0) {
+ throw VideoException("Could not set camera control register");
+ }
+}
+
+uint32_t FirewireVideo::GetControlRegister(uint64_t offset)
+{
+ uint32_t value = 0;
+ dc1394error_t err = dc1394_get_control_register(camera, offset, &value);
+ if (err < 0) {
+ throw VideoException("Could not get camera control register");
+ }
+ return value;
+}
+
+int FirewireVideo::nearest_value(int value, int step, int min, int max)
+{
+ int low, high;
+
+ low=value-(value%step);
+ high=value-(value%step)+step;
+ if (low<min)
+ low=min;
+ if (high>max)
+ high=max;
+
+ if (abs(low-value)<abs(high-value))
+ return low;
+ else
+ return high;
+}
+
+double FirewireVideo::bus_period_from_iso_speed(dc1394speed_t iso_speed)
+{
+ double bus_period;
+
+ switch(iso_speed){
+ case DC1394_ISO_SPEED_3200:
+ bus_period = 15.625e-6;
+ break;
+ case DC1394_ISO_SPEED_1600:
+ bus_period = 31.25e-6;
+ break;
+ case DC1394_ISO_SPEED_800:
+ bus_period = 62.5e-6;
+ break;
+ case DC1394_ISO_SPEED_400:
+ bus_period = 125e-6;
+ break;
+ case DC1394_ISO_SPEED_200:
+ bus_period = 250e-6;
+ break;
+ case DC1394_ISO_SPEED_100:
+ bus_period = 500e-6;
+ break;
+ default:
+ throw VideoException("iso speed not valid");
+ }
+
+ return bus_period;
+}
+
+dc1394video_mode_t get_firewire_format7_mode(const std::string fmt)
+{
+ const std::string FMT7_prefix = "FORMAT7_";
+
+ if( StartsWith(fmt, FMT7_prefix) )
+ {
+ int fmt7_mode = 0;
+ std::istringstream iss( fmt.substr(FMT7_prefix.size()) );
+ iss >> fmt7_mode;
+ if( !iss.fail() ) {
+ return (dc1394video_mode_t)(DC1394_VIDEO_MODE_FORMAT7_0 + fmt7_mode);
+ }
+ }
+
+ throw VideoException("Unknown video mode");
+}
+
+dc1394video_mode_t get_firewire_mode(unsigned width, unsigned height, const std::string fmt)
+{
+ for( dc1394video_mode_t video_mode=DC1394_VIDEO_MODE_MIN; video_mode<DC1394_VIDEO_MODE_MAX; video_mode = (dc1394video_mode_t)(video_mode +1) )
+ {
+ try {
+ unsigned w,h;
+ std::string format;
+ Dc1394ModeDetails(video_mode,w,h,format);
+
+ if( w == width && h==height && !fmt.compare(format) )
+ return video_mode;
+ } catch (VideoException e) {}
+ }
+
+ throw VideoException("Unknown video mode");
+}
+
+dc1394framerate_t get_firewire_framerate(float framerate)
+{
+ if(framerate==1.875) return DC1394_FRAMERATE_1_875;
+ else if(framerate==3.75) return DC1394_FRAMERATE_3_75;
+ else if(framerate==7.5) return DC1394_FRAMERATE_7_5;
+ else if(framerate==15) return DC1394_FRAMERATE_15;
+ else if(framerate==30) return DC1394_FRAMERATE_30;
+ else if(framerate==60) return DC1394_FRAMERATE_60;
+ else if(framerate==120) return DC1394_FRAMERATE_120;
+ else if(framerate==240) return DC1394_FRAMERATE_240;
+ else throw VideoException("Invalid framerate");
+}
+
+PANGOLIN_REGISTER_FACTORY(FirewireVideo)
+{
+ struct FirewireVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::string desired_format = uri.Get<std::string>("fmt","RGB24");
+ ToUpper(desired_format);
+ const ImageDim desired_dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const ImageDim desired_xy = uri.Get<ImageDim>("pos", ImageDim(0,0));
+ const int desired_dma = uri.Get<int>("dma", 10);
+ const int desired_iso = uri.Get<int>("iso", 400);
+ const float desired_fps = uri.Get<float>("fps", 30);
+ const bool deinterlace = uri.Get<bool>("deinterlace", 0);
+
+ Guid guid = 0;
+ unsigned deviceid = 0;
+ dc1394framerate_t framerate = get_firewire_framerate(desired_fps);
+ dc1394speed_t iso_speed = (dc1394speed_t)(log(desired_iso/100) / log(2));
+ int dma_buffers = desired_dma;
+
+ VideoInterface* video_raw = nullptr;
+
+ if( StartsWith(desired_format, "FORMAT7") )
+ {
+ dc1394video_mode_t video_mode = get_firewire_format7_mode(desired_format);
+ if( guid.guid == 0 ) {
+ video_raw = new FirewireVideo(deviceid,video_mode,FirewireVideo::MAX_FR, desired_dim.x, desired_dim.y, desired_xy.x, desired_xy.y, iso_speed, dma_buffers,true);
+ }else{
+ video_raw = new FirewireVideo(guid,video_mode,FirewireVideo::MAX_FR, desired_dim.x, desired_dim.y, desired_xy.x, desired_xy.y, iso_speed, dma_buffers,true);
+ }
+ }else{
+ dc1394video_mode_t video_mode = get_firewire_mode(desired_dim.x, desired_dim.y,desired_format);
+ if( guid.guid == 0 ) {
+ video_raw = new FirewireVideo(deviceid,video_mode,framerate,iso_speed,dma_buffers);
+ }else{
+ video_raw = new FirewireVideo(guid,video_mode,framerate,iso_speed,dma_buffers);
+ }
+ }
+
+ if(deinterlace) {
+ std::unique_ptr<VideoInterface> video(video_raw);
+ video_raw = new DeinterlaceVideo(video);
+ }
+
+ return std::unique_ptr<VideoInterface>(video_raw);
+ }
+ };
+
+ auto factory = std::make_shared<FirewireVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "firewire");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "dc1394");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/images.cpp b/externals/Pangolin/src/video/drivers/images.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..eeb919d633efd7c59a8b4a7c56db803fcfa99d79
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/images.cpp
@@ -0,0 +1,298 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/video/drivers/images.h>
+#include <pangolin/video/iostream_operators.h>
+
+#include <cstring>
+#include <fstream>
+
+namespace pangolin
+{
+
+bool ImagesVideo::LoadFrame(size_t i)
+{
+ if( i < num_files) {
+ Frame& frame = loaded[i];
+ for(size_t c=0; c< num_channels; ++c) {
+ const std::string& filename = Filename(i,c);
+ const ImageFileType file_type = FileType(filename);
+
+ if(file_type == ImageFileTypeUnknown && unknowns_are_raw) {
+ frame.push_back( LoadImage( filename, raw_fmt, raw_width, raw_height, raw_fmt.bpp * raw_width / 8) );
+ }else{
+ frame.push_back( LoadImage( filename, file_type ) );
+ }
+ }
+ return true;
+ }
+ return false;
+}
+
+void ImagesVideo::PopulateFilenamesFromJson(const std::string& filename)
+{
+ std::ifstream ifs( PathExpand(filename));
+ picojson::value json;
+ const std::string err = picojson::parse(json, ifs);
+ if(err.empty()) {
+ const std::string folder = PathParent(filename) + "/";
+ device_properties = json["device_properties"];
+ json_frames = json["frames"];
+
+ num_files = json_frames.size();
+ if(num_files == 0) {
+ throw VideoException("Empty Json Image archive.");
+ }
+
+ num_channels = json_frames[0]["stream_files"].size();
+ if(num_channels == 0) {
+ throw VideoException("Empty Json Image archive.");
+ }
+
+ filenames.resize(num_channels);
+ for(size_t c=0; c < num_channels; ++c) {
+ filenames[c].resize(num_files);
+ for(size_t i = 0; i < num_files; ++i) {
+ const std::string path = json_frames[i]["stream_files"][c].get<std::string>();
+ filenames[c][i] = (path.size() && path[0] == '/') ? path : (folder + path);
+ }
+ }
+ loaded.resize(num_files);
+ }else{
+ throw VideoException(err);
+ }
+}
+
+void ImagesVideo::PopulateFilenames(const std::string& wildcard_path)
+{
+ const std::vector<std::string> wildcards = Expand(wildcard_path, '[', ']', ',');
+ num_channels = wildcards.size();
+
+ if(wildcards.size() == 1 ) {
+ const std::string expanded_path = PathExpand(wildcards[0]);
+ const std::string possible_archive_path = expanded_path + "/archive.json";
+
+ if (FileLowercaseExtention(expanded_path) == ".json" ) {
+ PopulateFilenamesFromJson(wildcards[0]);
+ return;
+ }else if(FileExists(possible_archive_path)){
+ PopulateFilenamesFromJson(possible_archive_path);
+ return;
+ }
+ }
+
+ filenames.resize(num_channels);
+
+ for(size_t i = 0; i < wildcards.size(); ++i) {
+ const std::string channel_wildcard = PathExpand(wildcards[i]);
+ FilesMatchingWildcard(channel_wildcard, filenames[i]);
+ if(num_files == size_t(-1)) {
+ num_files = filenames[i].size();
+ }else{
+ if( num_files != filenames[i].size() ) {
+ std::cerr << "Warning: Video Channels have unequal number of files" << std::endl;
+ }
+ num_files = std::min(num_files, filenames[i].size());
+ }
+ if(num_files == 0) {
+ throw VideoException("No files found for wildcard '" + channel_wildcard + "'");
+ }
+ }
+
+ // Resize empty frames vector to hold future images.
+ loaded.resize(num_files);
+}
+
+void ImagesVideo::ConfigureStreamSizes()
+{
+ size_bytes = 0;
+ for(size_t c=0; c < num_channels; ++c) {
+ const TypedImage& img = loaded[0][c];
+ const StreamInfo stream_info(img.fmt, img.w, img.h, img.pitch, (unsigned char*)(size_bytes));
+ streams.push_back(stream_info);
+ size_bytes += img.h*img.pitch;
+ }
+}
+
+ImagesVideo::ImagesVideo(const std::string& wildcard_path)
+ : num_files(-1), num_channels(0), next_frame_id(0),
+ unknowns_are_raw(false)
+{
+ // Work out which files to sequence
+ PopulateFilenames(wildcard_path);
+
+ // Load first image in order to determine stream sizes etc
+ LoadFrame(next_frame_id);
+
+ ConfigureStreamSizes();
+
+ // TODO: Queue frames in another thread.
+}
+
+ImagesVideo::ImagesVideo(const std::string& wildcard_path,
+ const PixelFormat& raw_fmt,
+ size_t raw_width, size_t raw_height
+) : num_files(-1), num_channels(0), next_frame_id(0),
+ unknowns_are_raw(true), raw_fmt(raw_fmt),
+ raw_width(raw_width), raw_height(raw_height)
+{
+ // Work out which files to sequence
+ PopulateFilenames(wildcard_path);
+
+ // Load first image in order to determine stream sizes etc
+ LoadFrame(next_frame_id);
+
+ ConfigureStreamSizes();
+
+ // TODO: Queue frames in another thread.
+}
+
+ImagesVideo::~ImagesVideo()
+{
+}
+
+//! Implement VideoInput::Start()
+void ImagesVideo::Start()
+{
+
+}
+
+//! Implement VideoInput::Stop()
+void ImagesVideo::Stop()
+{
+
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t ImagesVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& ImagesVideo::Streams() const
+{
+ return streams;
+}
+
+//! Implement VideoInput::GrabNext()
+bool ImagesVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ if(next_frame_id < loaded.size()) {
+ Frame& frame = loaded[next_frame_id];
+
+ if(frame.size() != num_channels) {
+ LoadFrame(next_frame_id);
+ }
+
+ for(size_t c=0; c < num_channels; ++c){
+ TypedImage& img = frame[c];
+ if(!img.ptr || img.w != streams[c].Width() || img.h != streams[c].Height() ) {
+ return false;
+ }
+ const StreamInfo& si = streams[c];
+ std::memcpy(image + (size_t)si.Offset(), img.ptr, si.SizeBytes());
+ img.Deallocate();
+ }
+ frame.clear();
+
+ next_frame_id++;
+ return true;
+ }
+
+ return false;
+}
+
+//! Implement VideoInput::GrabNewest()
+bool ImagesVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+size_t ImagesVideo::GetCurrentFrameId() const
+{
+ return (int)next_frame_id - 1;
+}
+
+size_t ImagesVideo::GetTotalFrames() const
+{
+ return num_files;
+}
+
+size_t ImagesVideo::Seek(size_t frameid)
+{
+ next_frame_id = std::max(size_t(0), std::min(frameid, num_files));
+ return next_frame_id;
+}
+
+const picojson::value& ImagesVideo::DeviceProperties() const
+{
+ return device_properties;
+}
+
+const picojson::value& ImagesVideo::FrameProperties() const
+{
+ const size_t frame = GetCurrentFrameId();
+
+ if( json_frames.evaluate_as_boolean() && frame < json_frames.size()) {
+ const picojson::value& frame_props = json_frames[frame];
+ if(frame_props.contains("frame_properties")) {
+ return frame_props["frame_properties"];
+ }
+ }
+
+ return null_props;
+}
+
+PANGOLIN_REGISTER_FACTORY(ImagesVideo)
+{
+ struct ImagesVideoVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const bool raw = uri.Contains("fmt");
+ const std::string path = PathExpand(uri.url);
+
+ if(raw) {
+ const std::string sfmt = uri.Get<std::string>("fmt", "GRAY8");
+ const PixelFormat fmt = PixelFormatFromString(sfmt);
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ return std::unique_ptr<VideoInterface>( new ImagesVideo(path, fmt, dim.x, dim.y) );
+ }else{
+ return std::unique_ptr<VideoInterface>( new ImagesVideo(path) );
+ }
+ }
+ };
+
+ auto factory = std::make_shared<ImagesVideoVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 20, "file");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 20, "files");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "image");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "images");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/images_out.cpp b/externals/Pangolin/src/video/drivers/images_out.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3ad01db238733731b40b3f43b26a32b692d1bb74
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/images_out.cpp
@@ -0,0 +1,126 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <iomanip>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/image/image_io.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/video/drivers/images_out.h>
+
+namespace pangolin {
+
+ImagesVideoOutput::ImagesVideoOutput(const std::string& image_folder, const std::string& json_file_out, const std::string& image_file_extension)
+ : json_frames(picojson::array_type,true),
+ image_index(0), image_folder( PathExpand(image_folder) + "/" ), image_file_extension(image_file_extension)
+{
+ if(!json_file_out.empty()) {
+ file.open(json_file_out);
+ if(!file.is_open()) {
+ throw std::runtime_error("Unable to open json file for writing, " + json_file_out + ". Make sure output folder already exists.");
+ }
+ }
+}
+
+ImagesVideoOutput::~ImagesVideoOutput()
+{
+ if(file.is_open())
+ {
+ const std::string video_uri = "images://" + image_folder + "archive.json";
+ picojson::value json_file;
+ json_file["device_properties"] = device_properties;
+ json_file["frames"] = json_frames;
+ json_file["input_uri"] = input_uri;
+ json_file["video_uri"] = video_uri;
+
+ // Serialize json to file.
+ file << json_file.serialize(true);
+ }
+}
+
+const std::vector<StreamInfo>& ImagesVideoOutput::Streams() const
+{
+ return streams;
+}
+
+void ImagesVideoOutput::SetStreams(const std::vector<StreamInfo>& streams, const std::string& uri, const picojson::value& device_properties)
+{
+ this->streams = streams;
+ this->input_uri = uri;
+ this->device_properties = device_properties;
+}
+
+int ImagesVideoOutput::WriteStreams(const unsigned char* data, const picojson::value& frame_properties)
+{
+ picojson::value json_filenames(picojson::array_type, true);
+
+ // Write each stream image to file.
+ for(size_t s=0; s < streams.size(); ++s) {
+ const pangolin::StreamInfo& si = streams[s];
+ const std::string filename = pangolin::FormatString("image_%%%_%.%",std::setfill('0'),std::setw(10),image_index, s, image_file_extension);
+ json_filenames.push_back(filename);
+ const Image<unsigned char> img = si.StreamImage(data);
+ pangolin::SaveImage(img, si.PixFormat(), image_folder + filename);
+ }
+
+ // Add frame_properties to json file.
+ picojson::value json_frame;
+ json_frame["frame_properties"] = frame_properties;
+ json_frame["stream_files"] = json_filenames;
+ json_frames.push_back(json_frame);
+
+ ++image_index;
+ return 0;
+}
+
+bool ImagesVideoOutput::IsPipe() const
+{
+ return false;
+}
+
+PANGOLIN_REGISTER_FACTORY(ImagesVideoOutput)
+{
+ struct ImagesVideoFactory final : public FactoryInterface<VideoOutputInterface> {
+ std::unique_ptr<VideoOutputInterface> Open(const Uri& uri) override {
+ const std::string images_folder = PathExpand(uri.url);
+ const std::string json_filename = images_folder + "/archive.json";
+ const std::string image_extension = uri.Get<std::string>("fmt", "png");
+
+ if(FileExists(json_filename)) {
+ throw std::runtime_error("Dataset already exists in directory.");
+ }
+
+ return std::unique_ptr<VideoOutputInterface>(
+ new ImagesVideoOutput(images_folder, json_filename, image_extension)
+ );
+ }
+ };
+
+ auto factory = std::make_shared<ImagesVideoFactory>();
+ FactoryRegistry<VideoOutputInterface>::I().RegisterFactory(factory, 10, "images");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/join.cpp b/externals/Pangolin/src/video/drivers/join.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ae0440ee63b137a5700102a30508208d11a11d79
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/join.cpp
@@ -0,0 +1,429 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/join.h>
+#include <pangolin/video/iostream_operators.h>
+
+//#define DEBUGJOIN
+
+#ifdef DEBUGJOIN
+ #include <pangolin/utils/timer.h>
+ #define TSTART() pangolin::basetime start,last,now; start = pangolin::TimeNow(); last = start;
+ #define TGRABANDPRINT(...) now = pangolin::TimeNow(); fprintf(stderr,"JOIN: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, " %fms.\n",1000*pangolin::TimeDiff_s(last, now)); last = now;
+ #define DBGPRINT(...) fprintf(stderr,"JOIN: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr,"\n");
+#else
+ #define TSTART()
+ #define TGRABANDPRINT(...)
+ #define DBGPRINT(...)
+#endif
+
+namespace pangolin
+{
+JoinVideo::JoinVideo(std::vector<std::unique_ptr<VideoInterface> > &src_)
+ : storage(std::move(src_)), size_bytes(0), sync_tolerance_us(0)
+{
+ for(auto& p : storage) {
+ src.push_back(p.get());
+ }
+
+ // Add individual streams
+ for(size_t s=0; s< src.size(); ++s)
+ {
+ VideoInterface& vid = *src[s];
+ for(size_t i=0; i < vid.Streams().size(); ++i)
+ {
+ const StreamInfo si = vid.Streams()[i];
+ const PixelFormat fmt = si.PixFormat();
+ const Image<unsigned char> img_offset = si.StreamImage((unsigned char*)size_bytes);
+ streams.push_back(StreamInfo(fmt, img_offset));
+ }
+ size_bytes += src[s]->SizeBytes();
+ }
+}
+
+JoinVideo::~JoinVideo()
+{
+ for(size_t s=0; s< src.size(); ++s) {
+ src[s]->Stop();
+ }
+}
+
+size_t JoinVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+const std::vector<StreamInfo>& JoinVideo::Streams() const
+{
+ return streams;
+}
+
+void JoinVideo::Start()
+{
+ for(size_t s=0; s< src.size(); ++s) {
+ src[s]->Start();
+ }
+}
+
+void JoinVideo::Stop()
+{
+ for(size_t s=0; s< src.size(); ++s) {
+ src[s]->Stop();
+ }
+}
+
+bool JoinVideo::Sync(int64_t tolerance_us, double transfer_bandwidth_gbps)
+{
+ transfer_bandwidth_bytes_per_us = int64_t((transfer_bandwidth_gbps * 1E3) / 8.0);
+// std::cout << "transfer_bandwidth_gbps: " << transfer_bandwidth_gbps << std::endl;
+
+ for(size_t s=0; s< src.size(); ++s)
+ {
+ picojson::value props = GetVideoDeviceProperties(src[s]);
+ if(!props.get_value(PANGO_HAS_TIMING_DATA, false)) {
+ if (props.contains("streams")) {
+ picojson::value streams = props["streams"];
+ for (size_t i=0; i<streams.size(); ++i) {
+ if(!streams[i].get_value(PANGO_HAS_TIMING_DATA, false)) {
+ sync_tolerance_us = 0;
+ return false;
+ }
+ }
+ } else {
+ sync_tolerance_us = 0;
+ return false;
+ }
+ }
+ }
+
+ sync_tolerance_us = tolerance_us;
+
+// std::cout << "transfer_bandwidth_bytes_per_us: " << transfer_bandwidth_bytes_per_us << std::endl;
+ return true;
+}
+
+// Assuming that src_index supports VideoPropertiesInterface and has a valid PANGO_HOST_RECEPTION_TIME_US, or PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US
+// returns a capture time adjusted for transfer time and when possible also for exposure.
+int64_t JoinVideo::GetAdjustedCaptureTime(size_t src_index)
+{
+ picojson::value props = GetVideoFrameProperties(src[src_index]);
+ if(props.contains(PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US)) {
+ // great, the driver already gave us an estimated center of capture
+ return props[PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US].get<int64_t>();
+
+ } else {
+ if(props.contains(PANGO_HOST_RECEPTION_TIME_US)) {
+ int64_t transfer_time_us = 0;
+ if( transfer_bandwidth_bytes_per_us > 0 ) {
+ transfer_time_us = src[src_index]->SizeBytes() / transfer_bandwidth_bytes_per_us;
+ }
+ return props[PANGO_HOST_RECEPTION_TIME_US].get<int64_t>() - transfer_time_us;
+ } else {
+ if (props.contains("streams")) {
+ picojson::value streams = props["streams"];
+
+ if(streams.size()>0){
+ if(streams[0].contains(PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US)) {
+ // great, the driver already gave us an estimated center of capture
+ return streams[0][PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US].get<int64_t>();
+ }
+ else if( streams[0].contains(PANGO_HOST_RECEPTION_TIME_US)) {
+ int64_t transfer_time_us = 0;
+ if( transfer_bandwidth_bytes_per_us > 0 ) {
+ transfer_time_us = src[src_index]->SizeBytes() / transfer_bandwidth_bytes_per_us;
+ }
+ return streams[0][PANGO_HOST_RECEPTION_TIME_US].get<int64_t>() - transfer_time_us;
+ }
+ }
+ }
+ }
+
+ PANGO_ENSURE(false, "JoinVideo: Stream % does contain any timestamp info.\n", src_index);
+ return 0;
+ }
+}
+
+bool JoinVideo::GrabNext(unsigned char* image, bool wait)
+{
+ size_t offset = 0;
+ std::vector<size_t> offsets(src.size(), 0);
+ std::vector<int64_t> capture_us(src.size(), 0);
+
+ TSTART()
+ DBGPRINT("Entering GrabNext:")
+ for(size_t s=0; s<src.size(); ++s) {
+ if( src[s]->GrabNext(image+offset,wait) ) {
+ if(sync_tolerance_us > 0) {
+ capture_us[s] = GetAdjustedCaptureTime(s);
+ }else{
+ capture_us[s] = std::numeric_limits<int64_t>::max();
+ }
+ }
+ offsets[s] = offset;
+ offset += src[s]->SizeBytes();
+ TGRABANDPRINT("Stream %ld grab took ",s);
+ }
+
+ // Check if any streams didn't return an image. This means a stream is waiting on data or has finished.
+ if( std::any_of(capture_us.begin(), capture_us.end(), [](int64_t v){return v == 0;}) ){
+ return false;
+ }
+
+ // Check Sync if a tolerence has been specified.
+ if(sync_tolerance_us > 0)
+ {
+ auto range = std::minmax_element(capture_us.begin(), capture_us.end());
+ if( (*range.second - *range.first) > sync_tolerance_us)
+ {
+ pango_print_warn("JoinVideo: Source timestamps span %lu us, not within %lu us. Ignoring frames, trying to sync...\n", (unsigned long)((*range.second - *range.first)), (unsigned long)sync_tolerance_us);
+
+ // Attempt to resync...
+ for(size_t n=0; n<10; ++n){
+ for(size_t s=0; s<src.size(); ++s) {
+ // Catch up frames that are behind
+ if(capture_us[s] < (*range.second - sync_tolerance_us))
+ {
+ if(src[s]->GrabNext(image+offsets[s],true)) {
+ capture_us[s] = GetAdjustedCaptureTime(s);
+ }
+ }
+ }
+ }
+ }
+
+ // Check sync again
+ range = std::minmax_element(capture_us.begin(), capture_us.end());
+ if( (*range.second - *range.first) > sync_tolerance_us) {
+ TGRABANDPRINT("NOT IN SYNC oldest:%ld newest:%ld delta:%ld", *range.first, *range.second, (*range.second - *range.first));
+ return false;
+ } else {
+ TGRABANDPRINT(" IN SYNC oldest:%ld newest:%ld delta:%ld", *range.first, *range.second, (*range.second - *range.first));
+ return true;
+ }
+ }
+ else
+ {
+ pango_print_warn("JoinVideo: sync_tolerance_us = 0, frames are not synced!\n");
+ return true;
+ }
+}
+
+bool AllInterfacesAreBufferAware(std::vector<VideoInterface*>& src){
+ for(size_t s=0; s<src.size(); ++s) {
+ if(!dynamic_cast<BufferAwareVideoInterface*>(src[s])) return false;
+ }
+ return true;
+}
+
+bool JoinVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ // TODO: Tidy to correspond to GrabNext()
+ TSTART()
+ DBGPRINT("Entering GrabNewest:");
+ if(AllInterfacesAreBufferAware(src)) {
+ DBGPRINT("All interfaces are BufferAwareVideoInterface.")
+ unsigned int minN = std::numeric_limits<unsigned int>::max();
+ //Find smallest number of frames it is safe to drop.
+ for(size_t s=0; s<src.size(); ++s) {
+ auto bai = dynamic_cast<BufferAwareVideoInterface*>(src[s]);
+ unsigned int n = bai->AvailableFrames();
+ minN = std::min(n, minN);
+ DBGPRINT("Interface %ld has %u frames available.",s ,n)
+ }
+ TGRABANDPRINT("Quering avalable frames took ")
+ DBGPRINT("Safe number of buffers to drop: %d.",((minN > 1) ? (minN-1) : 0));
+
+ //Safely drop minN-1 frames on each interface.
+ if(minN > 1) {
+ for(size_t s=0; s<src.size(); ++s) {
+ auto bai = dynamic_cast<BufferAwareVideoInterface*>(src[s]);
+ if(!bai->DropNFrames(minN - 1)) {
+ pango_print_error("Stream %lu did not drop %u frames altough available.\n", (unsigned long)s, (minN-1));
+ return false;
+ }
+ }
+ TGRABANDPRINT("Dropping %u frames on each interface took ",(minN -1));
+ }
+ return GrabNext(image, wait);
+ } else {
+ DBGPRINT("NOT all interfaces are BufferAwareVideoInterface.")
+ // Simply calling GrabNewest on the child streams might cause loss of sync,
+ // instead we perform as many GrabNext as possible on the first stream and
+ // then pull the same number of frames from every other stream.
+ size_t offset = 0;
+ std::vector<size_t> offsets;
+ std::vector<int64_t> reception_times;
+ int64_t newest = std::numeric_limits<int64_t>::min();
+ int64_t oldest = std::numeric_limits<int64_t>::max();
+ bool grabbed_any = false;
+ int first_stream_backlog = 0;
+ int64_t rt = 0;
+ bool got_frame = false;
+
+ do {
+ got_frame = src[0]->GrabNext(image+offset,false);
+ if(got_frame) {
+ if(sync_tolerance_us > 0) {
+ rt = GetAdjustedCaptureTime(0);
+ }
+ first_stream_backlog++;
+ grabbed_any = true;
+ }
+ } while(got_frame);
+ offsets.push_back(offset);
+ offset += src[0]->SizeBytes();
+ if(sync_tolerance_us > 0) {
+ reception_times.push_back(rt);
+ if(newest < rt) newest = rt;
+ if(oldest > rt) oldest = rt;
+ }
+ TGRABANDPRINT("Stream 0 grab took ");
+
+ for(size_t s=1; s<src.size(); ++s) {
+ for (int i=0; i<first_stream_backlog; i++){
+ grabbed_any |= src[s]->GrabNext(image+offset,true);
+ if(sync_tolerance_us > 0) {
+ rt = GetAdjustedCaptureTime(s);
+ }
+ }
+ offsets.push_back(offset);
+ offset += src[s]->SizeBytes();
+ if(sync_tolerance_us > 0) {
+ reception_times.push_back(rt);
+ if(newest < rt) newest = rt;
+ if(oldest > rt) oldest = rt;
+ }
+ }
+ TGRABANDPRINT("Stream >=1 grab took ");
+
+ if(sync_tolerance_us > 0) {
+ if(std::abs(newest - oldest) > sync_tolerance_us){
+ pango_print_warn("Join timestamps not within %lu us trying to sync\n", (unsigned long)sync_tolerance_us);
+
+ for(size_t n=0; n<10; ++n){
+ for(size_t s=0; s<src.size(); ++s) {
+ if(reception_times[s] < (newest - sync_tolerance_us)) {
+ VideoInterface& vid = *src[s];
+ if(vid.GrabNewest(image+offsets[s],false)) {
+ rt = GetAdjustedCaptureTime(s);
+ if(newest < rt) newest = rt;
+ if(oldest > rt) oldest = rt;
+ reception_times[s] = rt;
+ }
+ }
+ }
+ }
+ }
+
+ if(std::abs(newest - oldest) > sync_tolerance_us ) {
+ TGRABANDPRINT("NOT IN SYNC newest:%ld oldest:%ld delta:%ld syncing took ", newest, oldest, (newest - oldest));
+ return false;
+ } else {
+ TGRABANDPRINT(" IN SYNC newest:%ld oldest:%ld delta:%ld syncing took ", newest, oldest, (newest - oldest));
+ return true;
+ }
+ } else {
+ return true;
+ }
+ }
+
+}
+
+std::vector<VideoInterface*>& JoinVideo::InputStreams()
+{
+ return src;
+}
+
+std::vector<std::string> SplitBrackets(const std::string src, char open = '{', char close = '}')
+{
+ std::vector<std::string> splits;
+
+ int nesting = 0;
+ int begin = -1;
+
+ for(size_t i=0; i < src.length(); ++i) {
+ if(src[i] == open) {
+ if(nesting==0) {
+ begin = (int)i;
+ }
+ nesting++;
+ }else if(src[i] == close) {
+ nesting--;
+ if(nesting == 0) {
+ // matching close bracket.
+ int str_start = begin+1;
+ splits.push_back( src.substr(str_start, i-str_start) );
+ }
+ }
+ }
+
+ return splits;
+}
+
+PANGOLIN_REGISTER_FACTORY(JoinVideo)
+{
+ struct JoinVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+
+ std::vector<std::string> uris = SplitBrackets(uri.url);
+
+ // Standard by which we should measure if frames are in sync.
+ const unsigned long sync_tol_us = uri.Get<unsigned long>("sync_tolerance_us", 0);
+
+ // Bandwidth used to compute exposure end time from reception time for sync logic
+ const double transfer_bandwidth_gbps = uri.Get<double>("transfer_bandwidth_gbps", 0.0);
+
+ if(uris.size() == 0) {
+ throw VideoException("No VideoSources found in join URL.", "Specify videos to join with curly braces, e.g. join://{test://}{test://}");
+ }
+
+ std::vector<std::unique_ptr<VideoInterface>> src;
+ for(size_t i=0; i<uris.size(); ++i) {
+ src.push_back( pangolin::OpenVideo(uris[i]) );
+ }
+
+ JoinVideo* video_raw = new JoinVideo(src);
+
+ if(sync_tol_us>0) {
+ if(!video_raw->Sync(sync_tol_us, transfer_bandwidth_gbps)) {
+ pango_print_error("WARNING: not all streams in join support sync_tolerance_us option. Not using tolerance.\n");
+ }
+ }
+
+ return std::unique_ptr<VideoInterface>(video_raw);
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<JoinVideoFactory>(), 10, "join");
+}
+
+}
+
+#undef TSTART
+#undef TGRABANDPRINT
+#undef DBGPRINT
diff --git a/externals/Pangolin/src/video/drivers/json.cpp b/externals/Pangolin/src/video/drivers/json.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..de20fec23aa93aa5f187be46a22ba1a60c030e26
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/json.cpp
@@ -0,0 +1,86 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/video.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/file_extension.h>
+#include <pangolin/utils/transform.h>
+
+#include <fstream>
+#include <functional>
+
+namespace pangolin {
+
+PANGOLIN_REGISTER_FACTORY(JsonVideo)
+{
+ struct JsonVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ if(uri.scheme == "json" || (uri.scheme == "file" && FileLowercaseExtention(uri.url) == ".json")) {
+ const std::string json_filename = PathExpand(uri.url);
+ std::ifstream f( json_filename );
+
+ // Parse json file to determine sub-video
+ if(f.is_open())
+ {
+ picojson::value file_json(picojson::object_type,true);
+ const std::string err = picojson::parse(file_json,f);
+ if(err.empty())
+ {
+ // Json loaded. Parse output.
+ std::string input_uri = file_json.get_value<std::string>("video_uri", "");
+ if(!input_uri.empty())
+ {
+ // Transform input_uri based on sub args.
+ const picojson::value input_uri_params = file_json.get_value<picojson::object>("video_uri_defaults", picojson::object());
+ input_uri = Transform(input_uri, [&](const std::string& k) {
+ return uri.Get<std::string>(k, input_uri_params.contains(k) ? input_uri_params[k].to_str() : "#");
+ });
+
+ return pangolin::OpenVideo(input_uri);
+ }else{
+ throw VideoException("JsonVideo failed.", "Bad input URI.");
+ }
+ }else{
+ throw VideoException("JsonVideo failed.", err);
+ }
+ }else{
+ throw VideoException("JsonVideo failed. Unable to load file.", json_filename);
+ }
+ }else{
+ // Not applicable for this factory.
+ return std::unique_ptr<VideoInterface>();
+ }
+ }
+ };
+
+ auto factory = std::make_shared<JsonVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "json");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 5, "file");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/merge.cpp b/externals/Pangolin/src/video/drivers/merge.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1822c404b2260fb6a675e5f0b6fd9a08143818d8
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/merge.cpp
@@ -0,0 +1,165 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/merge.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <pangolin/plot/range.h>
+#include <assert.h> // assert()
+
+#include <assert.h>
+
+namespace pangolin
+{
+
+MergeVideo::MergeVideo(std::unique_ptr<VideoInterface>& src_, const std::vector<Point>& stream_pos, size_t w = 0, size_t h = 0 )
+ : src( std::move(src_) ), buffer(new uint8_t[src->SizeBytes()]), stream_pos(stream_pos)
+{
+ videoin.push_back(src.get());
+
+ // Must be a stream_pos for each stream
+ // Each stream must have the same format.
+ assert(stream_pos.size() == src->Streams().size());
+ assert(src->Streams().size() > 0);
+ const PixelFormat fmt = src->Streams()[0].PixFormat();
+ for(size_t i=1; i < src->Streams().size(); ++i) {
+ assert(src->Streams()[i].PixFormat().format == fmt.format);
+ }
+
+ // Compute buffer regions for data copying.
+ XYRange<size_t> r = XYRange<size_t>::Empty();
+ for(size_t i=0; i < src->Streams().size(); ++i) {
+ const StreamInfo& si = src->Streams()[i];
+ const size_t x = stream_pos[i].x;
+ const size_t y = stream_pos[i].y;
+ XYRange<size_t> sr(x, x + si.Width(), y, y + si.Height());
+ r.Insert(sr);
+ }
+
+ // Use implied min / max based on points
+ if(!w && !h) {
+ w = r.x.max;
+ h = r.y.max;
+ }
+
+ size_bytes = w*h*fmt.bpp/8;
+ streams.emplace_back(fmt,w,h,w*fmt.bpp/8,(unsigned char*)0);
+}
+
+MergeVideo::~MergeVideo()
+{
+
+}
+
+//! Implement VideoInput::Start()
+void MergeVideo::Start()
+{
+ src->Start();
+}
+
+//! Implement VideoInput::Stop()
+void MergeVideo::Stop()
+{
+ src->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t MergeVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& MergeVideo::Streams() const
+{
+ return streams;
+}
+
+void MergeVideo::CopyBuffer(unsigned char* dst_bytes, unsigned char* src_bytes)
+{
+ Image<unsigned char> dst_image = Streams()[0].StreamImage(dst_bytes);
+ const size_t dst_pix_bytes = Streams()[0].PixFormat().bpp / 8;
+
+ for(size_t i=0; i < stream_pos.size(); ++i) {
+ const StreamInfo& src_stream = src->Streams()[i];
+ const Image<unsigned char> src_image = src_stream.StreamImage(src_bytes);
+ const Point& p = stream_pos[i];
+ for(size_t y=0; y < src_stream.Height(); ++y) {
+ // Copy row from src to dst
+ std::memcpy(
+ dst_image.RowPtr(y + p.y) + p.x * dst_pix_bytes,
+ src_image.RowPtr(y), src_stream.RowBytes()
+ );
+ }
+ }
+}
+
+//! Implement VideoInput::GrabNext()
+bool MergeVideo::GrabNext( unsigned char* image, bool wait )
+{
+ const bool success = src->GrabNext(buffer.get(), wait);
+ if(success) CopyBuffer(image, buffer.get());
+ return success;
+}
+
+//! Implement VideoInput::GrabNewest()
+bool MergeVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ const bool success = src->GrabNewest(buffer.get(), wait);
+ if(success) CopyBuffer(image, buffer.get());
+ return success;
+}
+
+std::vector<VideoInterface*>& MergeVideo::InputStreams()
+{
+ return videoin;
+}
+
+PANGOLIN_REGISTER_FACTORY(MergeVideo)
+{
+ struct MergeVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(0,0));
+
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ std::vector<Point> points;
+ Point p(0,0);
+ for(size_t s=0; s < subvid->Streams().size(); ++s) {
+ const StreamInfo& si = subvid->Streams()[s];
+ p = uri.Get<Point>("pos"+std::to_string(s+1), p);
+ points.push_back(p);
+ p.x += si.Width();
+ }
+
+ return std::unique_ptr<VideoInterface>(new MergeVideo(subvid, points, dim.x, dim.y));
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<MergeVideoFactory>(), 10, "merge");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/mirror.cpp b/externals/Pangolin/src/video/drivers/mirror.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..bcb87fc176f04d0696f8d08939d90d9985e24c2b
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/mirror.cpp
@@ -0,0 +1,589 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/mirror.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+
+MirrorVideo::MirrorVideo(std::unique_ptr<VideoInterface>& src, const std::vector<MirrorOptions>& flips)
+ : videoin(std::move(src)), flips(flips), size_bytes(0),buffer(0)
+{
+ if(!videoin) {
+ throw VideoException("MirrorVideo: VideoInterface in must not be null");
+ }
+
+ inputs.push_back(videoin.get());
+
+ for(size_t i=0;i<videoin->Streams().size();i++)
+ switch (flips[i]) {
+ case MirrorOptionsFlipX:
+ case MirrorOptionsFlipY:
+ case MirrorOptionsFlipXY:
+ case MirrorOptionsNone:
+ streams.push_back(videoin->Streams()[i]);
+ break;
+
+ case MirrorOptionsTranspose:
+ case MirrorOptionsRotateCW:
+ case MirrorOptionsRotateCCW:
+
+ unsigned char*ptr=videoin->Streams()[i].Offset();
+ size_t w=videoin->Streams()[i].Height();
+ size_t h=videoin->Streams()[i].Width();
+ size_t Bpp=videoin->Streams()[i].PixFormat().bpp / 8;
+
+ streams.emplace_back(videoin->Streams()[i].PixFormat(),pangolin::Image<unsigned char>(ptr,w,h,w*Bpp));
+ break;
+ };
+
+ size_bytes = videoin->SizeBytes();
+ buffer = new unsigned char[size_bytes];
+}
+
+MirrorVideo::~MirrorVideo()
+{
+ delete[] buffer;
+}
+
+//! Implement VideoInput::Start()
+void MirrorVideo::Start()
+{
+ videoin->Start();
+}
+
+//! Implement VideoInput::Stop()
+void MirrorVideo::Stop()
+{
+ videoin->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t MirrorVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& MirrorVideo::Streams() const
+{
+ return streams;
+}
+
+void PitchedImageCopy(
+ Image<unsigned char>& img_out,
+ const Image<unsigned char>& img_in,
+ size_t bytes_per_pixel
+) {
+ if( img_out.w != img_in.w || img_out.h != img_in.h ) {
+ throw std::runtime_error("PitchedImageCopy: Incompatible image sizes");
+ }
+
+ for(size_t y=0; y < img_out.h; ++y) {
+ std::memcpy(img_out.RowPtr((int)y), img_in.RowPtr((int)y), bytes_per_pixel * img_in.w);
+ }
+}
+
+void FlipY(
+ Image<unsigned char>& img_out,
+ const Image<unsigned char>& img_in,
+ size_t bytes_per_pixel
+) {
+ if( img_out.w != img_in.w || img_out.h != img_in.h ) {
+ throw std::runtime_error("FlipY: Incompatible image sizes");
+ }
+
+ for(size_t y_out=0; y_out < img_out.h; ++y_out) {
+ const size_t y_in = (img_in.h-1) - y_out;
+ std::memcpy(img_out.RowPtr((int)y_out), img_in.RowPtr((int)y_in), bytes_per_pixel * img_in.w);
+ }
+}
+
+template <typename T>
+void ChainSwap2(T& a, T& b)
+{
+ T t = a;
+ a = b;
+ b = t;
+}
+
+template <typename T>
+void ChainSwap4(T& a, T& b, T& c, T& d)
+{
+ T t = a;
+ a = b;
+ b = c;
+ c = d;
+ d = t;
+}
+
+template <size_t BPP, size_t TSZ>
+void TiledFlipX(Image<unsigned char>& img_out, const Image<unsigned char>& img_in)
+{
+ const size_t w = img_in.w;
+ const size_t h = img_in.h;
+
+ typedef struct
+ {
+ unsigned char d[BPP];
+ } T;
+ T d[TSZ][TSZ];
+
+ for(size_t xin = 0; xin < w; xin += TSZ)
+ for(size_t yin = 0; yin < h; yin += TSZ)
+ {
+ const size_t xspan = std::min(TSZ, w - xin);
+ const size_t yspan = std::min(TSZ, h - yin);
+ const size_t xout = w - xin - TSZ;
+ const size_t yout = yin;
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(d[y], img_in.RowPtr(yin + y) + xin * BPP, xspan * BPP);
+
+ for(size_t y = 0; y < TSZ; y++)
+ for(size_t x = 0; x < TSZ / 2; x++)
+ ChainSwap2(d[y][x], d[y][TSZ - 1 - x]);
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(img_out.RowPtr(yout + y) + (xout + TSZ - xspan) * BPP, d[y] + TSZ - xspan, xspan * BPP);
+ }
+}
+
+template <size_t BPP, size_t TSZ>
+void TiledRotate180(Image<unsigned char>& img_out, const Image<unsigned char>& img_in)
+{
+ static_assert(!(TSZ & 1), "Tilesize must be even.");
+
+ const size_t w = img_in.w;
+ const size_t h = img_in.h;
+
+ typedef struct
+ {
+ unsigned char d[BPP];
+ } T;
+ T d[TSZ][TSZ];
+
+ for(size_t xin = 0; xin < w; xin += TSZ)
+ for(size_t yin = 0; yin < h; yin += TSZ)
+ {
+ const size_t xspan = std::min(TSZ, w - xin);
+ const size_t yspan = std::min(TSZ, h - yin);
+ const size_t xout = w - xin - TSZ;
+ const size_t yout = h - yin - TSZ;
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(d[y], img_in.RowPtr(yin + y) + xin * BPP, xspan * BPP);
+
+ for(size_t y = 0; y < TSZ / 2; y++)
+ for(size_t x = 0; x < TSZ; x++)
+ ChainSwap2(d[y][x], d[TSZ - 1 - y][TSZ - 1 - x]);
+
+ for(size_t y = TSZ - yspan; y < TSZ; y++)
+ memcpy(img_out.RowPtr(yout + y) + (xout + TSZ - xspan) * BPP, d[y] + TSZ - xspan, xspan * BPP);
+ }
+}
+
+template <size_t BPP, size_t TSZ>
+void TiledTranspose(Image<unsigned char>& img_out, const Image<unsigned char>& img_in)
+{
+ const size_t w = img_in.w;
+ const size_t h = img_in.h;
+
+ typedef struct
+ {
+ unsigned char d[BPP];
+ } T;
+ T d[TSZ][TSZ];
+
+ for(size_t xin = 0; xin < w; xin += TSZ)
+ for(size_t yin = 0; yin < h; yin += TSZ)
+ {
+ const size_t xspan = std::min(TSZ, w - xin);
+ const size_t yspan = std::min(TSZ, h - yin);
+ const size_t dmin = std::min(xspan, yspan);
+ const size_t dmax = std::max(xspan, yspan);
+ const size_t xout = yin;
+ const size_t yout = xin;
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(d[y], img_in.RowPtr(yin + y) + xin * BPP, xspan * BPP);
+
+ for(size_t x = 0; x < dmin; x++)
+ for(size_t y = x + 1; y < dmax; y++)
+ ChainSwap2(d[x][y], d[y][x]);
+
+ for(size_t y = 0; y < xspan; y++)
+ memcpy(img_out.RowPtr(yout + y) + xout * BPP, d[y], yspan * BPP);
+ }
+}
+
+template <size_t BPP, size_t TSZ>
+void TiledRotateCW(Image<unsigned char>& img_out, const Image<unsigned char>& img_in)
+{
+ static_assert(!(TSZ & 1), "Tilesize must be even.");
+
+ const size_t w = img_in.w;
+ const size_t h = img_in.h;
+
+ typedef struct
+ {
+ unsigned char d[BPP];
+ } T;
+ T d[TSZ][TSZ];
+
+ for(size_t xin = 0; xin < w; xin += TSZ)
+ for(size_t yin = 0; yin < h; yin += TSZ)
+ {
+ const size_t xspan = std::min(TSZ, w - xin);
+ const size_t yspan = std::min(TSZ, h - yin);
+ const size_t xout = h - yin - TSZ;
+ const size_t yout = xin;
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(d[y], img_in.RowPtr(yin + y) + xin * BPP, xspan * BPP);
+
+ for(size_t y = 0; y < TSZ / 2; y++)
+ for(size_t x = 0; x < TSZ / 2; x++)
+ ChainSwap4(d[TSZ - 1 - x][y], d[TSZ - 1 - y][TSZ - 1 - x], d[x][TSZ - 1 - y], d[y][x]);
+
+ for(size_t y = 0; y < xspan; y++)
+ memcpy(img_out.RowPtr(yout + y) + (xout + TSZ - yspan) * BPP, d[y] + TSZ - yspan, yspan * BPP);
+ }
+}
+
+template <size_t BPP, size_t TSZ>
+void TiledRotateCCW(Image<unsigned char>& img_out, const Image<unsigned char>& img_in)
+{
+ static_assert(!(TSZ & 1), "Tilesize must be even.");
+
+ const size_t w = img_in.w;
+ const size_t h = img_in.h;
+
+ typedef struct
+ {
+ unsigned char d[BPP];
+ } T;
+ T d[TSZ][TSZ];
+
+ for(size_t xin = 0; xin < w; xin += TSZ)
+ for(size_t yin = 0; yin < h; yin += TSZ)
+ {
+ const size_t xspan = std::min(TSZ, w - xin);
+ const size_t yspan = std::min(TSZ, h - yin);
+ const size_t xout = yin;
+ const size_t yout = w - xin - TSZ;
+
+ for(size_t y = 0; y < yspan; y++)
+ memcpy(d[y], img_in.RowPtr(yin + y) + xin * BPP, xspan * BPP);
+
+ for(size_t y = 0; y < TSZ / 2; y++)
+ for(size_t x = 0; x < TSZ / 2; x++)
+ ChainSwap4(d[y][x], d[x][TSZ - 1 - y], d[TSZ - 1 - y][TSZ - 1 - x], d[TSZ - 1 - x][y]);
+
+ for(size_t y = TSZ - xspan; y < TSZ; y++)
+ memcpy(img_out.RowPtr(yout + y) + xout * BPP, d[y], yspan * BPP);
+ }
+}
+
+void FlipX(Image<unsigned char>& img_out, const Image<unsigned char>& img_in, size_t bytes_per_pixel)
+{
+ if(bytes_per_pixel == 1)
+ TiledFlipX<1, 160>(img_out, img_in);
+ else if(bytes_per_pixel == 2)
+ TiledFlipX<2, 120>(img_out, img_in);
+ else if(bytes_per_pixel == 3)
+ TiledFlipX<3, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 4)
+ TiledFlipX<4, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 6)
+ TiledFlipX<6, 64>(img_out, img_in);
+ else {
+ for(size_t y = 0; y < img_out.h; ++y) {
+ for(size_t x = 0; x < img_out.w; ++x) {
+ memcpy(img_out.RowPtr((int)y) + (img_out.w - 1 - x) * bytes_per_pixel,
+ img_in.RowPtr((int)y) + x * bytes_per_pixel,
+ bytes_per_pixel);
+ }
+ }
+ }
+}
+
+void FlipXY(Image<unsigned char>& img_out, const Image<unsigned char>& img_in, size_t bytes_per_pixel)
+{
+ if(bytes_per_pixel == 1)
+ TiledRotate180<1, 160>(img_out, img_in);
+ else if(bytes_per_pixel == 2)
+ TiledRotate180<2, 120>(img_out, img_in);
+ else if(bytes_per_pixel == 3)
+ TiledRotate180<3, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 4)
+ TiledRotate180<4, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 6)
+ TiledRotate180<6, 64>(img_out, img_in);
+ else {
+ for(size_t y_out = 0; y_out < img_out.h; ++y_out) {
+ for(size_t x = 0; x < img_out.w; ++x) {
+ const size_t y_in = (img_in.h - 1) - y_out;
+ memcpy(img_out.RowPtr((int)y_out) + (img_out.w - 1 - x) * bytes_per_pixel,
+ img_in.RowPtr((int)y_in) + x * bytes_per_pixel,
+ bytes_per_pixel);
+ }
+ }
+ }
+}
+
+void RotateCW(Image<unsigned char>& img_out, const Image<unsigned char>& img_in, size_t bytes_per_pixel)
+{
+ if(bytes_per_pixel == 1)
+ TiledRotateCW<1, 160>(img_out, img_in);
+ else if(bytes_per_pixel == 2)
+ TiledRotateCW<2, 120>(img_out, img_in);
+ else if(bytes_per_pixel == 3)
+ TiledRotateCW<3, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 4)
+ TiledRotateCW<4, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 6)
+ TiledRotateCW<6, 64>(img_out, img_in);
+ else {
+ for(size_t yout = 0; yout < img_out.h; ++yout)
+ for(size_t xout = 0; xout < img_out.w; ++xout) {
+ size_t xin = yout;
+ size_t yin = img_out.w - 1 - xout;
+ memcpy(img_out.RowPtr((int)yout) + xout * bytes_per_pixel,
+ img_in.RowPtr((int)yin) + xin * bytes_per_pixel,
+ bytes_per_pixel);
+ }
+ }
+}
+
+void Transpose(Image<unsigned char>& img_out, const Image<unsigned char>& img_in, size_t bytes_per_pixel)
+{
+ if(bytes_per_pixel == 1)
+ TiledTranspose<1, 160>(img_out, img_in);
+ else if(bytes_per_pixel == 2)
+ TiledTranspose<2, 120>(img_out, img_in);
+ else if(bytes_per_pixel == 3)
+ TiledTranspose<3, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 4)
+ TiledTranspose<4, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 6)
+ TiledTranspose<6, 64>(img_out, img_in);
+ else {
+ for(size_t yout = 0; yout < img_out.h; ++yout)
+ for(size_t xout = 0; xout < img_out.w; ++xout) {
+ size_t xin = yout;
+ size_t yin = xout;
+ memcpy(img_out.RowPtr((int)yout) + xout * bytes_per_pixel,
+ img_in.RowPtr((int)yin) + xin * bytes_per_pixel,
+ bytes_per_pixel);
+ }
+ }
+}
+
+void RotateCCW(Image<unsigned char>& img_out, const Image<unsigned char>& img_in, size_t bytes_per_pixel)
+{
+ if(bytes_per_pixel == 1)
+ TiledRotateCCW<1, 160>(img_out, img_in);
+ else if(bytes_per_pixel == 2)
+ TiledRotateCCW<2, 120>(img_out, img_in);
+ else if(bytes_per_pixel == 3)
+ TiledRotateCCW<3, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 4)
+ TiledRotateCCW<4, 80>(img_out, img_in);
+ else if(bytes_per_pixel == 6)
+ TiledRotateCCW<6, 64>(img_out, img_in);
+ else {
+ for(size_t yout = 0; yout < img_out.h; ++yout)
+ for(size_t xout = 0; xout < img_out.w; ++xout) {
+ size_t xin = img_out.h - 1 - yout;
+ size_t yin = xout;
+ memcpy(img_out.RowPtr((int)yout) + xout * bytes_per_pixel,
+ img_in.RowPtr((int)yin) + xin * bytes_per_pixel,
+ bytes_per_pixel);
+ }
+ }
+}
+
+
+void MirrorVideo::Process(unsigned char* buffer_out, const unsigned char* buffer_in)
+{
+
+ for(size_t s=0; s<streams.size(); ++s) {
+ Image<unsigned char> img_out = Streams()[s].StreamImage(buffer_out);
+ const Image<unsigned char> img_in = videoin->Streams()[s].StreamImage(buffer_in);
+ const size_t bytes_per_pixel = Streams()[s].PixFormat().bpp / 8;
+
+ switch (flips[s]) {
+ case MirrorOptionsFlipX:
+ FlipX(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsFlipY:
+ FlipY(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsFlipXY:
+ FlipXY(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsRotateCW:
+ RotateCW(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsRotateCCW:
+ RotateCCW(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsTranspose:
+ Transpose(img_out, img_in, bytes_per_pixel);
+ break;
+ case MirrorOptionsNone:
+ PitchedImageCopy(img_out, img_in, bytes_per_pixel);
+ break;
+ default:
+ pango_print_warn("MirrorVideo::Process(): Invalid enum %i.\n", flips[s]);
+ }
+ }
+
+}
+
+//! Implement VideoInput::GrabNext()
+bool MirrorVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin->GrabNext(buffer,wait)) {
+ Process(image, buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool MirrorVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin->GrabNewest(buffer,wait)) {
+ Process(image, buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+std::vector<VideoInterface*>& MirrorVideo::InputStreams()
+{
+ return inputs;
+}
+
+unsigned int MirrorVideo::AvailableFrames() const
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin.get());
+ if(!vpi)
+ {
+ pango_print_warn("Mirror: child interface is not buffer aware.");
+ return 0;
+ }
+ else
+ {
+ return vpi->AvailableFrames();
+ }
+}
+
+bool MirrorVideo::DropNFrames(uint32_t n)
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin.get());
+ if(!vpi)
+ {
+ pango_print_warn("Mirror: child interface is not buffer aware.");
+ return false;
+ }
+ else
+ {
+ return vpi->DropNFrames(n);
+ }
+}
+
+std::istream& operator>> (std::istream &is, MirrorOptions &mirror)
+{
+ std::string str_mirror;
+ is >> str_mirror;
+ std::transform(str_mirror.begin(), str_mirror.end(), str_mirror.begin(), toupper);
+
+ if(!str_mirror.compare("NONE")) {
+ mirror = MirrorOptionsNone;
+ }else if(!str_mirror.compare("FLIPX") || !str_mirror.compare("MIRROR")) {
+ mirror = MirrorOptionsFlipX;
+ }else if(!str_mirror.compare("FLIPY") || !str_mirror.compare("FLIP")) {
+ mirror = MirrorOptionsFlipY;
+ }else if(!str_mirror.compare("ROTATECW")) {
+ mirror = MirrorOptionsRotateCW;
+ }else if(!str_mirror.compare("ROTATECCW")) {
+ mirror = MirrorOptionsRotateCCW;
+ }else if(!str_mirror.compare("FLIPXY") || !str_mirror.compare("TRANSPOSE")) {
+ mirror = MirrorOptionsFlipXY;
+ }else{
+ pango_print_warn("Unknown mirror option %s.", str_mirror.c_str());
+ mirror = MirrorOptionsNone;
+ }
+
+ return is;
+}
+
+PANGOLIN_REGISTER_FACTORY(MirrorVideo)
+{
+ struct MirrorVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+
+ MirrorOptions default_opt = MirrorOptionsNone;
+ if(uri.scheme == "flip") default_opt = MirrorOptionsFlipY;
+ if(uri.scheme == "rotate") default_opt = MirrorOptionsFlipXY;
+ if(uri.scheme == "transpose") default_opt = MirrorOptionsTranspose;
+ if(uri.scheme == "rotateCW") default_opt = MirrorOptionsRotateCW;
+ if(uri.scheme == "rotateCCW") default_opt = MirrorOptionsRotateCCW;
+
+ std::vector<MirrorOptions> flips;
+
+ for(size_t i=0; i < subvid->Streams().size(); ++i){
+ std::stringstream ss;
+ ss << "stream" << i;
+ const std::string key = ss.str();
+ flips.push_back(uri.Get<MirrorOptions>(key, default_opt) );
+ }
+
+ return std::unique_ptr<VideoInterface> (new MirrorVideo(subvid, flips));
+ }
+ };
+
+ auto factory = std::make_shared<MirrorVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "mirror");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "flip");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "rotate");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "transpose");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "rotateCW");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "rotateCCW");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "transform");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/openni.cpp b/externals/Pangolin/src/video/drivers/openni.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c5d4f1e088827593c2e92277e5e36584054e78d3
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/openni.cpp
@@ -0,0 +1,299 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/openni.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+OpenNiVideo::OpenNiVideo(OpenNiSensorType s1, OpenNiSensorType s2, ImageDim dim, int fps)
+{
+ sensor_type[0] = s1;
+ sensor_type[1] = s2;
+
+ XnStatus nRetVal = XN_STATUS_OK;
+ nRetVal = context.Init();
+ if (nRetVal != XN_STATUS_OK) {
+ std::cerr << "context.Init: " << xnGetStatusString(nRetVal) << std::endl;
+ }
+
+ XnMapOutputMode mapMode;
+ mapMode.nXRes = dim.x;
+ mapMode.nYRes = dim.y;
+ mapMode.nFPS = fps;
+
+ sizeBytes = 0;
+
+ bool use_depth = false;
+ bool use_ir = false;
+ bool use_rgb = false;
+ bool depth_to_color = false;
+
+ for(int i=0; i<2; ++i) {
+ PixelFormat fmt;
+
+ // Establish output pixel format for sensor streams
+ switch( sensor_type[i] ) {
+ case OpenNiDepth_1mm_Registered:
+ case OpenNiDepth_1mm:
+ case OpenNiIr:
+ case OpenNiIrProj:
+ fmt = PixelFormatFromString("GRAY16LE");
+ break;
+ case OpenNiIr8bit:
+ case OpenNiIr8bitProj:
+ fmt = PixelFormatFromString("GRAY8");
+ break;
+ case OpenNiRgb:
+ fmt = PixelFormatFromString("RGB24");
+ break;
+ default:
+ continue;
+ }
+
+ switch( sensor_type[i] ) {
+ case OpenNiDepth_1mm_Registered:
+ depth_to_color = true;
+ case OpenNiDepth_1mm:
+ use_depth = true;
+ break;
+ case OpenNiIr:
+ case OpenNiIr8bit:
+ use_ir = true;
+ break;
+ case OpenNiIrProj:
+ case OpenNiIr8bitProj:
+ use_ir = true;
+ use_depth = true;
+ break;
+ case OpenNiRgb:
+ use_rgb = true;
+ break;
+ default:
+ break;
+ }
+
+ const StreamInfo stream(fmt, mapMode.nXRes, mapMode.nYRes, (mapMode.nXRes * fmt.bpp) / 8, (unsigned char*)0 + sizeBytes);
+ sizeBytes += stream.SizeBytes();
+ streams.push_back(stream);
+ }
+
+ if( use_depth ) {
+ nRetVal = depthNode.Create(context);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Unable to create DepthNode: " + xnGetStatusString(nRetVal) );
+ }else{
+ nRetVal = depthNode.SetMapOutputMode(mapMode);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Invalid DepthNode mode: " + xnGetStatusString(nRetVal) );
+ }
+ }
+ }
+
+ if( use_rgb ) {
+ nRetVal = imageNode.Create(context);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Unable to create ImageNode: " + xnGetStatusString(nRetVal) );
+ }else{
+ nRetVal = imageNode.SetMapOutputMode(mapMode);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Invalid ImageNode mode: " + xnGetStatusString(nRetVal) );
+ }
+ }
+ }
+
+ if (depth_to_color && use_rgb) {
+ //Registration
+ if( depthNode.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT) ) {
+ nRetVal = depthNode.GetAlternativeViewPointCap().SetViewPoint( imageNode );
+ if (nRetVal != XN_STATUS_OK) {
+ std::cerr << "depthNode.GetAlternativeViewPointCap().SetViewPoint(imageNode): " << xnGetStatusString(nRetVal) << std::endl;
+ }
+ }
+
+ // Frame Sync
+ if (depthNode.IsCapabilitySupported(XN_CAPABILITY_FRAME_SYNC))
+ {
+ if (depthNode.GetFrameSyncCap().CanFrameSyncWith(imageNode))
+ {
+ nRetVal = depthNode.GetFrameSyncCap().FrameSyncWith(imageNode);
+ if (nRetVal != XN_STATUS_OK) {
+ std::cerr << "depthNode.GetFrameSyncCap().FrameSyncWith(imageNode): " << xnGetStatusString(nRetVal) << std::endl;
+ }
+ }
+ }
+ }
+
+ if( use_ir ) {
+ nRetVal = irNode.Create(context);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Unable to create IrNode: " + xnGetStatusString(nRetVal) );
+ }else{
+ nRetVal = irNode.SetMapOutputMode(mapMode);
+ if (nRetVal != XN_STATUS_OK) {
+ throw VideoException( (std::string)"Invalid IrNode mode: " + xnGetStatusString(nRetVal) );
+ }
+ }
+ }
+
+ Start();
+}
+
+OpenNiVideo::~OpenNiVideo()
+{
+ context.Release();
+}
+
+size_t OpenNiVideo::SizeBytes() const
+{
+ return sizeBytes;
+}
+
+const std::vector<StreamInfo>& OpenNiVideo::Streams() const
+{
+ return streams;
+}
+
+void OpenNiVideo::Start()
+{
+ // XnStatus nRetVal =
+ context.StartGeneratingAll();
+}
+
+void OpenNiVideo::Stop()
+{
+ context.StopGeneratingAll();
+}
+
+bool OpenNiVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ // XnStatus nRetVal = context.WaitAndUpdateAll();
+ XnStatus nRetVal = context.WaitAnyUpdateAll();
+ // nRetVal = context.WaitOneUpdateAll(imageNode);
+
+ if (nRetVal != XN_STATUS_OK) {
+ std::cerr << "Failed updating data: " << xnGetStatusString(nRetVal) << std::endl;
+ return false;
+ }else{
+ unsigned char* out_img = image;
+
+ for(int i=0; i<2; ++i) {
+ switch (sensor_type[i]) {
+ case OpenNiDepth_1mm:
+ case OpenNiDepth_1mm_Registered:
+ {
+ const XnDepthPixel* pDepthMap = depthNode.GetDepthMap();
+ memcpy(out_img,pDepthMap, streams[i].SizeBytes() );
+ break;
+ }
+ case OpenNiIr:
+ case OpenNiIrProj:
+ {
+ const XnIRPixel* pIrMap = irNode.GetIRMap();
+ memcpy(out_img, pIrMap, streams[i].SizeBytes() );
+ break;
+ }
+ case OpenNiIr8bit:
+ case OpenNiIr8bitProj:
+ {
+ const XnIRPixel* pIr16Map = irNode.GetIRMap();
+
+ // rescale from 16-bit (10 effective) to 8-bit
+ xn::IRMetaData meta_data;
+ irNode.GetMetaData(meta_data);
+ int w = meta_data.XRes();
+ int h = meta_data.YRes();
+
+ // Copy to out_img with conversion
+ XnUInt8* pIrMapScaled = (XnUInt8*)out_img;
+ for (int v = 0; v < h; ++v)
+ for (int u = 0; u < w; ++u) {
+ int val = *pIr16Map >> 2; // 10bit to 8 bit
+ pIrMapScaled[w * v + u] = val;
+ pIr16Map++;
+ }
+
+ break;
+ }
+ case OpenNiRgb:
+ {
+ const XnUInt8* pImageMap = imageNode.GetImageMap();
+ memcpy(out_img,pImageMap, streams[i].SizeBytes());
+ break;
+ }
+ default:
+ continue;
+ break;
+ }
+
+ out_img += streams[i].SizeBytes();
+ }
+
+ return true;
+ }
+}
+
+bool OpenNiVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+PANGOLIN_REGISTER_FACTORY(OpenNiVideo)
+{
+ struct OpenNiVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const unsigned int fps = uri.Get<unsigned int>("fps", 30);
+ const bool autoexposure = uri.Get<bool>("autoexposure", true);
+
+ OpenNiSensorType img1 = OpenNiRgb;
+ OpenNiSensorType img2 = OpenNiUnassigned;
+
+ if( uri.Contains("img1") ){
+ img1 = openni_sensor(uri.Get<std::string>("img1", "depth"));
+ }
+
+ if( uri.Contains("img2") ){
+ img2 = openni_sensor(uri.Get<std::string>("img2","rgb"));
+ }
+
+ OpenNiVideo* oniv = new OpenNiVideo(img1, img2, dim, fps);
+ oniv->SetAutoExposure(autoexposure);
+ return std::unique_ptr<VideoInterface>(oniv);
+ }
+ };
+
+ auto factory = std::make_shared<OpenNiVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "openni1");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 100, "openni");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 100, "oni");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 100, "kinect");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/openni2.cpp b/externals/Pangolin/src/video/drivers/openni2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7c5459c3a6e6328bb37effd5bf1b356eee945f1e
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/openni2.cpp
@@ -0,0 +1,690 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Richard Newcombe
+ * 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/openni2.h>
+
+#include <OniVersion.h>
+#include <PS1080.h>
+
+namespace pangolin
+{
+
+PixelFormat VideoFormatFromOpenNI2(openni::PixelFormat fmt)
+{
+ std::string pvfmt;
+
+ switch (fmt) {
+ case openni::PIXEL_FORMAT_DEPTH_1_MM: pvfmt = "GRAY16LE"; break;
+ case openni::PIXEL_FORMAT_DEPTH_100_UM: pvfmt = "GRAY16LE"; break;
+ case openni::PIXEL_FORMAT_SHIFT_9_2: pvfmt = "GRAY16LE"; break; // ?
+ case openni::PIXEL_FORMAT_SHIFT_9_3: pvfmt = "GRAY16LE"; break; // ?
+ case openni::PIXEL_FORMAT_RGB888: pvfmt = "RGB24"; break;
+ case openni::PIXEL_FORMAT_GRAY8: pvfmt = "GRAY8"; break;
+ case openni::PIXEL_FORMAT_GRAY16: pvfmt = "GRAY16LE"; break;
+ case openni::PIXEL_FORMAT_YUV422: pvfmt = "YUYV422"; break;
+#if ONI_VERSION_MAJOR >= 2 && ONI_VERSION_MINOR >= 2
+ case openni::PIXEL_FORMAT_YUYV: pvfmt = "Y400A"; break;
+#endif
+ default:
+ throw VideoException("Unknown OpenNI pixel format");
+ break;
+ }
+
+ return PixelFormatFromString(pvfmt);
+}
+
+void OpenNi2Video::PrintOpenNI2Modes(openni::SensorType sensorType)
+{
+ // Query supported modes for device
+ const openni::Array<openni::VideoMode>& modes =
+ devices[0].getSensorInfo(sensorType)->getSupportedVideoModes();
+
+ switch (sensorType) {
+ case openni::SENSOR_COLOR: pango_print_info("OpenNI Colour Modes:\n"); break;
+ case openni::SENSOR_DEPTH: pango_print_info("OpenNI Depth Modes:\n"); break;
+ case openni::SENSOR_IR: pango_print_info("OpenNI IR Modes:\n"); break;
+ }
+
+ for(int i = 0; i < modes.getSize(); i++) {
+ std::string sfmt = "PangolinUnknown";
+ try{
+ sfmt = VideoFormatFromOpenNI2(modes[i].getPixelFormat()).format;
+ }catch(VideoException){}
+ pango_print_info( " %dx%d, %d fps, %s\n",
+ modes[i].getResolutionX(), modes[i].getResolutionY(),
+ modes[i].getFps(), sfmt.c_str()
+ );
+ }
+}
+
+openni::VideoMode OpenNi2Video::FindOpenNI2Mode(
+ openni::Device & device,
+ openni::SensorType sensorType,
+ int width, int height,
+ int fps, openni::PixelFormat fmt
+) {
+ // Query supported modes for device
+ const openni::Array<openni::VideoMode>& modes =
+ device.getSensorInfo(sensorType)->getSupportedVideoModes();
+
+ // Select last listed mode which matches parameters
+ int best_mode = -1;
+ for(int i = 0; i < modes.getSize(); i++) {
+ if( (!width || modes[i].getResolutionX() == width) &&
+ (!height || modes[i].getResolutionY() == height) &&
+ (!fps || modes[i].getFps() == fps) &&
+ (!fmt || modes[i].getPixelFormat() == fmt)
+ ) {
+ best_mode = i;
+ }
+ }
+
+ if(best_mode >= 0) {
+ return modes[best_mode];
+ }
+
+ throw pangolin::VideoException("Video mode not supported");
+}
+
+inline openni::SensorType SensorType(const OpenNiSensorType sensor)
+{
+ switch (sensor) {
+ case OpenNiRgb:
+ case OpenNiGrey:
+ return openni::SENSOR_COLOR;
+ case OpenNiDepth_1mm:
+ case OpenNiDepth_100um:
+ case OpenNiDepth_1mm_Registered:
+ return openni::SENSOR_DEPTH;
+ case OpenNiIr:
+ case OpenNiIr8bit:
+ case OpenNiIr24bit:
+ case OpenNiIrProj:
+ case OpenNiIr8bitProj:
+ return openni::SENSOR_IR;
+ default:
+ throw std::invalid_argument("OpenNI: Bad sensor type");
+ }
+}
+
+OpenNi2Video::OpenNi2Video(ImageDim dim, ImageRoi roi, int fps)
+{
+ InitialiseOpenNI();
+
+ openni::Array<openni::DeviceInfo> deviceList;
+ openni::OpenNI::enumerateDevices(&deviceList);
+
+ if (deviceList.getSize() < 1) {
+ throw VideoException("No OpenNI Devices available. Ensure your camera is plugged in.");
+ }
+
+ for(int i = 0 ; i < deviceList.getSize(); i ++) {
+ const char* device_uri = deviceList[i].getUri();
+ const int dev_id = AddDevice(device_uri);
+ AddStream(OpenNiStreamMode( OpenNiDepth_1mm, dim, roi, fps, dev_id) );
+ AddStream(OpenNiStreamMode( OpenNiRgb, dim, roi, fps, dev_id) );
+ }
+
+ SetupStreamModes();
+}
+
+OpenNi2Video::OpenNi2Video(const std::string& device_uri)
+{
+ InitialiseOpenNI();
+
+ const int dev_id = AddDevice(device_uri);
+ AddStream(OpenNiStreamMode( OpenNiDepth_1mm, ImageDim(), ImageRoi(), 30, dev_id) );
+ AddStream(OpenNiStreamMode( OpenNiRgb, ImageDim(), ImageRoi(), 30, dev_id) );
+
+ SetupStreamModes();
+}
+
+OpenNi2Video::OpenNi2Video(const std::string& device_uri, std::vector<OpenNiStreamMode> &stream_modes)
+{
+ InitialiseOpenNI();
+
+ AddDevice(device_uri);
+
+ for(size_t i=0; i < stream_modes.size(); ++i) {
+ OpenNiStreamMode& mode = stream_modes[i];
+ AddStream(mode);
+ }
+
+ SetupStreamModes();
+}
+
+OpenNi2Video::OpenNi2Video(std::vector<OpenNiStreamMode>& stream_modes)
+{
+ InitialiseOpenNI();
+
+ openni::Array<openni::DeviceInfo> deviceList;
+ openni::OpenNI::enumerateDevices(&deviceList);
+
+ if (deviceList.getSize() < 1) {
+ throw VideoException("OpenNI2: No devices available. Ensure your camera is plugged in.");
+ }
+
+ for(int i = 0 ; i < deviceList.getSize(); i ++) {
+ const char* device_uri = deviceList[i].getUri();
+ AddDevice(device_uri);
+ }
+
+ for(size_t i=0; i < stream_modes.size(); ++i) {
+ OpenNiStreamMode& mode = stream_modes[i];
+ AddStream(mode);
+ }
+
+ SetupStreamModes();
+}
+
+void OpenNi2Video::InitialiseOpenNI()
+{
+ // Initialise member variables
+ numDevices = 0;
+ numStreams = 0;
+ current_frame_index = 0;
+ total_frames = std::numeric_limits<size_t>::max();
+
+ openni::Status rc = openni::STATUS_OK;
+
+ rc = openni::OpenNI::initialize();
+ if (rc != openni::STATUS_OK) {
+ throw VideoException( "Unable to initialise OpenNI library", openni::OpenNI::getExtendedError() );
+ }
+}
+
+int OpenNi2Video::AddDevice(const std::string& device_uri)
+{
+ const size_t dev_id = numDevices;
+ openni::Status rc = devices[dev_id].open(device_uri.c_str());
+ if (rc != openni::STATUS_OK) {
+ throw VideoException( "OpenNI2: Couldn't open device.", openni::OpenNI::getExtendedError() );
+ }
+ ++numDevices;
+ return dev_id;
+}
+
+void OpenNi2Video::AddStream(const OpenNiStreamMode& mode)
+{
+ sensor_type[numStreams] = mode;
+ openni::Device& device = devices[mode.device];
+ openni::VideoStream& stream = video_stream[numStreams];
+ openni::Status rc = stream.create(device, SensorType(mode.sensor_type));
+ if (rc != openni::STATUS_OK) {
+ throw VideoException( "OpenNI2: Couldn't create stream.", openni::OpenNI::getExtendedError() );
+ }
+
+ openni::PlaybackControl* control = device.getPlaybackControl();
+ if(control && numStreams==0) {
+ total_frames = std::min(total_frames, (size_t)control->getNumberOfFrames(stream));
+ }
+
+ numStreams++;
+}
+
+void OpenNi2Video::SetupStreamModes()
+{
+ streams_properties = &frame_properties["streams"];
+ *streams_properties = picojson::value(picojson::array_type,false);
+ streams_properties->get<picojson::array>().resize(numStreams);
+
+ use_depth = false;
+ use_ir = false;
+ use_rgb = false;
+ depth_to_color = false;
+ use_ir_and_rgb = false;
+
+ sizeBytes =0;
+ for(size_t i=0; i<numStreams; ++i) {
+ const OpenNiStreamMode& mode = sensor_type[i];
+ openni::SensorType nisensortype;
+ openni::PixelFormat nipixelfmt;
+
+ switch( mode.sensor_type ) {
+ case OpenNiDepth_1mm_Registered:
+ depth_to_color = true;
+ nisensortype = openni::SENSOR_DEPTH;
+ nipixelfmt = openni::PIXEL_FORMAT_DEPTH_1_MM;
+ use_depth = true;
+ break;
+ case OpenNiDepth_1mm:
+ nisensortype = openni::SENSOR_DEPTH;
+ nipixelfmt = openni::PIXEL_FORMAT_DEPTH_1_MM;
+ use_depth = true;
+ break;
+ case OpenNiDepth_100um:
+ nisensortype = openni::SENSOR_DEPTH;
+ nipixelfmt = openni::PIXEL_FORMAT_DEPTH_100_UM;
+ use_depth = true;
+ break;
+ case OpenNiIrProj:
+ case OpenNiIr:
+ nisensortype = openni::SENSOR_IR;
+ nipixelfmt = openni::PIXEL_FORMAT_GRAY16;
+ use_ir = true;
+ break;
+ case OpenNiIr24bit:
+ nisensortype = openni::SENSOR_IR;
+ nipixelfmt = openni::PIXEL_FORMAT_RGB888;
+ use_ir = true;
+ break;
+ case OpenNiIr8bitProj:
+ case OpenNiIr8bit:
+ nisensortype = openni::SENSOR_IR;
+ nipixelfmt = openni::PIXEL_FORMAT_GRAY8;
+ use_ir = true;
+ break;
+ case OpenNiRgb:
+ nisensortype = openni::SENSOR_COLOR;
+ nipixelfmt = openni::PIXEL_FORMAT_RGB888;
+ use_rgb = true;
+ break;
+ case OpenNiGrey:
+ nisensortype = openni::SENSOR_COLOR;
+ nipixelfmt = openni::PIXEL_FORMAT_GRAY8;
+ use_rgb = true;
+ break;
+ case OpenNiUnassigned:
+ default:
+ continue;
+ }
+
+ openni::VideoMode onivmode;
+ try {
+ onivmode = FindOpenNI2Mode(devices[mode.device], nisensortype, mode.dim.x, mode.dim.y, mode.fps, nipixelfmt);
+ }catch(VideoException e) {
+ pango_print_error("Unable to find compatible OpenNI Video Mode. Please choose from:\n");
+ PrintOpenNI2Modes(nisensortype);
+ fflush(stdout);
+ throw e;
+ }
+
+ openni::Status rc;
+ if(!devices[mode.device].isFile()){//trying to setVideoMode on a file results in an OpenNI error
+ rc = video_stream[i].setVideoMode(onivmode);
+ if(rc != openni::STATUS_OK)
+ throw VideoException("Couldn't set OpenNI VideoMode", openni::OpenNI::getExtendedError());
+ }
+
+ int outputWidth = onivmode.getResolutionX();
+ int outputHeight = onivmode.getResolutionY();
+
+ if (mode.roi.w && mode.roi.h) {
+ rc = video_stream[i].setCropping(mode.roi.x,mode.roi.y,mode.roi.w,mode.roi.h);
+ if(rc != openni::STATUS_OK)
+ throw VideoException("Couldn't set OpenNI cropping", openni::OpenNI::getExtendedError());
+
+ outputWidth = mode.roi.w;
+ outputHeight = mode.roi.h;
+ }
+
+ const PixelFormat fmt = VideoFormatFromOpenNI2(nipixelfmt);
+ const StreamInfo stream(
+ fmt, outputWidth, outputHeight,
+ (outputWidth * fmt.bpp) / 8,
+ (unsigned char*)0 + sizeBytes
+ );
+
+ sizeBytes += stream.SizeBytes();
+ streams.push_back(stream);
+ }
+
+ SetRegisterDepthToImage(depth_to_color);
+
+ use_ir_and_rgb = use_rgb && use_ir;
+}
+
+void OpenNi2Video::UpdateProperties()
+{
+ picojson::value& jsopenni = device_properties["openni"];
+
+ picojson::value& jsdevices = jsopenni["devices"];
+ jsdevices = picojson::value(picojson::array_type,false);
+ jsdevices.get<picojson::array>().resize(numDevices);
+ for (size_t i=0; i<numDevices; ++i) {
+ picojson::value& jsdevice = jsdevices[i];
+#define SET_PARAM(param_type, param) \
+ { \
+ param_type val; \
+ if(devices[i].getProperty(param, &val) == openni::STATUS_OK) { \
+ jsdevice[#param] = val; \
+ } \
+ }
+ SET_PARAM( unsigned long long, XN_MODULE_PROPERTY_USB_INTERFACE );
+ SET_PARAM( bool, XN_MODULE_PROPERTY_MIRROR );
+ char serialNumber[1024];
+ devices[i].getProperty(ONI_DEVICE_PROPERTY_SERIAL_NUMBER, &serialNumber);
+ jsdevice["ONI_DEVICE_PROPERTY_SERIAL_NUMBER"] = std::string(serialNumber);
+#undef SET_PARAM
+ }
+
+ picojson::value& stream = jsopenni["streams"];
+ stream = picojson::value(picojson::array_type,false);
+ stream.get<picojson::array>().resize(Streams().size());
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ if(sensor_type[i].sensor_type != OpenNiUnassigned)
+ {
+#define SET_PARAM(param_type, param) \
+ {\
+ param_type val; \
+ if(video_stream[i].getProperty(param, &val) == openni::STATUS_OK) { \
+ jsstream[#param] = val; \
+ } \
+ }
+
+ picojson::value& jsstream = stream[i];
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_INPUT_FORMAT );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_CROPPING_MODE );
+
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_CLOSE_RANGE );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_WHITE_BALANCE_ENABLED );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_GAIN );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_HOLE_FILTER );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_REGISTRATION_TYPE );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_CONST_SHIFT );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_PIXEL_SIZE_FACTOR );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_MAX_SHIFT );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_PARAM_COEFF );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_SHIFT_SCALE );
+ SET_PARAM( unsigned long long, XN_STREAM_PROPERTY_ZERO_PLANE_DISTANCE );
+ SET_PARAM( double, XN_STREAM_PROPERTY_ZERO_PLANE_PIXEL_SIZE );
+ SET_PARAM( double, XN_STREAM_PROPERTY_EMITTER_DCMOS_DISTANCE );
+ SET_PARAM( double, XN_STREAM_PROPERTY_DCMOS_RCMOS_DISTANCE );
+#undef SET_PARAM
+ }
+ }
+}
+
+void OpenNi2Video::SetMirroring(bool enable)
+{
+ // Set this property on all streams. It doesn't matter if it fails.
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ video_stream[i].setMirroringEnabled(enable);
+ }
+}
+
+void OpenNi2Video::SetAutoExposure(bool enable)
+{
+ // Set this property on all streams exposing CameraSettings
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ openni::CameraSettings* cam = video_stream[i].getCameraSettings();
+ if(cam) cam->setAutoExposureEnabled(enable);
+ }
+}
+
+void OpenNi2Video::SetAutoWhiteBalance(bool enable)
+{
+ // Set this property on all streams exposing CameraSettings
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ openni::CameraSettings* cam = video_stream[i].getCameraSettings();
+ if(cam) cam->setAutoWhiteBalanceEnabled(enable);
+ }
+}
+
+void OpenNi2Video::SetDepthCloseRange(bool enable)
+{
+ // Set this property on all streams. It doesn't matter if it fails.
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ video_stream[i].setProperty(XN_STREAM_PROPERTY_CLOSE_RANGE, enable);
+ }
+}
+
+void OpenNi2Video::SetDepthHoleFilter(bool enable)
+{
+ // Set this property on all streams. It doesn't matter if it fails.
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ video_stream[i].setProperty(XN_STREAM_PROPERTY_HOLE_FILTER, enable);
+ video_stream[i].setProperty(XN_STREAM_PROPERTY_GAIN,50);
+ }
+}
+
+void OpenNi2Video::SetDepthColorSyncEnabled(bool enable)
+{
+ for(size_t i = 0 ; i < numDevices; i++) {
+ devices[i].setDepthColorSyncEnabled(enable);
+ }
+}
+
+void OpenNi2Video::SetFastCrop(bool enable)
+{
+ const uint32_t pango_XN_STREAM_PROPERTY_FAST_ZOOM_CROP = 0x1080F009;
+ for (unsigned int i = 0; i < Streams().size(); ++i) {
+ video_stream[i].setProperty(pango_XN_STREAM_PROPERTY_FAST_ZOOM_CROP, enable);
+ video_stream[i].setProperty(XN_STREAM_PROPERTY_CROPPING_MODE, enable ? XN_CROPPING_MODE_INCREASED_FPS : XN_CROPPING_MODE_NORMAL);
+ }
+}
+
+void OpenNi2Video::SetRegisterDepthToImage(bool enable)
+{
+ if(enable) {
+ for(size_t i = 0 ; i < numDevices; i++) {
+ devices[i].setImageRegistrationMode(openni::IMAGE_REGISTRATION_DEPTH_TO_COLOR);
+ }
+ }else{
+ for(size_t i = 0 ; i < numDevices ; i++) {
+ devices[i].setImageRegistrationMode(openni::IMAGE_REGISTRATION_OFF);
+ }
+ }
+}
+
+void OpenNi2Video::SetPlaybackSpeed(float speed)
+{
+ for(size_t i = 0 ; i < numDevices; i++) {
+ openni::PlaybackControl* control = devices[i].getPlaybackControl();
+ if(control) control->setSpeed(speed);
+ }
+}
+
+void OpenNi2Video::SetPlaybackRepeat(bool enabled)
+{
+ for(size_t i = 0 ; i < numDevices; i++) {
+ openni::PlaybackControl* control = devices[i].getPlaybackControl();
+ if(control) control->setRepeatEnabled(enabled);
+ }
+}
+
+OpenNi2Video::~OpenNi2Video()
+{
+ Stop();
+
+ for(size_t i=0; i<numStreams; ++i) {
+ if( video_stream[i].isValid()) {
+ video_stream[i].destroy();
+ }
+ }
+
+ openni::OpenNI::shutdown();
+}
+
+size_t OpenNi2Video::SizeBytes() const
+{
+ return sizeBytes;
+}
+
+const std::vector<StreamInfo>& OpenNi2Video::Streams() const
+{
+ return streams;
+}
+
+void OpenNi2Video::Start()
+{
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ video_stream[i].start();
+ }
+}
+
+void OpenNi2Video::Stop()
+{
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ video_stream[i].stop();
+ }
+}
+
+openni::VideoStream * OpenNi2Video::GetVideoStream(int stream){
+ if(video_stream[stream].isValid()) {
+ return &video_stream[stream];
+ }else{
+ pango_print_error("Error getting stream: %d \n%s",stream, openni::OpenNI::getExtendedError() );
+ return NULL;
+ }
+}
+
+bool OpenNi2Video::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ unsigned char* out_img = image;
+
+ openni::Status rc = openni::STATUS_OK;
+
+ for(unsigned int i=0; i<Streams().size(); ++i) {
+ if(sensor_type[i].sensor_type == OpenNiUnassigned) {
+ rc = openni::STATUS_OK;
+ continue;
+ }
+
+ if(!video_stream[i].isValid()) {
+ rc = openni::STATUS_NO_DEVICE;
+ continue;
+ }
+
+ if(use_ir_and_rgb) video_stream[i].start();
+
+ rc = video_stream[i].readFrame(&video_frame[i]);
+ video_frame[0].getFrameIndex();
+ if(rc != openni::STATUS_OK) {
+ pango_print_error("Error reading frame:\n%s", openni::OpenNI::getExtendedError() );
+ }
+
+ const bool toGreyscale = false;
+ if(toGreyscale) {
+ const int w = streams[i].Width();
+ const int h = streams[i].Height();
+
+ openni::RGB888Pixel* pColour = (openni::RGB888Pixel*)video_frame[i].getData();
+ for(int i = 0 ; i < w*h;i++){
+ openni::RGB888Pixel rgb = pColour[i];
+ int grey = ((int)(rgb.r&0xFF) + (int)(rgb.g&0xFF) + (int)(rgb.b&0xFF))/3;
+ grey = std::min(255,std::max(0,grey));
+ out_img[i] = grey;
+ }
+ }else{
+ memcpy(out_img, video_frame[i].getData(), streams[i].SizeBytes());
+ }
+
+ // update frame properties
+ (*streams_properties)[i]["devtime_us"] = video_frame[i].getTimestamp();
+
+ if(use_ir_and_rgb) video_stream[i].stop();
+
+ out_img += streams[i].SizeBytes();
+ }
+
+ current_frame_index = video_frame[0].getFrameIndex();
+
+ return rc == openni::STATUS_OK;
+}
+
+bool OpenNi2Video::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+size_t OpenNi2Video::GetCurrentFrameId() const
+{
+ return current_frame_index;
+}
+
+size_t OpenNi2Video::GetTotalFrames() const
+{
+ return total_frames;
+}
+
+size_t OpenNi2Video::Seek(size_t frameid)
+{
+ openni::PlaybackControl* control = devices[0].getPlaybackControl();
+ if(control) {
+ control->seek(video_stream[0], frameid);
+ return frameid;
+ }else{
+ return -1;
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(OpenNi2Video)
+{
+ struct OpenNI2VideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const bool realtime = uri.Contains("realtime");
+ const ImageDim default_dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const ImageRoi default_roi = uri.Get<ImageRoi>("roi", ImageRoi(0,0,0,0));
+ const unsigned int default_fps = uri.Get<unsigned int>("fps", 30);
+
+ std::vector<OpenNiStreamMode> stream_modes;
+
+ int num_streams = 0;
+ std::string simg= "img1";
+ while(uri.Contains(simg)) {
+ OpenNiStreamMode stream = uri.Get<OpenNiStreamMode>(simg, OpenNiStreamMode(OpenNiRgb,default_dim,default_roi,default_fps,0));
+ stream_modes.push_back(stream);
+ ++num_streams;
+ simg = "img" + ToString(num_streams+1);
+ }
+
+ OpenNi2Video* nivid;
+ if(!uri.url.empty()) {
+ nivid = new OpenNi2Video(pangolin::PathExpand(uri.url));
+ }else if(stream_modes.size()) {
+ nivid = new OpenNi2Video(stream_modes);
+ }else{
+ nivid = new OpenNi2Video(default_dim, default_roi, default_fps);
+ }
+
+ nivid->SetDepthCloseRange( uri.Get<bool>("closerange",false) );
+ nivid->SetDepthHoleFilter( uri.Get<bool>("holefilter",false) );
+ nivid->SetDepthColorSyncEnabled( uri.Get<bool>("coloursync",false) );
+ nivid->SetFastCrop( uri.Get<bool>("fastcrop",false) );
+ nivid->SetPlaybackSpeed(realtime ? 1.0f : -1.0f);
+ nivid->SetAutoExposure(true);
+ nivid->SetAutoWhiteBalance(true);
+ nivid->SetMirroring(false);
+
+ nivid->UpdateProperties();
+
+ nivid->Start();
+
+ return std::unique_ptr<VideoInterface>(nivid);
+ }
+ };
+
+ auto factory = std::make_shared<OpenNI2VideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "openni");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "openni2");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "oni");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/pack.cpp b/externals/Pangolin/src/video/drivers/pack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..792aed1f3c49f4e99ceab9d0614b58b6f4638bfa
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/pack.cpp
@@ -0,0 +1,260 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/pack.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+#ifdef DEBUGUNPACK
+ #include <pangolin/utils/timer.h>
+ #define TSTART() pangolin::basetime start,last,now; start = pangolin::TimeNow(); last = start;
+ #define TGRABANDPRINT(...) now = pangolin::TimeNow(); fprintf(stderr,"UNPACK: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, " %fms.\n",1000*pangolin::TimeDiff_s(last, now)); last = now;
+ #define DBGPRINT(...) fprintf(stderr,"UNPACK: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr,"\n");
+#else
+ #define TSTART()
+ #define TGRABANDPRINT(...)
+ #define DBGPRINT(...)
+#endif
+
+namespace pangolin
+{
+
+PackVideo::PackVideo(std::unique_ptr<VideoInterface> &src_, PixelFormat out_fmt)
+ : src(std::move(src_)), size_bytes(0), buffer(0)
+{
+ if( !src || out_fmt.channels != 1) {
+ throw VideoException("PackVideo: Only supports single channel input.");
+ }
+
+ videoin.push_back(src.get());
+
+ for(size_t s=0; s< src->Streams().size(); ++s) {
+ const size_t w = src->Streams()[s].Width();
+ const size_t h = src->Streams()[s].Height();
+
+ // Check compatibility of formats
+ const PixelFormat in_fmt = src->Streams()[s].PixFormat();
+ if(in_fmt.channels > 1 || in_fmt.bpp > 16) {
+ throw VideoException("PackVideo: Only supports one channel input.");
+ }
+
+ // round up to ensure enough bytes for packing
+ const size_t pitch = (w*out_fmt.bpp)/ 8 + ((w*out_fmt.bpp) % 8 > 0? 1 : 0);
+ streams.push_back(pangolin::StreamInfo( out_fmt, w, h, pitch, (unsigned char*)0 + size_bytes ));
+ size_bytes += h*pitch;
+ }
+
+ buffer = new unsigned char[src->SizeBytes()];
+}
+
+PackVideo::~PackVideo()
+{
+ delete[] buffer;
+}
+
+//! Implement VideoInput::Start()
+void PackVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+//! Implement VideoInput::Stop()
+void PackVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t PackVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& PackVideo::Streams() const
+{
+ return streams;
+}
+
+template<typename T>
+void ConvertTo8bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ T* pout = (T*)(out.ptr + r*out.pitch);
+ uint8_t* pin = in.ptr + r*in.pitch;
+ const uint8_t* pin_end = in.ptr + (r+1)*in.pitch;
+ while(pin != pin_end) {
+ *(pout++) = *(pin++);
+ }
+ }
+}
+
+template<typename T>
+void ConvertTo10bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ uint8_t* pout = out.ptr + r*out.pitch;
+ T* pin = (T*)(in.ptr + r*in.pitch);
+ const T* pin_end = (T*)(in.ptr + (r+1)*in.pitch);
+ while(pin != pin_end) {
+ uint64_t val = (*(pin++) & 0x00000003FF);
+ val |= uint64_t(*(pin++) & 0x00000003FF) << 10;
+ val |= uint64_t(*(pin++) & 0x00000003FF) << 20;
+ val |= uint64_t(*(pin++) & 0x00000003FF) << 30;
+ *(pout++) = uint8_t( val & 0x00000000FF);
+ *(pout++) = uint8_t((val & 0x000000FF00) >> 8);
+ *(pout++) = uint8_t((val & 0x0000FF0000) >> 16);
+ *(pout++) = uint8_t((val & 0x00FF000000) >> 24);
+ *(pout++) = uint8_t((val & 0xFF00000000) >> 32);
+ }
+ }
+}
+
+template<typename T>
+void ConvertTo12bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ uint8_t* pout = out.ptr + r*out.pitch;
+ T* pin = (T*)(in.ptr + r*in.pitch);
+ const T* pin_end = (T*)(in.ptr + (r+1)*in.pitch);
+ while(pin != pin_end) {
+ uint32_t val = (*(pin++) & 0x00000FFF);
+ val |= uint32_t(*(pin++) & 0x00000FFF) << 12;
+ *(pout++) = uint8_t( val & 0x000000FF);
+ *(pout++) = uint8_t((val & 0x0000FF00) >> 8);
+ *(pout++) = uint8_t((val & 0x00FF0000) >> 16);
+ }
+ }
+}
+
+void PackVideo::Process(unsigned char* image, const unsigned char* buffer)
+{
+ TSTART()
+ for(size_t s=0; s<streams.size(); ++s) {
+ const Image<unsigned char> img_in = videoin[0]->Streams()[s].StreamImage(buffer);
+ Image<unsigned char> img_out = Streams()[s].StreamImage(image);
+
+ const int bits_out = Streams()[s].PixFormat().bpp;
+
+ if(videoin[0]->Streams()[s].PixFormat().format == "GRAY16LE") {
+ if(bits_out == 8) {
+ ConvertTo8bit<uint16_t>(img_out, img_in);
+ }else if( bits_out == 10) {
+ ConvertTo10bit<uint16_t>(img_out, img_in);
+ }else if( bits_out == 12){
+ ConvertTo12bit<uint16_t>(img_out, img_in);
+ }else{
+ throw pangolin::VideoException("Unsupported bitdepths.");
+ }
+ }else{
+ throw pangolin::VideoException("Unsupported input pix format.");
+ }
+ }
+ TGRABANDPRINT("Packing took ")
+}
+
+//! Implement VideoInput::GrabNext()
+bool PackVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNext(buffer,wait)) {
+ Process(image,buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool PackVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNewest(buffer,wait)) {
+ Process(image,buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+std::vector<VideoInterface*>& PackVideo::InputStreams()
+{
+ return videoin;
+}
+
+unsigned int PackVideo::AvailableFrames() const
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Pack: child interface is not buffer aware.");
+ return 0;
+ }
+ else
+ {
+ return vpi->AvailableFrames();
+ }
+}
+
+bool PackVideo::DropNFrames(uint32_t n)
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Pack: child interface is not buffer aware.");
+ return false;
+ }
+ else
+ {
+ return vpi->DropNFrames(n);
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(PackVideo)
+{
+ struct PackVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ const std::string fmt = uri.Get("fmt", std::string("GRAY16LE") );
+ return std::unique_ptr<VideoInterface>(
+ new PackVideo(subvid, PixelFormatFromString(fmt) )
+ );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<PackVideoFactory>(), 10, "pack");
+}
+
+}
+
+#undef TSTART
+#undef TGRABANDPRINT
+#undef DBGPRINT
diff --git a/externals/Pangolin/src/video/drivers/pango.cpp b/externals/Pangolin/src/video/drivers/pango.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..744bcb29b5919b3ce0041c0c9e03dd04174ebef1
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/pango.cpp
@@ -0,0 +1,243 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/log/playback_session.h>
+#include <pangolin/utils/file_extension.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/signal_slot.h>
+#include <pangolin/video/drivers/pango.h>
+#include <pangolin/video/iostream_operators.h>
+
+#include <functional>
+
+namespace pangolin
+{
+
+const std::string pango_video_type = "raw_video";
+
+PangoVideo::PangoVideo(const std::string& filename, std::shared_ptr<PlaybackSession> playback_session)
+ : _filename(filename),
+ _playback_session(playback_session),
+ _reader(_playback_session->Open(filename)),
+ _event_promise(_playback_session->Time()),
+ _src_id(FindPacketStreamSource()),
+ _source(nullptr)
+{
+ PANGO_ENSURE(_src_id != -1, "No appropriate video streams found in log.");
+
+ _source = &_reader->Sources()[_src_id];
+ SetupStreams(*_source);
+
+ // Make sure we time-seek with other playback devices
+ session_seek = _playback_session->Time().OnSeek.Connect(
+ [&](SyncTime::TimePoint t){
+ _event_promise.Cancel();
+ _reader->Seek(_src_id, t);
+ _event_promise.WaitAndRenew(_source->NextPacketTime());
+ }
+ );
+
+ _event_promise.WaitAndRenew(_source->NextPacketTime());
+}
+
+PangoVideo::~PangoVideo()
+{
+}
+
+size_t PangoVideo::SizeBytes() const
+{
+ return _size_bytes;
+}
+
+const std::vector<StreamInfo>& PangoVideo::Streams() const
+{
+ return _streams;
+}
+
+void PangoVideo::Start()
+{
+
+}
+
+void PangoVideo::Stop()
+{
+
+}
+
+bool PangoVideo::GrabNext(unsigned char* image, bool /*wait*/)
+{
+ try
+ {
+ Packet fi = _reader->NextFrame(_src_id);
+ _frame_properties = fi.meta;
+
+ if(_fixed_size) {
+ fi.Stream().read(reinterpret_cast<char*>(image), _size_bytes);
+ }else{
+ for(size_t s=0; s < _streams.size(); ++s) {
+ StreamInfo& si = _streams[s];
+ pangolin::Image<unsigned char> dst = si.StreamImage(image);
+
+ if(stream_decoder[s]) {
+ pangolin::TypedImage img = stream_decoder[s](fi.Stream());
+ PANGO_ENSURE(img.IsValid());
+
+ // TODO: We can avoid this copy by decoding directly into img
+ for(size_t row =0; row < dst.h; ++row) {
+ std::memcpy(dst.RowPtr(row), img.RowPtr(row), si.RowBytes());
+ }
+ }else{
+ for(size_t row =0; row < dst.h; ++row) {
+ fi.Stream().read((char*)dst.RowPtr(row), si.RowBytes());
+ }
+ }
+ }
+ }
+
+ _event_promise.WaitAndRenew(_source->NextPacketTime());
+ return true;
+ }
+ catch(...)
+ {
+ _frame_properties = picojson::value();
+ return false;
+ }
+}
+
+bool PangoVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image, wait);
+}
+
+size_t PangoVideo::GetCurrentFrameId() const
+{
+ return (int)(_reader->Sources()[_src_id].next_packet_id) - 1;
+}
+
+size_t PangoVideo::GetTotalFrames() const
+{
+ return _source->index.size();
+}
+
+size_t PangoVideo::Seek(size_t next_frame_id)
+{
+ // Get time for seek
+ if(next_frame_id < _source->index.size()) {
+ const int64_t capture_time = _source->index[next_frame_id].capture_time;
+ _playback_session->Time().Seek(SyncTime::TimePoint(std::chrono::microseconds(capture_time)));
+ return next_frame_id;
+ }else{
+ return _source->next_packet_id;
+ }
+}
+
+std::string PangoVideo::GetSourceUri()
+{
+ return _source_uri;
+}
+
+int PangoVideo::FindPacketStreamSource()
+{
+ for(const auto& src : _reader->Sources())
+ {
+ if (!src.driver.compare(pango_video_type))
+ {
+ return static_cast<int>(src.id);
+ }
+ }
+
+ return -1;
+}
+
+void PangoVideo::SetupStreams(const PacketStreamSource& src)
+{
+ // Read sources header
+ _fixed_size = src.data_size_bytes != 0;
+ _size_bytes = src.data_size_bytes;
+ _source_uri = src.uri;
+
+ _device_properties = src.info["device"];
+ const picojson::value& json_streams = src.info["streams"];
+ const size_t num_streams = json_streams.size();
+
+ for (size_t i = 0; i < num_streams; ++i)
+ {
+ const picojson::value& json_stream = json_streams[i];
+
+ std::string encoding = json_stream["encoding"].get<std::string>();
+
+ // Check if the stream is compressed
+ if(json_stream.contains("decoded")) {
+ const std::string compressed_encoding = encoding;
+ encoding = json_stream["decoded"].get<std::string>();
+ const PixelFormat decoded_fmt = PixelFormatFromString(encoding);
+ stream_decoder.push_back(StreamEncoderFactory::I().GetDecoder(compressed_encoding, decoded_fmt));
+ }else{
+ stream_decoder.push_back(nullptr);
+ }
+
+ PixelFormat fmt = PixelFormatFromString(encoding);
+
+ fmt.channel_bit_depth = json_stream.get_value<int64_t>("channel_bit_depth", 0);
+
+ StreamInfo si(
+ fmt,
+ json_stream["width"].get<int64_t>(),
+ json_stream["height"].get<int64_t>(),
+ json_stream["pitch"].get<int64_t>(),
+ (unsigned char*) 0 + json_stream["offset"].get<int64_t>()
+ );
+
+ if(!_fixed_size) {
+ _size_bytes += si.SizeBytes();
+ }
+
+
+ _streams.push_back(si);
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(PangoVideo)
+{
+ struct PangoVideoFactory : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const std::string path = PathExpand(uri.url);
+
+ if( !uri.scheme.compare("pango") || FileType(uri.url) == ImageFileTypePango ) {
+ return std::unique_ptr<VideoInterface>(new PangoVideo(path.c_str(), PlaybackSession::ChooseFromParams(uri)));
+ }
+ return std::unique_ptr<VideoInterface>();
+ }
+ };
+
+ auto factory = std::make_shared<PangoVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "pango");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 5, "file");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/pango_video_output.cpp b/externals/Pangolin/src/video/drivers/pango_video_output.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7984ea394e4e2a151959d7fd7f12e101ff9b1705
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/pango_video_output.cpp
@@ -0,0 +1,303 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/memstreambuf.h>
+#include <pangolin/utils/picojson.h>
+#include <pangolin/utils/sigstate.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/video/drivers/pango_video_output.h>
+#include <pangolin/video/iostream_operators.h>
+#include <pangolin/video/video_interface.h>
+#include <set>
+#include <future>
+
+#ifndef _WIN_
+# include <unistd.h>
+#endif
+
+namespace pangolin
+{
+
+const std::string pango_video_type = "raw_video";
+
+void SigPipeHandler(int sig)
+{
+ SigState::I().sig_callbacks.at(sig).value = true;
+}
+
+PangoVideoOutput::PangoVideoOutput(const std::string& filename, size_t buffer_size_bytes, const std::map<size_t, std::string> &stream_encoder_uris)
+ : filename(filename),
+ packetstream_buffer_size_bytes(buffer_size_bytes),
+ packetstreamsrcid(-1),
+ total_frame_size(0),
+ is_pipe(pangolin::IsPipe(filename)),
+ fixed_size(true),
+ stream_encoder_uris(stream_encoder_uris)
+{
+ if(!is_pipe)
+ {
+ packetstream.Open(filename, packetstream_buffer_size_bytes);
+ }
+ else
+ {
+ RegisterNewSigCallback(&SigPipeHandler, (void*)this, SIGPIPE);
+ }
+
+ // Instantiate encoders
+}
+
+PangoVideoOutput::~PangoVideoOutput()
+{
+}
+
+const std::vector<StreamInfo>& PangoVideoOutput::Streams() const
+{
+ return streams;
+}
+
+bool PangoVideoOutput::IsPipe() const
+{
+ return is_pipe;
+}
+
+void PangoVideoOutput::SetStreams(const std::vector<StreamInfo>& st, const std::string& uri, const picojson::value& properties)
+{
+ std::set<unsigned char*> unique_ptrs;
+ for (size_t i = 0; i < st.size(); ++i)
+ {
+ unique_ptrs.insert(st[i].Offset());
+ }
+
+ if (unique_ptrs.size() < st.size())
+ throw std::invalid_argument("Each image must have unique offset into buffer.");
+
+ if (packetstreamsrcid == -1)
+ {
+ input_uri = uri;
+ streams = st;
+ device_properties = properties;
+
+ picojson::value json_header(picojson::object_type, false);
+ picojson::value& json_streams = json_header["streams"];
+ json_header["device"] = device_properties;
+
+ stream_encoders.resize(streams.size());
+
+ fixed_size = true;
+
+ total_frame_size = 0;
+ for (unsigned int i = 0; i < streams.size(); ++i)
+ {
+ StreamInfo& si = streams[i];
+ total_frame_size = std::max(total_frame_size, (size_t) si.Offset() + si.SizeBytes());
+
+ picojson::value& json_stream = json_streams.push_back();
+
+ std::string encoder_name = si.PixFormat().format;
+ if(stream_encoder_uris.find(i) != stream_encoder_uris.end() && !stream_encoder_uris[i].empty() ) {
+ // instantiate encoder and write it's name to the stream properties
+ json_stream["decoded"] = si.PixFormat().format;
+ encoder_name = stream_encoder_uris[i];
+ stream_encoders[i] = StreamEncoderFactory::I().GetEncoder(encoder_name, si.PixFormat());
+ fixed_size = false;
+ }
+
+ json_stream["channel_bit_depth"] = si.PixFormat().channel_bit_depth;
+ json_stream["encoding"] = encoder_name;
+ json_stream["width"] = si.Width();
+ json_stream["height"] = si.Height();
+ json_stream["pitch"] = si.Pitch();
+ json_stream["offset"] = (size_t) si.Offset();
+ }
+
+ PacketStreamSource pss;
+ pss.driver = pango_video_type;
+ pss.uri = input_uri;
+ pss.info = json_header;
+ pss.data_size_bytes = fixed_size ? total_frame_size : 0;
+ pss.data_definitions = "struct Frame{ uint8 stream_data[" + pangolin::Convert<std::string, size_t>::Do(total_frame_size) + "];};";
+
+ packetstreamsrcid = (int)packetstream.AddSource(pss);
+ } else {
+ throw std::runtime_error("Unable to add new streams");
+ }
+}
+
+int PangoVideoOutput::WriteStreams(const unsigned char* data, const picojson::value& frame_properties)
+{
+ const int64_t host_reception_time_us = frame_properties.get_value(PANGO_HOST_RECEPTION_TIME_US, Time_us(TimeNow()));
+
+#ifndef _WIN_
+ if (is_pipe)
+ {
+ // If there is a reader waiting on the other side of the pipe, open
+ // a file descriptor to the file and close it only after the file
+ // has been opened by the PacketStreamWriter. This avoids the reader
+ // from seeing EOF on its next read because all file descriptors on
+ // the write side have been closed.
+ //
+ // When the stream is already open but the reader has disappeared,
+ // opening a file descriptor will fail and errno will be ENXIO.
+ int fd = WritablePipeFileDescriptor(filename);
+
+ if (!packetstream.IsOpen())
+ {
+ if (fd != -1)
+ {
+ packetstream.Open(filename, packetstream_buffer_size_bytes);
+ close(fd);
+ }
+ }
+ else
+ {
+ if (fd != -1)
+ {
+ // There's a reader on the other side of the pipe.
+ close(fd);
+ }
+ else
+ {
+ if (errno == ENXIO)
+ {
+ packetstream.ForceClose();
+ SigState::I().sig_callbacks.at(SIGPIPE).value = false;
+
+ // This should be unnecessary since per the man page,
+ // data should be dropped from the buffer upon closing the
+ // writable file descriptors.
+ pangolin::FlushPipe(filename);
+ }
+ }
+ }
+
+ if (!packetstream.IsOpen())
+ return 0;
+ }
+#endif
+
+ if(!fixed_size) {
+ // TODO: Make this more efficient (without so many allocs and memcpy's)
+
+ std::vector<memstreambuf> encoded_stream_data;
+
+ // Create buffers for compressed data: the first will be reused for all the data later
+ encoded_stream_data.emplace_back(total_frame_size);
+ for(size_t i=1; i < streams.size(); ++i) {
+ encoded_stream_data.emplace_back(streams[i].SizeBytes());
+ }
+
+ // lambda encodes frame data i to encoded_stream_data[i]
+ auto encode_stream = [&](int i){
+ encoded_stream_data[i].clear();
+ std::ostream encode_stream(&encoded_stream_data[i]);
+
+ const StreamInfo& si = streams[i];
+ const Image<unsigned char> stream_image = si.StreamImage(data);
+
+ if(stream_encoders[i]) {
+ // Encode to buffer
+ stream_encoders[i](encode_stream, stream_image);
+ }else{
+ if(stream_image.IsContiguous()) {
+ encode_stream.write((char*)stream_image.ptr, streams[i].SizeBytes());
+ }else{
+ for(size_t row=0; row < stream_image.h; ++row) {
+ encode_stream.write((char*)stream_image.RowPtr(row), si.RowBytes());
+ }
+ }
+ }
+ return true;
+ };
+
+ // Compress each stream (>0 in another thread)
+ std::vector<std::future<bool>> encode_finished;
+ for(size_t i=1; i < streams.size(); ++i) {
+ encode_finished.emplace_back(std::async(std::launch::async, [&,i](){
+ return encode_stream(i);
+ }));
+ }
+ // Encode stream 0 in this thread
+ encode_stream(0);
+
+ // Reuse our first compression stream for the rest of the data too.
+ std::vector<uint8_t>& encoded = encoded_stream_data[0].buffer;
+
+ // Wait on all threads to finish and copy into data packet
+ for(size_t i=1; i < streams.size(); ++i) {
+ encode_finished[i-1].get();
+ encoded.insert(encoded.end(), encoded_stream_data[i].buffer.begin(), encoded_stream_data[i].buffer.end());
+ }
+
+ packetstream.WriteSourcePacket(packetstreamsrcid, reinterpret_cast<const char*>(encoded.data()), host_reception_time_us, encoded.size(), frame_properties);
+ }else{
+ packetstream.WriteSourcePacket(packetstreamsrcid, reinterpret_cast<const char*>(data), host_reception_time_us, total_frame_size, frame_properties);
+ }
+
+ return 0;
+}
+
+PANGOLIN_REGISTER_FACTORY(PangoVideoOutput)
+{
+ struct PangoVideoFactory final : public FactoryInterface<VideoOutputInterface> {
+ std::unique_ptr<VideoOutputInterface> Open(const Uri& uri) override {
+ const size_t mb = 1024*1024;
+ const size_t buffer_size_bytes = uri.Get("buffer_size_mb", 100) * mb;
+ std::string filename = uri.url;
+
+ if(uri.Contains("unique_filename")) {
+ filename = MakeUniqueFilename(filename);
+ }
+
+ // Default encoder
+ std::string default_encoder = "";
+
+ if(uri.Contains("encoder")) {
+ default_encoder = uri.Get<std::string>("encoder","");
+ }
+
+ // Encoders for each stream
+ std::map<size_t, std::string> stream_encoder_uris;
+ for(size_t i=0; i<100; ++i)
+ {
+ const std::string encoder_key = pangolin::FormatString("encoder%",i+1);
+ stream_encoder_uris[i] = uri.Get<std::string>(encoder_key, default_encoder);
+ }
+
+ return std::unique_ptr<VideoOutputInterface>(
+ new PangoVideoOutput(filename, buffer_size_bytes, stream_encoder_uris)
+ );
+ }
+ };
+
+ auto factory = std::make_shared<PangoVideoFactory>();
+ FactoryRegistry<VideoOutputInterface>::I().RegisterFactory(factory, 10, "pango");
+ FactoryRegistry<VideoOutputInterface>::I().RegisterFactory(factory, 10, "file");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/pleora.cpp b/externals/Pangolin/src/video/drivers/pleora.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ab9e8a9de1d7d579e3b28d6ec20467a0c72ee887
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/pleora.cpp
@@ -0,0 +1,744 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/pleora.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+#include <thread>
+
+#ifdef DEBUGPLEORA
+ #include <pangolin/utils/timer.h>
+ #define TSTART() pangolin::basetime start,last,now; start = pangolin::TimeNow(); last = start;
+ #define TGRABANDPRINT(...) now = pangolin::TimeNow(); fprintf(stderr," PLEORA: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, " %fms.\n",1000*pangolin::TimeDiff_s(last, now)); last = now;
+ #define DBGPRINT(...) fprintf(stderr," PLEORA: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr,"\n");
+#else
+ #define TSTART()
+ #define TGRABANDPRINT(...)
+ #define DBGPRINT(...)
+#endif
+
+namespace pangolin
+{
+
+inline void ThrowOnFailure(const PvResult& res)
+{
+ if(res.IsFailure()) {
+ throw std::runtime_error("Failure: " + std::string(res.GetCodeString().GetAscii()) );
+ }
+}
+
+template<typename T>
+struct PleoraParamTraits;
+
+template<> struct PleoraParamTraits<bool> {
+ typedef PvGenBoolean PvType;
+};
+template<> struct PleoraParamTraits<int64_t> {
+ typedef PvGenInteger PvType;
+};
+template<> struct PleoraParamTraits<float> {
+ typedef PvGenFloat PvType;
+};
+template<> struct PleoraParamTraits<std::string> {
+ typedef PvGenString PvType;
+};
+
+template<typename T>
+T GetParam(PvGenParameterArray* params, const char* name)
+{
+ typedef typename PleoraParamTraits<T>::PvType PvType;
+ PvType* param = dynamic_cast<PvType*>( params->Get(name) );
+ if(!param) {
+ throw std::runtime_error("Incorrect type");
+ }
+ T ret;
+ PvResult res = param->GetValue(ret);
+ if(res.IsFailure()) {
+ throw std::runtime_error("Cannot get value: " + std::string(res.GetCodeString().GetAscii()) );
+ }
+ return ret;
+}
+
+template<typename T>
+bool SetParam(PvGenParameterArray* params, const char* name, T val)
+{
+ typedef typename PleoraParamTraits<T>::PvType PvType;
+ PvType* param = dynamic_cast<PvType*>( params->Get(name) );
+ if(!param) {
+ throw std::runtime_error("Unable to get parameter handle: " + std::string(name) );
+ }
+
+ if(!param->IsWritable()) {
+ throw std::runtime_error("Cannot set value for " + std::string(name) );
+ }
+
+ PvResult res = param->SetValue(val);
+ if(res.IsFailure()) {
+ throw std::runtime_error("Cannot set value: " + std::string(res.GetCodeString().GetAscii()) );
+ }
+ return true;
+}
+
+inline const PvDeviceInfo* SelectDevice( PvSystem& aSystem, const char* model_name = 0, const char* serial_num = 0, size_t index = 0 )
+{
+ aSystem.Find();
+
+ // Enumerate all devices, select first that matches criteria
+ size_t matches = 0;
+ for ( uint32_t i = 0; i < aSystem.GetInterfaceCount(); i++ ) {
+ const PvInterface *lInterface = dynamic_cast<const PvInterface *>( aSystem.GetInterface( i ) );
+ if ( lInterface ) {
+ for ( uint32_t j = 0; j < lInterface->GetDeviceCount(); j++ ) {
+ const PvDeviceInfo *lDI = dynamic_cast<const PvDeviceInfo *>( lInterface->GetDeviceInfo( j ) );
+ if ( lDI && lDI->IsConfigurationValid() ) {
+ if( model_name && strcmp(lDI->GetModelName().GetAscii(), model_name) )
+ continue;
+ if( serial_num && strcmp(lDI->GetSerialNumber().GetAscii(), serial_num) )
+ continue;
+ if(matches == index) {
+ return lDI;
+ }
+ ++matches;
+ }
+ }
+ }
+ }
+
+ return 0;
+}
+
+PixelFormat PleoraFormat(const PvGenEnum* pfmt)
+{
+ std::string spfmt = pfmt->ToString().GetAscii();
+ if( !spfmt.compare("Mono8") ) {
+ return PixelFormatFromString("GRAY8");
+ } else if( !spfmt.compare("Mono10p") ) {
+ return PixelFormatFromString("GRAY10");
+ } else if( !spfmt.compare("Mono12p") ) {
+ return PixelFormatFromString("GRAY12");
+ } else if( !spfmt.compare("Mono10") || !spfmt.compare("Mono12")) {
+ return PixelFormatFromString("GRAY16LE");
+ } else if( !spfmt.compare("RGB8") ) {
+ return PixelFormatFromString("RGB24");
+ } else if( !spfmt.compare("BGR8") ) {
+ return PixelFormatFromString("BGR24");
+ } else if( !spfmt.compare("BayerBG8") ) {
+ return PixelFormatFromString("GRAY8");
+ } else if( !spfmt.compare("BayerBG12") ) {
+ return PixelFormatFromString("GRAY16LE");
+ } else {
+ throw VideoException("Unknown Pleora pixel format", spfmt);
+ }
+}
+
+PleoraVideo::PleoraVideo(const Params& p): size_bytes(0), lPvSystem(0), lDevice(0), lStream(0), lDeviceParams(0), lStart(0), lStop(0),
+ lTemperatureCelcius(0), getTemp(false), lStreamParams(0), validGrabbedBuffers(0)
+{
+ std::string sn;
+ std::string mn;
+ int index = 0;
+ size_t buffer_count = PleoraVideo::DEFAULT_BUFFER_COUNT;
+ Params device_params;
+
+ for(Params::ParamMap::const_iterator it = p.params.begin(); it != p.params.end(); it++) {
+ if(it->first == "model"){
+ mn = it->second;
+ } else if(it->first == "sn"){
+ sn = it->second;
+ } else if(it->first == "idx"){
+ index = p.Get<int>("idx", 0);
+ } else if(it->first == "size") {
+ const ImageDim dim = p.Get<ImageDim>("size", ImageDim(0,0) );
+ device_params.Set("Width" , dim.x);
+ device_params.Set("Height" , dim.y);
+ } else if(it->first == "pos") {
+ const ImageDim pos = p.Get<ImageDim>("pos", ImageDim(0,0) );
+ device_params.Set("OffsetX" , pos.x);
+ device_params.Set("OffsetY" , pos.y);
+ } else if(it->first == "roi") {
+ const ImageRoi roi = p.Get<ImageRoi>("roi", ImageRoi(0,0,0,0) );
+ device_params.Set("Width" , roi.w);
+ device_params.Set("Height" , roi.h);
+ device_params.Set("OffsetX", roi.x);
+ device_params.Set("OffsetY", roi.y);
+ } else {
+ device_params.Set(it->first, it->second);
+ }
+ }
+
+ InitDevice(mn.empty() ? 0 : mn.c_str(), sn.empty() ? 0 : sn.c_str(), index);
+ SetDeviceParams(device_params);
+ InitStream();
+
+ InitPangoStreams();
+ InitPangoDeviceProperties();
+ InitBuffers(buffer_count);
+}
+
+PleoraVideo::~PleoraVideo()
+{
+ Stop();
+ DeinitBuffers();
+ DeinitStream();
+ DeinitDevice();
+}
+
+std::string PleoraVideo::GetParameter(const std::string& name) {
+ PvGenParameter* par = lDeviceParams->Get(PvString(name.c_str()));
+ if(par) {
+ PvString ret = par->ToString();
+ return std::string(ret.GetAscii());
+ } else {
+ pango_print_error("Parameter %s not recognized\n", name.c_str());
+ return "";
+ }
+}
+
+void PleoraVideo::SetParameter(const std::string& name, const std::string& value) {
+ PvGenParameter* par = lDeviceParams->Get(PvString(name.c_str()));
+ if(par) {
+ PvResult r = par->FromString(PvString(value.c_str()));
+ if(!r.IsOK()){
+ pango_print_error("Error setting parameter %s to:%s Reason:%s\n", name.c_str(), value.c_str(), r.GetDescription().GetAscii());
+ } else {
+ pango_print_info("Setting parameter %s to:%s\n", name.c_str(), value.c_str());
+ }
+ } else {
+ pango_print_error("Parameter %s not recognized\n", name.c_str());
+ }
+}
+
+void PleoraVideo::InitDevice(
+ const char* model_name, const char* serial_num, size_t index
+) {
+ lPvSystem = new PvSystem();
+ if ( !lPvSystem ) {
+ throw pangolin::VideoException("Pleora: Unable to create PvSystem");
+ }
+
+ lDeviceInfo = SelectDevice(*lPvSystem, model_name, serial_num, index);
+ if ( !lDeviceInfo ) {
+ delete lPvSystem;
+ throw pangolin::VideoException("Pleora: Unable to select device");
+ }
+
+ PvResult lResult;
+ lDevice = PvDevice::CreateAndConnect( lDeviceInfo, &lResult );
+ if ( !lDevice ) {
+ delete lPvSystem;
+ throw pangolin::VideoException("Pleora: Unable to connect to device", lResult.GetDescription().GetAscii() );
+ }
+
+ lDeviceParams = lDevice->GetParameters();
+}
+
+
+void PleoraVideo::DeinitDevice()
+{
+ if(lDevice) {
+ lDevice->Disconnect();
+ PvDevice::Free( lDevice );
+ lDevice = 0;
+ }
+
+ delete lPvSystem;
+ lPvSystem = 0;
+}
+
+void PleoraVideo::InitStream()
+{
+ // Setup Stream
+ PvResult lResult;
+ lStream = PvStream::CreateAndOpen( lDeviceInfo->GetConnectionID(), &lResult );
+ if ( !lStream ) {
+ DeinitDevice();
+ throw pangolin::VideoException("Pleora: Unable to open stream", lResult.GetDescription().GetAscii() );
+ }
+ lStreamParams = lStream->GetParameters();
+}
+
+void PleoraVideo::DeinitStream()
+{
+ if(lStream) {
+ lStream->Close();
+ PvStream::Free( lStream );
+ lStream = 0;
+ }
+}
+
+void PleoraVideo::SetDeviceParams(Params& p) {
+
+ lStart = dynamic_cast<PvGenCommand*>( lDeviceParams->Get( "AcquisitionStart" ) );
+ lStop = dynamic_cast<PvGenCommand*>( lDeviceParams->Get( "AcquisitionStop" ) );
+
+ for(Params::ParamMap::iterator it = p.params.begin(); it != p.params.end(); it++) {
+ if(it->first == "get_temperature"){
+ getTemp = p.Get<bool>("get_temperature",false);
+ } else {
+ if (it->second == "Execute") {
+ // This is a command, deal with it accordingly.
+ PvGenCommand* cmd = dynamic_cast<PvGenCommand*>(lDeviceParams->Get(it->first.c_str()));
+ if(cmd) {
+ PvResult r = cmd->Execute();
+ if(!r.IsOK()){
+ pango_print_error("Error executing command %s Reason:%s\n", it->first.c_str(), r.GetDescription().GetAscii());
+ } else {
+ pango_print_info("Executed Command %s\n", it->first.c_str());
+ }
+ bool done;
+ int attempts = 20;
+ do {
+ cmd->IsDone(done);
+ std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+ attempts--;
+ } while(!done && (attempts > 0));
+ if(attempts == 0) {
+ pango_print_error("Timeout while waiting for command %s done\n", it->first.c_str());
+ }
+ } else {
+ pango_print_error("Command %s not recognized\n", it->first.c_str());
+ }
+ } else {
+ try {
+ PvGenParameter* par = lDeviceParams->Get(PvString(it->first.c_str()));
+ if(par) {
+ PvResult r = par->FromString(PvString(it->second.c_str()));
+ if(!r.IsOK()){
+ pango_print_error("Error setting parameter %s to:%s Reason:%s\n", it->first.c_str(), it->second.c_str(), r.GetDescription().GetAscii());
+ } else {
+ pango_print_info("Setting parameter %s to:%s\n", it->first.c_str(), it->second.c_str());
+ }
+ } else {
+ pango_print_error("Parameter %s not recognized\n", it->first.c_str());
+ }
+ } catch(std::runtime_error e) {
+ pango_print_error("Set parameter %s: %s\n", it->first.c_str(), e.what());
+ }
+ }
+ }
+ }
+
+ // Get Handles to properties we'll be using.
+ lAnalogGain = lDeviceParams->GetInteger("AnalogGain");
+ lGamma = lDeviceParams->GetFloat("Gamma");
+ lAnalogBlackLevel = lDeviceParams->GetInteger("AnalogBlackLevel");
+ lExposure = lDeviceParams->GetFloat("ExposureTime");
+ lAquisitionMode = lDeviceParams->GetEnum("AcquisitionMode");
+ lTriggerSource = lDeviceParams->GetEnum("TriggerSource");
+ lTriggerMode = lDeviceParams->GetEnum("TriggerMode");
+
+ if(getTemp) {
+ lTemperatureCelcius = lDeviceParams->GetFloat("DeviceTemperatureCelsius");
+ pango_print_warn("Warning: get_temperature might add a blocking call taking several ms to each frame read.");
+ }
+
+}
+
+void PleoraVideo::InitBuffers(size_t buffer_count)
+{
+ // Reading payload size from device
+ const uint32_t lSize = lDevice->GetPayloadSize();
+
+ // Use buffer_count or the maximum number of buffers, whichever is smaller
+ const uint32_t lBufferCount = ( lStream->GetQueuedBufferMaximum() < buffer_count ) ?
+ lStream->GetQueuedBufferMaximum() :
+ buffer_count;
+
+ // Allocate buffers and queue
+ for( uint32_t i = 0; i < lBufferCount; i++ ) {
+ PvBuffer *lBuffer = new PvBuffer;
+ lBuffer->Alloc( static_cast<uint32_t>( lSize ) );
+ lBufferList.push_back( lBuffer );
+ }
+}
+
+void PleoraVideo::DeinitBuffers()
+{
+ // Free buffers
+ for( BufferList::iterator lIt = lBufferList.begin(); lIt != lBufferList.end(); lIt++ ) {
+ delete *lIt;
+ }
+ lBufferList.clear();
+}
+
+void PleoraVideo::InitPangoStreams()
+{
+ // Get actual width, height and payload size
+ const int w = DeviceParam<int64_t>("Width");
+ const int h = DeviceParam<int64_t>("Height");
+ const uint32_t lSize = lDevice->GetPayloadSize();
+
+ // Setup pangolin for stream
+ PvGenEnum* lpixfmt = dynamic_cast<PvGenEnum*>( lDeviceParams->Get("PixelFormat") );
+ const PixelFormat fmt = PleoraFormat(lpixfmt);
+ streams.push_back(StreamInfo(fmt, w, h, (w*fmt.bpp)/8));
+ size_bytes = lSize;
+}
+
+void PleoraVideo::InitPangoDeviceProperties()
+{
+ // Store camera details in device properties
+ device_properties["SerialNumber"] = std::string(lDeviceInfo->GetSerialNumber().GetAscii());
+ device_properties["VendorName"] = std::string(lDeviceInfo->GetVendorName().GetAscii());
+ device_properties["ModelName"] = std::string(lDeviceInfo->GetModelName().GetAscii());
+ device_properties["ManufacturerInfo"] = std::string(lDeviceInfo->GetManufacturerInfo().GetAscii());
+ device_properties["Version"] = std::string(lDeviceInfo->GetVersion().GetAscii());
+ device_properties["DisplayID"] = std::string(lDeviceInfo->GetDisplayID().GetAscii());
+ device_properties["UniqueID"] = std::string(lDeviceInfo->GetUniqueID().GetAscii());
+ device_properties["ConnectionID"] = std::string(lDeviceInfo->GetConnectionID().GetAscii());
+
+ picojson::value props(picojson::object_type, true);
+ for(size_t i=0; i < lDeviceParams->GetCount(); ++i) {
+ PvGenParameter* p = (*lDeviceParams)[i];
+ if(p->IsReadable()) {
+ props[p->GetName().GetAscii()] = p->ToString().GetAscii();
+ }
+ }
+
+ device_properties["properties"] = props;
+}
+
+unsigned int PleoraVideo::AvailableFrames() const
+{
+ return validGrabbedBuffers;
+}
+
+bool PleoraVideo::DropNFrames(uint32_t n)
+{
+ if(n > validGrabbedBuffers) return false;
+
+ while(n > 0) {
+ lStream->QueueBuffer(lGrabbedBuffList.front().buff);
+ lGrabbedBuffList.pop_front();
+ --validGrabbedBuffers;
+ --n;
+ DBGPRINT("DropNFrames: removed 1 frame from the list and requeued it.")
+ }
+
+ return true;
+}
+
+void PleoraVideo::Start()
+{
+ if(lStream->GetQueuedBufferCount() == 0) {
+ // Queue all buffers in the stream
+ for( BufferList::iterator lIt = lBufferList.begin(); lIt != lBufferList.end(); lIt++ ) {
+ lStream->QueueBuffer( *lIt );
+ }
+ lDevice->StreamEnable();
+ lStart->Execute();
+ } else {
+// // It isn't an error to repeatedly start
+// pango_print_warn("PleoraVideo: Already started.\n");
+ }
+}
+
+void PleoraVideo::Stop()
+{
+ // stop grab thread
+ if(lStream->GetQueuedBufferCount() > 0) {
+ lStop->Execute();
+ lDevice->StreamDisable();
+
+ // Abort all buffers from the stream and dequeue
+ lStream->AbortQueuedBuffers();
+ while ( lStream->GetQueuedBufferCount() > 0 ) {
+ PvBuffer *lBuffer = NULL;
+ PvResult lOperationResult;
+ lStream->RetrieveBuffer( &lBuffer, &lOperationResult );
+ }
+ } else {
+// // It isn't an error to repeatedly stop
+// pango_print_warn("PleoraVideo: Already stopped.\n");
+ }
+}
+
+size_t PleoraVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+const std::vector<StreamInfo>& PleoraVideo::Streams() const
+{
+ return streams;
+}
+
+bool PleoraVideo::ParseBuffer(PvBuffer* lBuffer, unsigned char* image)
+{
+ TSTART()
+ if ( lBuffer->GetPayloadType() == PvPayloadTypeImage ) {
+ PvImage *lImage = lBuffer->GetImage();
+ TGRABANDPRINT("GetImage took ")
+ std::memcpy(image, lImage->GetDataPointer(), size_bytes);
+ TGRABANDPRINT("memcpy took ")
+ // Required frame properties
+ frame_properties[PANGO_CAPTURE_TIME_US] = picojson::value(lBuffer->GetTimestamp());
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(lBuffer->GetReceptionTime());
+ TGRABANDPRINT("Frame properties took ")
+
+ // Optional frame properties
+ if(lTemperatureCelcius != 0) {
+ double val;
+ PvResult lResult = lTemperatureCelcius->GetValue(val);
+ if(lResult.IsSuccess()) {
+ frame_properties[PANGO_SENSOR_TEMPERATURE_C] = picojson::value(val);
+ } else {
+ pango_print_error("DeviceTemperatureCelsius %f fail\n", val);
+ }
+ }
+ TGRABANDPRINT("GetTemperature took ")
+ return true;
+ } else {
+ return false;
+ }
+
+}
+
+bool PleoraVideo::GrabNext( unsigned char* image, bool wait)
+{
+ const uint32_t timeout = wait ? 1000 : 0;
+ bool good = false;
+ TSTART()
+ DBGPRINT("GrabNext no thread:")
+
+ RetriveAllAvailableBuffers((validGrabbedBuffers==0) ? timeout : 0);
+ TGRABANDPRINT("Retriving all available buffers (valid frames in queue=%d, queue size=%ld) took ",validGrabbedBuffers ,lGrabbedBuffList.size())
+
+ if(validGrabbedBuffers == 0) return false;
+
+ // Retrieve next buffer from list and parse it
+ GrabbedBufferList::iterator front = lGrabbedBuffList.begin();
+ if ( front->res.IsOK() ) {
+ good = ParseBuffer(front->buff, image);
+ }
+ TGRABANDPRINT("Parsing buffer took ")
+
+ lStream->QueueBuffer(front->buff);
+ TGRABANDPRINT("\tPLEORA:QueueBuffer: ")
+
+ // Remove used buffer from list.
+ lGrabbedBuffList.pop_front();
+ --validGrabbedBuffers;
+
+ return good;
+}
+
+
+bool PleoraVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ const uint32_t timeout = wait ? 0xFFFFFFFF : 0;
+ bool good = false;
+
+ TSTART()
+ DBGPRINT("GrabNewest no thread:")
+ RetriveAllAvailableBuffers((validGrabbedBuffers==0) ? timeout : 0);
+ TGRABANDPRINT("Retriving all available buffers (valid frames in queue=%d, queue size=%ld) took ",validGrabbedBuffers ,lGrabbedBuffList.size())
+
+ if(validGrabbedBuffers == 0) {
+ DBGPRINT("No valid buffers, returning.")
+ return false;
+ }
+ if(validGrabbedBuffers > 1) DropNFrames(validGrabbedBuffers-1);
+ TGRABANDPRINT("Dropping %d frames took ", (validGrabbedBuffers-1))
+
+ // Retrieve next buffer from list and parse it
+ GrabbedBufferList::iterator front = lGrabbedBuffList.begin();
+ if ( front->res.IsOK() ) {
+ good = ParseBuffer(front->buff, image);
+ }
+ TGRABANDPRINT("Parsing buffer took ")
+
+ lStream->QueueBuffer(front->buff);
+ TGRABANDPRINT("Requeueing buffer took ")
+
+ // Remove used buffer from list.
+ lGrabbedBuffList.pop_front();
+ --validGrabbedBuffers;
+
+ return good;
+}
+
+void PleoraVideo::RetriveAllAvailableBuffers(uint32_t timeout){
+ PvBuffer *lBuffer = NULL;
+ PvResult lOperationResult;
+ PvResult lResult;
+ TSTART()
+ do {
+ lResult = lStream->RetrieveBuffer( &lBuffer, &lOperationResult, timeout);
+ if ( !lResult.IsOK() ) {
+ if(lResult && !(lResult.GetCode() == PvResult::Code::TIMEOUT)) {
+ pango_print_warn("Pleora error: %s,\n'%s'\n", lResult.GetCodeString().GetAscii(), lResult.GetDescription().GetAscii() );
+ }
+ return;
+ } else if( !lOperationResult.IsOK() ) {
+ pango_print_warn("Pleora error %s,\n'%s'\n", lOperationResult.GetCodeString().GetAscii(), lResult.GetDescription().GetAscii() );
+ lStream->QueueBuffer( lBuffer );
+ return;
+ }
+ lGrabbedBuffList.push_back(GrabbedBuffer(lBuffer,lOperationResult,true));
+ ++validGrabbedBuffers;
+ TGRABANDPRINT("Attempt retrieving buffer (timeout=%d validbuffer=%d) took ", timeout, validGrabbedBuffers)
+ timeout = 0;
+ } while (lResult.IsOK());
+}
+
+int64_t PleoraVideo::GetGain()
+{
+ int64_t val;
+ if(lAnalogGain) {
+ ThrowOnFailure( lAnalogGain->GetValue(val) );
+ }
+ return val;
+}
+
+void PleoraVideo::SetGain(int64_t val)
+{
+ if(val >= 0 && lAnalogGain && lAnalogGain->IsWritable()) {
+ ThrowOnFailure( lAnalogGain->SetValue(val) );
+ frame_properties[PANGO_ANALOG_GAIN] = picojson::value(val);
+ }
+}
+
+int64_t PleoraVideo::GetAnalogBlackLevel()
+{
+ int64_t val;
+ if(lAnalogBlackLevel) {
+ ThrowOnFailure( lAnalogBlackLevel->GetValue(val) );
+ }
+ return val;
+}
+
+void PleoraVideo::SetAnalogBlackLevel(int64_t val)
+{
+ if(val >= 0 && lAnalogBlackLevel&& lAnalogBlackLevel->IsWritable()) {
+ ThrowOnFailure( lAnalogBlackLevel->SetValue(val) );
+ frame_properties[PANGO_ANALOG_BLACK_LEVEL] = picojson::value(val);
+ }
+}
+
+double PleoraVideo::GetExposure()
+{
+ double val;
+ if( lExposure ) {
+ ThrowOnFailure( lExposure->GetValue(val));
+ }
+ return val;
+}
+
+void PleoraVideo::SetExposure(double val)
+{
+ if(val > 0 && lExposure && lExposure->IsWritable() ) {
+ ThrowOnFailure( lExposure->SetValue(val) );
+ frame_properties[PANGO_EXPOSURE_US] = picojson::value(val);
+ }
+}
+
+
+double PleoraVideo::GetGamma()
+{
+ double val;
+ if(lGamma) {
+ ThrowOnFailure(lGamma->GetValue(val));
+ }
+ return val;
+}
+
+void PleoraVideo::SetGamma(double val)
+{
+ if(val > 0 && lGamma && lGamma->IsWritable() ) {
+ ThrowOnFailure( lGamma->SetValue(val) );
+ frame_properties[PANGO_GAMMA] = picojson::value(val);
+ }
+}
+
+//use 0,0,1 for line0 hardware trigger.
+//use 2,252,0 for software continuous
+void PleoraVideo::SetupTrigger(bool triggerActive, int64_t triggerSource, int64_t acquisitionMode)
+{
+ if(lAquisitionMode && lTriggerSource && lTriggerMode &&
+ lAquisitionMode->IsWritable() && lTriggerSource->IsWritable() && lTriggerMode->IsWritable() ) {
+ // Check input is valid.
+ const PvGenEnumEntry* entry_src;
+ const PvGenEnumEntry* entry_acq;
+ lTriggerSource->GetEntryByValue(triggerSource, &entry_src);
+ lAquisitionMode->GetEntryByValue(acquisitionMode, &entry_acq);
+
+ if(entry_src && entry_acq) {
+ ThrowOnFailure(lTriggerMode->SetValue(triggerActive ? 1 : 0));
+ if(triggerActive) {
+ pango_print_debug("Pleora: external trigger active\n");
+ ThrowOnFailure(lTriggerSource->SetValue(triggerSource));
+ ThrowOnFailure(lAquisitionMode->SetValue(acquisitionMode));
+ }
+ }else{
+ pango_print_error("Bad values for trigger options.");
+ }
+ }
+}
+
+template<typename T>
+T PleoraVideo::DeviceParam(const char* name)
+{
+ return GetParam<T>(lDeviceParams, name);
+}
+
+template<typename T>
+bool PleoraVideo::SetDeviceParam(const char* name, T val)
+{
+ return SetParam<T>(lDeviceParams, name, val);
+}
+
+template<typename T>
+T PleoraVideo::StreamParam(const char* name)
+{
+ return GetParam<T>(lStreamParams, name);
+}
+
+template<typename T>
+bool PleoraVideo::SetStreamParam(const char* name, T val)
+{
+ return SetParam<T>(lStreamParams, name, val);
+}
+
+PANGOLIN_REGISTER_FACTORY(PleoraVideo)
+{
+ struct PleoraVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ return std::unique_ptr<VideoInterface>(new PleoraVideo(uri));
+ }
+ };
+
+ auto factory = std::make_shared<PleoraVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "pleora");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "u3v");
+}
+
+}
+
+#undef TSTART
+#undef TGRABANDPRINT
+#undef DBGPRINT
diff --git a/externals/Pangolin/src/video/drivers/pvn.cpp b/externals/Pangolin/src/video/drivers/pvn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6b49724d99f527cd2176e77ba34b64046dbce36c
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/pvn.cpp
@@ -0,0 +1,136 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/video/drivers/pvn.h>
+#include <pangolin/video/iostream_operators.h>
+
+#include <iostream>
+
+using namespace std;
+
+namespace pangolin
+{
+
+PvnVideo::PvnVideo(const std::string& filename, bool realtime )
+ : frame_size_bytes(0), realtime(realtime), last_frame(TimeNow())
+{
+ file.open( PathExpand(filename).c_str(), ios::binary );
+
+ if(!file.is_open() )
+ throw VideoException("Cannot open file - does not exist or bad permissions.");
+
+ ReadFileHeader();
+}
+
+PvnVideo::~PvnVideo()
+{
+}
+
+void PvnVideo::ReadFileHeader()
+{
+ string sfmt;
+ float framerate;
+ unsigned w, h;
+
+ file >> sfmt;
+ file >> w;
+ file >> h;
+ file >> framerate;
+ file.get();
+
+ if(file.bad() || !(w >0 && h >0) )
+ throw VideoException("Unable to read video header");
+
+ const PixelFormat fmt = PixelFormatFromString(sfmt);
+ StreamInfo strm0( fmt, w, h, (w*fmt.bpp) / 8, 0);
+
+ frame_size_bytes += strm0.Pitch() * strm0.Height();
+// frame_interval = TimeFromSeconds( 1.0 / framerate);
+
+ streams.push_back(strm0);
+}
+
+
+void PvnVideo::Start()
+{
+}
+
+void PvnVideo::Stop()
+{
+}
+
+size_t PvnVideo::SizeBytes() const
+{
+ return frame_size_bytes;
+}
+
+const std::vector<StreamInfo>& PvnVideo::Streams() const
+{
+ return streams;
+}
+
+bool PvnVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ file.read((char*)image, frame_size_bytes);
+
+ const basetime next_frame = TimeAdd(last_frame, frame_interval);
+
+ if( realtime ) {
+ WaitUntil(next_frame);
+ }
+
+ last_frame = TimeNow();
+ return file.good();
+}
+
+bool PvnVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+PANGOLIN_REGISTER_FACTORY(PvnVideo)
+{
+ struct PvnVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const std::string path = PathExpand(uri.url);
+
+ if( !uri.scheme.compare("pvn") || FileType(uri.url) == ImageFileTypePvn ) {
+ const bool realtime = uri.Contains("realtime");
+ return std::unique_ptr<VideoInterface>(new PvnVideo(path.c_str(), realtime));
+ }
+ return std::unique_ptr<VideoInterface>();
+ }
+ };
+
+ auto factory = std::make_shared<PvnVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "pvn");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "file");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/realsense.cpp b/externals/Pangolin/src/video/drivers/realsense.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..23bc40ffb9ced3b6072a5e032bb242e4e70dc520
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/realsense.cpp
@@ -0,0 +1,100 @@
+#include <librealsense/rs.hpp>
+#include <pangolin/video/drivers/realsense.h>
+#include <pangolin/factory/factory_registry.h>
+
+namespace pangolin {
+
+RealSenseVideo::RealSenseVideo(ImageDim dim, int fps)
+ : dim_(dim), fps_(fps) {
+ ctx_ = new rs::context();
+ sizeBytes = 0;
+ for (int32_t i=0; i<ctx_->get_device_count(); ++i) {
+ devs_.push_back(ctx_->get_device(i));
+
+ devs_[i]->enable_stream(rs::stream::depth, dim_.x, dim_.y, rs::format::z16, fps_);
+ StreamInfo streamD(PixelFormatFromString("GRAY16LE"), dim_.x, dim_.y, dim_.x*2, 0);
+ streams.push_back(streamD);
+
+ sizeBytes += streamD.SizeBytes();
+ devs_[i]->enable_stream(rs::stream::color, dim_.x, dim_.y, rs::format::rgb8, fps_);
+ StreamInfo streamRGB(PixelFormatFromString("RGB24"), dim_.x, dim_.y, dim_.x*3, (uint8_t*)0+sizeBytes);
+ streams.push_back(streamRGB);
+ sizeBytes += streamRGB.SizeBytes();
+
+ devs_[i]->start();
+ }
+ total_frames = std::numeric_limits<int>::max();
+}
+
+RealSenseVideo::~RealSenseVideo() {
+ delete ctx_;
+}
+
+void RealSenseVideo::Start() {
+ for (int32_t i=0; i<ctx_->get_device_count(); ++i) {
+ devs_[i]->stop();
+ devs_[i]->start();
+ }
+ current_frame_index = 0;
+}
+
+void RealSenseVideo::Stop() {
+ for (int32_t i=0; i<ctx_->get_device_count(); ++i) {
+ devs_[i]->stop();
+ }
+}
+
+size_t RealSenseVideo::SizeBytes() const {
+ return sizeBytes;
+}
+
+const std::vector<StreamInfo>& RealSenseVideo::Streams() const {
+ return streams;
+}
+
+bool RealSenseVideo::GrabNext(unsigned char* image, bool wait) {
+
+ unsigned char* out_img = image;
+ for (int32_t i=0; i<ctx_->get_device_count(); ++i) {
+ if (wait) {
+ devs_[i]->wait_for_frames();
+ }
+ memcpy(out_img, devs_[i]->get_frame_data(rs::stream::depth), streams[i*2].SizeBytes());
+ out_img += streams[i*2].SizeBytes();
+ memcpy(out_img, devs_[i]->get_frame_data(rs::stream::color), streams[i*2+1].SizeBytes());
+ out_img += streams[i*2+1].SizeBytes();
+ }
+ return true;
+}
+
+bool RealSenseVideo::GrabNewest(unsigned char* image, bool wait) {
+ return GrabNext(image, wait);
+}
+
+size_t RealSenseVideo::GetCurrentFrameId() const {
+ return current_frame_index;
+}
+
+size_t RealSenseVideo::GetTotalFrames() const {
+ return total_frames;
+}
+
+size_t RealSenseVideo::Seek(size_t frameid) {
+ // TODO
+ return -1;
+}
+
+PANGOLIN_REGISTER_FACTORY(RealSenseVideo)
+{
+ struct RealSenseVideoFactory : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const unsigned int fps = uri.Get<unsigned int>("fps", 30);
+ return std::unique_ptr<VideoInterface>( new RealSenseVideo(dim, fps) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<RealSenseVideoFactory>(), 10, "realsense");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/realsense2.cpp b/externals/Pangolin/src/video/drivers/realsense2.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..591d3b72acda2d9bf31bf9f0b47c5910d0154e2d
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/realsense2.cpp
@@ -0,0 +1,114 @@
+#include <librealsense2/rs.hpp>
+#include <pangolin/video/drivers/realsense2.h>
+#include <pangolin/factory/factory_registry.h>
+
+namespace pangolin {
+
+RealSense2Video::RealSense2Video(ImageDim dim, int fps)
+ : dim_(dim), fps_(fps) {
+
+ sizeBytes = 0;
+
+ // Create RealSense pipeline, encapsulating the actual device and sensors
+ pipe = new rs2::pipeline();
+
+ //Configure the pipeline
+ cfg = new rs2::config();
+
+ { //config depth
+ cfg->enable_stream(RS2_STREAM_DEPTH, dim_.x, dim_.y, RS2_FORMAT_Z16, fps_);
+ StreamInfo streamD(PixelFormatFromString("GRAY16LE"), dim_.x, dim_.y, dim_.x*2, 0);
+ streams.push_back(streamD);
+ sizeBytes += streamD.SizeBytes();
+ }
+
+ { //config color
+ cfg->enable_stream(RS2_STREAM_COLOR, dim_.x, dim_.y, RS2_FORMAT_RGB8, fps_);
+ StreamInfo streamRGB(PixelFormatFromString("RGB24"), dim_.x, dim_.y, dim_.x*3, (uint8_t*)0+sizeBytes);
+ streams.push_back(streamRGB);
+ sizeBytes += streamRGB.SizeBytes();
+ }
+
+ // Start streaming with default recommended configuration
+ pipe->start(*cfg);
+ rs2::pipeline_profile profile = pipe->get_active_profile();
+ auto sensor = profile.get_device().first<rs2::depth_sensor>();
+ auto scale = sensor.get_depth_scale();
+ std::cout << "Depth scale is: " << scale << std::endl;
+
+ total_frames = std::numeric_limits<int>::max();
+}
+
+RealSense2Video::~RealSense2Video() {
+ delete pipe;
+ pipe = nullptr;
+
+ delete cfg;
+ cfg = nullptr;
+}
+
+void RealSense2Video::Start() {
+ pipe->start(*cfg);
+ current_frame_index = 0;
+}
+
+void RealSense2Video::Stop() {
+ pipe->stop();
+}
+
+size_t RealSense2Video::SizeBytes() const {
+ return sizeBytes;
+}
+
+const std::vector<StreamInfo>& RealSense2Video::Streams() const {
+ return streams;
+}
+
+bool RealSense2Video::GrabNext(unsigned char* image, bool /*wait*/) {
+
+ unsigned char* out_img = image;
+
+ rs2::frameset data = pipe->wait_for_frames(); // Wait for next set of frames from the camera
+ rs2::frame depth = data.get_depth_frame(); // Get the depth data
+ rs2::frame color = data.get_color_frame(); // Get the color data
+
+ memcpy(out_img, depth.get_data(), streams[0].SizeBytes());
+ out_img += streams[0].SizeBytes();
+
+ memcpy(out_img, color.get_data(), streams[1].SizeBytes());
+ out_img += streams[1].SizeBytes();
+
+ return true;
+}
+
+bool RealSense2Video::GrabNewest(unsigned char* image, bool wait) {
+ return GrabNext(image, wait);
+}
+
+size_t RealSense2Video::GetCurrentFrameId() const {
+ return current_frame_index;
+}
+
+size_t RealSense2Video::GetTotalFrames() const {
+ return total_frames;
+}
+
+size_t RealSense2Video::Seek(size_t /*frameid*/) {
+ // TODO
+ return -1;
+}
+
+PANGOLIN_REGISTER_FACTORY(RealSense2Video)
+{
+ struct RealSense2VideoFactory : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const unsigned int fps = uri.Get<unsigned int>("fps", 30);
+ return std::unique_ptr<VideoInterface>( new RealSense2Video(dim, fps) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<RealSense2VideoFactory>(), 10, "realsense2");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/shared_memory.cpp b/externals/Pangolin/src/video/drivers/shared_memory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..2d7433d7883663453d56168fc3a2b7914bcee3fb
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/shared_memory.cpp
@@ -0,0 +1,99 @@
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/shared_memory.h>
+#include <pangolin/video/iostream_operators.h>
+
+using namespace std;
+
+namespace pangolin
+{
+
+SharedMemoryVideo::SharedMemoryVideo(size_t w, size_t h, std::string pix_fmt,
+ const std::shared_ptr<SharedMemoryBufferInterface>& shared_memory,
+ const std::shared_ptr<ConditionVariableInterface>& buffer_full) :
+ _fmt(PixelFormatFromString(pix_fmt)),
+ _frame_size(w*h*_fmt.bpp/8),
+ _shared_memory(shared_memory),
+ _buffer_full(buffer_full)
+{
+ const size_t pitch = w * _fmt.bpp/8;
+ const StreamInfo stream(_fmt, w, h, pitch, 0);
+ _streams.push_back(stream);
+}
+
+SharedMemoryVideo::~SharedMemoryVideo()
+{
+}
+
+void SharedMemoryVideo::Start()
+{
+}
+
+void SharedMemoryVideo::Stop()
+{
+}
+
+size_t SharedMemoryVideo::SizeBytes() const
+{
+ return _frame_size;
+}
+
+const std::vector<StreamInfo>& SharedMemoryVideo::Streams() const
+{
+ return _streams;
+}
+
+bool SharedMemoryVideo::GrabNext(unsigned char* image, bool wait)
+{
+ // If a condition variable exists, try waiting on it.
+ if(_buffer_full) {
+ timespec ts;
+ clock_gettime(CLOCK_REALTIME, &ts);
+
+ if (wait) {
+ _buffer_full->wait();
+ } else if (!_buffer_full->wait(ts)) {
+ return false;
+ }
+ }
+
+ // Read the buffer.
+ _shared_memory->lock();
+ memcpy(image, _shared_memory->ptr(), _frame_size);
+ _shared_memory->unlock();
+
+ return true;
+}
+
+bool SharedMemoryVideo::GrabNewest(unsigned char* image, bool wait)
+{
+ return GrabNext(image,wait);
+}
+
+PANGOLIN_REGISTER_FACTORY(SharedMemoryVideo)
+{
+ struct SharedMemoryVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(0, 0));
+ const std::string sfmt = uri.Get<std::string>("fmt", "GRAY8");
+ const PixelFormat fmt = PixelFormatFromString(sfmt);
+ const std::string shmem_name = std::string("/") + uri.url;
+ std::shared_ptr<SharedMemoryBufferInterface> shmem_buffer =
+ open_named_shared_memory_buffer(shmem_name, true);
+ if (dim.x == 0 || dim.y == 0 || !shmem_buffer) {
+ throw VideoException("invalid shared memory parameters");
+ }
+
+ const std::string cond_name = shmem_name + "_cond";
+ std::shared_ptr<ConditionVariableInterface> buffer_full =
+ open_named_condition_variable(cond_name);
+
+ return std::unique_ptr<VideoInterface>(
+ new SharedMemoryVideo(dim.x, dim.y, fmt, shmem_buffer,buffer_full)
+ );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<SharedMemoryVideoFactory>(), 10, "shmem");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/shift.cpp b/externals/Pangolin/src/video/drivers/shift.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ccf0e8e7eead9bf737c4a26ae1f8e4e20f656ad8
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/shift.cpp
@@ -0,0 +1,159 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/shift.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+ShiftVideo::ShiftVideo(std::unique_ptr<VideoInterface> &src_, PixelFormat out_fmt, int shift_right_bits, unsigned int mask)
+ : src(std::move(src_)), size_bytes(0), buffer(0), shift_right_bits(shift_right_bits), mask(mask)
+{
+ if(!src) {
+ throw VideoException("ShiftVideo: VideoInterface in must not be null");
+ }
+ videoin.push_back(src.get());
+
+ for(size_t s=0; s< src->Streams().size(); ++s) {
+ const size_t w = src->Streams()[s].Width();
+ const size_t h = src->Streams()[s].Height();
+
+ // Check compatibility of formats
+ const PixelFormat in_fmt = src->Streams()[s].PixFormat();
+ if(in_fmt.channels != out_fmt.channels) {
+ throw VideoException("ShiftVideo: output format is not compatible with input format for shifting.");
+ }
+ if(out_fmt.channels > 1 || out_fmt.bpp != 8 || in_fmt.bpp != 16) {
+ // TODO: Remove restriction
+ throw VideoException("ShiftVideo: currently only supports one channel input formats of 16 bits.");
+ }
+
+ streams.push_back(pangolin::StreamInfo( out_fmt, w, h, w*out_fmt.bpp / 8, (unsigned char*)0 + size_bytes ));
+ size_bytes += w*h*out_fmt.bpp / 8;
+ }
+
+ buffer = new unsigned char[src->SizeBytes()];
+}
+
+ShiftVideo::~ShiftVideo()
+{
+ delete[] buffer;
+}
+
+//! Implement VideoInput::Start()
+void ShiftVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+//! Implement VideoInput::Stop()
+void ShiftVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t ShiftVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& ShiftVideo::Streams() const
+{
+ return streams;
+}
+
+void DoShift16to8(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in,
+ int shift_right_bits,
+ unsigned int mask
+) {
+ for(size_t y=0; y<out.h; ++y) {
+ for(size_t x=0; x<out.w; ++x) {
+ const unsigned short vin = ((unsigned short*)(in.ptr + y * in.pitch))[x];
+ out.ptr[y*out.pitch+x] = (vin >> shift_right_bits) & mask;
+ }
+ }
+}
+
+//! Implement VideoInput::GrabNext()
+bool ShiftVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNext(buffer,wait)) {
+ for(size_t s=0; s<streams.size(); ++s) {
+ Image<unsigned char> img_in = videoin[0]->Streams()[s].StreamImage(buffer);
+ Image<unsigned char> img_out = Streams()[s].StreamImage(image);
+ DoShift16to8(img_out, img_in, shift_right_bits, mask);
+ }
+ return true;
+ }else{
+ return false;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool ShiftVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNewest(buffer,wait)) {
+ for(size_t s=0; s<streams.size(); ++s) {
+ Image<unsigned char> img_in = videoin[0]->Streams()[s].StreamImage(buffer);
+ Image<unsigned char> img_out = Streams()[s].StreamImage(image);
+ DoShift16to8(img_out, img_in, shift_right_bits, mask);
+ }
+ return true;
+ }else{
+ return false;
+ }
+}
+
+std::vector<VideoInterface*>& ShiftVideo::InputStreams()
+{
+ return videoin;
+}
+
+PANGOLIN_REGISTER_FACTORY(ShiftVideo)
+{
+ struct ShiftVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const int shift_right = uri.Get<int>("shift", 0);
+ const int mask = uri.Get<int>("mask", 0xffff);
+
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ return std::unique_ptr<VideoInterface>(
+ new ShiftVideo(subvid, PixelFormatFromString("GRAY8"), shift_right, mask)
+ );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<ShiftVideoFactory>(), 10, "shift");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/split.cpp b/externals/Pangolin/src/video/drivers/split.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4e74c699ae929a0c58155e065e2364f0ffcfa0d
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/split.cpp
@@ -0,0 +1,158 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/split.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+SplitVideo::SplitVideo(std::unique_ptr<VideoInterface> &src_, const std::vector<StreamInfo>& streams)
+ : src(std::move(src_)), streams(streams)
+{
+ videoin.push_back(src.get());
+
+ // Warn if stream over-runs input stream
+ for(unsigned int i=0; i < streams.size(); ++i) {
+ if(src->SizeBytes() < (size_t)streams[i].Offset() + streams[i].SizeBytes() ) {
+ pango_print_warn("VideoSplitter: stream extends past end of input.\n");
+ break;
+ }
+ }
+}
+
+SplitVideo::~SplitVideo()
+{
+}
+
+size_t SplitVideo::SizeBytes() const
+{
+ return videoin[0]->SizeBytes();
+}
+
+const std::vector<StreamInfo>& SplitVideo::Streams() const
+{
+ return streams;
+}
+
+void SplitVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+void SplitVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+bool SplitVideo::GrabNext( unsigned char* image, bool wait )
+{
+ return videoin[0]->GrabNext(image, wait);
+}
+
+bool SplitVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return videoin[0]->GrabNewest(image, wait);
+}
+
+std::vector<VideoInterface*>& SplitVideo::InputStreams()
+{
+ return videoin;
+}
+
+PANGOLIN_REGISTER_FACTORY(SplitVideo)
+{
+ struct SplitVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::vector<StreamInfo> streams;
+
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ if(subvid->Streams().size() == 0) {
+ throw VideoException("VideoSplitter input must have at least one stream");
+ }
+
+ while(true) {
+ const size_t n = streams.size() + 1;
+ std::string key_roi = std::string("roi") + pangolin::Convert<std::string, size_t>::Do(n);
+ std::string key_mem = std::string("mem") + pangolin::Convert<std::string, size_t>::Do(n);
+ std::string key_str = std::string("stream") + pangolin::Convert<std::string, size_t>::Do(n);
+
+ if(uri.Contains(key_roi)) {
+ const StreamInfo& st1 = subvid->Streams()[0];
+ const ImageRoi& roi = uri.Get<ImageRoi>(key_roi, ImageRoi() );
+ if(roi.w == 0 || roi.h == 0) {
+ throw VideoException("split: empty ROI.");
+ }
+ const size_t start1 = roi.y * st1.Pitch() + st1.PixFormat().bpp * roi.x / 8;
+ streams.push_back( StreamInfo( st1.PixFormat(), roi.w, roi.h, st1.Pitch(), (unsigned char*)0 + start1 ) );
+ }else if(uri.Contains(key_mem)) {
+ const StreamInfo& info = uri.Get<StreamInfo>(key_mem, subvid->Streams()[0] );
+ streams.push_back(info);
+ }else if(uri.Contains(key_str)) {
+ const size_t old_stream = uri.Get<size_t>(key_str, 0) -1;
+ if(old_stream >= subvid->Streams().size()) {
+ throw VideoException("split: requesting source stream which does not exist.");
+ }
+ streams.push_back(subvid->Streams()[old_stream]);
+ }else{
+ break;
+ }
+ }
+
+ // Default split if no arguments
+ if(streams.size() == 0) {
+ const StreamInfo& st1 = subvid->Streams()[0];
+ const size_t subw = st1.Width();
+ const size_t subh = st1.Height();
+
+ ImageRoi roi1, roi2;
+
+ if(subw > subh) {
+ // split horizontally
+ roi1 = ImageRoi(0,0, subw/2, subh );
+ roi2 = ImageRoi(subw/2,0, subw/2, subh );
+ }else{
+ // split vertically
+ roi1 = ImageRoi(0,0, subw, subh/2 );
+ roi2 = ImageRoi(0,subh/2, subw, subh/2 );
+ }
+
+ const size_t start1 = roi1.y * st1.Pitch() + st1.PixFormat().bpp * roi1.x / 8;
+ const size_t start2 = roi2.y * st1.Pitch() + st1.PixFormat().bpp * roi2.x / 8;
+ streams.push_back( StreamInfo( st1.PixFormat(), roi1.w, roi1.h, st1.Pitch(), (unsigned char*)(start1) ) );
+ streams.push_back( StreamInfo( st1.PixFormat(), roi2.w, roi2.h, st1.Pitch(), (unsigned char*)(start2) ) );
+ }
+
+ return std::unique_ptr<VideoInterface>( new SplitVideo(subvid,streams) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<SplitVideoFactory>(), 10, "split");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/teli.cpp b/externals/Pangolin/src/video/drivers/teli.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a5589d51535661254c5e87274e0e09fe4eaffc3f
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/teli.cpp
@@ -0,0 +1,550 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2015 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <XmlFeatures.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/teli.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+// Represet lifetime of Teli SDK. Destructed by static deinitialisation.
+class TeliSystem
+{
+public:
+ static TeliSystem& Instance() {
+ static TeliSystem sys;
+ return sys;
+ }
+
+private:
+ TeliSystem()
+ {
+ Teli::CAM_API_STATUS uiStatus = Teli::Sys_Initialize();
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS && uiStatus != Teli::CAM_API_STS_ALREADY_INITIALIZED)
+ throw pangolin::VideoException("Unable to initialise TeliSDK.");
+ }
+
+ ~TeliSystem()
+ {
+ Teli::CAM_API_STATUS uiStatus = Teli::Sys_Terminate();
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS) {
+ pango_print_warn("TeliSDK: Error uninitialising.");
+ }
+ }
+};
+
+std::string GetNodeValStr(Teli::CAM_HANDLE cam, Teli::CAM_NODE_HANDLE node, std::string node_str)
+{
+ Teli::TC_NODE_TYPE node_type;
+ Teli::CAM_API_STATUS st = Teli::Nd_GetType(cam, node, &node_type);
+ if(st != Teli::CAM_API_STS_SUCCESS) {
+ throw std::runtime_error("TeliSDK: Unable to get Teli node type.");
+ }
+
+ Teli::CAM_API_STATUS status;
+
+ switch(node_type) {
+ case Teli::TC_NODE_TYPE_INTEGER:
+ {
+ int64_t val;
+ status = Teli::Nd_GetIntValue(cam, node, &val);
+ if(status == Teli::CAM_API_STS_SUCCESS) {
+ return pangolin::Convert<std::string, int64_t>::Do(val);
+ }
+ break;
+ }
+ case Teli::TC_NODE_TYPE_BOOLEAN:
+ {
+ bool8_t val;
+ status = Teli::Nd_GetBoolValue(cam, node, &val);
+ if(status == Teli::CAM_API_STS_SUCCESS) {
+ return pangolin::Convert<std::string, bool8_t>::Do(val);
+ }
+ break;
+ }
+ case Teli::TC_NODE_TYPE_FLOAT:
+ {
+ float64_t val;
+ status = Teli::Nd_GetFloatValue(cam, node, &val);
+ if(status == Teli::CAM_API_STS_SUCCESS) {
+ return pangolin::Convert<std::string, float64_t>::Do(val);
+ }
+ break;
+ }
+ case Teli::TC_NODE_TYPE_STRING:
+ {
+ uint32_t buffer_size = 10*1024;
+ char* buffer = new char[buffer_size];
+ status = Teli::Nd_GetStrValue(cam, node, buffer, &buffer_size);
+ std::string val(buffer);
+ delete[] buffer;
+ if(status == Teli::CAM_API_STS_SUCCESS) {
+ return val;
+ }
+ break;
+ }
+ case Teli::TC_NODE_TYPE_ENUMERATION:
+ {
+ uint32_t buffer_size = 10*1024;
+ char* buffer = new char[buffer_size];
+ status = Teli::Nd_GetEnumStrValue(cam, node, buffer, &buffer_size);
+ std::string val(buffer);
+ if(status == Teli::CAM_API_STS_SUCCESS) {
+ return val;
+ }
+ break;
+ }
+ case Teli::TC_NODE_TYPE_COMMAND:
+ case Teli::TC_NODE_TYPE_REGISTER:
+ case Teli::TC_NODE_TYPE_CATEGORY:
+ case Teli::TC_NODE_TYPE_ENUM_ENTRY:
+ case Teli::TC_NODE_TYPE_PORT:
+ default:
+ throw VideoException("TeliSDK: Unsupported node_type: " + node_type);
+ }
+
+ if(status != Teli::CAM_API_STS_SUCCESS) {
+ Teli::CAM_GENICAM_ERR_MSG psErrMsg;
+ Teli::Misc_GetLastGenICamError(&psErrMsg);
+ throw VideoException("TeliSDK: Unable to get Teli parameter, " + node_str, psErrMsg.pszDescription);
+ }else{
+ throw VideoException("TeliSDK: Unable to get Teli parameter, " + node_str);
+ }
+}
+
+void TeliVideo::SetNodeValStr(Teli::CAM_HANDLE cam, Teli::CAM_NODE_HANDLE node, std::string node_str, std::string val_str)
+{
+ Teli::TC_NODE_TYPE node_type;
+ Teli::CAM_API_STATUS st = Teli::Nd_GetType(cam, node, &node_type);
+ if(st != Teli::CAM_API_STS_SUCCESS) {
+ throw VideoException("TeliSDK: Unable to get Teli node type.");
+ }
+
+ Teli::CAM_API_STATUS status = Teli::CAM_API_STS_SUCCESS;
+
+ switch(node_type) {
+ case Teli::TC_NODE_TYPE_INTEGER:
+ {
+ const int64_t val = pangolin::Convert<int64_t, std::string>::Do(val_str);
+ status = Teli::Nd_SetIntValue(cam, node, val);
+ break;
+ }
+ case Teli::TC_NODE_TYPE_BOOLEAN:
+ {
+ const bool8_t val = pangolin::Convert<bool8_t, std::string>::Do(val_str);
+ status = Teli::Nd_SetBoolValue(cam, node, val);
+ break;
+ }
+ case Teli::TC_NODE_TYPE_FLOAT:
+ {
+ const float64_t val = pangolin::Convert<float64_t, std::string>::Do(val_str);
+ status = Teli::Nd_SetFloatValue(cam, node, val);
+ break;
+ }
+ case Teli::TC_NODE_TYPE_STRING:
+ {
+ status = Teli::Nd_SetStrValue(cam, node, val_str.c_str());
+ break;
+ }
+ case Teli::TC_NODE_TYPE_ENUMERATION:
+ {
+ status = Teli::Nd_SetEnumStrValue(cam, node, val_str.c_str());
+ break;
+ }
+ case Teli::TC_NODE_TYPE_COMMAND:
+ {
+ status = Teli::Nd_CmdExecute(cam, node, true);
+
+ if (status != Teli::CAM_API_STS_SUCCESS) {
+ pango_print_error("TeliVideo: Nd_CmdExecute returned error, %u", status);
+ break;
+ }
+
+ // Confirm command is successful
+ bool done = false;
+ for(int attempts=20; attempts > 0; --attempts) {
+ // Confirm whether the execution has been accomplished.
+ status = Teli::Nd_GetCmdIsDone(cam, node, &done);
+ if (status != Teli::CAM_API_STS_SUCCESS) {
+ pango_print_error("TeliVideo: Nd_GetCmdIsDone returned error, %u", status);
+ break;
+ }
+ if(done) break;
+ }
+
+ pango_print_error("Timeout while waiting for command %s done\n", node_str.c_str());
+ break;
+ }
+ case Teli::TC_NODE_TYPE_REGISTER:
+ case Teli::TC_NODE_TYPE_CATEGORY:
+ case Teli::TC_NODE_TYPE_ENUM_ENTRY:
+ case Teli::TC_NODE_TYPE_PORT:
+ default:
+ throw VideoException("TeliSDK: Unsupported node_type: " + node_type);
+ }
+
+ if(status != Teli::CAM_API_STS_SUCCESS) {
+ Teli::CAM_GENICAM_ERR_MSG psErrMsg;
+ Teli::Misc_GetLastGenICamError(&psErrMsg);
+ throw VideoException("TeliSDK: Unable to set Teli parameter, " + node_str, psErrMsg.pszDescription);
+ }
+}
+
+TeliVideo::TeliVideo(const Params& p)
+ : cam(0), strm(0), hStrmCmpEvt(0), transfer_bandwidth_gbps(0), exposure_us(0)
+{
+ TeliSystem::Instance();
+
+ uint32_t num_cams = 0;
+ Teli::CAM_API_STATUS uiStatus = Teli::Sys_GetNumOfCameras(&num_cams);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("Unable to enumerate TeliSDK cameras.");
+
+ if (num_cams == 0)
+ throw pangolin::VideoException("No TeliSDK Cameras available.");
+
+ // Default to rogue values
+ std::string sn;
+ std::string mn;
+ int cam_index = 0;
+
+ Params device_params;
+
+ for(Params::ParamMap::const_iterator it = p.params.begin(); it != p.params.end(); it++) {
+ if(it->first == "model"){
+ mn = it->second;
+ } else if(it->first == "sn"){
+ sn = it->second;
+ } else if(it->first == "idx"){
+ cam_index = p.Get<int>("idx", 0);
+ } else if(it->first == "size") {
+ const ImageDim dim = p.Get<ImageDim>("size", ImageDim(0,0) );
+ device_params.Set("Width" , dim.x);
+ device_params.Set("Height" , dim.y);
+ } else if(it->first == "pos") {
+ const ImageDim pos = p.Get<ImageDim>("pos", ImageDim(0,0) );
+ device_params.Set("OffsetX" , pos.x);
+ device_params.Set("OffsetY" , pos.y);
+ } else if(it->first == "roi") {
+ const ImageRoi roi = p.Get<ImageRoi>("roi", ImageRoi(0,0,0,0) );
+ device_params.Set("Width" , roi.w);
+ device_params.Set("Height" , roi.h);
+ device_params.Set("OffsetX", roi.x);
+ device_params.Set("OffsetY", roi.y);
+ } else {
+ device_params.Set(it->first, it->second);
+ }
+ }
+
+ if(sn.empty() && mn.empty()) {
+ uiStatus = Teli::Cam_Open(cam_index, &cam, 0, true, 0);
+ }else{
+ uiStatus = Teli::Cam_OpenFromInfo(
+ (sn.empty() ? 0 : sn.c_str()),
+ (mn.empty() ? 0 : mn.c_str()),
+ 0, &cam, 0, true, 0
+ );
+ }
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException(FormatString("TeliSDK: Error opening camera, sn='%'", sn));
+
+ SetDeviceParams(device_params);
+ Initialise();
+}
+
+ bool TeliVideo::GetParameter(const std::string& name, std::string& result)
+{
+ Teli::CAM_NODE_HANDLE node;
+ Teli::CAM_API_STATUS st = Teli::Nd_GetNode(cam, name.c_str(), &node);
+ if( st == Teli::CAM_API_STS_SUCCESS) {
+ result = GetNodeValStr(cam, node, name);
+ return true;
+ }else{
+ pango_print_warn("TeliSDK: Unable to get reference to node: %s", name.c_str());
+ return false;
+ }
+}
+
+bool TeliVideo::SetParameter(const std::string& name, const std::string& value)
+{
+ Teli::CAM_NODE_HANDLE node;
+ Teli::CAM_API_STATUS st = Teli::Nd_GetNode(cam, name.c_str(), &node);
+ if( st == Teli::CAM_API_STS_SUCCESS) {
+ SetNodeValStr(cam, node, name, value);
+ return true;
+ }else{
+ pango_print_warn("TeliSDK: Unable to get reference to node: %s", name.c_str());
+ return false;
+ }
+}
+
+void TeliVideo::Initialise()
+{
+ Teli::CAM_API_STATUS uiStatus = Teli::CAM_API_STS_SUCCESS;
+
+ // Create completion event object for stream.
+#ifdef _WIN_
+ hStrmCmpEvt = CreateEvent(NULL, FALSE, FALSE, NULL);
+ if (hStrmCmpEvt == NULL)
+ throw pangolin::VideoException("TeliSDK: Error creating event.");
+#endif
+#ifdef _LINUX_
+ uiStatus = Teli::Sys_CreateSignal(&hStrmCmpEvt);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error creating event.");
+#endif
+ uint32_t uiPyldSize = 0;
+ uiStatus = Teli::Strm_OpenSimple(cam, &strm, &uiPyldSize, hStrmCmpEvt);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error opening camera stream.");
+
+ // Read pixel format
+ PixelFormat pfmt;
+ Teli::CAM_PIXEL_FORMAT teli_fmt;
+ uiStatus = Teli::GetCamPixelFormat(cam, &teli_fmt);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error calling GetCamPixelFormat.");
+
+ switch( teli_fmt) {
+ case Teli::PXL_FMT_Mono8:
+ case Teli::PXL_FMT_BayerGR8:
+ case Teli::PXL_FMT_BayerBG8:
+ pfmt = pangolin::PixelFormatFromString("GRAY8");
+ break;
+ case Teli::PXL_FMT_Mono10:
+ case Teli::PXL_FMT_Mono12:
+ case Teli::PXL_FMT_Mono16:
+ case Teli::PXL_FMT_BayerGR10:
+ case Teli::PXL_FMT_BayerGR12:
+ case Teli::PXL_FMT_BayerBG10:
+ case Teli::PXL_FMT_BayerBG12:
+ pfmt = pangolin::PixelFormatFromString("GRAY16LE");
+ break;
+ case Teli::PXL_FMT_RGB8:
+ pfmt = pangolin::PixelFormatFromString("RGB24");
+ break;
+ case Teli::PXL_FMT_BGR8:
+ pfmt = pangolin::PixelFormatFromString("BGR24");
+ break;
+ default:
+ throw std::runtime_error("TeliSDK: Unknown pixel format: " + ToString<int>(teli_fmt) );
+ }
+
+ size_bytes = 0;
+
+ // Use width and height reported by camera
+ uint32_t w = 0;
+ uint32_t h = 0;
+ if( Teli::GetCamWidth(cam, &w) != Teli::CAM_API_STS_SUCCESS || Teli::GetCamHeight(cam, &h) != Teli::CAM_API_STS_SUCCESS) {
+ throw pangolin::VideoException("TeliSDK: Unable to establish stream dimensions.");
+ }
+
+ const int n = 1;
+ for(size_t c=0; c < n; ++c) {
+ const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp) / 8, 0);
+ streams.push_back(stream_info);
+ size_bytes += uiPyldSize;
+ }
+
+ InitPangoDeviceProperties();
+}
+
+void TeliVideo::InitPangoDeviceProperties()
+{
+
+ Teli::CAM_INFO info;
+ Teli::Cam_GetInformation(cam, 0, &info);
+
+ // Store camera details in device properties
+ device_properties["SerialNumber"] = std::string(info.szSerialNumber);
+ device_properties["VendorName"] = std::string(info.szManufacturer);
+ device_properties["ModelName"] = std::string(info.szModelName);
+ device_properties["ManufacturerInfo"] = std::string(info.sU3vCamInfo.szManufacturerInfo);
+ device_properties["Version"] = std::string(info.sU3vCamInfo.szDeviceVersion);
+ device_properties[PANGO_HAS_TIMING_DATA] = true;
+
+ // TODO: Enumerate other settings.
+}
+
+void TeliVideo::SetDeviceParams(const Params& p)
+{
+ for(Params::ParamMap::const_iterator it = p.params.begin(); it != p.params.end(); it++) {
+ if(it->first == "transfer_bandwidth_gbps") {
+ transfer_bandwidth_gbps = atof(it->second.c_str());
+ } else {
+ try{
+ if (it->second == "Execute") {
+ //
+ std::runtime_error("TeliSDK: Execution commands not yet supported.");
+ } else {
+ SetParameter(it->first, it->second);
+ }
+ }catch(std::exception& e) {
+ std::cerr << e.what() << std::endl;
+ }
+ }
+ }
+}
+
+TeliVideo::~TeliVideo()
+{
+ Teli::CAM_API_STATUS uiStatus = Teli::Strm_Close(strm);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ pango_print_warn("TeliSDK: Error closing camera stream.");
+
+ uiStatus = Teli::Cam_Close(cam);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ pango_print_warn("TeliSDK: Error closing camera.");
+}
+
+//! Implement VideoInput::Start()
+void TeliVideo::Start()
+{
+ Teli::CAM_API_STATUS uiStatus = Teli::Strm_Start(strm);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error starting stream.");
+}
+
+//! Implement VideoInput::Stop()
+void TeliVideo::Stop()
+{
+ Teli::CAM_API_STATUS uiStatus = Teli::Strm_Stop(strm);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error stopping stream.");
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t TeliVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& TeliVideo::Streams() const
+{
+ return streams;
+}
+
+void TeliVideo::PopulateEstimatedCenterCaptureTime(basetime host_reception_time)
+{
+ if(transfer_bandwidth_gbps) {
+ const float transfer_time_us = size_bytes / int64_t((transfer_bandwidth_gbps * 1E3) / 8.0);
+ frame_properties[PANGO_ESTIMATED_CENTER_CAPTURE_TIME_US] = picojson::value(int64_t(pangolin::Time_us(host_reception_time) - (exposure_us/2.0) - transfer_time_us));
+ }
+}
+
+bool TeliVideo::GrabNext(unsigned char* image, bool /*wait*/)
+{
+#ifdef _WIN_
+ unsigned int uiRet = WaitForSingleObject(hStrmCmpEvt, 2000);
+ if (uiRet == WAIT_OBJECT_0) {
+#endif
+#ifdef _LINUX_
+ unsigned int uiRet = Teli::Sys_WaitForSignal(hStrmCmpEvt, 2000);
+ if (uiRet == Teli::CAM_API_STS_SUCCESS) {
+#endif
+ Teli::CAM_IMAGE_INFO sImageInfo;
+ uint32_t uiPyldSize = (uint32_t)size_bytes;
+ Teli::CAM_API_STATUS uiStatus = Teli::Strm_ReadCurrentImage(strm, image, &uiPyldSize, &sImageInfo);
+ frame_properties[PANGO_EXPOSURE_US] = picojson::value(exposure_us);
+ frame_properties[PANGO_CAPTURE_TIME_US] = picojson::value(sImageInfo.ullTimestamp/1000);
+ basetime now = pangolin::TimeNow();
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(now));
+ PopulateEstimatedCenterCaptureTime(now);
+ return (uiStatus == Teli::CAM_API_STS_SUCCESS);
+ }
+
+ return false;
+}
+
+//! Implement VideoInput::GrabNewest()
+bool TeliVideo::GrabNewest(unsigned char* image, bool wait)
+{
+ return GrabNext(image,wait);
+}
+
+//! Returns number of available frames
+uint32_t TeliVideo::AvailableFrames() const
+{
+ uint32_t puiCount = 0;
+ Teli::CAM_API_STATUS uiStatus = Teli::GetCamImageBufferFrameCount(cam, &puiCount);
+ if (uiStatus != Teli::CAM_API_STS_SUCCESS)
+ throw pangolin::VideoException("TeliSDK: Error reading frame buffer frame count.");
+ return puiCount;
+}
+
+//! Drops N frames in the queue starting from the oldest
+//! returns false if less than n frames arae available
+bool TeliVideo::DropNFrames(uint32_t n)
+{
+ for (uint32_t i=0;i<n;++i) {
+#ifdef _WIN_
+ unsigned int uiRet = WaitForSingleObject(hStrmCmpEvt, 2000);
+ if (uiRet == WAIT_OBJECT_0) {
+#endif
+#ifdef _LINUX_
+ unsigned int uiRet = Teli::Sys_WaitForSignal(hStrmCmpEvt, 2000);
+ if (uiRet == Teli::CAM_API_STS_SUCCESS) {
+#endif
+ Teli::CAM_IMAGE_INFO sImageInfo;
+ uint32_t uiPyldSize = 0 ;
+ Teli::Strm_ReadCurrentImage(strm, 0, &uiPyldSize, &sImageInfo);
+ } else {
+ return false;
+ }
+ }
+ return true;
+}
+
+//! Access JSON properties of device
+const picojson::value& TeliVideo::DeviceProperties() const
+{
+ return device_properties;
+}
+
+//! Access JSON properties of most recently captured frame
+const picojson::value& TeliVideo::FrameProperties() const
+{
+ return frame_properties;
+}
+
+PANGOLIN_REGISTER_FACTORY(TeliVideo)
+{
+ struct TeliVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ return std::unique_ptr<VideoInterface>(new TeliVideo(uri));
+ }
+ };
+
+ auto factory = std::make_shared<TeliVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "teli");
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "u3v");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/test.cpp b/externals/Pangolin/src/video/drivers/test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be28202c400848713a7841816223bfeeb863d406
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/test.cpp
@@ -0,0 +1,111 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/test.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+void setRandomData(unsigned char * arr, size_t size){
+ for(size_t i = 0 ; i < size;i++) {
+// arr[i] = (unsigned char)(i * 255.0 / size);
+ arr[i] = (unsigned char)(rand()/(RAND_MAX/255.0));
+ }
+}
+
+TestVideo::TestVideo(size_t w, size_t h, size_t n, std::string pix_fmt)
+{
+ const PixelFormat pfmt = PixelFormatFromString(pix_fmt);
+
+ size_bytes = 0;
+
+ for(size_t c=0; c < n; ++c) {
+ const StreamInfo stream_info(pfmt, w, h, (w*pfmt.bpp)/8, 0);
+ streams.push_back(stream_info);
+ size_bytes += w*h*(pfmt.bpp)/8;
+ }
+}
+
+TestVideo::~TestVideo()
+{
+
+}
+
+//! Implement VideoInput::Start()
+void TestVideo::Start()
+{
+
+}
+
+//! Implement VideoInput::Stop()
+void TestVideo::Stop()
+{
+
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t TestVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& TestVideo::Streams() const
+{
+ return streams;
+}
+
+//! Implement VideoInput::GrabNext()
+bool TestVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ setRandomData(image, size_bytes);
+ return true;
+}
+
+//! Implement VideoInput::GrabNewest()
+bool TestVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image,wait);
+}
+
+PANGOLIN_REGISTER_FACTORY(TestVideo)
+{
+ struct TestVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const int n = uri.Get<int>("n", 1);
+ std::string fmt = uri.Get<std::string>("fmt","RGB24");
+ return std::unique_ptr<VideoInterface>(new TestVideo(dim.x,dim.y,n,fmt));
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<TestVideoFactory>(), 10, "test");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/thread.cpp b/externals/Pangolin/src/video/drivers/thread.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..be5baa7e862c5de29fd5f9228739b02a39754a0b
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/thread.cpp
@@ -0,0 +1,269 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/thread.h>
+#include <pangolin/video/iostream_operators.h>
+
+#ifdef DEBUGTHREAD
+ #include <pangolin/utils/timer.h>
+ #define TSTART() pangolin::basetime start,last,now; start = pangolin::TimeNow(); last = start;
+ #define TGRABANDPRINT(...) now = pangolin::TimeNow(); fprintf(stderr,"THREAD: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, " %fms.\n",1000*pangolin::TimeDiff_s(last, now)); last = now;
+ #define DBGPRINT(...) fprintf(stderr,"THREAD: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr,"\n");
+#else
+ #define TSTART()
+ #define TGRABANDPRINT(...)
+ #define DBGPRINT(...)
+#endif
+
+namespace pangolin
+{
+
+const uint64_t grab_fail_thread_sleep_us = 1000;
+const uint64_t capture_timout_ms = 5000;
+
+ThreadVideo::ThreadVideo(std::unique_ptr<VideoInterface> &src_, size_t num_buffers)
+ : src(std::move(src_)), quit_grab_thread(true)
+{
+ if(!src) {
+ throw VideoException("ThreadVideo: VideoInterface in must not be null");
+ }
+ videoin.push_back(src.get());
+
+// // queue init allocates buffers.
+// queue.init(num_buffers, (unsigned int)videoin[0]->SizeBytes());
+
+ for(size_t i=0; i < num_buffers; ++i)
+ {
+ queue.returnOrAddUsedBuffer( GrabResult(videoin[0]->SizeBytes()) );
+ }
+}
+
+ThreadVideo::~ThreadVideo()
+{
+ Stop();
+
+ src.reset();
+}
+
+//! Implement VideoInput::Start()
+void ThreadVideo::Start()
+{
+ // Only start thread if not already running.
+ if(quit_grab_thread) {
+ videoin[0]->Start();
+ quit_grab_thread = false;
+ grab_thread = std::thread(std::ref(*this));
+ }
+}
+
+//! Implement VideoInput::Stop()
+void ThreadVideo::Stop()
+{
+ quit_grab_thread = true;
+ if(grab_thread.joinable()) {
+ grab_thread.join();
+ }
+ videoin[0]->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t ThreadVideo::SizeBytes() const
+{
+ return videoin[0]->SizeBytes();
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& ThreadVideo::Streams() const
+{
+ return videoin[0]->Streams();
+}
+
+const picojson::value& ThreadVideo::DeviceProperties() const
+{
+ device_properties = GetVideoDeviceProperties(videoin[0]);
+ return device_properties;
+}
+
+const picojson::value& ThreadVideo::FrameProperties() const
+{
+ return frame_properties;
+}
+
+uint32_t ThreadVideo::AvailableFrames() const
+{
+ return (uint32_t)queue.AvailableFrames();
+}
+
+bool ThreadVideo::DropNFrames(uint32_t n)
+{
+ return queue.DropNFrames(n);
+}
+
+//! Implement VideoInput::GrabNext()
+bool ThreadVideo::GrabNext( unsigned char* image, bool wait )
+{
+ TSTART()
+
+ if(queue.EmptyBuffers() == 0) {
+ pango_print_warn("Thread %12p has run out of %d buffers\n", this, (int)queue.AvailableFrames());
+ }
+
+ if(queue.AvailableFrames() == 0 && !wait) {
+ // No frames available, no wait, simply return false.
+ DBGPRINT("GrabNext no available frames no wait.");
+ return false;
+ }else{
+ if(queue.AvailableFrames() == 0 && wait) {
+ // Must return a frame so block on notification from grab thread.
+ std::unique_lock<std::mutex> lk(cvMtx);
+ DBGPRINT("GrabNext no available frames wait for notification.");
+ if(cv.wait_for(lk, std::chrono::milliseconds(capture_timout_ms)) == std::cv_status::timeout)
+ {
+ pango_print_warn("ThreadVideo: GrabNext blocking read for frames reached timeout.");
+ return false;
+ }
+ }
+
+ // At least one valid frame in queue, return it.
+ GrabResult grab = queue.getNext();
+ if(grab.return_status) {
+ DBGPRINT("GrabNext at least one frame available.");
+ std::memcpy(image, grab.buffer.get(), videoin[0]->SizeBytes());
+ frame_properties = grab.frame_properties;
+ }else{
+ DBGPRINT("GrabNext returned false")
+ }
+ queue.returnOrAddUsedBuffer(std::move(grab));
+
+ TGRABANDPRINT("GrabNext took")
+ return grab.return_status;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool ThreadVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ TSTART()
+ if(queue.AvailableFrames() == 0 && !wait) {
+ // No frames available, no wait, simply return false.
+ DBGPRINT("GrabNext no available frames no wait.");
+ return false;
+ }else{
+ if(queue.AvailableFrames() == 0 && wait) {
+ // Must return a frame so block on notification from grab thread.
+ std::unique_lock<std::mutex> lk(cvMtx);
+ DBGPRINT("GrabNewest no available frames wait for notification.");
+ if(cv.wait_for(lk, std::chrono::milliseconds(capture_timout_ms)) == std::cv_status::timeout)
+ {
+ pango_print_warn("ThreadVideo: GrabNext blocking read for frames reached timeout.");
+ return false;
+ }
+ }
+
+ // At least one valid frame in queue, return it.
+ DBGPRINT("GrabNewest at least one frame available.");
+ GrabResult grab = queue.getNewest();
+ const bool success = grab.return_status;
+ if(success) {
+ std::memcpy(image, grab.buffer.get(), videoin[0]->SizeBytes());
+ frame_properties = grab.frame_properties;
+ }
+ queue.returnOrAddUsedBuffer(std::move(grab));
+ TGRABANDPRINT("GrabNewest memcpy of available frame took")
+
+ return success;
+ }
+}
+
+void ThreadVideo::operator()()
+{
+ DBGPRINT("Grab thread Started.")
+ // Spinning thread attempting to read from videoin[0] as fast as possible
+ // relying on the videoin[0] blocking grab.
+ while(!quit_grab_thread) {
+ // Get a buffer from the queue;
+
+ if(queue.EmptyBuffers() > 0) {
+ GrabResult grab = queue.getFreeBuffer();
+
+ // Blocking grab (i.e. GrabNext with wait = true).
+ try{
+ grab.return_status = videoin[0]->GrabNext(grab.buffer.get(), true);
+ }catch(const VideoException& e) {
+ // User doesn't have the opportunity to catch exceptions here.
+ pango_print_warn("ThreadVideo caught VideoException (%s)\n", e.what());
+ grab.return_status = false;
+ }catch(const std::exception& e){
+ // User doesn't have the opportunity to catch exceptions here.
+ pango_print_warn("ThreadVideo caught exception (%s)\n", e.what());
+ grab.return_status = false;
+ }
+
+ if(grab.return_status){
+ grab.frame_properties = GetVideoFrameProperties(videoin[0]);
+ }else{
+ std::this_thread::sleep_for(std::chrono::microseconds(grab_fail_thread_sleep_us) );
+ }
+ queue.addValidBuffer(std::move(grab));
+
+ DBGPRINT("Grab thread got frame. valid:%d free:%d",queue.AvailableFrames(),queue.EmptyBuffers())
+ // Let listening threads know we got a frame in case they are waiting.
+ cv.notify_all();
+ }else{
+ std::this_thread::sleep_for(std::chrono::microseconds(grab_fail_thread_sleep_us) );
+ }
+ std::this_thread::yield();
+ }
+ DBGPRINT("Grab thread Stopped.")
+
+ return;
+}
+
+std::vector<VideoInterface*>& ThreadVideo::InputStreams()
+{
+ return videoin;
+}
+
+PANGOLIN_REGISTER_FACTORY(ThreadVideo)
+{
+ struct ThreadVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ const int num_buffers = uri.Get<int>("num_buffers", 30);
+ return std::unique_ptr<VideoInterface>(new ThreadVideo(subvid, num_buffers));
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<ThreadVideoFactory>(), 10, "thread");
+}
+
+}
+
+#undef TSTART
+#undef TGRABANDPRINT
+#undef DBGPRINT
diff --git a/externals/Pangolin/src/video/drivers/truncate.cpp b/externals/Pangolin/src/video/drivers/truncate.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c00502836da96f65803c1e2d6b11f6fe95aadd6c
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/truncate.cpp
@@ -0,0 +1,110 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/truncate.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+TruncateVideo::TruncateVideo(std::unique_ptr<VideoInterface> &src_, size_t begin, size_t end)
+ : src(std::move(src_)), streams(src->Streams()), begin(begin), end(end), next_frame_to_grab(0)
+{
+ videoin.push_back(src.get());
+
+ if(VideoPlaybackInterface* v = GetVideoPlaybackInterface()){
+ // Guard against the obscure case of nested TruncateVideo filters
+ if( !pangolin::FindFirstMatchingVideoInterface<TruncateVideo>(*src_) ) {
+ next_frame_to_grab = v->Seek(begin);
+ }
+ }
+}
+
+TruncateVideo::~TruncateVideo()
+{
+}
+
+size_t TruncateVideo::SizeBytes() const
+{
+ return videoin[0]->SizeBytes();
+}
+
+const std::vector<StreamInfo>& TruncateVideo::Streams() const
+{
+ return streams;
+}
+
+void TruncateVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+void TruncateVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+bool TruncateVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(next_frame_to_grab < end) {
+ bool grab_success = videoin[0]->GrabNext(image, wait);
+ return grab_success && (next_frame_to_grab++) >= begin;
+ }
+ return false;
+}
+
+bool TruncateVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return videoin[0]->GrabNewest(image, wait);
+}
+
+std::vector<VideoInterface*>& TruncateVideo::InputStreams()
+{
+ return videoin;
+}
+
+PANGOLIN_REGISTER_FACTORY(TruncateVideo)
+{
+ struct TruncateVideoFactory : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ if(subvid->Streams().size() == 0) {
+ throw VideoException("VideoTruncater input must have at least one stream");
+ }
+
+ const size_t start = uri.Get<size_t>("begin", 0);
+ const size_t end = uri.Get<size_t>("end", std::numeric_limits<size_t>::max());
+
+ return std::unique_ptr<VideoInterface>( new TruncateVideo(subvid,start,end) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<TruncateVideoFactory>(), 10, "truncate");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/unpack.cpp b/externals/Pangolin/src/video/drivers/unpack.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..3e32435e69e9d4bfa9b0ffddd3882c7b5e18f13a
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/unpack.cpp
@@ -0,0 +1,268 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2014 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/drivers/unpack.h>
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/iostream_operators.h>
+
+#ifdef DEBUGUNPACK
+ #include <pangolin/utils/timer.h>
+ #define TSTART() pangolin::basetime start,last,now; start = pangolin::TimeNow(); last = start;
+ #define TGRABANDPRINT(...) now = pangolin::TimeNow(); fprintf(stderr,"UNPACK: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr, " %fms.\n",1000*pangolin::TimeDiff_s(last, now)); last = now;
+ #define DBGPRINT(...) fprintf(stderr,"UNPACK: "); fprintf(stderr, __VA_ARGS__); fprintf(stderr,"\n");
+#else
+ #define TSTART()
+ #define TGRABANDPRINT(...)
+ #define DBGPRINT(...)
+#endif
+
+namespace pangolin
+{
+
+UnpackVideo::UnpackVideo(std::unique_ptr<VideoInterface> &src_, PixelFormat out_fmt)
+ : src(std::move(src_)), size_bytes(0), buffer(0)
+{
+ if( !src || out_fmt.channels != 1) {
+ throw VideoException("UnpackVideo: Only supports single channel output.");
+ }
+
+ videoin.push_back(src.get());
+
+ for(size_t s=0; s< src->Streams().size(); ++s) {
+ const size_t w = src->Streams()[s].Width();
+ const size_t h = src->Streams()[s].Height();
+
+ // Check compatibility of formats
+ const PixelFormat in_fmt = src->Streams()[s].PixFormat();
+ if(in_fmt.channels > 1 || in_fmt.bpp > 16) {
+ throw VideoException("UnpackVideo: Only supports one channel input.");
+ }
+
+ const size_t pitch = (w*out_fmt.bpp)/ 8;
+ streams.push_back(pangolin::StreamInfo( out_fmt, w, h, pitch, (unsigned char*)0 + size_bytes ));
+ size_bytes += h*pitch;
+ }
+
+ buffer = new unsigned char[src->SizeBytes()];
+}
+
+UnpackVideo::~UnpackVideo()
+{
+ delete[] buffer;
+}
+
+//! Implement VideoInput::Start()
+void UnpackVideo::Start()
+{
+ videoin[0]->Start();
+}
+
+//! Implement VideoInput::Stop()
+void UnpackVideo::Stop()
+{
+ videoin[0]->Stop();
+}
+
+//! Implement VideoInput::SizeBytes()
+size_t UnpackVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+//! Implement VideoInput::Streams()
+const std::vector<StreamInfo>& UnpackVideo::Streams() const
+{
+ return streams;
+}
+
+template<typename T>
+void ConvertFrom8bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ T* pout = (T*)(out.ptr + r*out.pitch);
+ uint8_t* pin = in.ptr + r*in.pitch;
+ const uint8_t* pin_end = in.ptr + (r+1)*in.pitch;
+ while(pin != pin_end) {
+ *(pout++) = *(pin++);
+ }
+ }
+}
+
+template<typename T>
+void ConvertFrom10bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ T* pout = (T*)(out.ptr + r*out.pitch);
+ uint8_t* pin = in.ptr + r*in.pitch;
+ const uint8_t* pin_end = in.ptr + (r+1)*in.pitch;
+ while(pin != pin_end) {
+ uint64_t val = *(pin++);
+ val |= uint64_t(*(pin++)) << 8;
+ val |= uint64_t(*(pin++)) << 16;
+ val |= uint64_t(*(pin++)) << 24;
+ val |= uint64_t(*(pin++)) << 32;
+ *(pout++) = T( val & 0x00000003FF);
+ *(pout++) = T((val & 0x00000FFC00) >> 10);
+ *(pout++) = T((val & 0x003FF00000) >> 20);
+ *(pout++) = T((val & 0xFFC0000000) >> 30);
+ }
+ }
+}
+
+template<typename T>
+void ConvertFrom12bit(
+ Image<unsigned char>& out,
+ const Image<unsigned char>& in
+) {
+ for(size_t r=0; r<out.h; ++r) {
+ T* pout = (T*)(out.ptr + r*out.pitch);
+ uint8_t* pin = in.ptr + r*in.pitch;
+ const uint8_t* pin_end = in.ptr + (r+1)*in.pitch;
+ while(pin != pin_end) {
+ uint32_t val = *(pin++);
+ val |= uint32_t(*(pin++)) << 8;
+ val |= uint32_t(*(pin++)) << 16;
+ *(pout++) = T( val & 0x000FFF);
+ *(pout++) = T((val & 0xFFF000) >> 12);
+ }
+ }
+}
+
+void UnpackVideo::Process(unsigned char* image, const unsigned char* buffer)
+{
+ TSTART()
+ for(size_t s=0; s<streams.size(); ++s) {
+ const Image<unsigned char> img_in = videoin[0]->Streams()[s].StreamImage(buffer);
+ Image<unsigned char> img_out = Streams()[s].StreamImage(image);
+
+ const int bits_in = videoin[0]->Streams()[s].PixFormat().bpp;
+
+ if(Streams()[s].PixFormat().format == "GRAY32F") {
+ if( bits_in == 8) {
+ ConvertFrom8bit<float>(img_out, img_in);
+ }else if( bits_in == 10) {
+ ConvertFrom10bit<float>(img_out, img_in);
+ }else if( bits_in == 12){
+ ConvertFrom12bit<float>(img_out, img_in);
+ }else{
+ throw pangolin::VideoException("Unsupported bitdepths.");
+ }
+ }else if(Streams()[s].PixFormat().format == "GRAY16LE") {
+ if( bits_in == 8) {
+ ConvertFrom8bit<uint16_t>(img_out, img_in);
+ }else if( bits_in == 10) {
+ ConvertFrom10bit<uint16_t>(img_out, img_in);
+ }else if( bits_in == 12){
+ ConvertFrom12bit<uint16_t>(img_out, img_in);
+ }else{
+ throw pangolin::VideoException("Unsupported bitdepths.");
+ }
+ }else{
+ }
+ }
+ TGRABANDPRINT("Unpacking took ")
+}
+
+//! Implement VideoInput::GrabNext()
+bool UnpackVideo::GrabNext( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNext(buffer,wait)) {
+ Process(image,buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+//! Implement VideoInput::GrabNewest()
+bool UnpackVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ if(videoin[0]->GrabNewest(buffer,wait)) {
+ Process(image,buffer);
+ return true;
+ }else{
+ return false;
+ }
+}
+
+std::vector<VideoInterface*>& UnpackVideo::InputStreams()
+{
+ return videoin;
+}
+
+unsigned int UnpackVideo::AvailableFrames() const
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Unpack: child interface is not buffer aware.");
+ return 0;
+ }
+ else
+ {
+ return vpi->AvailableFrames();
+ }
+}
+
+bool UnpackVideo::DropNFrames(uint32_t n)
+{
+ BufferAwareVideoInterface* vpi = dynamic_cast<BufferAwareVideoInterface*>(videoin[0]);
+ if(!vpi)
+ {
+ pango_print_warn("Unpack: child interface is not buffer aware.");
+ return false;
+ }
+ else
+ {
+ return vpi->DropNFrames(n);
+ }
+}
+
+PANGOLIN_REGISTER_FACTORY(UnpackVideo)
+{
+ struct UnpackVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ std::unique_ptr<VideoInterface> subvid = pangolin::OpenVideo(uri.url);
+ const std::string fmt = uri.Get("fmt", std::string("GRAY16LE") );
+ return std::unique_ptr<VideoInterface>(
+ new UnpackVideo(subvid, PixelFormatFromString(fmt) )
+ );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<UnpackVideoFactory>(), 10, "unpack");
+}
+
+}
+
+#undef TSTART
+#undef TGRABANDPRINT
+#undef DBGPRINT
diff --git a/externals/Pangolin/src/video/drivers/uvc.cpp b/externals/Pangolin/src/video/drivers/uvc.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..2aa53ac2cd3f1aaf03f4d0cf998302c8d75a469d
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/uvc.cpp
@@ -0,0 +1,364 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/drivers/uvc.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+UvcVideo::UvcVideo(int vendor_id, int product_id, const char* sn, int device_id, int width, int height, int fps)
+ : ctx_(NULL),
+ dev_(NULL),
+ devh_(NULL),
+ frame_(NULL),
+ is_streaming(false)
+{
+ uvc_init(&ctx_, NULL);
+ if(!ctx_) {
+ throw VideoException("Unable to open UVC Context");
+ }
+
+ InitDevice(vendor_id, product_id, sn, device_id, width, height, fps);
+ InitPangoDeviceProperties();
+
+ // FIX: CRASHING IF WE DON'T START STREAMING STRAIGHT AWAY
+
+ Start();
+}
+
+UvcVideo::~UvcVideo()
+{
+ DeinitDevice();
+
+ if(devh_) uvc_close(devh_);
+ if(dev_) uvc_unref_device(dev_);
+
+ if (ctx_) {
+ uvc_exit(ctx_);
+ ctx_ = 0;
+ }
+}
+
+uvc_error_t UvcVideo::FindDevice(
+ uvc_context_t *ctx, uvc_device_t **dev,
+ int vid, int pid, const char *sn, int device_id) {
+ uvc_error_t ret = UVC_SUCCESS;
+
+ uvc_device_t **list;
+ uvc_device_t *test_dev;
+
+ ret = uvc_get_device_list(ctx, &list);
+
+ int cnt = 0;
+ while(list[cnt++]!=NULL);
+ pango_print_info("UVC Descriptor list contains %d devices.\n", (cnt-1));
+
+ if (ret != UVC_SUCCESS) {
+ return ret;
+ }
+
+ int dev_idx = 0;
+ int num_found = 0;
+ bool found_dev = false;
+
+ while (!found_dev && (test_dev = list[dev_idx++]) != NULL) {
+ uvc_device_descriptor_t *desc;
+
+ if (uvc_get_device_descriptor(test_dev, &desc) != UVC_SUCCESS)
+ continue;
+
+
+ const bool matches = (!vid || desc->idVendor == vid)
+ && (!pid || desc->idProduct == pid)
+ && (!sn || (desc->serialNumber && !strcmp(desc->serialNumber, sn)));
+
+ uvc_free_device_descriptor(desc);
+
+ if (matches) {
+ if(device_id == num_found) {
+ found_dev = true;
+ break;
+ }
+ num_found++;
+ }
+ }
+
+ if (found_dev)
+ uvc_ref_device(test_dev);
+
+ uvc_free_device_list(list, 1);
+
+ if (found_dev) {
+ *dev = test_dev;
+ return UVC_SUCCESS;
+ } else {
+ return UVC_ERROR_NO_DEVICE;
+ }
+}
+
+void UvcVideo::InitDevice(int vid, int pid, const char* sn, int device_id, int width, int height, int fps)
+{
+ uvc_error_t find_err = FindDevice(ctx_, &dev_, vid, pid, sn, device_id );
+ if (find_err != UVC_SUCCESS) {
+ uvc_perror(find_err, "uvc_find_device");
+ throw VideoException("Unable to open UVC Device");
+ }
+ if(!dev_) {
+ throw VideoException("Unable to open UVC Device - no pointer returned.");
+ }
+
+ uvc_error_t open_err = uvc_open(dev_, &devh_);
+ if (open_err != UVC_SUCCESS) {
+ uvc_perror(open_err, "uvc_open");
+ uvc_unref_device(dev_);
+ throw VideoException("Unable to open device");
+ }
+
+ //uvc_print_diag(devh_, stderr);
+
+ uvc_error_t mode_err = uvc_get_stream_ctrl_format_size(
+ devh_, &ctrl_,
+ UVC_FRAME_FORMAT_ANY,
+ width, height,
+ fps);
+
+ //uvc_print_stream_ctrl(&ctrl_, stderr);
+
+ if (mode_err != UVC_SUCCESS) {
+ uvc_perror(mode_err, "uvc_get_stream_ctrl_format_size");
+ uvc_close(devh_);
+ uvc_unref_device(dev_);
+ throw VideoException("Unable to set device mode.");
+ }
+
+ uvc_error_t strm_err = uvc_stream_open_ctrl(devh_, &strm_, &ctrl_);
+ if(strm_err != UVC_SUCCESS) {
+ uvc_perror(strm_err, "uvc_stream_open_ctrl");
+ uvc_close(devh_);
+ uvc_unref_device(dev_);
+ throw VideoException("Unable to open device stream.");
+ }
+
+ // Default to greyscale.
+ PixelFormat pfmt = PixelFormatFromString("GRAY8");
+
+ const uvc_format_desc_t* uvc_fmt = uvc_get_format_descs(devh_);
+ while( uvc_fmt->bFormatIndex != ctrl_.bFormatIndex && uvc_fmt ) {
+ uvc_fmt = uvc_fmt->next;
+ }
+
+ if(uvc_fmt) {
+ // TODO: Use uvc_fmt->fourccFormat
+ if( uvc_fmt->bBitsPerPixel == 16 ) {
+ pfmt = PixelFormatFromString("GRAY16LE");
+ }
+ }
+
+ const StreamInfo stream_info(pfmt, width, height, (width*pfmt.bpp)/8, 0);
+ streams.push_back(stream_info);
+}
+
+void UvcVideo::InitPangoDeviceProperties()
+{
+ // Store camera details in device properties
+ device_properties["BusNumber"] = std::to_string(uvc_get_bus_number(dev_));
+ device_properties["DeviceAddress"] = std::to_string(uvc_get_device_address(dev_));
+ device_properties[PANGO_HAS_TIMING_DATA] = true;
+}
+
+void UvcVideo::DeinitDevice()
+{
+ Stop();
+
+ if (frame_) {
+ uvc_free_frame(frame_);
+ frame_ = 0;
+ }
+}
+
+void UvcVideo::Start()
+{
+ if(!is_streaming) {
+ uvc_error_t stream_err = uvc_stream_start(strm_, NULL, this, 0);
+
+ if (stream_err != UVC_SUCCESS) {
+ uvc_perror(stream_err, "uvc_stream_start");
+ uvc_close(devh_);
+ uvc_unref_device(dev_);
+ throw VideoException("Unable to start streaming.");
+ }else{
+ is_streaming = true;
+ }
+
+ if (frame_) {
+ uvc_free_frame(frame_);
+ }
+
+ size_bytes = ctrl_.dwMaxVideoFrameSize;
+ frame_ = uvc_allocate_frame(size_bytes);
+ if(!frame_) {
+ throw VideoException("Unable to allocate frame.");
+ }
+ }
+}
+
+void UvcVideo::Stop()
+{
+ if(is_streaming && devh_) {
+ uvc_stop_streaming(devh_);
+ }
+ is_streaming = false;
+}
+
+size_t UvcVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+const std::vector<StreamInfo>& UvcVideo::Streams() const
+{
+ return streams;
+}
+
+bool UvcVideo::GrabNext( unsigned char* image, bool wait )
+{
+ uvc_frame_t* frame = NULL;
+ uvc_error_t err = uvc_stream_get_frame(strm_, &frame, wait ? 0 : -1);
+
+ if(err!= UVC_SUCCESS) {
+ pango_print_error("UvcVideo Error: %s", uvc_strerror(err) );
+ return false;
+ }else{
+ if(frame) {
+ memcpy(image, frame->data, frame->data_bytes );
+ // This is a hack, this ts sould come from the device.
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(pangolin::TimeNow()));
+ return true;
+ }else{
+ if(wait) {
+ pango_print_debug("UvcVideo: No frame data");
+ }
+ return false;
+ }
+ }
+}
+
+bool UvcVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ return GrabNext(image, wait);
+}
+
+int UvcVideo::IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code)
+{
+ if(req_code == UVC_SET_CUR) {
+ return uvc_set_ctrl(devh_, unit, ctrl, data, len);
+ }else{
+ return uvc_get_ctrl(devh_, unit, ctrl, data, len, (uvc_req_code)req_code);
+ }
+}
+
+bool UvcVideo::SetExposure(int exp_us)
+{
+ uint32_t e = uint32_t(exp_us);
+
+ if (uvc_set_exposure_abs(devh_, e) < 0) {
+ pango_print_warn("UvcVideo::setExposure() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool UvcVideo::GetExposure(int& exp_us)
+{
+ uint32_t e;
+ if (uvc_get_exposure_abs(devh_, &e, uvc_req_code::UVC_GET_CUR) < 0) {
+ pango_print_warn("UvcVideo::GetExposure() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ exp_us = e;
+ return true;
+ }
+}
+
+bool UvcVideo::SetGain(float gain)
+{
+ uint16_t g = uint16_t(gain);
+
+ if (uvc_set_gain(devh_, g) < 0) {
+ pango_print_warn("UvcVideo::setGain() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool UvcVideo::GetGain(float& gain)
+{
+ uint16_t g;
+ if (uvc_get_gain(devh_, &g, uvc_req_code::UVC_GET_CUR) < 0) {
+ pango_print_warn("UvcVideo::GetGain() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ gain = g;
+ return true;
+ }
+}
+
+//! Access JSON properties of device
+const picojson::value& UvcVideo::DeviceProperties() const
+{
+ return device_properties;
+}
+
+//! Access JSON properties of most recently captured frame
+const picojson::value& UvcVideo::FrameProperties() const
+{
+ return frame_properties;
+}
+
+PANGOLIN_REGISTER_FACTORY(UvcVideo)
+{
+ struct UvcVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ int vid = 0;
+ int pid = 0;
+ std::istringstream(uri.Get<std::string>("vid","0x0000")) >> std::hex >> vid;
+ std::istringstream(uri.Get<std::string>("pid","0x0000")) >> std::hex >> pid;
+ const unsigned int dev_id = uri.Get<int>("num",0);
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640,480));
+ const unsigned int fps = uri.Get<unsigned int>("fps", 0); // 0 means unspecified.
+ return std::unique_ptr<VideoInterface>( new UvcVideo(vid,pid,0,dev_id,dim.x,dim.y,fps) );
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<UvcVideoFactory>(), 10, "uvc");
+}
+
+}
diff --git a/externals/Pangolin/src/video/drivers/uvc_mediafoundation.cpp b/externals/Pangolin/src/video/drivers/uvc_mediafoundation.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..122c71fa4391d5fcdea1b1d01929fe42c8dbb904
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/uvc_mediafoundation.cpp
@@ -0,0 +1,660 @@
+#ifndef NOMINMAX
+# define NOMINMAX
+#endif
+
+#include <mfapi.h>
+#include <mferror.h>
+#include <mfidl.h>
+#include <mfreadwrite.h>
+
+#include <dshow.h>
+#include <ks.h>
+#include <ksmedia.h>
+#include <ksproxy.h>
+#include <vidcap.h>
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/video/drivers/uvc_mediafoundation.h>
+#include <pangolin/video/iostream_operators.h>
+
+namespace pangolin
+{
+
+static constexpr DWORD KS_CONTROL_NODE_ID_INVALID = ~0;
+
+const GUID GUID_EXTENSION_UNIT_DESCRIPTOR_OV580{
+ 0x2ccb0bda, 0x6331, 0x4fdb, 0x85, 0x0e, 0x79, 0x05, 0x4d, 0xbd, 0x56, 0x71};
+
+UvcMediaFoundationVideo::UvcMediaFoundationVideo(int vendorId, int productId, int deviceId, size_t width, size_t height, int fps)
+ : size_bytes(0),
+ mediaSource(nullptr),
+ sourceReader(nullptr),
+ baseFilter(nullptr),
+ ksControl(nullptr),
+ ksControlNodeId(KS_CONTROL_NODE_ID_INVALID)
+{
+ if(FAILED(CoInitializeEx(nullptr, COINIT_APARTMENTTHREADED)))
+ {
+ throw VideoException("CoInitializeEx failed");
+ }
+
+ if(FAILED(MFStartup(MF_VERSION)))
+ {
+ throw VideoException("MfStartup failed");
+ }
+
+ if(!FindDevice(vendorId, productId, deviceId))
+ {
+ throw VideoException("Unable to open UVC media source");
+ }
+ InitDevice(width, height, fps);
+
+ device_properties[PANGO_HAS_TIMING_DATA] = true;
+}
+
+UvcMediaFoundationVideo::~UvcMediaFoundationVideo()
+{
+ DeinitDevice();
+ HRESULT hr = MFShutdown();
+ if(FAILED(hr))
+ {
+ pango_print_warn("MFShutdown failed with result %X", hr);
+ }
+ CoUninitialize();
+}
+
+void UvcMediaFoundationVideo::Start()
+{
+
+}
+
+void UvcMediaFoundationVideo::Stop()
+{
+}
+
+size_t UvcMediaFoundationVideo::SizeBytes() const
+{
+ return size_bytes;
+}
+
+const std::vector<pangolin::StreamInfo>& UvcMediaFoundationVideo::Streams() const
+{
+ return streams;
+}
+
+bool UvcMediaFoundationVideo::GrabNext(unsigned char* image, bool wait)
+{
+ IMFSample* sample = nullptr;
+ DWORD streamIndex = 0;
+ DWORD flags = 0;
+ LONGLONG timeStamp;
+
+ HRESULT hr = sourceReader->ReadSample(
+ (DWORD)MF_SOURCE_READER_FIRST_VIDEO_STREAM, 0, &streamIndex, &flags, &timeStamp, &sample);
+ if(SUCCEEDED(hr))
+ {
+ if((flags & MF_SOURCE_READERF_ENDOFSTREAM) != 0)
+ {
+ return false;
+ }
+
+ if(!sample)
+ {
+ return false;
+ }
+ }
+
+ IMFMediaBuffer* mediaBuffer = nullptr;
+ if(SUCCEEDED(hr))
+ {
+ hr = sample->ConvertToContiguousBuffer(&mediaBuffer);
+ }
+ if(SUCCEEDED(hr))
+ {
+ // Use the 2D buffer interface if it's available
+ IMF2DBuffer* mediaBuffer2d = nullptr;
+ hr = mediaBuffer->QueryInterface(&mediaBuffer2d);
+ if(SUCCEEDED(hr))
+ {
+ hr = mediaBuffer2d->ContiguousCopyTo(image, (DWORD)size_bytes);
+ mediaBuffer2d->Release();
+ mediaBuffer2d = nullptr;
+ }
+ else
+ {
+ // No 2D buffer is available
+ byte* buffer;
+ DWORD bufferSize = 0;
+ hr = mediaBuffer->Lock(&buffer, nullptr, &bufferSize);
+ if(SUCCEEDED(hr))
+ {
+ size_t copySize = std::min((size_t)bufferSize, size_bytes);
+ memcpy(image, buffer, copySize);
+ mediaBuffer->Unlock();
+ }
+ }
+ }
+
+ if(SUCCEEDED(hr))
+ {
+ using namespace std::chrono;
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(pangolin::TimeNow()));
+ }
+
+ if(mediaBuffer)
+ {
+ mediaBuffer->Release();
+ mediaBuffer = nullptr;
+ }
+
+ if(sample)
+ {
+ sample->Release();
+ sample = nullptr;
+ }
+
+ return SUCCEEDED(hr);
+}
+
+bool UvcMediaFoundationVideo::GrabNewest(unsigned char* image, bool wait)
+{
+ return GrabNext(image, wait);
+}
+
+int UvcMediaFoundationVideo::IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code)
+{
+ if(!ksControl || ksControlNodeId == KS_CONTROL_NODE_ID_INVALID)
+ {
+ return -1;
+ }
+
+ HRESULT hr;
+ KSP_NODE s = {};
+ ULONG ulBytesReturned;
+
+ s.Property.Set = GUID_EXTENSION_UNIT_DESCRIPTOR_OV580;
+ s.Property.Id = 2;
+ s.NodeId = ksControlNodeId;
+
+ s.Property.Flags = KSPROPERTY_TYPE_TOPOLOGY;
+ if(req_code == UVC_GET_CUR)
+ {
+ s.Property.Flags |= KSPROPERTY_TYPE_GET;
+ }
+ else if(req_code == UVC_SET_CUR)
+ {
+ s.Property.Flags |= KSPROPERTY_TYPE_SET;
+ }
+ hr = ksControl->KsProperty((PKSPROPERTY)&s, sizeof(s), &data[0], len, &ulBytesReturned);
+ if(FAILED(hr))
+ {
+ pango_print_error("KsProperty failed on UVC device");
+ return -1;
+ }
+
+ return 0;
+}
+
+bool UvcMediaFoundationVideo::GetExposure(int& exp_us)
+{
+ pango_print_warn("GetExposure not implemented for UvcMediaFoundationVideo");
+ return false;
+}
+
+bool UvcMediaFoundationVideo::SetExposure(int exp_us)
+{
+ pango_print_warn("SetExposure not implemented for UvcMediaFoundationVideo");
+ return false;
+}
+
+bool UvcMediaFoundationVideo::GetGain(float& gain)
+{
+ pango_print_warn("GetGain not implemented for UvcMediaFoundationVideo");
+ return false;
+}
+
+bool UvcMediaFoundationVideo::SetGain(float gain)
+{
+ pango_print_warn("SetGain not implemented for UvcMediaFoundationVideo");
+ return false;
+}
+
+
+const picojson::value& UvcMediaFoundationVideo::DeviceProperties() const
+{
+ return device_properties;
+}
+
+const picojson::value& UvcMediaFoundationVideo::FrameProperties() const
+{
+ return frame_properties;
+}
+
+bool UvcMediaFoundationVideo::FindDevice(int vendorId, int productId, int deviceId)
+{
+ // Create attributes for finding UVC devices
+ IMFAttributes* searchAttributes = nullptr;
+ HRESULT hr = MFCreateAttributes(&searchAttributes, 1);
+ if(FAILED(hr))
+ {
+ pango_print_error("Unable to create UVC device search attributes");
+ }
+
+ // Request video capture devices
+ if(SUCCEEDED(hr))
+ {
+ hr = searchAttributes->SetGUID(MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE,
+ MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_GUID);
+ if(FAILED(hr))
+ {
+ pango_print_error("Unable to set UVC source type attribute");
+ }
+ }
+
+ // Enumerate the devices
+ IMFActivate** devices = nullptr;
+ UINT32 deviceCount = 0;
+ if(SUCCEEDED(hr))
+ {
+ hr = MFEnumDeviceSources(searchAttributes, &devices, &deviceCount);
+ if(FAILED(hr))
+ {
+ pango_print_error("Unable to enumerate UVC device sources");
+ }
+ }
+
+ std::wstring symLink;
+ bool activatedDevice = false;
+ if(SUCCEEDED(hr))
+ {
+ for(UINT32 i = 0; i < deviceCount; ++i)
+ {
+ // Get this device's sym link
+ WCHAR* symLinkWCStr = nullptr;
+ UINT32 symLinkLength = 0;
+ hr = devices[i]->GetAllocatedString(
+ MF_DEVSOURCE_ATTRIBUTE_SOURCE_TYPE_VIDCAP_SYMBOLIC_LINK, &symLinkWCStr, &symLinkLength);
+ if(FAILED(hr))
+ {
+ hr = S_OK;
+ continue;
+ }
+
+ std::wstring checkSymLink(symLinkWCStr);
+
+ // Check if this device matches the requested vendor ID and product ID
+ if(!DeviceMatches(checkSymLink, vendorId, productId))
+ {
+ continue;
+ }
+
+ if(deviceId == i)
+ {
+ hr = devices[i]->ActivateObject(IID_PPV_ARGS(&mediaSource));
+ activatedDevice = SUCCEEDED(hr);
+ if(activatedDevice)
+ {
+ symLink = std::move(checkSymLink);
+ }
+ break;
+ }
+ }
+
+ if(!activatedDevice)
+ {
+ pango_print_error("Unable to activate UVC device source");
+ hr = E_FAIL;
+ }
+ }
+
+ for(UINT32 i = 0; i < deviceCount; ++i)
+ {
+ devices[i]->Release();
+ devices[i] = nullptr;
+ }
+ devices = nullptr;
+
+ CoTaskMemFree(devices);
+
+ if(searchAttributes != nullptr)
+ {
+ searchAttributes->Release();
+ searchAttributes = nullptr;
+ }
+
+ // Find the DirectShow device
+ ICreateDevEnum* dshowDevices = nullptr;
+ if(SUCCEEDED(hr))
+ {
+ hr = CoCreateInstance(CLSID_SystemDeviceEnum, 0, CLSCTX_INPROC, IID_ICreateDevEnum, (void**)&dshowDevices);
+ }
+
+ IEnumMoniker* videoInputEnumerator = nullptr;
+ if(SUCCEEDED(hr))
+ {
+ hr = dshowDevices->CreateClassEnumerator(CLSID_VideoInputDeviceCategory, &videoInputEnumerator, 0);
+ }
+
+ if(SUCCEEDED(hr))
+ {
+ IMoniker* moniker = nullptr;
+ while((hr = videoInputEnumerator->Next(1, &moniker, 0)) == S_OK)
+ {
+ IPropertyBag* propertyBag = nullptr;
+ hr = moniker->BindToStorage(0, 0, IID_IPropertyBag, (void**)&propertyBag);
+ if(FAILED(hr))
+ {
+ moniker->Release();
+ moniker = nullptr;
+ continue;
+ }
+
+ VARIANT variantPath;
+ VariantInit(&variantPath);
+ hr = propertyBag->Read(L"DevicePath", &variantPath, nullptr);
+
+ bool pathMatches;
+ if(SUCCEEDED(hr) && variantPath.vt == VT_BSTR)
+ {
+ // Determine if this is the correct device by comparing its path against the source's symbolic link
+ // This breaks the rules, but it seems to be the only way to make sure it's the correct device
+
+ // DirectShow and MediaFoundation appear to each create their own symbolic link which contains a GUID
+ // Ignore the GUID portion of the link, leaving just the path information
+ size_t braceOffset = symLink.find(L'{');
+
+ pathMatches = 0 == std::wcsncmp(symLink.c_str(), variantPath.bstrVal, braceOffset);
+ }
+ else
+ {
+ pathMatches = false;
+ }
+
+ VARIANT variantFriendlyName;
+ VariantInit(&variantFriendlyName);
+ hr = propertyBag->Read(L"FriendlyName", &variantFriendlyName, nullptr);
+
+ if(!pathMatches)
+ {
+ moniker->Release();
+ moniker = nullptr;
+ continue;
+ }
+
+ // Found the correct video input
+ break;
+ }
+
+ if(moniker)
+ {
+ hr = moniker->BindToObject(0, 0, IID_IBaseFilter, (void**)&baseFilter);
+ }
+ else
+ {
+ hr = E_FAIL;
+ }
+
+ IKsTopologyInfo* ksTopologyInfo = nullptr;
+ if(SUCCEEDED(hr))
+ {
+ hr = baseFilter->QueryInterface(__uuidof(IKsTopologyInfo), (void**)&ksTopologyInfo);
+ }
+
+ DWORD numberOfNodes = 0;
+ if(SUCCEEDED(hr))
+ {
+ hr = ksTopologyInfo->get_NumNodes(&numberOfNodes);
+ }
+
+ GUID nodeGuid;
+ for(DWORD nodeIndex = 0; nodeIndex < numberOfNodes; ++nodeIndex)
+ {
+ if(FAILED(ksTopologyInfo->get_NodeType(nodeIndex, &nodeGuid)))
+ {
+ continue;
+ }
+
+ if(nodeGuid == KSNODETYPE_DEV_SPECIFIC)
+ {
+ // This is the extension node
+ IKsNodeControl* pUnknown = nullptr;
+ hr = ksTopologyInfo->CreateNodeInstance(nodeIndex, __uuidof(IUnknown), (void**)&pUnknown);
+
+ if(SUCCEEDED(hr) && pUnknown != nullptr)
+ {
+ hr = pUnknown->QueryInterface(__uuidof(IKsControl), (void**)&ksControl);
+ }
+ if(SUCCEEDED(hr))
+ {
+ ksControlNodeId = nodeIndex;
+ }
+
+ if(pUnknown)
+ {
+ pUnknown->Release();
+ pUnknown = nullptr;
+ }
+ }
+ }
+
+ if(ksTopologyInfo)
+ {
+ ksTopologyInfo->Release();
+ ksTopologyInfo = nullptr;
+ }
+
+ if(moniker)
+ {
+ moniker->Release();
+ moniker = nullptr;
+ }
+ }
+
+ if(videoInputEnumerator)
+ {
+ videoInputEnumerator->Release();
+ }
+
+ if(dshowDevices)
+ {
+ dshowDevices->Release();
+ }
+
+ return SUCCEEDED(hr);
+}
+
+void UvcMediaFoundationVideo::InitDevice(size_t width, size_t height, int fps)
+{
+ HRESULT hr = MFCreateSourceReaderFromMediaSource(mediaSource, nullptr, &sourceReader);
+ if(FAILED(hr))
+ {
+ pango_print_error("Unable to create source reader from UVC media source");
+ }
+
+ // Find the closest supported resolution
+ UINT32 stride;
+ PixelFormat pixelFormat;
+ if(SUCCEEDED(hr))
+ {
+ IMFMediaType* bestMediaType = nullptr;
+ int bestError = std::numeric_limits<int>::max();
+ UINT32 bestWidth;
+ UINT32 bestHeight;
+ UINT32 bestStride;
+ GUID bestGuid;
+
+ for(DWORD i = 0;; ++i)
+ {
+ IMFMediaType* checkMediaType;
+ hr = sourceReader->GetNativeMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, i, &checkMediaType);
+ if(FAILED(hr))
+ {
+ if(hr == MF_E_NO_MORE_TYPES)
+ {
+ // Reached the end of the available media types
+ hr = S_OK;
+ }
+ else
+ {
+ pango_print_error("Failed to get UVC native media type");
+ }
+ break;
+ }
+
+ UINT32 checkWidth;
+ UINT32 checkHeight;
+ if(FAILED(MFGetAttributeSize(checkMediaType, MF_MT_FRAME_SIZE, &checkWidth, &checkHeight)))
+ {
+ checkWidth = 0;
+ checkHeight = 0;
+ }
+
+ int checkError = abs(int(checkWidth) - int(width)) + abs(int(checkHeight) - int(height));
+ if(bestError > checkError)
+ {
+ // Release the previous best
+ if(bestMediaType)
+ {
+ bestMediaType->Release();
+ }
+
+ bestError = checkError;
+ bestMediaType = checkMediaType;
+ bestWidth = checkWidth;
+ bestHeight = checkHeight;
+
+ if(FAILED(checkMediaType->GetGUID(MF_MT_SUBTYPE, &bestGuid)))
+ {
+ bestGuid = {0};
+ }
+
+ bestStride = MFGetAttributeUINT32(checkMediaType, MF_MT_DEFAULT_STRIDE, checkWidth);
+ }
+ else
+ {
+ checkMediaType->Release();
+ }
+ }
+ if(bestMediaType)
+ {
+ if(SUCCEEDED(hr))
+ {
+ sourceReader->SetCurrentMediaType(MF_SOURCE_READER_FIRST_VIDEO_STREAM, nullptr, bestMediaType);
+ width = bestWidth;
+ height = bestHeight;
+ stride = bestStride;
+
+ if(bestGuid == MFVideoFormat_YUY2)
+ {
+ pixelFormat = PixelFormatFromString("GRAY8");
+ }
+ else
+ {
+ pango_print_warn("Unexpected MFVideoFormat with FOURCC %c%c%c%c",
+ (unsigned char)((bestGuid.Data1 >> 0) & 0xff),
+ (unsigned char)((bestGuid.Data1 >> 8) & 0xff),
+ (unsigned char)((bestGuid.Data1 >> 16) & 0xff),
+ (unsigned char)((bestGuid.Data1 >> 24) & 0xff));
+ }
+ }
+
+ bestMediaType->Release();
+ }
+ else
+ {
+ width = 0;
+ height = 0;
+ }
+ }
+
+ size_bytes = stride * height;
+
+ streams.emplace_back(pixelFormat, width, height, stride);
+}
+
+void UvcMediaFoundationVideo::DeinitDevice()
+{
+ if(ksControl)
+ {
+ ksControl->Release();
+ ksControl = nullptr;
+ }
+
+ ksControlNodeId = KS_CONTROL_NODE_ID_INVALID;
+
+ if(baseFilter)
+ {
+ baseFilter->Release();
+ baseFilter = nullptr;
+ }
+
+ if(sourceReader)
+ {
+ sourceReader->Release();
+ sourceReader = nullptr;
+ }
+
+ if(mediaSource)
+ {
+ mediaSource->Shutdown();
+ mediaSource->Release();
+ mediaSource = nullptr;
+ }
+
+ streams.clear();
+}
+
+bool UvcMediaFoundationVideo::DeviceMatches(const std::wstring& symLink, int vendorId, int productId)
+{
+ // Example symlink:
+ // \\?\usb#vid_05a9&pid_0581&mi_00#6&2ff327a4&2&0000#{e5323777-f976-4f5b-9b55-b94699c46e44}\global
+ // ^^^^^^^^ ^^^^^^^^
+ std::wstring symLinkString(symLink);
+ if(vendorId != 0 && !SymLinkIDMatches(symLinkString, L"vid_", vendorId))
+ {
+ return false;
+ }
+
+ if(productId != 0 && !SymLinkIDMatches(symLinkString, L"pid_", productId))
+ {
+ return false;
+ }
+
+ return true;
+}
+
+bool UvcMediaFoundationVideo::SymLinkIDMatches(const std::wstring& symLink, const wchar_t* idStr, int id)
+{
+ // Find the ID prefix
+ size_t idOffset = symLink.find(idStr);
+ if(idOffset == std::wstring::npos)
+ {
+ // Unable to find the prefix
+ return false;
+ }
+
+ // Parse the ID as a hexadecimal number
+ return id == std::wcstol(&symLink[idOffset + std::wcslen(idStr)], nullptr, 16);
+}
+
+PANGOLIN_REGISTER_FACTORY(UvcMediaFoundationVideo)
+{
+ struct UvcVideoFactory final : public FactoryInterface<VideoInterface>
+ {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override
+ {
+ int vendorId = 0;
+ int productId = 0;
+ std::istringstream(uri.Get<std::string>("vid", "0x0000")) >> std::hex >> vendorId;
+ std::istringstream(uri.Get<std::string>("pid", "0x0000")) >> std::hex >> productId;
+ const unsigned int deviceId = uri.Get<int>("num", 0);
+ const ImageDim dim = uri.Get<ImageDim>("size", ImageDim(640, 480));
+ const unsigned int fps = uri.Get<unsigned int>("fps", 0); // 0 means unspecified
+ return std::unique_ptr<VideoInterface>(new UvcMediaFoundationVideo(vendorId, productId, deviceId, dim.x, dim.y, fps));
+ }
+ };
+
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(std::make_shared<UvcVideoFactory>(), 10, "uvc");
+}
+}
diff --git a/externals/Pangolin/src/video/drivers/v4l.cpp b/externals/Pangolin/src/video/drivers/v4l.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..076e8eee3a1bbfd09fa3c949efe1b37ba7efe5e4
--- /dev/null
+++ b/externals/Pangolin/src/video/drivers/v4l.cpp
@@ -0,0 +1,798 @@
+/* This file is part of the Pangolin Project,
+ * http://github.com/stevenlovegrove/Pangolin
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * adapted from V4L2 video capture example
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/utils/timer.h>
+#include <pangolin/video/drivers/v4l.h>
+#include <pangolin/video/iostream_operators.h>
+
+#include <assert.h>
+#include <iostream>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#include <errno.h>
+#include <fcntl.h>
+#include <linux/usb/video.h>
+#include <linux/uvcvideo.h>
+#include <malloc.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#define CLEAR(x) memset (&(x), 0, sizeof (x))
+
+using namespace std;
+
+namespace pangolin
+{
+
+static int xioctl(int fd, int request, void* arg)
+{
+ int r;
+ do r = ioctl (fd, request, arg);
+ while (-1 == r && EINTR == errno);
+ return r;
+}
+
+inline std::string V4lToString(int32_t v)
+{
+ // v = ((__u32)(a) | ((__u32)(b) << 8) | ((__u32)(c) << 16) | ((__u32)(d) << 24))
+ char cc[5];
+ cc[0] = v & 0xff;
+ cc[1] = (v>>8) & 0xff;
+ cc[2] = (v>>16) & 0xff;
+ cc[3] = (v>>24) & 0xff;
+ cc[4] = 0;
+ return std::string(cc);
+}
+
+V4lVideo::V4lVideo(const char* dev_name, io_method io, unsigned iwidth, unsigned iheight)
+ : io(io), fd(-1), buffers(0), n_buffers(0), running(false)
+{
+ open_device(dev_name);
+ init_device(dev_name,iwidth,iheight,0);
+ InitPangoDeviceProperties();
+
+ Start();
+}
+
+V4lVideo::~V4lVideo()
+{
+ if(running)
+ {
+ Stop();
+ }
+
+ uninit_device();
+ close_device();
+}
+
+void V4lVideo::InitPangoDeviceProperties()
+{
+ // Store camera details in device properties
+ device_properties[PANGO_HAS_TIMING_DATA] = true;
+}
+
+const std::vector<StreamInfo>& V4lVideo::Streams() const
+{
+ return streams;
+}
+
+size_t V4lVideo::SizeBytes() const
+{
+ return image_size;
+}
+
+bool V4lVideo::GrabNext( unsigned char* image, bool /*wait*/ )
+{
+ for (;;) {
+ fd_set fds;
+ struct timeval tv;
+ int r;
+
+ FD_ZERO (&fds);
+ FD_SET (fd, &fds);
+
+ /* Timeout. */
+ tv.tv_sec = 2;
+ tv.tv_usec = 0;
+
+ r = select (fd + 1, &fds, NULL, NULL, &tv);
+
+ if (-1 == r) {
+ if (EINTR == errno)
+ continue;
+
+ // This is a terminal condition that must be propogated up.
+ throw VideoException ("select", strerror(errno));
+ }
+
+ if (0 == r) {
+ // Timeout has occured - This is longer than any reasonable frame interval,
+ // but not necessarily terminal, so return false to indicate that no frame was captured.
+ return false;
+ }
+
+ if (ReadFrame(image))
+ break;
+
+ /* EAGAIN - continue select loop. */
+ }
+
+ return true;
+}
+
+bool V4lVideo::GrabNewest( unsigned char* image, bool wait )
+{
+ // TODO: Implement
+ return GrabNext(image,wait);
+}
+
+int V4lVideo::ReadFrame(unsigned char* image)
+{
+ struct v4l2_buffer buf;
+ unsigned int i;
+
+ switch (io) {
+ case IO_METHOD_READ:
+ if (-1 == read (fd, buffers[0].start, buffers[0].length)) {
+ switch (errno) {
+ case EAGAIN:
+ return 0;
+
+ case EIO:
+ /* Could ignore EIO, see spec. */
+
+ /* fall through */
+
+ default:
+ throw VideoException("read", strerror(errno));
+ }
+ }
+ // This is a hack, this ts sould come from the device.
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(pangolin::TimeNow()));
+
+ // process_image(buffers[0].start);
+ memcpy(image,buffers[0].start,buffers[0].length);
+
+ break;
+
+ case IO_METHOD_MMAP:
+ CLEAR (buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+
+ if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return 0;
+
+ case EIO:
+ /* Could ignore EIO, see spec. */
+
+ /* fall through */
+
+ default:
+ throw VideoException("VIDIOC_DQBUF", strerror(errno));
+ }
+ }
+ // This is a hack, this ts sould come from the device.
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(pangolin::TimeNow()));
+
+ assert (buf.index < n_buffers);
+
+ // process_image (buffers[buf.index].start);
+ memcpy(image,buffers[buf.index].start,buffers[buf.index].length);
+
+
+ if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
+ throw VideoException("VIDIOC_QBUF", strerror(errno));
+
+ break;
+
+ case IO_METHOD_USERPTR:
+ CLEAR (buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_USERPTR;
+
+ if (-1 == xioctl (fd, VIDIOC_DQBUF, &buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return 0;
+
+ case EIO:
+ /* Could ignore EIO, see spec. */
+
+ /* fall through */
+
+ default:
+ throw VideoException("VIDIOC_DQBUF", strerror(errno));
+ }
+ }
+ // This is a hack, this ts sould come from the device.
+ frame_properties[PANGO_HOST_RECEPTION_TIME_US] = picojson::value(pangolin::Time_us(pangolin::TimeNow()));
+
+ for (i = 0; i < n_buffers; ++i)
+ if (buf.m.userptr == (unsigned long) buffers[i].start
+ && buf.length == buffers[i].length)
+ break;
+
+ assert (i < n_buffers);
+
+ // process_image ((void *) buf.m.userptr);
+ memcpy(image,(void *)buf.m.userptr,buf.length);
+
+
+ if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
+ throw VideoException("VIDIOC_QBUF", strerror(errno));
+
+ break;
+ }
+
+ return 1;
+}
+
+void V4lVideo::Stop()
+{
+ if(running) {
+ enum v4l2_buf_type type;
+
+ switch (io) {
+ case IO_METHOD_READ:
+ /* Nothing to do. */
+ break;
+
+ case IO_METHOD_MMAP:
+ case IO_METHOD_USERPTR:
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (-1 == xioctl (fd, VIDIOC_STREAMOFF, &type))
+ throw VideoException("VIDIOC_STREAMOFF", strerror(errno));
+
+ break;
+ }
+
+ running = false;
+ }
+}
+
+void V4lVideo::Start()
+{
+ if(!running) {
+ unsigned int i;
+ enum v4l2_buf_type type;
+
+ switch (io) {
+ case IO_METHOD_READ:
+ /* Nothing to do. */
+ break;
+
+ case IO_METHOD_MMAP:
+ for (i = 0; i < n_buffers; ++i) {
+ struct v4l2_buffer buf;
+
+ CLEAR (buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = i;
+
+ if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
+ throw VideoException("VIDIOC_QBUF", strerror(errno));
+ }
+
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
+ throw VideoException("VIDIOC_STREAMON", strerror(errno));
+
+ break;
+
+ case IO_METHOD_USERPTR:
+ for (i = 0; i < n_buffers; ++i) {
+ struct v4l2_buffer buf;
+
+ CLEAR (buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_USERPTR;
+ buf.index = i;
+ buf.m.userptr = (unsigned long) buffers[i].start;
+ buf.length = buffers[i].length;
+
+ if (-1 == xioctl (fd, VIDIOC_QBUF, &buf))
+ throw VideoException("VIDIOC_QBUF", strerror(errno));
+ }
+
+ type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (-1 == xioctl (fd, VIDIOC_STREAMON, &type))
+ throw VideoException ("VIDIOC_STREAMON", strerror(errno));
+
+ break;
+ }
+
+ running = true;
+ }
+}
+
+void V4lVideo::uninit_device()
+{
+ unsigned int i;
+
+ switch (io) {
+ case IO_METHOD_READ:
+ free (buffers[0].start);
+ break;
+
+ case IO_METHOD_MMAP:
+ for (i = 0; i < n_buffers; ++i)
+ if (-1 == munmap (buffers[i].start, buffers[i].length))
+ throw VideoException ("munmap");
+ break;
+
+ case IO_METHOD_USERPTR:
+ for (i = 0; i < n_buffers; ++i)
+ free (buffers[i].start);
+ break;
+ }
+
+ free (buffers);
+}
+
+void V4lVideo::init_read(unsigned int buffer_size)
+{
+ buffers = (buffer*)calloc (1, sizeof (buffer));
+
+ if (!buffers) {
+ throw VideoException("Out of memory\n");
+ }
+
+ buffers[0].length = buffer_size;
+ buffers[0].start = malloc (buffer_size);
+
+ if (!buffers[0].start) {
+ throw VideoException("Out of memory\n");
+ }
+}
+
+void V4lVideo::init_mmap(const char* /*dev_name*/)
+{
+ struct v4l2_requestbuffers req;
+
+ CLEAR (req);
+
+ req.count = 4;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_MMAP;
+
+ if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
+ if (EINVAL == errno) {
+ throw VideoException("does not support memory mapping", strerror(errno));
+ } else {
+ throw VideoException ("VIDIOC_REQBUFS", strerror(errno));
+ }
+ }
+
+ if (req.count < 2) {
+ throw VideoException("Insufficient buffer memory");
+ }
+
+ buffers = (buffer*)calloc(req.count, sizeof(buffer));
+
+ if (!buffers) {
+ throw VideoException( "Out of memory\n");
+ }
+
+ for (n_buffers = 0; n_buffers < req.count; ++n_buffers) {
+ struct v4l2_buffer buf;
+
+ CLEAR (buf);
+
+ buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ buf.memory = V4L2_MEMORY_MMAP;
+ buf.index = n_buffers;
+
+ if (-1 == xioctl (fd, VIDIOC_QUERYBUF, &buf))
+ throw VideoException ("VIDIOC_QUERYBUF", strerror(errno));
+
+ buffers[n_buffers].length = buf.length;
+ buffers[n_buffers].start =
+ mmap (NULL /* start anywhere */,
+ buf.length,
+ PROT_READ | PROT_WRITE /* required */,
+ MAP_SHARED /* recommended */,
+ fd, buf.m.offset);
+
+ if (MAP_FAILED == buffers[n_buffers].start)
+ throw VideoException ("mmap");
+ }
+}
+
+void V4lVideo::init_userp(const char* /*dev_name*/, unsigned int buffer_size)
+{
+ struct v4l2_requestbuffers req;
+ unsigned int page_size;
+
+ page_size = getpagesize ();
+ buffer_size = (buffer_size + page_size - 1) & ~(page_size - 1);
+
+ CLEAR (req);
+
+ req.count = 4;
+ req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ req.memory = V4L2_MEMORY_USERPTR;
+
+ if (-1 == xioctl (fd, VIDIOC_REQBUFS, &req)) {
+ if (EINVAL == errno) {
+ throw VideoException( "Does not support user pointer i/o", strerror(errno));
+ } else {
+ throw VideoException ("VIDIOC_REQBUFS", strerror(errno));
+ }
+ }
+
+ buffers = (buffer*)calloc(4, sizeof(buffer));
+
+ if (!buffers) {
+ throw VideoException( "Out of memory\n");
+ }
+
+ for (n_buffers = 0; n_buffers < 4; ++n_buffers) {
+ buffers[n_buffers].length = buffer_size;
+ buffers[n_buffers].start = memalign (/* boundary */ page_size,
+ buffer_size);
+
+ if (!buffers[n_buffers].start) {
+ throw VideoException( "Out of memory\n");
+ }
+ }
+}
+
+void V4lVideo::init_device(const char* dev_name, unsigned iwidth, unsigned iheight, unsigned ifps, unsigned v4l_format, v4l2_field field)
+{
+ struct v4l2_capability cap;
+ struct v4l2_cropcap cropcap;
+ struct v4l2_crop crop;
+ struct v4l2_format fmt;
+ struct v4l2_streamparm strm;
+
+ unsigned int min;
+
+ if (-1 == xioctl (fd, VIDIOC_QUERYCAP, &cap)) {
+ if (EINVAL == errno) {
+ throw VideoException("Not a V4L2 device", strerror(errno));
+ } else {
+ throw VideoException ("VIDIOC_QUERYCAP", strerror(errno));
+ }
+ }
+
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE)) {
+ throw VideoException("Not a video capture device");
+ }
+
+ switch (io) {
+ case IO_METHOD_READ:
+ if (!(cap.capabilities & V4L2_CAP_READWRITE)) {
+ throw VideoException("Does not support read i/o");
+ }
+
+ break;
+
+ case IO_METHOD_MMAP:
+ case IO_METHOD_USERPTR:
+ if (!(cap.capabilities & V4L2_CAP_STREAMING)) {
+ throw VideoException("Does not support streaming i/o");
+ }
+
+ break;
+ }
+
+
+ /* Select video input, video standard and tune here. */
+
+ CLEAR (cropcap);
+
+ cropcap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ if (0 == xioctl (fd, VIDIOC_CROPCAP, &cropcap)) {
+ crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ crop.c = cropcap.defrect; /* reset to default */
+
+ if (-1 == xioctl (fd, VIDIOC_S_CROP, &crop)) {
+ switch (errno) {
+ case EINVAL:
+ /* Cropping not supported. */
+ break;
+ default:
+ /* Errors ignored. */
+ break;
+ }
+ }
+ } else {
+ /* Errors ignored. */
+ }
+
+ CLEAR (fmt);
+
+ if(iwidth!=0 && iheight!=0) {
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = iwidth;
+ fmt.fmt.pix.height = iheight;
+ fmt.fmt.pix.pixelformat = v4l_format;
+ fmt.fmt.pix.field = field;
+
+ if (-1 == xioctl (fd, VIDIOC_S_FMT, &fmt))
+ throw VideoException("VIDIOC_S_FMT", strerror(errno));
+ }else{
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+ /* Preserve original settings as set by v4l2-ctl for example */
+ if (-1 == xioctl(fd, VIDIOC_G_FMT, &fmt))
+ throw VideoException("VIDIOC_G_FMT", strerror(errno));
+ }
+
+ /* Buggy driver paranoia. */
+ min = fmt.fmt.pix.width * 2;
+ if (fmt.fmt.pix.bytesperline < min)
+ fmt.fmt.pix.bytesperline = min;
+ min = fmt.fmt.pix.bytesperline * fmt.fmt.pix.height;
+ if (fmt.fmt.pix.sizeimage < min)
+ fmt.fmt.pix.sizeimage = min;
+
+ /* Note VIDIOC_S_FMT may change width and height. */
+ width = fmt.fmt.pix.width;
+ height = fmt.fmt.pix.height;
+ image_size = fmt.fmt.pix.sizeimage;
+
+ if(ifps!=0)
+ {
+ CLEAR(strm);
+ strm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ strm.parm.capture.capability = V4L2_CAP_TIMEPERFRAME;
+ strm.parm.capture.timeperframe.numerator = 1;
+ strm.parm.capture.timeperframe.denominator = ifps;
+
+ if (-1 == xioctl (fd, VIDIOC_S_PARM, &fmt))
+ throw VideoException("VIDIOC_S_PARM", strerror(errno));
+
+ fps = (float)strm.parm.capture.timeperframe.denominator / strm.parm.capture.timeperframe.numerator;
+ }else{
+ fps = 0;
+ }
+
+ switch (io) {
+ case IO_METHOD_READ:
+ init_read (fmt.fmt.pix.sizeimage);
+ break;
+
+ case IO_METHOD_MMAP:
+ init_mmap (dev_name );
+ break;
+
+ case IO_METHOD_USERPTR:
+ init_userp (dev_name, fmt.fmt.pix.sizeimage);
+ break;
+ }
+
+ uint32_t bit_depth = 0;
+
+ std::string spix="GRAY8";
+ if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_GREY) {
+ spix="GRAY8";
+ bit_depth = 8;
+ }else if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_YUYV) {
+ spix="YUYV422";
+ bit_depth = 8;
+ } else if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_UYVY) {
+ spix="UYVY422";
+ bit_depth = 8;
+ }else if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_Y16) {
+ spix="GRAY16LE";
+ bit_depth = 16;
+ }else if(fmt.fmt.pix.pixelformat == V4L2_PIX_FMT_Y10) {
+ spix="GRAY10";
+ bit_depth = 10;
+ }else{
+ // TODO: Add method to translate from V4L to FFMPEG type.
+ std::cerr << "V4L Format " << V4lToString(fmt.fmt.pix.pixelformat)
+ << " not recognised. Defaulting to '" << spix << std::endl;
+ }
+
+ PixelFormat pfmt = PixelFormatFromString(spix);
+ pfmt.channel_bit_depth = bit_depth;
+ const StreamInfo stream_info(pfmt, width, height, (width*pfmt.bpp)/8, 0);
+
+ streams.push_back(stream_info);
+}
+
+bool V4lVideo::SetExposure(int exposure_us)
+{
+ struct v4l2_ext_controls ctrls = {};
+ struct v4l2_ext_control ctrl = {};
+
+ ctrl.id = V4L2_CID_EXPOSURE_ABSOLUTE;
+ // v4l specifies exposure in 100us units
+ ctrl.value = int(exposure_us / 100.0);
+ ctrls.ctrl_class = V4L2_CTRL_CLASS_CAMERA;
+ ctrls.count = 1;
+ ctrls.controls = &ctrl;
+
+ if (-1 == xioctl(fd, VIDIOC_S_EXT_CTRLS, &ctrls)){
+ pango_print_warn("V4lVideo::SetExposure() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool V4lVideo::GetExposure(int& exposure_us)
+{
+ struct v4l2_ext_controls ctrls = {};
+ struct v4l2_ext_control ctrl = {};
+
+ ctrl.id = V4L2_CID_EXPOSURE_ABSOLUTE;
+ ctrls.ctrl_class = V4L2_CTRL_CLASS_CAMERA;
+ ctrls.count = 1;
+ ctrls.controls = &ctrl;
+
+ if (-1 == xioctl(fd, VIDIOC_G_EXT_CTRLS, &ctrls)){
+ pango_print_warn("V4lVideo::GetExposure() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ // v4l specifies exposure in 100us units
+ exposure_us = ctrls.controls->value * 100;
+ return true;
+ }
+}
+
+bool V4lVideo::SetGain(float gain)
+{
+ struct v4l2_control control;
+ control.id = V4L2_CID_GAIN;
+ control.value = gain;
+
+ if (-1 == xioctl (fd, VIDIOC_S_CTRL, &control)) {
+ pango_print_warn("V4lVideo::SetGain() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ return true;
+ }
+}
+
+bool V4lVideo::GetGain(float& gain)
+{
+ struct v4l2_control control;
+ control.id = V4L2_CID_GAIN;
+
+ if (-1 == xioctl (fd, VIDIOC_G_CTRL, &control)) {
+ pango_print_warn("V4lVideo::GetGain() ioctl error: %s\n", strerror(errno));
+ return false;
+ } else {
+ gain = control.value;
+ return true;
+ }
+}
+
+void V4lVideo::close_device()
+{
+ if (-1 == close (fd))
+ throw VideoException("close");
+
+ fd = -1;
+}
+
+void V4lVideo::open_device(const char* dev_name)
+{
+ struct stat st;
+
+ if (-1 == stat (dev_name, &st)) {
+ throw VideoException("Cannot stat device", strerror(errno));
+ }
+
+ if (!S_ISCHR (st.st_mode)) {
+ throw VideoException("Not device");
+ }
+
+ fd = open (dev_name, O_RDWR /* required */ | O_NONBLOCK, 0);
+
+ if (-1 == fd) {
+ throw VideoException("Cannot open device");
+ }
+}
+
+int V4lVideo::IoCtrl(uint8_t unit, uint8_t ctrl, unsigned char* data, int len, UvcRequestCode req_code)
+{
+ struct uvc_xu_control_query xu;
+ xu.unit = unit;
+ xu.selector = ctrl;
+ xu.size = len;
+ xu.data = data;
+ xu.query = req_code;
+
+ int ret = ioctl(fd, UVCIOC_CTRL_QUERY, &xu);
+ if (ret == -1) {
+ pango_print_warn("V4lVideo::IoCtrl() ioctl error: %d\n", errno);
+ return ret;
+ }
+ return 0;
+}
+
+//! Access JSON properties of device
+const picojson::value& V4lVideo::DeviceProperties() const
+{
+ return device_properties;
+}
+
+//! Access JSON properties of most recently captured frame
+const picojson::value& V4lVideo::FrameProperties() const
+{
+ return frame_properties;
+}
+
+PANGOLIN_REGISTER_FACTORY(V4lVideo)
+{
+ struct V4lVideoFactory final : public FactoryInterface<VideoInterface> {
+ std::unique_ptr<VideoInterface> Open(const Uri& uri) override {
+ const std::string smethod = uri.Get<std::string>("method","mmap");
+ const ImageDim desired_dim = uri.Get<ImageDim>("size", ImageDim(0,0));
+
+ io_method method = IO_METHOD_MMAP;
+
+ if(smethod == "read" ) {
+ method = IO_METHOD_READ;
+ }else if(smethod == "mmap" ) {
+ method = IO_METHOD_MMAP;
+ }else if(smethod == "userptr" ) {
+ method = IO_METHOD_USERPTR;
+ }
+
+ V4lVideo* video_raw = new V4lVideo(uri.url.c_str(), method, desired_dim.x, desired_dim.y );
+ if(video_raw && uri.Contains("ExposureTime")) {
+ static_cast<V4lVideo*>(video_raw)->SetExposure(uri.Get<int>("ExposureTime", 10000));
+ }
+ if(video_raw && uri.Contains("Gain")) {
+ static_cast<V4lVideo*>(video_raw)->SetGain(uri.Get<int>("Gain", 1));
+ }
+ return std::unique_ptr<VideoInterface>(video_raw);
+ }
+ };
+
+ auto factory = std::make_shared<V4lVideoFactory>();
+ FactoryRegistry<VideoInterface>::I().RegisterFactory(factory, 10, "v4l");
+}
+
+
+}
diff --git a/externals/Pangolin/src/video/stream_encoder_factory.cpp b/externals/Pangolin/src/video/stream_encoder_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d01a66017a8c88d4a42148fc8e3dec49bbb8e105
--- /dev/null
+++ b/externals/Pangolin/src/video/stream_encoder_factory.cpp
@@ -0,0 +1,62 @@
+#include <pangolin/video/stream_encoder_factory.h>
+
+#include <cctype>
+#include <pangolin/utils/file_utils.h>
+#include <pangolin/utils/type_convert.h>
+
+namespace pangolin {
+
+StreamEncoderFactory& StreamEncoderFactory::I()
+{
+ static StreamEncoderFactory instance;
+ return instance;
+}
+
+struct EncoderDetails
+{
+ std::string encoder_name;
+ ImageFileType file_type;
+ float quality;
+};
+
+inline EncoderDetails EncoderDetailsFromString(const std::string& encoder_spec)
+{
+ std::string::const_reverse_iterator rit = encoder_spec.rbegin();
+ for(; std::isdigit(*rit) && rit != encoder_spec.rend(); ++rit );
+
+ // png, tga, ...
+ std::string encoder_name(encoder_spec.begin(), rit.base());
+ ToLower(encoder_name);
+
+ // Quality of encoding for lossy encoders [0..100]
+ float quality = 100.0;
+ if(rit != encoder_spec.rbegin()) {
+ quality = pangolin::Convert<float,std::string>::Do(std::string(rit.base(),encoder_spec.end()));
+ }
+
+
+ return { encoder_name, NameToImageFileType(encoder_name), quality};
+}
+
+ImageEncoderFunc StreamEncoderFactory::GetEncoder(const std::string& encoder_spec, const PixelFormat& fmt)
+{
+ const EncoderDetails encdet = EncoderDetailsFromString(encoder_spec);
+ if(encdet.file_type == ImageFileTypeUnknown)
+ throw std::invalid_argument("Unsupported encoder format: " + encoder_spec);
+
+ return [fmt,encdet](std::ostream& os, const Image<unsigned char>& img){
+ SaveImage(img,fmt,os,encdet.file_type,true,encdet.quality);
+ };
+}
+
+ImageDecoderFunc StreamEncoderFactory::GetDecoder(const std::string& encoder_spec, const PixelFormat& fmt)
+{
+ const EncoderDetails encdet = EncoderDetailsFromString(encoder_spec);
+ PANGO_ENSURE(encdet.file_type != ImageFileTypeUnknown);
+
+ return [fmt,encdet](std::istream& is){
+ return LoadImage(is,encdet.file_type);
+ };
+}
+
+}
diff --git a/externals/Pangolin/src/video/video.cpp b/externals/Pangolin/src/video/video.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1852b3710961fd46ddbc6c5eab24c2662622543e
--- /dev/null
+++ b/externals/Pangolin/src/video/video.cpp
@@ -0,0 +1,80 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/factory/factory_registry.h>
+#include <pangolin/video/video.h>
+#include <pangolin/video/video_output.h>
+#include <pangolin/video_drivers.h>
+
+namespace pangolin
+{
+
+bool one_time_init = false;
+
+std::unique_ptr<VideoInterface> OpenVideo(const std::string& str_uri)
+{
+ return OpenVideo( ParseUri(str_uri) );
+}
+
+std::unique_ptr<VideoInterface> OpenVideo(const Uri& uri)
+{
+ if(!one_time_init) {
+ one_time_init = LoadBuiltInVideoDrivers();
+ }
+
+ std::unique_ptr<VideoInterface> video =
+ FactoryRegistry<VideoInterface>::I().Open(uri);
+
+ if(!video) {
+ throw VideoExceptionNoKnownHandler(uri.scheme);
+ }
+
+ return video;
+}
+
+std::unique_ptr<VideoOutputInterface> OpenVideoOutput(const std::string& str_uri)
+{
+ return OpenVideoOutput( ParseUri(str_uri) );
+}
+
+std::unique_ptr<VideoOutputInterface> OpenVideoOutput(const Uri& uri)
+{
+ if(!one_time_init) {
+ one_time_init = LoadBuiltInVideoDrivers();
+ }
+
+ std::unique_ptr<VideoOutputInterface> video =
+ FactoryRegistry<VideoOutputInterface>::I().Open(uri);
+
+ if(!video) {
+ throw VideoException("No known video handler for URI '" + uri.scheme + "'");
+ }
+
+ return video;
+}
+
+}
diff --git a/externals/Pangolin/src/video/video_input.cpp b/externals/Pangolin/src/video/video_input.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9432fd5ca0bd629b38f18c3c1ec214c514275d84
--- /dev/null
+++ b/externals/Pangolin/src/video/video_input.cpp
@@ -0,0 +1,220 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/video_input.h>
+#include <pangolin/video/video_output.h>
+
+namespace pangolin
+{
+
+VideoInput::VideoInput()
+ : frame_num(0), record_frame_skip(1), record_once(false), record_continuous(false)
+{
+}
+
+VideoInput::VideoInput(
+ const std::string& input_uri,
+ const std::string& output_uri
+ ) : frame_num(0), record_frame_skip(1), record_once(false), record_continuous(false)
+{
+ Open(input_uri, output_uri);
+}
+
+void VideoInput::Open(
+ const std::string& input_uri,
+ const std::string& output_uri
+ )
+{
+ uri_input = ParseUri(input_uri);
+ uri_output = ParseUri(output_uri);
+
+ if (uri_output.scheme == "file") {
+ // Default to pango output
+ uri_output.scheme = "pango";
+ }
+
+ // Start off playing from video_src
+ video_src = OpenVideo(input_uri);
+
+ // Reset state
+ frame_num = 0;
+ videos.resize(1);
+ videos[0] = video_src.get();
+}
+
+void VideoInput::Close()
+{
+ // Reset this first so that recording data gets written out to disk ASAP.
+ video_recorder.reset();
+
+ video_src.reset();
+ videos.clear();
+}
+
+VideoInput::~VideoInput()
+{
+ Close();
+}
+
+const std::string& VideoInput::LogFilename() const
+{
+ return uri_output.url;
+}
+
+std::string& VideoInput::LogFilename()
+{
+ return uri_output.url;
+}
+
+bool VideoInput::Grab( unsigned char* buffer, std::vector<Image<unsigned char> >& images, bool wait, bool newest)
+{
+ if( !video_src ) throw VideoException("No video source open");
+
+ bool success;
+
+ if(newest) {
+ success = GrabNewest(buffer, wait);
+ }else{
+ success = GrabNext(buffer, wait);
+ }
+
+ if(success) {
+ images.clear();
+ for(size_t s=0; s < Streams().size(); ++s) {
+ images.push_back(Streams()[s].StreamImage(buffer));
+ }
+ }
+
+ return success;
+}
+
+void VideoInput::InitialiseRecorder()
+{
+ video_recorder.reset();
+ video_recorder = OpenVideoOutput(uri_output);
+ video_recorder->SetStreams(
+ video_src->Streams(), uri_input.full_uri,
+ GetVideoDeviceProperties(video_src.get())
+ );
+}
+
+void VideoInput::Record()
+{
+ // Switch sub-video
+ videos.resize(1);
+ videos[0] = video_src.get();
+
+ // Initialise recorder and ensure src is started
+ InitialiseRecorder();
+ video_src->Start();
+ frame_num = 0;
+ record_continuous = true;
+}
+
+void VideoInput::RecordOneFrame()
+{
+ // Append to existing video.
+ if(!video_recorder) {
+ InitialiseRecorder();
+ }
+ record_continuous = false;
+ record_once = true;
+
+ // Switch sub-video
+ videos.resize(1);
+ videos[0] = video_src.get();
+}
+
+size_t VideoInput::SizeBytes() const
+{
+ if( !video_src ) throw VideoException("No video source open");
+ return video_src->SizeBytes();
+}
+
+const std::vector<StreamInfo>& VideoInput::Streams() const
+{
+ return video_src->Streams();
+}
+
+void VideoInput::Start()
+{
+ video_src->Start();
+}
+
+void VideoInput::Stop()
+{
+ if(IsRecording()) {
+ video_recorder.reset();
+ }else{
+ video_src->Stop();
+ }
+}
+
+bool VideoInput::GrabNext( unsigned char* image, bool wait )
+{
+ frame_num++;
+
+ const bool should_record = (record_continuous && !(frame_num % record_frame_skip)) || record_once;
+
+ const bool success = video_src->GrabNext(image, wait);
+
+ if( should_record && video_recorder != 0 && success) {
+ video_recorder->WriteStreams(image, GetVideoFrameProperties(video_src.get()) );
+ record_once = false;
+ }
+
+ return success;
+}
+
+bool VideoInput::GrabNewest( unsigned char* image, bool wait )
+{
+ frame_num++;
+
+ const bool should_record = (record_continuous && !(frame_num % record_frame_skip)) || record_once;
+ const bool success = video_src->GrabNewest(image,wait);
+
+ if( should_record && video_recorder != 0 && success)
+ {
+ video_recorder->WriteStreams(image, GetVideoFrameProperties(video_src.get()) );
+ record_once = false;
+ }
+
+ return success;
+}
+
+void VideoInput::SetTimelapse(size_t one_in_n_frames)
+{
+ record_frame_skip = one_in_n_frames;
+}
+
+bool VideoInput::IsRecording() const
+{
+ return video_recorder != 0;
+}
+
+}
+
diff --git a/externals/Pangolin/src/video/video_interface_factory.cpp b/externals/Pangolin/src/video/video_interface_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..39f409bc69d70d7f5a3db899098ff006faedfabf
--- /dev/null
+++ b/externals/Pangolin/src/video/video_interface_factory.cpp
@@ -0,0 +1,42 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/video_interface.h>
+#include <pangolin/factory/factory_registry.h>
+
+namespace pangolin
+{
+
+template<>
+FactoryRegistry<VideoInterface>& FactoryRegistry<VideoInterface>::I()
+{
+ // Singleton instance
+ static FactoryRegistry instance;
+ return instance;
+}
+
+}
diff --git a/externals/Pangolin/src/video/video_output.cpp b/externals/Pangolin/src/video/video_output.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d95f3fdd00abab0533533ae771b431691e66e23f
--- /dev/null
+++ b/externals/Pangolin/src/video/video_output.cpp
@@ -0,0 +1,137 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/video.h>
+#include <pangolin/video/video_output.h>
+
+#include <pangolin/video/drivers/pango_video_output.h>
+
+#include <pangolin/utils/file_utils.h>
+
+namespace pangolin
+{
+VideoOutput::VideoOutput()
+{
+}
+
+VideoOutput::VideoOutput(const std::string& uri)
+{
+ Open(uri);
+}
+
+VideoOutput::~VideoOutput()
+{
+}
+
+bool VideoOutput::IsOpen() const
+{
+ return recorder.get() != nullptr;
+}
+
+void VideoOutput::Open(const std::string& str_uri)
+{
+ Close();
+ uri = ParseUri(str_uri);
+ recorder = OpenVideoOutput(uri);
+}
+
+void VideoOutput::Close()
+{
+ recorder.reset();
+}
+
+const std::vector<StreamInfo>& VideoOutput::Streams() const
+{
+ return recorder->Streams();
+}
+
+void VideoOutput::SetStreams(const std::vector<StreamInfo>& streams,
+ const std::string& uri,
+ const picojson::value& properties)
+{
+ recorder->SetStreams(streams, uri, properties);
+}
+
+int VideoOutput::WriteStreams(const unsigned char* data, const picojson::value& frame_properties)
+{
+ return recorder->WriteStreams(data, frame_properties);
+}
+
+bool VideoOutput::IsPipe() const
+{
+ return recorder->IsPipe();
+}
+
+void VideoOutput::AddStream(const PixelFormat& pf, size_t w, size_t h, size_t pitch)
+{
+ streams.emplace_back(pf, w, h, pitch, nullptr);
+}
+
+void VideoOutput::AddStream(const PixelFormat& pf, size_t w, size_t h)
+{
+ AddStream(pf, w, h, w * pf.bpp / 8);
+}
+
+void VideoOutput::SetStreams(const std::string& uri, const picojson::value& properties)
+{
+ size_t offset = 0;
+ for(size_t i = 0; i < streams.size(); i++)
+ {
+ streams[i] = StreamInfo(streams[i].PixFormat(),
+ streams[i].Width(),
+ streams[i].Height(),
+ streams[i].Pitch(),
+ (unsigned char*)offset);
+ offset += streams[i].SizeBytes();
+ }
+ SetStreams(streams, uri, properties);
+}
+
+size_t VideoOutput::SizeBytes(void) const
+{
+ size_t total = 0;
+ for(const StreamInfo& si : recorder->Streams())
+ total += si.SizeBytes();
+ return total;
+}
+
+std::vector<Image<unsigned char>> VideoOutput::GetOutputImages(unsigned char* buffer) const
+{
+ std::vector<Image<unsigned char>> images;
+ for(size_t s = 0; s < recorder->Streams().size(); ++s)
+ {
+ images.push_back(recorder->Streams()[s].StreamImage(buffer));
+ }
+ return images;
+}
+
+std::vector<Image<unsigned char>> VideoOutput::GetOutputImages(std::vector<unsigned char>& buffer) const
+{
+ buffer.resize(SizeBytes());
+ return GetOutputImages(buffer.data());
+}
+}
diff --git a/externals/Pangolin/src/video/video_output_interface_factory.cpp b/externals/Pangolin/src/video/video_output_interface_factory.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..28b87f0987df9e05805765bb40e4ae7bc5774e42
--- /dev/null
+++ b/externals/Pangolin/src/video/video_output_interface_factory.cpp
@@ -0,0 +1,42 @@
+/* This file is part of the Pangolin Project.
+ * http://github.com/stevenlovegrove/Pangolin
+ *
+ * Copyright (c) 2011-2013 Steven Lovegrove
+ *
+ * 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.
+ */
+
+#include <pangolin/video/video_output_interface.h>
+#include <pangolin/factory/factory_registry.h>
+
+namespace pangolin
+{
+
+template<>
+FactoryRegistry<VideoOutputInterface>& FactoryRegistry<VideoOutputInterface>::I()
+{
+ // Singleton instance
+ static FactoryRegistry instance;
+ return instance;
+}
+
+}
diff --git a/externals/Pangolin/test/log/testlog.cpp b/externals/Pangolin/test/log/testlog.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..65199e8d749f13414687b72e9a5e4378c16e4e74
--- /dev/null
+++ b/externals/Pangolin/test/log/testlog.cpp
@@ -0,0 +1,282 @@
+#include <iostream>
+#include <mutex>
+#include <sstream>
+#include <thread>
+
+#include <pangolin/log/packetstream_reader.h>
+#include <pangolin/log/packetstream_writer.h>
+
+using namespace std;
+using namespace pangolin;
+
+mutex output_m;
+auto& output = cout;
+
+void writeFakeFrame(const PacketStreamSource& source, size_t sequence_number, PacketStreamWriter& target)
+{
+ stringstream r;
+ r << "Hello, I am frame number " << sequence_number << " from source number " << source.id << ", named '" << source.driver << "'";
+
+ picojson::value meta;
+ meta["source"] = source.id;
+ meta["frame number"] = sequence_number;
+ meta["driver name"] = source.driver;
+
+ target.WriteSourcePacket(source.id, r.str().c_str(), r.str().size(), meta);
+}
+
+
+Packet readFakeFrame(PacketStreamSourceId id, PacketStreamReader& source)
+{
+ char buffer[1024];
+ // source.lock();
+ Packet fi = source.NextFrame(id);
+ fi.ReadRaw(buffer, fi.size);
+
+ output_m.lock();
+ output << "Thread " << this_thread::get_id() << " read a frame from src " << fi.src << ", of size " << fi.size << "..." << endl;
+ fi.meta.serialize(ostream_iterator<char>(output), true);
+ output.write(buffer, fi.size);
+ output << endl << endl;
+ output_m.unlock();
+ return fi;
+}
+
+void test_simple()
+{
+ PacketStreamWriter write("test_simple");
+ SyncTime t;
+
+ PacketStreamSource video;
+ video.driver = "video driver name";
+ video.uri = "video driver fake uri";
+
+ PacketStreamSource infrared;
+ infrared.driver = "infrared driver name";
+ infrared.uri = "infrared driver fake uri";
+
+ write.AddSource(video);
+ write.AddSource(infrared);
+
+ output << "Opened an oPacketStream, writing some fake frames with a time delay to test sync: " << endl;
+ for (size_t seqnum = 0; seqnum < 10; seqnum++)
+ {
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ writeFakeFrame(video, seqnum, write);
+ output << ".";
+ output.flush();
+ }
+
+ output << endl;
+ write.WriteEnd();
+ write.Close();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ output << "Done writing" << endl;
+
+ PacketStreamReader read("test_simple");
+ output << "Opened an iPacketStream, reading frames with a sync timer. You should see a 1 second delay between each frame." << endl;
+
+ while(readFakeFrame(video.id, read));
+
+ output << "Done reading frames" << endl;
+ output << "Testing seek... let's look for frame 5" << endl;
+
+ if (!read.Seek(video.id, 5))
+ throw runtime_error("Something terrible has happened.");
+
+ if (!readFakeFrame(video.id, read))
+ throw runtime_error("Something terrible has happened.");
+
+ output << "Now let's test resyncing, by reading through the other frames with timesync enabled. You should see the same 1 second delay." << endl;
+
+ while(readFakeFrame(video.id, read));
+
+ output << "TEST COMPLETE" << endl << endl;
+
+}
+
+void test_parallel_multistream_singlefile()
+{
+ output << "Okay, now we test parallel processing with sync. " << endl
+ << "The easiest way is to open multiple iPacketStreams (each wraps its own filehandle), and read in parallel, using a shared SyncTime object." << endl
+ << endl
+ << "First, hang on while we make a new stream to test on: " << endl;
+
+ PacketStreamWriter write("test_parallel_multistream_singlefile");
+
+ PacketStreamSource video;
+ video.driver = "video driver name";
+ video.uri = "video driver fake uri";
+
+ PacketStreamSource infrared;
+ infrared.driver = "infrared driver name";
+ infrared.uri = "infrared driver fake uri";
+
+ write.AddSource(video);
+ write.AddSource(infrared);
+
+ output << "Okay, now let's write some fake frames: three \"infrared\" frames between each \"video\" frame, each spaced by 1 second: " << endl;
+
+ size_t jseq = 0;
+ for (size_t i = 0; i < 10; ++i)
+ {
+ writeFakeFrame(video, i, write);
+ output << ".";
+ output.flush();
+
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ for (size_t j = 0; j < 3; ++j)
+ {
+ writeFakeFrame(infrared, jseq++, write);
+ output << ".";
+ output.flush();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ }
+ }
+
+ write.WriteEnd();
+ write.Close();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ output << endl << "All finished. Now we start our sync timer, and launch two reader threads: " << endl << endl;
+
+ PacketStreamReader read1("test_parallel_multistream_singlefile");
+ PacketStreamReader read2("test_parallel_multistream_singlefile");
+ SyncTime t;
+
+ thread video_reader([&video, &read1, &t]() { while(readFakeFrame(video.id, read1)); });
+ thread infrared_reader([&infrared, &read2, &t]() { while(readFakeFrame(infrared.id, read2)); });
+
+ video_reader.join();
+ infrared_reader.join();
+
+ output << "TEST COMPLETE" << endl << endl;
+}
+
+void test_parallel_multistream_multifile()
+{
+ output << "Next, we shall test sync two streams recorded at different times. This should work just fine, so long as the sync clock is started at the same time, or is shared between threads." << endl
+ << "Preparing stream #1: " << endl;
+
+ PacketStreamWriter write_vid("test_parallel_multistream_multifile_vid");
+ PacketStreamSource video;
+ video.driver = "video driver name";
+ video.uri = "video driver fake uri";
+
+ write_vid.AddSource(video);
+ for (size_t i = 0; i < 10; ++i)
+ {
+ writeFakeFrame(video, i, write_vid);
+ output << ".";
+ output.flush();
+ std::this_thread::sleep_for(std::chrono::seconds(4));
+ }
+
+ write_vid.WriteEnd();
+ write_vid.Close();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ output << endl << "Preparing stream #2: " << endl;
+
+ PacketStreamWriter write_inf("test_parallel_multistream_multifile_inf");
+ PacketStreamSource infrared;
+ infrared.driver = "infrared driver name";
+ infrared.uri = "infrared driver fake uri";
+
+ write_inf.AddSource(infrared);
+ size_t jseq = 0;
+ for (size_t i = 0; i < 10; ++i)
+ {
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ for (size_t j = 0; j < 3; ++j)
+ {
+ writeFakeFrame(infrared, jseq++, write_inf);
+ output << ".";
+ output.flush();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ }
+ }
+ write_inf.WriteEnd();
+ write_inf.Close();
+
+ output << endl << "All finished. Now we start our sync timer, and launch two reader threads: " << endl << endl;
+
+ PacketStreamReader read_vid("test_parallel_multistream_multifile_vid");
+ PacketStreamReader read_inf("test_parallel_multistream_multifile_inf");
+ SyncTime t;
+
+ thread video_reader([&video, &read_vid, &t]() { while(readFakeFrame(video.id, read_vid)); });
+ thread infrared_reader([&infrared, &read_inf, &t]() { while(readFakeFrame(infrared.id, read_inf)); });
+ video_reader.join();
+ infrared_reader.join();
+}
+
+//Doesn't work yet
+void test_parallel_singlestream_singlefile()
+{
+ output << "The complex way to do parallel processing involves multiple threads reading a single file. This may be necessary when we cannot open the file multiple times (socket or pipe), or hypothetically if we are very resource constrained."
+ << endl << endl
+ << "First, hang on while we make a new stream to test on: " << endl;
+
+ PacketStreamWriter write("test_parallel_singlestream_singlefile");
+
+ PacketStreamSource video;
+ video.driver = "video driver name";
+ video.uri = "video driver fake uri";
+
+ PacketStreamSource infrared;
+ infrared.driver = "infrared driver name";
+ infrared.uri = "infrared driver fake uri";
+
+ write.AddSource(video);
+ write.AddSource(infrared);
+
+ output << "Okay, now let's write some fake frames: three \"infrared\" frames between each \"video\" frame, each spaced by 1 second: " << endl;
+
+ size_t jseq = 0;
+ for (size_t i = 0; i < 10; ++i)
+ {
+ writeFakeFrame(video, i, write);
+ output << ".";
+ output.flush();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ for (size_t j = 0; j < 3; ++j)
+ {
+ writeFakeFrame(infrared, jseq++, write);
+ output << ".";
+ output.flush();
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+ }
+ }
+
+ write.WriteEnd();
+ write.Close();
+
+ output << endl << "All finished. Now we start our sync timer, and launch two reader threads: " << endl << endl;
+
+ PacketStreamReader read("test_parallel_singlestream_singlefile");
+ SyncTime t;
+
+ std::thread video_reader([&video, &read, &t]() { while(readFakeFrame(video.id, read)); });
+ std::thread infrared_reader([&infrared, &read, &t]() { while(readFakeFrame(infrared.id, read)); });
+ video_reader.join();
+ infrared_reader.join();
+
+}
+
+int main (int, char**)
+{
+ output << "Some function tests for oPacketStream and iPacketStream. " << endl
+ << "These are just developer tests at this point, with human readable end-to-end function checks, rather than a formal set of unit test which returns pass/fail and provides full coverage. " << endl << endl;
+
+ test_simple();
+ test_parallel_multistream_singlefile();
+ test_parallel_multistream_multifile();
+ // test_parallel_singlestream_singlefile();
+
+}
+
+
+
diff --git a/source/ViewerAR.cc b/source/ViewerAR.cc
new file mode 100644
index 0000000000000000000000000000000000000000..6937866a4850decee934efc029ea5952c1b90967
--- /dev/null
+++ b/source/ViewerAR.cc
@@ -0,0 +1,906 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ *
+ * To be used with ORB-SLAM2.
+ * Includes GPL3 licensed code taken from the ORB-SLAM2 project.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+ * For more information see <https://github.com/raulmur/ORB_SLAM2>
+ */
+
+#include "ViewerAR.h"
+
+#ifndef RENDER_WITH_PANGOLIN
+#include <stdio.h>
+#include <stdlib.h>
+#include <GL/gl.h>
+#define GLAPI __attribute__((visibility("default")))
+#include <GL/osmesa.h>
+#include <pangolin/display/display_internal.h>
+#endif
+
+#include <opencv2/highgui/highgui.hpp>
+#include <mutex>
+#include <thread>
+#include <cstdlib>
+#include <QString>
+#include <QDateTime>
+#include <GL/gl.h>
+#include <QElapsedTimer>
+
+using namespace std;
+
+namespace MRLeo
+{
+
+QStringList ViewerAR::AR_OBJECT_TYPES = QStringList()
+ << "CUBE_A" << "CUBE_B" << "CUBE_C";
+
+
+/**
+ * @brief ViewerAR::ViewerAR
+ */
+ViewerAR::ViewerAR(int width, int height, const bool benchmarking,
+ const bool logTime) {
+ mRunning = true;
+ mWidth = width;
+ mHeight = height;
+ m3DObjectType = "CUBE_A";
+ mBenchmarking = benchmarking;
+ mLogTime = logTime;
+ mTotalTrackedPoints = 0;
+ mNumFramesDrawn = 0;
+}
+
+/**
+ * @brief ViewerAR::set3DObjectType
+ * @param objectTypeName
+ */
+void ViewerAR::set3DObjectType(QString objectTypeName)
+{
+ m3DObjectMutex.lock();
+ mPoseReady = true;
+ if (!ViewerAR::AR_OBJECT_TYPES.contains(objectTypeName)) {
+ objectTypeName = ViewerAR::AR_OBJECT_TYPES.first();
+ }
+ m3DObjectType = objectTypeName;
+ m3DObjectMutex.unlock();
+}
+
+/**
+ * @brief ViewerAR::Run
+ */
+void ViewerAR::Run()
+{
+ cv::Mat im, Tcw;
+ int slamStatus;
+ vector<cv::KeyPoint> vKeys;
+ vector<ORB_SLAM2::MapPoint*> vMPs;
+ quint32 frameid;
+
+ while (mRunning) {
+ getImagePose(frameid, im, Tcw, slamStatus, vKeys, vMPs);
+ if (!im.empty()) {
+ mWidth = im.cols;
+ mHeight = im.rows;
+ break;
+ }
+ }
+
+ QElapsedTimer processingTimeTimer;
+ processingTimeTimer.start();
+ qsrand(QTime::currentTime().msecsSinceStartOfDay());
+
+#ifndef DISABLE_IMAGE_OUTPUT
+#ifdef RENDER_WITH_PANGOLIN
+#ifdef RENDER_PANGOLIN_HEADLESS
+ pangolin::CreateWindowAndBind(QString::number(qrand()).toStdString(),
+ mWidth, mHeight, pangolin::Params({{"scheme", "headless"}}));
+#else
+ pangolin::CreateWindowAndBind(QString::number(qrand()).toStdString(),
+ mWidth, mHeight);
+#endif
+ pangolin::View& d_image = pangolin::Display("image");
+ d_image.SetBounds(0, 1.0f, 0, 1.0f, (float)mWidth / mHeight); // -?
+ d_image.SetLock(pangolin::LockLeft, pangolin::LockTop);
+
+ pangolin::GlTexture color_buffer(mWidth, mHeight);
+ pangolin::GlRenderBuffer depth_buffer(mWidth, mHeight);
+ pangolin::GlFramebuffer fbo_buffer(color_buffer, depth_buffer);
+ fbo_buffer.Bind();
+
+#else
+ OSMesaContext ctx = OSMesaCreateContextExt(OSMESA_RGB, 16, 0, 0, nullptr);
+ if (!ctx) {
+ fDebug << "OSMesaCreateContext failed";
+ return;
+ }
+ // Allocate the image buffer
+ GLubyte *buffer = new GLubyte[static_cast<size_t>(mWidth * mHeight) * 3];
+ if (!buffer) {
+ fDebug << "Alloc image buffer failed";
+ return;
+ }
+ // Bind the buffer to the context and make it current
+ if (!OSMesaMakeCurrent(ctx, buffer, GL_UNSIGNED_BYTE, mWidth, mHeight )) {
+ fDebug << "OSMesaMakeCurrent failed";
+ }
+#endif
+
+ glEnable(GL_DEPTH_TEST);
+ glEnable (GL_BLEND);
+
+ pangolin::GlTexture imageTexture(mWidth, mHeight, GL_RGB, false, 0, GL_RGB, GL_UNSIGNED_BYTE);
+ pangolin::OpenGlMatrixSpec P = pangolin::ProjectionMatrixRDF_TopLeft(mWidth, mHeight, fx, fy, cx, cy, 0.001, 1000);
+
+ double cubesize = 0.05;
+#endif
+ vector<Plane*> m3dObjectPoses;
+
+ mpSystem->DeactivateLocalizationMode();
+
+ qint64 startTime = 0;
+ int framesWithObject = 0;
+
+ QMap<int, qint64> framesTime;
+
+ int frames = 0;
+
+ while (mRunning) {
+#ifndef DISABLE_IMAGE_OUTPUT
+ glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
+
+ frames++;
+
+ // Activate camera view
+#ifdef RENDER_WITH_PANGOLIN
+ d_image.Activate();
+#else
+ glViewport(0, 0, mWidth, mHeight);
+#endif
+#endif
+
+ int arData = 0;
+
+ // Get last image and its computed pose from SLAM
+ getImagePose(frameid, im, Tcw, slamStatus, vKeys, vMPs);
+
+ if (mLogTime) {
+ startTime = processingTimeTimer.nsecsElapsed();
+ }
+
+#ifndef DISABLE_IMAGE_OUTPUT
+ glColor3f(1.0, 1.0, 1.0);
+ // Draw point cloud
+ drawTrackedPoints(vKeys, vMPs, im);
+ // Draw image
+ drawImageTexture(imageTexture, im);
+
+ glClear(GL_DEPTH_BUFFER_BIT);
+
+ // Load camera projection
+ glMatrixMode(GL_PROJECTION);
+ P.Load();
+ glMatrixMode(GL_MODELVIEW);
+ // Load camera pose
+ loadCameraPose(Tcw);
+#endif
+
+ framesWithObject++;
+
+ // Draw virtual things
+ if (slamStatus == 2) {
+ if (mBenchmarking) {
+ if (!m3dObjectPoses.empty()) {
+ if (framesWithObject > 20) {
+ mRemove3dObject = true;
+ framesWithObject = 0;
+ }
+ }
+ }
+ if (mAdd3dObject || mRemove3dObject) {
+ if (!m3dObjectPoses.empty()) {
+ for (size_t i = 0; i < m3dObjectPoses.size(); i++) {
+ delete m3dObjectPoses[i];
+ }
+ m3dObjectPoses.clear();
+ fDebug << "All 3D objects erased!";
+ framesWithObject = 0;
+ }
+ mRemove3dObject = false;
+ }
+ if (mAdd3dObject) {
+ Plane* pPlane = detectPlane(Tcw, vMPs, 50);
+ if (pPlane) {
+ fDebug << "New virtual cube inserted!";
+ m3dObjectPoses.push_back(pPlane);
+ mAdd3dObject = false;
+ framesWithObject = 0;
+ }
+ }
+ if (!m3dObjectPoses.empty()) {
+ // Recompute plane if there has been a loop closure or global BA
+ // In localization mode, map is not updated so we do not need to recompute
+ bool bRecompute = false;
+ if (mpSystem->MapChanged()) {
+ fDebug << "Map changed. All virtual elements are recomputed!";
+ bRecompute = true;
+ }
+ for (size_t i = 0; i < m3dObjectPoses.size(); i++) {
+ Plane* pPlane = m3dObjectPoses[i];
+ if (pPlane) {
+ if (bRecompute) {
+ pPlane->Recompute();
+ }
+ arData = 1;
+ // Draw cube
+#ifndef DISABLE_IMAGE_OUTPUT
+ glPushMatrix();
+ pPlane->glTpw.Multiply();
+ draw3DObject(cubesize);
+ glPopMatrix();
+#endif
+ if (mBenchmarking && !mForceColor) {
+#ifndef DISABLE_IMAGE_OUTPUT
+ glDisable(GL_DEPTH_TEST);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluOrtho2D(-1000, 1000, -1000, 1000);
+ glLoadIdentity();
+ glBegin(GL_POLYGON);
+ glColor3f(0, 1, 0);
+ glVertex2f(-1.0, 1.0);
+ glVertex2f(1.0, 1.0);
+ glVertex2f(1.0, 0.5);
+ glVertex2f(-1.0, 0.5);
+ glEnd();
+ glEnable(GL_DEPTH_TEST);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+#endif
+ }
+ }
+ }
+ }
+ } else if (mBenchmarking) {
+ framesWithObject = 0;
+ }
+ if (mForceColor) {
+ arData = 2;
+ glDisable(GL_DEPTH_TEST);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ glMatrixMode(GL_PROJECTION);
+ glLoadIdentity();
+ gluOrtho2D(-1000, 1000, -1000, 1000);
+ glLoadIdentity();
+ glBegin(GL_POLYGON);
+ glColor3f(1, 0, 1);
+ glVertex2f(-1.0, 1.0);
+ glVertex2f(1.0, 1.0);
+ glVertex2f(1.0, 0.5);
+ glVertex2f(-1.0, 0.5);
+ glEnd();
+ glEnable(GL_DEPTH_TEST);
+ glMatrixMode(GL_MODELVIEW);
+ glLoadIdentity();
+ }
+
+#ifdef DISABLE_IMAGE_OUTPUT
+ QImage outImage(new QImage());
+#else
+ QImage outImage(mWidth, mHeight, QImage::Format::Format_RGB888);
+ uchar* outimagePtr = outImage.bits();
+
+#ifdef RENDER_WITH_PANGOLIN
+#ifdef RENDER_PANGOLIN_HEADLESS
+ glFinish();
+ glReadBuffer(GL_BACK);
+ glPixelStorei(GL_PACK_ALIGNMENT, 1);
+ glReadPixels(0, 0, mWidth, mHeight, GL_RGB, GL_UNSIGNED_BYTE, outimagePtr);
+ outImage = outImage.mirrored(false, true);
+#else
+ outimagePtr = nullptr;
+#endif
+ pangolin::FinishFrame();
+#else
+ // Retrieved image is bottom to top, left to right and BGR.
+ // QImage is top to bottom, left to right and RGB.
+ for (int y = mHeight - 1; y >= 0; y--) {
+ for (int x = 0; x < mWidth; x++) {
+ int i = (y * mWidth + x) * 3 + 2;
+ *outimagePtr = buffer[i];
+ outimagePtr++;
+ i--;
+ *outimagePtr = buffer[i];
+ outimagePtr++;
+ i--;
+ *outimagePtr = buffer[i];
+ outimagePtr++;
+ }
+ }
+#endif
+#endif
+
+ emit newImageReady(frameid, outImage, arData);
+
+ if (mLogTime) {
+ framesTime.insert(frames, (processingTimeTimer.nsecsElapsed() - startTime) / 1000000);
+ }
+
+#ifndef USE_QMUTEX_AR
+ usleep(mT * 1000);
+#endif
+ }
+
+ QMapIterator<int, qint64> framesTimeIt(framesTime);
+ while (framesTimeIt.hasNext()) {
+ framesTimeIt.next();
+ fDebug << QString("frametime,%1,%2")
+ .arg(framesTimeIt.key()).arg(framesTimeIt.value());
+ }
+
+
+#ifndef DISABLE_IMAGE_OUTPUT
+#ifndef RENDER_WITH_PANGOLIN
+ OSMesaDestroyContext(ctx);
+#else
+ fbo_buffer.Unbind();
+#endif
+#endif
+
+ if (mLogTime) {
+ fDebug << "------------------------------------------------------";
+ fDebug << "Frames drawn: " << mNumFramesDrawn;
+ fDebug << "Total tracked points: " << mTotalTrackedPoints;
+ fDebug << "Average tracked points per frame: " << mTotalTrackedPoints /
+ qMax(mNumFramesDrawn, 1);
+ fDebug << "------------------------------------------------------";
+ }
+}
+
+/**
+ * @brief ViewerAR::Stop
+ * Stop the thread.
+ */
+void ViewerAR::Stop()
+{
+ mRunning = false;
+ mMutexPoseImage.lock();
+#ifdef USE_QMUTEX_AR
+ mQWaitCondition.wakeAll();
+#endif
+ mMutexPoseImage.unlock();
+}
+
+/**
+ * @brief ViewerAR::setImagePose
+ * @param im
+ * @param Tcw
+ * @param status
+ * @param vKeys
+ * @param vMPs
+ */
+void ViewerAR::setImagePose(
+ quint32 id, bool forcecolor,
+ const cv::Mat &im, const cv::Mat &Tcw, const int &status,
+ const vector<cv::KeyPoint> &vKeys, const vector<ORB_SLAM2::MapPoint*> &vMPs)
+{
+ mMutexPoseImage.lock();
+ mPoseReady = true;
+ mFrameId = id;
+ mForceColor = forcecolor;
+ mImage = im;//.clone();
+ mTcw = Tcw;//.clone();
+ mStatus = status;
+ mvKeys = vKeys;
+ mvMPs = vMPs;
+#ifdef USE_QMUTEX_AR
+ mQWaitCondition.wakeAll();
+#endif
+ mMutexPoseImage.unlock();
+}
+
+/**
+ * @brief ViewerAR::getImagePose
+ * @param im
+ * @param Tcw
+ * @param status
+ * @param vKeys
+ * @param vMPs
+ */
+void ViewerAR::getImagePose(
+ quint32 &frameid, cv::Mat &im, cv::Mat &Tcw, int &status,
+ std::vector<cv::KeyPoint> &vKeys, std::vector<ORB_SLAM2::MapPoint*> &vMPs)
+{
+ mMutexPoseImage.lock();
+#ifdef USE_QMUTEX_AR
+ if (!mPoseReady) {
+ mQWaitCondition.wait(&mMutexPoseImage);
+ }
+#endif
+ mPoseReady = false;
+ frameid = mFrameId;
+ im = mImage;
+ Tcw = mTcw;
+ status = mStatus;
+ vKeys = mvKeys;
+ vMPs = mvMPs;
+ mMutexPoseImage.unlock();
+}
+
+/**
+ * @brief ViewerAR::loadCameraPose
+ * @param Tcw
+ */
+void ViewerAR::loadCameraPose(const cv::Mat &Tcw)
+{
+ mMutexPoseImage.lock();
+ if (Tcw.empty()) {
+ mMutexPoseImage.unlock();
+ return;
+ }
+ pangolin::OpenGlMatrix M;
+ M.m[0] = Tcw.at<float>(0, 0);
+ M.m[1] = Tcw.at<float>(1, 0);
+ M.m[2] = Tcw.at<float>(2, 0);
+ M.m[3] = 0.0;
+ M.m[4] = Tcw.at<float>(0, 1);
+ M.m[5] = Tcw.at<float>(1, 1);
+ M.m[6] = Tcw.at<float>(2, 1);
+ M.m[7] = 0.0;
+ M.m[8] = Tcw.at<float>(0, 2);
+ M.m[9] = Tcw.at<float>(1, 2);
+ M.m[10] = Tcw.at<float>(2, 2);
+ M.m[11] = 0.0;
+ M.m[12] = Tcw.at<float>(0, 3);
+ M.m[13] = Tcw.at<float>(1, 3);
+ M.m[14] = Tcw.at<float>(2, 3);
+ M.m[15] = 1.0;
+ mMutexPoseImage.unlock();
+#ifdef RENDER_WITH_PANGOLIN
+ M.Load();
+#else
+ glLoadMatrixd(M.m);
+#endif
+}
+
+/**
+ * @brief ViewerAR::printStatus
+ * @param status
+ * @param bLocMode
+ * @param im
+ */
+void ViewerAR::drawStatus(const int &status, const bool &bLocMode, cv::Mat &im)
+{
+ Q_UNUSED(bLocMode);
+ switch(status) {
+ case 1:
+ addTextToImage("", im, 255, 0, 0);
+ break;
+ case 2:
+ addTextToImage("", im, 0, 255, 0);
+ break;
+ case 3:
+ addTextToImage("*", im, 255, 0, 0);
+ break;
+ }
+}
+
+/**
+ * @brief ViewerAR::addTextToImage
+ * @param s
+ * @param im
+ * @param r
+ * @param g
+ * @param b
+ */
+void ViewerAR::addTextToImage(const string &s, cv::Mat &im, const int r, const int g, const int b)
+{
+ cv::putText(im, s, cv::Point(10, im.rows - 10), cv::FONT_HERSHEY_PLAIN, 1.5, cv::Scalar(r, g, b), 2, 8);
+}
+
+/**
+ * @brief ViewerAR::drawImageTexture
+ * @param imageTexture
+ * @param im
+ */
+void ViewerAR::drawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im)
+{
+ if (!im.empty())
+ {
+ imageTexture.Upload(im.data, GL_RGB, GL_UNSIGNED_BYTE);
+ imageTexture.RenderToViewportFlipY();
+ }
+}
+
+/**
+ * @brief ViewerAR::draw3DObject
+ * @param size
+ * @param x
+ * @param y
+ * @param z
+ */
+void ViewerAR::draw3DObject(const double &size, const double x, const double y, const double z)
+{
+ pangolin::OpenGlMatrix M = pangolin::OpenGlMatrix::Translate(-x, -size - y, -z);
+ glPushMatrix();
+ M.Multiply();
+ GLfloat axis_min= static_cast<GLfloat>(-size);
+ GLfloat axis_max = static_cast<GLfloat>(size);
+
+ const GLfloat min = axis_min;
+ const GLfloat max = axis_max;
+
+ const GLfloat mi2 = min * 2;
+ const GLfloat ma2 = max * 2;
+
+ const GLfloat mi3 = min * 3;
+ const GLfloat ma3 = max * 3;
+
+ const GLfloat verts[] = {
+ mi2, min, max, ma2, min, max, mi2, max, max, ma2, max, max, // FRONT
+ mi2, min, min, mi2, max, min, ma2, min, min, ma2, max, min, // BACK
+
+ mi2, min, max, /**/ mi2, mi2, 0, /**/ mi2, max, max, mi2, min, min, mi2, max, min, // LEFT
+ ma2, min, min, /**/ ma2, mi2, 0, /**/ ma2, max, min, ma2, min, max, ma2, max, max, // RIGHT
+
+ mi2, max, max, ma2, max, max, mi2, max, min, ma2, max, min, // TOP
+
+ mi3, max, ma2, ma3, max, ma2, mi3, max, mi2, ma3, max, mi2, // GRASS
+
+ mi2, min, max, ma2, min, max, mi2, mi2, 0, ma2, mi2, 0, // ROOF 1
+ mi2, mi2, 0, ma2, mi2, 0, mi2, min, min, ma2, min, min, // ROOF 2
+ };
+
+ glVertexPointer(3, GL_FLOAT, 0, verts);
+ glEnableClientState(GL_VERTEX_ARRAY);
+
+ QColor colora;
+ QColor colorb;
+ QColor colorc;
+
+ m3DObjectMutex.lock();
+ switch (ViewerAR::AR_OBJECT_TYPES.indexOf(m3DObjectType)) {
+ case 0:
+ colora = QColor(206, 239, 255);
+ colorb = QColor(223, 245, 255);
+ colorc = QColor(239, 65, 53);
+ break;
+ case 1:
+ colora = QColor(249, 205, 48);
+ colorb = QColor(255, 255, 255);
+ colorc = QColor(22, 101, 161);
+ break;
+ default:
+ colora = QColor(255, 0, 0);
+ colorb = QColor(0, 0, 255);
+ colorc = QColor(0, 0, 255);
+ }
+ m3DObjectMutex.unlock();
+
+ glColor4f(colora.redF(), colora.greenF(), colora.blueF(), 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+ glDrawArrays(GL_TRIANGLE_STRIP, 4, 4);
+
+ glColor4f(colorb.redF(), colorb.greenF(), colorb.blueF(), 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 8, 5);
+ glDrawArrays(GL_TRIANGLE_STRIP, 13, 5);
+
+ glColor4f(0, 0.8f, 0, 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 22, 4);
+
+ glColor4f(0.8f, 0.5f, 0, 1.0f);
+ glDrawArrays(GL_TRIANGLE_STRIP, 26, 4);
+ glDrawArrays(GL_TRIANGLE_STRIP, 30, 4);
+
+ glDisableClientState(GL_VERTEX_ARRAY);
+ glPopMatrix();
+}
+
+/**
+ * @brief ViewerAR::drawTrackedPoints
+ * @param vKeys Tracked points
+ * @param vMPs Significant Point cloud points
+ * @param im
+ */
+void ViewerAR::drawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys, const std::vector<ORB_SLAM2::MapPoint *> &vMPs, cv::Mat &im)
+{
+ int mTotalTrackedPointsThisFrame = 0;
+ const size_t N = vKeys.size();
+ for (size_t i = 0; i < N; i++) {
+ if (vMPs[i]) {
+ mTotalTrackedPointsThisFrame++;
+ cv::circle(im, vKeys[i].pt, 1, cv::Scalar(0, 255, 0), -1);
+ }
+ }
+ mTotalTrackedPoints += mTotalTrackedPointsThisFrame;
+ mNumFramesDrawn += 1;
+}
+
+/**
+ * @brief ViewerAR::setCameraCalibration
+ * @param fx_
+ * @param fy_
+ * @param cx_
+ * @param cy_
+ */
+void ViewerAR::setCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_) {
+ fx = static_cast<double>(fx_);
+ fy = static_cast<double>(fy_);
+ cx = static_cast<double>(cx_);
+ cy = static_cast<double>(cy_);
+}
+
+
+/**
+ * @brief ViewerAR::detectPlane
+ * @param Tcw
+ * @param vMPs Point Cloud points
+ * @param iterations
+ * @return
+ */
+ViewerAR::Plane* ViewerAR::detectPlane(const cv::Mat Tcw, const std::vector<ORB_SLAM2::MapPoint*> &vMPs, const int iterations)
+{
+ // Retrieve 3D points
+ vector<cv::Mat> vPoints;
+ vPoints.reserve(vMPs.size());
+ vector<ORB_SLAM2::MapPoint *> vPointMP;
+ vPointMP.reserve(vMPs.size());
+ for(size_t i=0; i < vMPs.size(); i++) {
+ ORB_SLAM2::MapPoint * pMP = vMPs[i];
+ if (pMP) {
+ if (pMP->Observations() > 5) {
+ vPoints.push_back(pMP->GetWorldPos());
+ vPointMP.push_back(pMP);
+ }
+ }
+ }
+ const int N = vPoints.size();
+ if (N < 50) {
+ return nullptr;
+ }
+ // Indices for minimum set selection
+ vector<size_t> vAllIndices;
+ vAllIndices.reserve(N);
+ vector<size_t> vAvailableIndices;
+ for (int i = 0; i < N; i++) {
+ vAllIndices.push_back(i);
+ }
+ float bestDist = 1e10;
+ vector<float> bestvDist;
+ //RANSAC
+ for (int n=0; n<iterations; n++) {
+ vAvailableIndices = vAllIndices;
+ cv::Mat A(3, 4, CV_32F);
+ A.col(3) = cv::Mat::ones(3, 1, CV_32F);
+ // Get min set of points
+ for (short i = 0; i < 3; ++i) {
+ int randi = DUtils::Random::RandomInt(0, vAvailableIndices.size() - 1);
+ int idx = vAvailableIndices[randi];
+ A.row(i).colRange(0, 3) = vPoints[idx].t();
+ vAvailableIndices[randi] = vAvailableIndices.back();
+ vAvailableIndices.pop_back();
+ }
+ cv::Mat u, w, vt;
+ cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+ const float a = vt.at<float>(3, 0);
+ const float b = vt.at<float>(3, 1);
+ const float c = vt.at<float>(3, 2);
+ const float d = vt.at<float>(3, 3);
+ vector<float> vDistances(N, 0);
+ const float f = 1.0f / sqrt(a * a + b * b + c * c + d * d);
+ for(int i = 0; i < N; i++) {
+ vDistances[i] = fabs(
+ vPoints[i].at<float>(0) * a +
+ vPoints[i].at<float>(1) * b +
+ vPoints[i].at<float>(2) * c +
+ d) * f;
+ }
+ vector<float> vSorted = vDistances;
+ sort(vSorted.begin(),vSorted.end());
+ int nth = max((int)(0.2 * N), 20);
+ const float medianDist = vSorted[nth];
+ if (medianDist<bestDist) {
+ bestDist = medianDist;
+ bestvDist = vDistances;
+ }
+ }
+ // Compute threshold inlier/outlier
+ const float th = 1.4 * bestDist;
+ vector<bool> vbInliers(N, false);
+ int nInliers = 0;
+ for (int i = 0; i < N; i++) {
+ if (bestvDist[i] < th) {
+ nInliers++;
+ vbInliers[i] = true;
+ }
+ }
+ vector<ORB_SLAM2::MapPoint *> vInlierMPs(nInliers, nullptr);
+ int nin = 0;
+ for (int i = 0; i < N; i++) {
+ if (vbInliers[i]) {
+ vInlierMPs[nin] = vPointMP[i];
+ nin++;
+ }
+ }
+ return new Plane(vInlierMPs, Tcw);
+}
+
+/**
+ * @brief ViewerAR::Plane::Plane
+ * @param vMPs Point cloud points
+ * @param Tcw
+ */
+ViewerAR::Plane::Plane(const std::vector<ORB_SLAM2::MapPoint *> &vMPs, const cv::Mat &Tcw)
+ : mvMPs(vMPs), mTcw(Tcw.clone())
+{
+ rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f;
+ Recompute();
+}
+
+/**
+ * @brief ViewerAR::Plane::Recompute
+ */
+void ViewerAR::Plane::Recompute()
+{
+ const int N = mvMPs.size();
+ // Recompute plane with all points
+ cv::Mat A = cv::Mat(N, 4, CV_32F);
+ A.col(3) = cv::Mat::ones(N, 1, CV_32F);
+ o = cv::Mat::zeros(3, 1, CV_32F);
+ int nPoints = 0;
+ for (int i=0; i<N; i++) {
+ ORB_SLAM2::MapPoint* pMP = mvMPs[i];
+ if (!pMP->isBad()) {
+ cv::Mat Xw = pMP->GetWorldPos();
+ o+=Xw;
+ A.row(nPoints).colRange(0, 3) = Xw.t();
+ nPoints++;
+ }
+ }
+ A.resize(nPoints);
+ cv::Mat u, w, vt;
+ cv::SVDecomp(A, w, u, vt, cv::SVD::MODIFY_A | cv::SVD::FULL_UV);
+ float a = vt.at<float>(3, 0);
+ float b = vt.at<float>(3, 1);
+ float c = vt.at<float>(3, 2);
+ o = o * (1.0f / nPoints);
+ const float f = 1.0f / sqrt(a * a + b * b + c * c);
+ // Compute XC just the first time
+ if (XC.empty()) {
+ cv::Mat Oc = -mTcw.colRange(0, 3).rowRange(0, 3).t() * mTcw.rowRange(0, 3).col(3);
+ XC = Oc-o;
+ }
+ if ((XC.at<float>(0) * a + XC.at<float>(1) * b + XC.at<float>(2) * c) > 0) {
+ a = -a;
+ b = -b;
+ c = -c;
+ }
+ const float nx = a * f;
+ const float ny = b * f;
+ const float nz = c * f;
+ n = (cv::Mat_<float>(3, 1)<<nx, ny, nz);
+ cv::Mat up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
+ cv::Mat v = up.cross(n);
+ const float sa = cv::norm(v);
+ const float ca = up.dot(n);
+ const float ang = atan2(sa, ca);
+ Tpw = cv::Mat::eye(4, 4, CV_32F);
+ Tpw.rowRange(0, 3).colRange(0, 3) = ExpSO3(v * ang / sa) * ExpSO3(up * rang);
+ o.copyTo(Tpw.col(3).rowRange(0, 3));
+ glTpw.m[0] = Tpw.at<float>(0, 0);
+ glTpw.m[1] = Tpw.at<float>(1, 0);
+ glTpw.m[2] = Tpw.at<float>(2, 0);
+ glTpw.m[3] = 0.0;
+ glTpw.m[4] = Tpw.at<float>(0, 1);
+ glTpw.m[5] = Tpw.at<float>(1, 1);
+ glTpw.m[6] = Tpw.at<float>(2, 1);
+ glTpw.m[7] = 0.0;
+ glTpw.m[8] = Tpw.at<float>(0, 2);
+ glTpw.m[9] = Tpw.at<float>(1, 2);
+ glTpw.m[10] = Tpw.at<float>(2, 2);
+ glTpw.m[11] = 0.0;
+ glTpw.m[12] = Tpw.at<float>(0, 3);
+ glTpw.m[13] = Tpw.at<float>(1, 3);
+ glTpw.m[14] = Tpw.at<float>(2, 3);
+ glTpw.m[15] = 1.0;
+}
+
+
+/**
+ * @brief ViewerAR::Plane::Plane
+ * @param nx
+ * @param ny
+ * @param nz
+ * @param ox
+ * @param oy
+ * @param oz
+ */
+ViewerAR::Plane::Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz)
+{
+ n = (cv::Mat_<float>(3, 1) << nx, ny, nz);
+ o = (cv::Mat_<float>(3, 1) << ox, oy, oz);
+ cv::Mat up = (cv::Mat_<float>(3, 1) << 0.0f, 1.0f, 0.0f);
+ cv::Mat v = up.cross(n);
+ const float s = cv::norm(v);
+ const float c = up.dot(n);
+ const float a = atan2(s, c);
+ Tpw = cv::Mat::eye(4, 4, CV_32F);
+ const float rang = -3.14f / 2 + ((float)rand() / RAND_MAX) * 3.14f;
+ cout << rang;
+ Tpw.rowRange(0,3).colRange(0, 3) = ExpSO3(v * a / s)*ExpSO3(up * rang);
+ o.copyTo(Tpw.col(3).rowRange(0, 3));
+ glTpw.m[0] = Tpw.at<float>(0, 0);
+ glTpw.m[1] = Tpw.at<float>(1, 0);
+ glTpw.m[2] = Tpw.at<float>(2, 0);
+ glTpw.m[3] = 0.0;
+ glTpw.m[4] = Tpw.at<float>(0, 1);
+ glTpw.m[5] = Tpw.at<float>(1, 1);
+ glTpw.m[6] = Tpw.at<float>(2, 1);
+ glTpw.m[7] = 0.0;
+ glTpw.m[8] = Tpw.at<float>(0, 2);
+ glTpw.m[9] = Tpw.at<float>(1, 2);
+ glTpw.m[10] = Tpw.at<float>(2, 2);
+ glTpw.m[11] = 0.0;
+ glTpw.m[12] = Tpw.at<float>(0, 3);
+ glTpw.m[13] = Tpw.at<float>(1, 3);
+ glTpw.m[14] = Tpw.at<float>(2, 3);
+ glTpw.m[15] = 1.0;
+}
+
+/**
+ * @brief ExpSO3
+ * @param x
+ * @param y
+ * @param z
+ * @return
+ */
+cv::Mat ViewerAR::Plane::ExpSO3(const float &x, const float &y, const float &z)
+{
+ cv::Mat I = cv::Mat::eye(3, 3, CV_32F);
+ const float d2 = x * x + y * y + z * z;
+ const float d = sqrt(d2);
+ cv::Mat W = (cv::Mat_<float>(3, 3) <<
+ 0, -z, y,
+ z, 0, -x,
+ -y, x, 0);
+ const float eps = 1e-4;
+ if (d < eps) {
+ return (I + W + 0.5f * W * W);
+ }
+ return (I + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2);
+}
+
+/**
+ * @brief ExpSO3
+ * @param v
+ * @return
+ */
+cv::Mat ViewerAR::Plane::ExpSO3(const cv::Mat &v)
+{
+ return ExpSO3(v.at<float>(0), v.at<float>(1), v.at<float>(2));
+}
+
+}
+
+#ifndef RENDER_WITH_PANGOLIN
+namespace pangolin
+{
+const GLubyte gNotErrorLookup[] = "XX";
+const GLubyte* glErrorString(GLenum) {return gNotErrorLookup; }
+bool ShouldQuit() { return true; }
+void Handler::Keyboard(View&, unsigned char, int, int, bool) {}
+void HandlerScroll::Mouse(View&, MouseButton, int, int, bool, int) {}
+void Handler::Mouse(View&, MouseButton, int, int, bool, int) {}
+void Handler::MouseMotion(View&, int, int, int) {}
+void Handler::PassiveMouseMotion(View&, int, int, int) {}
+void HandlerScroll::Special(View&, InputSpecial, float, float, float, float, float, float, int) {}
+void Handler::Special(View&, InputSpecial, float, float, float, float, float, float, int) {}
+extern PangolinGl dummyContext;
+}
+#endif
diff --git a/source/ViewerAR.h b/source/ViewerAR.h
new file mode 100644
index 0000000000000000000000000000000000000000..089caa77c3a8008d9b10b26a3cb3a60002f6bd24
--- /dev/null
+++ b/source/ViewerAR.h
@@ -0,0 +1,137 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ *
+ * To be used with ORB-SLAM2.
+ * Includes GPL3 licensed code taken from the ORB-SLAM2 project.
+ * Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
+ * For more information see <https://github.com/raulmur/ORB_SLAM2>
+ */
+
+#ifndef VIEWERAR_H
+#define VIEWERAR_H
+
+#include <mutex>
+#include <opencv2/core/core.hpp>
+#include <pangolin/pangolin.h>
+#include <string>
+#include "System.h"
+#include <QImage>
+#include "global.h"
+#include <QMutex>
+#include <QWaitCondition>
+
+namespace MRLeo
+{
+
+/**
+ * @brief The ViewerAR class
+ */
+class ViewerAR : public QObject {
+
+ Q_OBJECT
+
+public:
+ class Plane {
+ public:
+ Plane(const std::vector<ORB_SLAM2::MapPoint*> &vMPs, const cv::Mat &Tcw);
+ Plane(const float &nx, const float &ny, const float &nz, const float &ox, const float &oy, const float &oz);
+ void Recompute();
+ //transformation from world to the plane
+ pangolin::OpenGlMatrix glTpw;
+ private:
+ cv::Mat ExpSO3(const float &x, const float &y, const float &z);
+ cv::Mat ExpSO3(const cv::Mat &v);
+ //normal
+ cv::Mat n;
+ //origin
+ cv::Mat o;
+ //arbitrary orientation along normal
+ float rang;
+ //transformation from world to the plane
+ cv::Mat Tpw;
+ //MapPoints that define the plane
+ std::vector<ORB_SLAM2::MapPoint*> mvMPs;
+ //camera pose when the plane was first observed (to compute normal direction)
+ cv::Mat mTcw, XC;
+ };
+
+ static QStringList AR_OBJECT_TYPES;
+
+ ViewerAR(int width, int height, const bool benchmarking, const bool logTime);
+ void setFPS(const double fps) { mFPS = fps; mT = 1e3/fps; }
+ void setSLAM(ORB_SLAM2::System* pSystem) { mpSystem = pSystem; }
+ void Run();
+ void Stop();
+ void setImagePose(
+ quint32 frameid, bool forcecolor, const cv::Mat &im, const cv::Mat &Tcw, const int &status,
+ const std::vector<cv::KeyPoint> &vKeys, const std::vector<ORB_SLAM2::MapPoint*> &vMPs);
+ void getImagePose(
+ quint32 &frameid, cv::Mat &im, cv::Mat &Tcw, int &status,
+ std::vector<cv::KeyPoint> &vKeys, std::vector<ORB_SLAM2::MapPoint*> &vMPs);
+ void set3DObjectType(QString object);
+ void add3DObject() { mRemove3dObject = true; mAdd3dObject = true; }
+ void remove3DObject() { mRemove3dObject = true; }
+ void setCameraCalibration(const float &fx_, const float &fy_, const float &cx_, const float &cy_);
+
+signals:
+ // Extend second parameter from int to larger data structure
+ // if needed to store larger.
+ void newImageReady(quint32 frameid, QImage image, int metadata);
+
+private:
+ void drawStatus(const int &status, const bool &bLocMode, cv::Mat &im);
+ void addTextToImage(const std::string &s, cv::Mat &im, const int r=0, const int g=0, const int b=0);
+ void loadCameraPose(const cv::Mat &Tcw);
+ void drawImageTexture(pangolin::GlTexture &imageTexture, cv::Mat &im);
+ void draw3DObject(const double &size, const double x=0, const double y=0, const double z=0);
+ void drawTrackedPoints(const std::vector<cv::KeyPoint> &vKeys, const std::vector<ORB_SLAM2::MapPoint*> &vMPs, cv::Mat &im);
+ Plane* detectPlane(const cv::Mat Tcw, const std::vector<ORB_SLAM2::MapPoint*> &vMPs, const int iterations=50);
+
+ bool mRunning;
+
+ //SLAM
+ ORB_SLAM2::System* mpSystem;
+
+ // frame rate
+ double mFPS, mT;
+ // calibration
+ double fx, fy, cx, cy;
+
+ // Last processed image and computed pose by the SLAM
+#ifdef USE_QMUTEX_AR
+ QMutex mMutexPoseImage;
+ QMutex m3DObjectMutex;
+ QWaitCondition mQWaitCondition;
+#else
+ std::mutex mMutexPoseImage;
+ std::mutex m3DObjectMutex;
+#endif
+ bool mPoseReady = true;
+ cv::Mat mTcw;
+ cv::Mat mImage;
+ quint32 mFrameId;
+ int mStatus;
+ std::vector<cv::KeyPoint> mvKeys;
+ std::vector<ORB_SLAM2::MapPoint*> mvMPs;
+ bool mAdd3dObject = false;
+ bool mRemove3dObject = false;
+ int mWidth;
+ int mHeight;
+ bool mForceColor;
+ QString m3DObjectType;
+ bool mBenchmarking;
+ bool mLogTime;
+ int mTotalTrackedPoints;
+ int mNumFramesDrawn;
+};
+
+
+}
+
+
+#endif // VIEWERAR_H
+
+
diff --git a/source/cannyfilter.cpp b/source/cannyfilter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..ac985331a6eb2d0d9b989d899649bb69f3b7e183
--- /dev/null
+++ b/source/cannyfilter.cpp
@@ -0,0 +1,75 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "cannyfilter.h"
+#include <QtCore/qbuffer.h>
+#include <QDebug>
+#include <QImage>
+#include <QtMath>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/ximgproc.hpp>
+
+namespace MRLeo {
+
+/**
+ * @brief CannyFilter::medianMat
+ * @param mat OpenCV Mat image
+ * @return The median value for all pixels in the mat image.
+ */
+double CannyFilter::medianMat(cv::Mat mat)
+{
+ mat = mat.reshape(0, 1); // spread Input Mat to single row
+ std::vector<double> vecFromMat;
+ mat.copyTo(vecFromMat);
+ // Partial sort the vector.
+ std::nth_element(vecFromMat.begin(),
+ vecFromMat.begin() + vecFromMat.size() / 2,
+ vecFromMat.end());
+ // Pick the middle element, the median
+ return vecFromMat[vecFromMat.size() / 2];
+}
+
+/**
+ * @brief CannyFilter::process
+ * @param session Session id
+ * @param image OpenCV Mat
+ * @param frameid Image id.
+ *
+ * Run Canny edge detector on the received image.
+ */
+void CannyFilter::process(qint32 session, quint32 frameid, cvMatPtr image)
+{
+ fDebug << "CannyFilter::process";
+ Q_UNUSED(session);
+ cv::Mat dst;
+ dst.create(image->size(), image->type());
+ cv::Mat src_gray;
+ cvtColor(*image, src_gray, cv::COLOR_RGB2GRAY);
+ const int kernel_size = 3;
+ cv::Mat detected_edges;
+ blur(src_gray, detected_edges, cv::Size(3, 3));
+ double sigma = 0.33;
+ double v = medianMat(detected_edges);
+ int lower = static_cast<int>(std::max(0.0, (1.0 - sigma) * v));
+ int upper = static_cast<int>(std::min(255.0, (1.0 + sigma) * v));
+ cv::Canny(detected_edges, detected_edges, lower, upper, kernel_size);
+ dst = cv::Scalar::all(0);
+ image->copyTo(dst, detected_edges);
+ if (mEmitJPEG) {
+ fDebug << "Emitting JPEG";
+ emit sendFile(mSession, NetworkConnection::File(
+ NetworkConnection::FileType::IMAGE, frameid, jpegFromMat(dst)));
+ }
+ if (mEmitQImage) {
+ fDebug << "Emitting QImage";
+ emit sendQImage(mSession, frameid, qImageFromMat(dst));
+ }
+}
+
+}
diff --git a/source/cannyfilter.h b/source/cannyfilter.h
new file mode 100644
index 0000000000000000000000000000000000000000..997f331d8e01ab04b79f58400d89972791a56061
--- /dev/null
+++ b/source/cannyfilter.h
@@ -0,0 +1,47 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef CANNYFILTER_H
+#define CANNYFILTER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QImage>
+#include <QMutex>
+#include <QObject>
+#include <opencv/cxcore.hpp>
+#include "imageprocesser.h"
+
+namespace MRLeo {
+
+/**
+ * @brief The CannyFilter class
+ *
+ * Example class that inherits ImageProcessor and runs OpenCV's
+ * Canny Edge Detector on OpenCV Mat images and outputs the result
+ * using sendImage() and sendFile() signals.
+ */
+class CannyFilter : public ImageProcesser {
+ Q_OBJECT
+
+public:
+ CannyFilter(qint32 session) { mSession = session; }
+ void setConfig(QJsonObject) override {}
+ void setCalibrateMode(bool) override {}
+ void calibrateCamera() override {}
+ void setUserInteractionConfiguration(QJsonObject) override {}
+
+protected:
+ virtual void process(qint32 session, quint32 frameid, cvMatPtr image) override;
+
+private:
+ double medianMat(cv::Mat Input);
+};
+
+}
+
+#endif // CANNYFILTER_H
diff --git a/source/echoimage.cpp b/source/echoimage.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9b0ad7cc951519d8b7d25276aeda716e7b6b9579
--- /dev/null
+++ b/source/echoimage.cpp
@@ -0,0 +1,84 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "echoimage.h"
+#include <QtCore/qbuffer.h>
+#include <QDebug>
+#include <QImage>
+#include <QtMath>
+#include <QElapsedTimer>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/ximgproc.hpp>
+
+namespace MRLeo {
+
+/**
+ * @brief EchoImage::EchoImage
+ * @param session
+ */
+EchoImage::EchoImage(qint32 session, bool benchmarking) {
+ mRunning = true;
+ mSession = session;
+ mBenchmarking = benchmarking;
+}
+
+/**
+ * @brief EchoImage::~EchoImage
+ */
+EchoImage::~EchoImage() {
+ mRunning = false;
+}
+
+/**
+ * @brief EchoImage::process
+ * @param session Session id
+ * @param image OpenCV Mat
+ * @param frameid Image id.
+ *
+ * Return the received image.
+ */
+void EchoImage::process(qint32 session, quint32 frameid, cvMatPtr image)
+{
+ Q_UNUSED(session);
+ if (mRunning && mLogTime) {
+ mProcessingFinishedTimes.insert(frameid, mUptime->nsecsElapsed());
+ }
+ int metadata = 0;
+ cv::Mat dstImage = *image;
+
+ bool colorFrame = false;
+ if (mIdentifyColorFrame) {
+ colorFrame = true;
+ for (int i = 0; i < 10; i++) {
+ unsigned char * p = dstImage.ptr(20 * i, 20 * i);
+ if (p[0] != 252 || p[1] != 0 || p[2] != 252) {
+ colorFrame = false;
+ break;
+ }
+ }
+ if (colorFrame) {
+ //fDebug << "Color frame found";
+ metadata = 2;
+ dstImage = cv::Scalar(255, 0, 255);
+ }
+ }
+ if (mEmitJPEG) {
+ cvtColor(*image, dstImage, cv::COLOR_BGR2RGB);
+ emit sendFile(mSession, NetworkConnection::File(
+ (mBenchmarking ?
+ NetworkConnection::FileType::IMAGE_WITH_METADATA :
+ NetworkConnection::FileType::IMAGE),
+ frameid, jpegFromMat(dstImage, mBenchmarking, metadata)));
+ }
+ if (mEmitQImage) {
+ emit sendQImage(mSession, frameid, qImageFromMat(dstImage));
+ }
+}
+
+}
diff --git a/source/echoimage.h b/source/echoimage.h
new file mode 100644
index 0000000000000000000000000000000000000000..7a1b4941a0e2df102712cbf3524b73e2200b43e9
--- /dev/null
+++ b/source/echoimage.h
@@ -0,0 +1,48 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef ECHOIMAGE_H
+#define ECHOIMAGE_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QImage>
+#include <QMutex>
+#include <QObject>
+#include <opencv/cxcore.hpp>
+#include "imageprocesser.h"
+
+namespace MRLeo {
+
+/**
+ * @brief The EchoImage class
+ *
+ * Example class that inherits ImageProcessor and runs OpenCV's
+ * Canny Edge Detector on OpenCV Mat images and outputs the result
+ * using sendImage() and sendFile() signals.
+ */
+class EchoImage : public ImageProcesser {
+ Q_OBJECT
+
+public:
+ EchoImage(qint32 session, bool benchmarking);
+ virtual ~EchoImage() override;
+ void setConfig(QJsonObject) override {}
+ void setCalibrateMode(bool) override {}
+ void calibrateCamera() override {}
+ void setUserInteractionConfiguration(QJsonObject) override {}
+
+protected:
+ virtual void process(qint32 session, quint32 frameid, cvMatPtr image) override;
+
+private:
+ double medianMat(cv::Mat Input);
+};
+
+}
+
+#endif // ECHOIMAGE_H
diff --git a/source/global.h b/source/global.h
new file mode 100644
index 0000000000000000000000000000000000000000..35b1ebd19e1d84d48ba283ffa5cce63513f876e1
--- /dev/null
+++ b/source/global.h
@@ -0,0 +1,28 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef GLOBAL_H
+#define GLOBAL_H
+
+#include <QByteArray>
+#include <QImage>
+#include <QSharedPointer>
+#include <opencv/cxcore.hpp>
+#include <string.h>
+#include <QDebug>
+
+namespace MRLeo {
+
+typedef QSharedPointer<QByteArray> QByteArrayPtr;
+typedef QSharedPointer<cv::Mat> cvMatPtr;
+
+#define __FILENAME__ (strrchr(__FILE__, '/') ? strrchr(__FILE__, '/') + 1 : __FILE__)
+#define fDebug qDebug().noquote() << __FILENAME__ << ":"
+
+}
+
+#endif // GLOBAL_H
diff --git a/source/imageprocesser.cpp b/source/imageprocesser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4cb4d887323a2312a335423600654fed80a757fe
--- /dev/null
+++ b/source/imageprocesser.cpp
@@ -0,0 +1,319 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include <QDebug>
+#include <QImage>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <QtCore/QBuffer>
+#include <QElapsedTimer>
+#include "imageprocesser.h"
+#include <mutex>
+
+namespace MRLeo {
+
+ImageProcesser::ImageProcesser()
+{
+ mUptime = new QElapsedTimer;
+ mUptime->start();
+}
+
+/**
+ * @brief ImageProcesser::matFromQImage
+ * @param img QImage
+ * @return OpenCV Mat image
+ *
+ * Convert the image. Supports 24 bit RGB and 32 bit RGBA.
+ */
+cv::Mat ImageProcesser::matFromQImage(QImage img)
+{
+ if (img.format() == QImage::Format_RGB32) {
+ img = img.convertToFormat(QImage::Format_RGB888);
+ }
+ return cv::Mat(img.height(), img.width(), CV_8UC3, img.bits(),
+ static_cast<size_t>(img.bytesPerLine())).clone();
+}
+
+/**
+ * @brief ImageProcesser::qImageFromMat
+ * @param mat OpenCV Mat image
+ * @return QImage
+ *
+ * Converts the Mat image to 24 bit RGB QImage.
+ */
+QImage ImageProcesser::qImageFromMat(cv::Mat mat)
+{
+ return QImage(static_cast<uchar *>(mat.data), mat.cols, mat.rows,
+ static_cast<int>(mat.step), QImage::Format_RGB888);
+}
+
+/**
+ * @brief ImageProcesser::jpegFromQImage
+ * @param image The image to convert.
+ * @return Byte array with JPEG file.
+ *
+ * Converts the QImage to a JPEG byte array with header.
+ */
+QByteArrayPtr ImageProcesser::jpegFromQImage(
+ QImage image, bool addmetadata, int metadata)
+{
+ QByteArrayPtr retarray(new QByteArray());
+ if (!image.isNull()) {
+ QBuffer qBuffer(retarray.data());
+ qBuffer.open(QIODevice::WriteOnly);
+ image.save(&qBuffer, "JPG", mJpegQualityLevel);
+ if (mImagesLogJpegSize > 0) {
+ mImagesLogJpegSize--;
+ fDebug << "JPEG image size: " << (retarray->size() * 8 / 1000) << " kbit";
+ }
+ }
+ if (addmetadata) {
+ retarray->append(4, reinterpret_cast<unsigned char>(static_cast<quint8>(metadata)));
+ }
+ return retarray;
+}
+
+/**
+ * @brief ImageProcesser::jpegFromMat
+ * @param image The image to convert
+ * @return Byte array with JPEG file.
+ *
+ * Converts the OpenCV Mat to a JPEG byte array with header.
+ */
+QByteArrayPtr ImageProcesser::jpegFromMat(
+ cv::Mat mat, bool addmetadata, int metadata)
+{
+ std::vector<uchar> buffer;
+ std::vector<int> param(2);
+ param[0] = cv::IMWRITE_JPEG_QUALITY;
+ param[1] = mJpegQualityLevel; // 0-100
+ cv::imencode(".jpg", mat, buffer, param);
+ auto retarray = QByteArrayPtr(
+ new QByteArray(
+ reinterpret_cast<const char *>(buffer.data()),
+ static_cast<int>(buffer.size())));
+ if (mImagesLogJpegSize > 0) {
+ mImagesLogJpegSize--;
+ fDebug << "JPEG image size: " << (retarray->size() * 8 / 1000) << " kbit";
+ }
+ if (addmetadata) {
+ retarray->append(4, static_cast<char>(metadata));
+ }
+ return retarray;
+}
+
+/**
+ * @brief ImageProcesser::addFileToProcessQueue
+ * @param session Session id
+ * @param file Image to be added.
+ *
+ * Add a JPEG file to the process queue.
+ */
+void ImageProcesser::addFileToProcessQueue(qint32 session, NetworkConnection::File file)
+{
+ if (mAllowAllSources || session == mSession) {
+ mProcessQueueMutex.lock();
+ mFileProcessQueue = file;
+ mFileQueuePosition = file.id;
+ mProcessQueueMutex.unlock();
+ }
+}
+
+/**
+ * @brief ImageProcesser::addQImageToProcessQueue
+ * @param session Session id
+ * @param image Image to be added.
+ * @param frameid Image's id number.
+ *
+ * Add a QImage to the process queue.
+ */
+void ImageProcesser::addQImageToProcessQueue(qint32 session, quint32 frameid, QImage image)
+{
+ if (mAllowAllSources || session == mSession) {
+ mProcessQueueMutex.lock();
+ mQImageProcessQueue = image;
+ mQImageQueuePosition = frameid;
+ mProcessQueueMutex.unlock();
+ }
+}
+
+/**
+ * @brief ImageProcesser::addQImageToProcessQueue
+ * @param session Session id
+ * @param image Image to be added.
+ * @param frameid Image's id number.
+ *
+ * Add an OpenCV Mat to the process queue.
+ */
+void ImageProcesser::addMatToProcessQueue(qint32 session, quint32 frameid, cvMatPtr image)
+{
+ if (mAllowAllSources || session == mSession) {
+ mProcessQueueMutex.lock();
+ mMatProcessQueue = image;
+ mMatQueuePosition = frameid;
+ mProcessQueueMutex.unlock();
+ }
+}
+
+/**
+ * @brief ImageProcesser::processFile
+ * @param session
+ * @param image Replaced if more recent image exists in process queue.
+ *
+ * Process the latest image in the JPEG file process queue.
+ */
+void ImageProcesser::processFile(qint32 session, NetworkConnection::File file)
+{
+ if ((!mAllowAllSources && session != mSession) ||
+ file.type != NetworkConnection::FileType::IMAGE) {
+ return;
+ }
+ mProcessQueueMutex.lock();
+ file = mFileProcessQueue;
+ mFileProcessQueue = NetworkConnection::File(NetworkConnection::FileType::NONE, 0, nullptr);
+ mProcessQueueMutex.unlock();
+ if (file.data.isNull()) {
+ mSkippedImages++;
+ return;
+ }
+ if (mSkippedImages > 0) {
+ fDebug << "Images arrived faster than could be processed. Skipped:"
+ << mSkippedImages;
+ mSkippedImages = 0;
+ }
+ QImage qimg = QImage::fromData(*file.data, "JPEG");
+ process(mSession, file.id, cvMatPtr(new cv::Mat(matFromQImage(qimg))));
+}
+
+/**
+ * @brief ImageProcesser::processQImage
+ * @param session Session id
+ * @param image QImage replaced if more recent image exists in process queue.
+ * @param frameid Image id.
+ *
+ * Process the latest image in the QImage process queue.
+ */
+void ImageProcesser::processQImage(qint32 session, quint32 frameid, QImage image)
+{
+ if (!mAllowAllSources && session != mSession) {
+ return;
+ }
+ mProcessQueueMutex.lock();
+ image = mQImageProcessQueue;
+ frameid = mQImageQueuePosition;
+ mQImageProcessQueue = QImage();
+ mProcessQueueMutex.unlock();
+ if (image.isNull()) {
+ mSkippedImages++;
+ return;
+ }
+ if (mSkippedImages > 0) {
+ fDebug << "Images arrived faster than could be processed. Skipped:"
+ << mSkippedImages;
+ mSkippedImages = 0;
+ }
+ process(mSession, frameid, cvMatPtr(new cv::Mat(matFromQImage(image))));
+}
+
+/**
+ * @brief ImageProcesser::processMat
+ * @param session Session id
+ * @param mat OpenCV Mat replaced if more recent image exists in process queue.
+ * @param frameid Image id
+ *
+ * Process the latest image in the Mat process queue.
+ */
+void ImageProcesser::processMat(qint32 session, quint32 frameid, cvMatPtr mat)
+{
+ if (!mAllowAllSources && session != mSession) {
+ return;
+ }
+ mProcessQueueMutex.lock();
+ mat = mMatProcessQueue;
+ frameid = mMatQueuePosition;
+ mMatProcessQueue.clear();
+ mProcessQueueMutex.unlock();
+ if (mat.isNull()) {
+ mSkippedImages++;
+ return;
+ }
+ if (mSkippedImages > 0) {
+ fDebug << "Images arrived faster than could be processed. Skipped:"
+ << mSkippedImages;
+ mSkippedImages = 0;
+ }
+ if (mLogTime) {
+ mProcessingStartedTimes.insert(frameid, mUptime->nsecsElapsed());
+ }
+ process(mSession, frameid, mat);
+}
+
+
+/**
+ * @brief ImageProcesser::getProcessingTimes
+ * @return
+ */
+const QMap<quint32, qint32> ImageProcesser::getProcessingTimes()
+{
+ mRunning = false;
+ QMapIterator<quint32, qint64> processedIt(mProcessingFinishedTimes);
+ QMap<quint32, qint32> retMap;
+ while (processedIt.hasNext()) {
+ processedIt.next();
+ if (mProcessingStartedTimes.contains(processedIt.key())) {
+ retMap.insert(processedIt.key(),
+ ((processedIt.value() - mProcessingStartedTimes.value(processedIt.key()))
+ / 1000000));
+ }
+ }
+ return retMap;
+}
+
+/**
+ * @brief ImageProcesser::triggerActionA
+ * Triggers action A.
+ * What action A means needs to be defined in the
+ * implementing class.
+ */
+void ImageProcesser::triggerActionA()
+{
+ fDebug << "ImageProcesser::triggerA";
+ // After parsing the value the implementing class
+ // needs to set it to false.
+ mTriggeredA = true;
+}
+
+/**
+ * @brief ImageProcesser::triggerActionB
+ * Triggers action B.
+ * What action B means needs to be defined in the
+ * implementing class.
+ */
+void ImageProcesser::triggerActionB()
+{
+ fDebug << "ImageProcesser::triggerB";
+ // After parsing the value the implementing class
+ // needs to set it to false.
+ mTriggeredB = true;
+}
+
+/**
+ * @brief ImageProcesser::triggerActionC
+ * Triggers action C.
+ * What action C means needs to be defined in the
+ * implementing class.
+ */
+void ImageProcesser::triggerActionC()
+{
+ fDebug << "ImageProcesser::triggerC";
+ // After parsing the value the implementing class
+ // needs to set it to false.
+ mTriggeredC = true;
+}
+
+}
diff --git a/source/imageprocesser.h b/source/imageprocesser.h
new file mode 100644
index 0000000000000000000000000000000000000000..f580aa0c779f03ff6a021eb377d278855f85ef12
--- /dev/null
+++ b/source/imageprocesser.h
@@ -0,0 +1,96 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef IMAGEPROCESSER_H
+#define IMAGEPROCESSER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QImage>
+#include <QMutex>
+#include <QObject>
+
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class ImageProcesser : public QObject
+{
+ Q_OBJECT
+
+public:
+ QImage qImageFromMat(cv::Mat img);
+ QByteArrayPtr jpegFromQImage(
+ QImage image, bool addmetadata=false, int metadata=0);
+ QByteArrayPtr jpegFromMat(
+ cv::Mat image, bool addmetadata=false, int metadata=0);
+ cv::Mat matFromQImage(QImage img);
+
+ ImageProcesser();
+ virtual ~ImageProcesser() {}
+
+ const QMap<quint32, qint32> getProcessingTimes();
+
+signals:
+ void sendFile(qint32 session, NetworkConnection::File file);
+ void sendQImage(qint32 session, quint32 frameid, QImage image);
+
+public slots:
+ void setEmitJPEG(bool enable) { mEmitJPEG = enable; }
+ void setEmitQImage(bool enable) { mEmitQImage = enable; }
+ void setIdentifyColorFrame(bool enable) { mIdentifyColorFrame = enable; }
+ virtual void setDebugMode(bool enable) { mDebugMode = enable; }
+ void setAllowAllSources(bool allow) { mAllowAllSources = allow; }
+ void setJpgegQualityLevel(int level) { mJpegQualityLevel = level; }
+ void setLogTime(bool enable) { mLogTime = enable; }
+ void triggerActionA();
+ void triggerActionB();
+ void triggerActionC();
+ void addFileToProcessQueue(qint32 session, NetworkConnection::File file);
+ void addMatToProcessQueue(qint32 session, quint32 frameid, cvMatPtr image);
+ void addQImageToProcessQueue(qint32 session, quint32 frameid, QImage image);
+ void processFile(qint32 session, NetworkConnection::File file);
+ void processMat(qint32 session, quint32 frameid, cvMatPtr image);
+ void processQImage(qint32 session, quint32 frameid, QImage image);
+ virtual void setConfig(QJsonObject calibration) = 0;
+ virtual void setCalibrateMode(bool enabled) = 0;
+ virtual void calibrateCamera() = 0;
+ virtual void setUserInteractionConfiguration(QJsonObject obj) = 0;
+
+protected:
+ virtual void process(qint32 session, quint32 frameid, cvMatPtr image) = 0;
+
+ qint32 mSession;
+ QMutex mProcessQueueMutex;
+ NetworkConnection::File mFileProcessQueue;
+ cvMatPtr mMatProcessQueue;
+ QImage mQImageProcessQueue;
+ quint32 mMatQueuePosition = 0;
+ quint32 mFileQueuePosition = 0;
+ quint32 mQImageQueuePosition = 0;
+ bool mBenchmarking = false;
+ bool mEmitJPEG = false;
+ bool mEmitQImage = false;
+ bool mTriggeredA = false;
+ bool mTriggeredB = false;
+ bool mTriggeredC = false;
+ bool mDebugMode = false;
+ bool mAllowAllSources = false;
+ bool mIdentifyColorFrame = false;
+ int mSkippedImages = 0;
+ bool mLogTime = false;
+ int mJpegQualityLevel = 80;
+ int mImagesLogJpegSize = 10;
+ QMap<quint32, qint64> mProcessingStartedTimes;
+ QMap<quint32, qint64> mProcessingFinishedTimes;
+ QElapsedTimer* mUptime;
+ bool mRunning;
+};
+
+}
+
+#endif // IMAGEPROCESSER_H
diff --git a/source/imagewriter.cpp b/source/imagewriter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..929d3da8b4c92b9e87317490452a07d6bbcf77a7
--- /dev/null
+++ b/source/imagewriter.cpp
@@ -0,0 +1,58 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "imagewriter.h"
+#include <QDataStream>
+#include <QDebug>
+#include <QDir>
+#include <QFile>
+#include <vector>
+#include <stdio.h>
+#include <opencv2/opencv.hpp>
+
+namespace MRLeo {
+
+void ImageWriter::writeImage(qint32 session, NetworkConnection::File file)
+{
+ if (!mEnabled || file.type != NetworkConnection::FileType::IMAGE) {
+ return;
+ }
+ QDir dir(mPath);
+ if (!dir.exists()) {
+ dir.mkpath(".");
+ }
+ QString filename =
+ QString(dir.path() + QDir::separator() + "mrleo_image_%1_%2_%3.jpg")
+ .arg(session).arg(file.type).arg(file.id);
+ fDebug << "Writing: " << filename;
+ QFile filestream(filename);
+ filestream.open(QIODevice::WriteOnly);
+ filestream.write(*file.data);
+ filestream.close();
+}
+
+
+void ImageWriter::writeMat(qint32 session, quint32 frameid, cvMatPtr image)
+{
+ if (!mEnabled) {
+ return;
+ }
+ QDir dir(mPath);
+ if (!dir.exists()) {
+ dir.mkpath(".");
+ }
+ QString filename =
+ QString(dir.path() + QDir::separator() + "mrleo_mat_%1_%2.jpg")
+ .arg(session).arg(frameid);
+ std::vector<int> params(2);
+ params[0] = cv::IMWRITE_JPEG_QUALITY;
+ params[1] = 100; // 0-100
+ imwrite(filename.toStdString().c_str(), *image, params);
+}
+
+}
+
diff --git a/source/imagewriter.h b/source/imagewriter.h
new file mode 100644
index 0000000000000000000000000000000000000000..e88e112f22973934c12beba79d11e4bcf3b54453
--- /dev/null
+++ b/source/imagewriter.h
@@ -0,0 +1,40 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef IMAGEWRITER_H
+#define IMAGEWRITER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QHostAddress>
+#include <QObject>
+
+namespace MRLeo {
+
+class ImageWriter : public QObject {
+ Q_OBJECT
+
+public:
+ ImageWriter(QString path) : mPath(path) {}
+ ~ImageWriter() {}
+
+public slots:
+ void writeImage(qint32 session, NetworkConnection::File file);
+ void writeMat(qint32 session, quint32 frameid, cvMatPtr image);
+ void setEnabled(bool enabled) {
+ mEnabled = enabled;
+ }
+
+private:
+ bool mEnabled = true;
+ QString mPath;
+
+};
+
+}
+
+#endif // IMAGEWRITER_H
diff --git a/source/main.cpp b/source/main.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1e24b34bb317fe1cab5cccd7cc6bf28861edc297
--- /dev/null
+++ b/source/main.cpp
@@ -0,0 +1,221 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "global.h"
+#include "mrserver.h"
+#include "networkconnection.h"
+#include "videotransmitter.h"
+#include "videoreceiver.h"
+#include <QVector3D>
+#include <QMatrix4x4>
+#include <QCommandLineParser>
+#ifdef ENABLE_WIDGET_SUPPORT
+#include <QApplication>
+#else
+#include <QGuiApplication>
+#endif
+
+using namespace MRLeo;
+
+Q_DECLARE_METATYPE(QByteArrayPtr)
+Q_DECLARE_METATYPE(NetworkConnection::File)
+Q_DECLARE_METATYPE(VideoStreamer::Format)
+
+int main(int argc, char *argv[])
+{
+ qRegisterMetaType<QByteArrayPtr>("QByteArrayPtr");
+ qRegisterMetaType<QList<QVector3D>>("QList<QVector3D>");
+ qRegisterMetaType<QMatrix4x4>("QMatrix4x4");
+ qRegisterMetaType<cvMatPtr>("cvMatPtr");
+ qRegisterMetaType<QImage>("QImage");
+ qRegisterMetaType<NetworkConnection::File>("File");
+ qRegisterMetaType<VideoStreamer::Format>("Format");
+
+#ifdef ENABLE_WIDGET_SUPPORT
+ QApplication app(argc, argv);
+#else
+ QGuiApplication app(argc, argv);
+#endif
+ QCoreApplication::setApplicationVersion("0.1");
+ QCoreApplication::setApplicationName("MR-Leo");
+ QCoreApplication::setOrganizationName("Linköping University");
+
+ QCommandLineParser parser;
+ parser.setApplicationDescription("Mixed Reality using Edge Computing");
+ parser.addHelpOption();
+ parser.addVersionOption();
+
+ quint16 tcpPort = 39200;
+ quint16 udpPort = 39585;
+
+ QCommandLineOption tcpOption(
+ QStringList() << "t" << "tcp",
+ QString("TCP port number. Default is %1.")
+ .arg(tcpPort),
+ "port");
+ parser.addOption(tcpOption);
+
+ QCommandLineOption udpOption(
+ QStringList() << "u" << "udp",
+ QString("UDP port number. Default is %1.")
+ .arg(udpPort),
+ "port");
+ parser.addOption(udpOption);
+
+ QCommandLineOption mockOption(
+ QStringList() << "m" << "mock",
+ "Add mock client that uses images in <directory>.",
+ "directory");
+ parser.addOption(mockOption);
+
+ QCommandLineOption camOption(
+ QStringList() << "c" << "camera",
+ "Add mock client that sends video " \
+ "retrieved from the computer's camera with id number <id> [0, 1, ..].",
+ "id");
+ parser.addOption(camOption);
+
+ QCommandLineOption vocOption(
+ QStringList() << "o" << "o",
+ "Specify a specific ORBvoc.txt. If not specified it's " \
+ "assumed the file's in the current directory.",
+ "path");
+ parser.addOption(vocOption);
+
+ QCommandLineOption writerOption(
+ QStringList() << "w" << "write",
+ "Write result images to <directory>.",
+ "directory");
+ parser.addOption(writerOption);
+
+ QCommandLineOption displayOption(
+ QStringList() << "d" << "display",
+ "Displays the input and output as GUI windows.");
+ parser.addOption(displayOption);
+
+ QCommandLineOption cannyFilterOption(
+ QStringList() << "f" << "cannyfilter",
+ "Test the video stack using only the canny filter.");
+ parser.addOption(cannyFilterOption);
+
+ QCommandLineOption echoImageOption(
+ QStringList() << "e" << "echoimage",
+ "Don't modify the received image, just return it.");
+ parser.addOption(echoImageOption);
+
+ QCommandLineOption benchmarkingOption(
+ QStringList() << "b" << "benchmarking",
+ "Fill the screen with a solid color when point cloud or MR objects are visible.");
+ parser.addOption(benchmarkingOption);
+
+ QCommandLineOption identifyColorFrameOption(
+ QStringList() << "i" << "identifycolor",
+ "Look for pictures filled with the data color #F0F and act on it. Used for benchmarking.");
+ parser.addOption(identifyColorFrameOption);
+
+ QCommandLineOption replaceVideoInOption(
+ QStringList() << "r" << "replaceinput",
+ "Replace video feeds sent from the end devices with video " \
+ "retrieved from the computer's camera with id number <id> [0, 1, ..].",
+ "id");
+ parser.addOption(replaceVideoInOption);
+
+ QCommandLineOption logTimeOption(
+ QStringList() << "l" << "logtime",
+ "Log the time and print results after 60 seconds.");
+ parser.addOption(logTimeOption);
+
+ QCommandLineOption disableLoopClosingOption(
+ QStringList() << "n" << "loopclosing",
+ "Disable loop closing in ORB-SLAM2.");
+ parser.addOption(disableLoopClosingOption);
+
+ QCommandLineOption jpegQualityOption(
+ QStringList() << "j" << "jpegquality",
+ "Set the MJPEG image quality level. Default is 80.",
+ "level");
+ parser.addOption(jpegQualityOption);
+
+ QCommandLineOption vocPoolSizeOption(
+ QStringList() << "p" << "pool",
+ "Set the <size> [0, 1, 2, ..] of the pool of vocabulary files at system startup. " \
+ "Default is 1.",
+ "size");
+ parser.addOption(vocPoolSizeOption);
+
+
+ parser.process(app);
+
+ if (parser.isSet(tcpOption)) {
+ tcpPort = parser.value(tcpOption).toUShort();
+ }
+ if (parser.isSet(udpOption)) {
+ udpPort = parser.value(udpOption).toUShort();
+ }
+ QString vocPath = "";
+ if (parser.isSet(vocOption)) {
+ vocPath = parser.value(vocOption);
+ }
+
+ int vocPoolSize = 1;
+ if (parser.isSet(vocPoolSizeOption)) {
+ vocPoolSize = parser.value(vocPoolSizeOption).toInt();
+ }
+
+ bool benchmarkingOptionEnabled = parser.isSet(benchmarkingOption);
+
+#ifndef ENABLE_WIDGET_SUPPORT
+ if (displayOptionEnabled) {
+ qDebug() << "*** MR-Leo-server has not been compiled with widget support and can't support parameter --display";
+ qDebug() << "*** Enable DISPLAY_MOCK_DATA in MR-Leo-server.pro and rebuild.";
+ }
+#endif
+
+ MRServer myServer;
+
+ myServer.setBenchmarkingMode(benchmarkingOptionEnabled);
+ myServer.setLogTime(parser.isSet(logTimeOption));
+ myServer.setDisplayResults(parser.isSet(displayOption));
+ myServer.setIdentifyColorFrame(parser.isSet(identifyColorFrameOption));
+ if (parser.isSet(jpegQualityOption)) {
+ myServer.setJpegQualityLevel(parser.value(jpegQualityOption).toInt());
+ }
+
+ if (parser.isSet(cannyFilterOption)) {
+ fDebug << "Using CANNY FILTER";
+ myServer.setMixedRealityFramework(MRServer::CANNYFILTER);
+ } else if (parser.isSet(echoImageOption)) {
+ fDebug << "Using ECHO IMAGE";
+ myServer.setMixedRealityFramework(MRServer::ECHOIMAGE);
+ } else if (parser.isSet(disableLoopClosingOption)) {
+ myServer.setMixedRealityFramework(MRServer::ORB_SLAM2_NO_LC);
+ myServer.loadVoc(vocPath, vocPoolSize);
+ } else {
+ myServer.setMixedRealityFramework(MRServer::ORB_SLAM2);
+ myServer.loadVoc(vocPath, vocPoolSize);
+ }
+
+ myServer.startServer(tcpPort, udpPort);
+
+ if (parser.isSet(writerOption)) {
+ myServer.addFilewriter(parser.value(writerOption));
+ }
+ if (parser.isSet(mockOption)) {
+ myServer.addMockClient(false, parser.value(mockOption));
+ }
+ if (parser.isSet(camOption)) {
+ myServer.addMockClient(true, parser.value(camOption));
+ fDebug << "Adding mock client";
+ }
+ if (parser.isSet(replaceVideoInOption)) {
+ myServer.forceVideoInputFromCamera(parser.value(replaceVideoInOption));
+ fDebug << "Replacing end device's input with feed from camera "
+ << parser.value(replaceVideoInOption);
+ }
+
+ return app.exec();
+}
diff --git a/source/mockclient.cpp b/source/mockclient.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..41312f2fc03916a60c92c2b887b7ca8c4bb1fb46
--- /dev/null
+++ b/source/mockclient.cpp
@@ -0,0 +1,152 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "mockclient.h"
+#include "networkconnection.h"
+#include <QDebug>
+#include <QDir>
+#include <QFile>
+#include <QString>
+#include <QThread>
+#include <QJsonObject>
+#include <QJsonDocument>
+#include <QElapsedTimer>
+#include <QTimer>
+#include "mrserver.h"
+#include <opencv2/core/core.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+namespace MRLeo {
+
+MockClient::MockClient()
+{
+ mSessionId = static_cast<qint32>(qrand());
+ QObject::connect(this, &MockClient::startSignal,
+ this, &MockClient::startThread);
+}
+
+/**
+ * @brief MockClient::start
+ * @param delay Milliseconds before sending the first image.
+ * @param fps Frames per seconds in output stream.
+ * @param repeat Start over when the last image in the directory has been sent.
+ * @param webcam
+ * @param path
+ */
+void MockClient::start(unsigned long delay, int fps,
+ bool repeat, bool webcam, QString path,
+ bool sendCommands)
+{
+ fDebug << "Adding mock client.";
+ emit startSignal(delay, fps, repeat, webcam, path, sendCommands);
+}
+
+/**
+ * @brief MockClient::startThread
+ * @param delay Milliseconds before sending the first image.
+ * @param fps Frames per seconds in output stream.
+ * @param repeat Start over when the last image in the directory has been sent.
+ * @param webcam
+ * @param path
+ */
+void MockClient::startThread(unsigned long delay, int fps,
+ bool repeat, bool webcam, QString path,
+ bool sendCommands)
+{
+ mRun = true;
+ if (sendCommands) {
+ emit newSession(getSession(), getHost(), getPort());
+
+ QJsonObject retObject;
+ retObject.insert("JpegStream", false);
+ retObject.insert("TransportProtocol", "NULL");
+ retObject.insert("Camera.width", 640);
+ retObject.insert("Camera.height", 480);
+ emit fileReady(getSession(), NetworkConnection::File(
+ NetworkConnection::FileType::JSON, 1,
+ QByteArrayPtr(new QByteArray(QJsonDocument(retObject).toJson()))));
+ }
+ quint32 imagecounter = 0;
+
+ if (webcam) {
+ fDebug << "Using Webcam as source";
+ cv::VideoCapture cap(path.toInt());
+ cap.set(cv::CAP_PROP_FRAME_WIDTH, 640);
+ cap.set(cv::CAP_PROP_FRAME_HEIGHT, 480);
+
+ if (!cap.isOpened()) {
+ fDebug << "Webcam could not be opened";
+ return;
+ }
+ while (repeat && mRun) {
+ fDebug << "Image id: " << imagecounter;
+ imagecounter++;
+ cv::Mat frame;
+ cap >> frame; // get a new frame from camera
+ emit matReady(getSession(), imagecounter, cvMatPtr(new cv::Mat(frame)));
+ //if ((imagecounter % 500) == 100) {
+ // fDebug << "Trigger B";
+ // emit fileReady(getSession(), NetworkConnection::File(
+ // NetworkConnection::FileType::TRIGGER_B, 1, nullptr));
+ //}
+ //if ((imagecounter % 500) == 130) {
+ // fDebug << "Trigger A";
+ // emit fileReady(getSession(), NetworkConnection::File(
+ // NetworkConnection::FileType::TRIGGER_A, 1, nullptr));
+ //}
+ }
+ } else {
+
+ if (delay && this->thread()) {
+ thread()->msleep(delay);
+ }
+ mElapsedTimer = new QElapsedTimer;
+ mElapsedTimer->start();
+ if (path.isEmpty()) {
+ path = "Mock";
+ }
+ QDir directory(path);
+ fDebug << "Using images in " << directory.absolutePath();
+ QStringList *files = new QStringList(directory.entryList(
+ QStringList() << "*.jpg" << "*.JPG",
+ QDir::Files, QDir::Name));
+ if (files->empty()) {
+ fDebug << "No mock files in directory " << path;
+ return;
+ }
+ files->sort();
+ if (fps < 1) {
+ fps = 10;
+ }
+ int interval = (double) 1000 / fps;
+ if (fps < 1) {
+ fps = 100;
+ }
+ int* ptrimagecounter = new int;
+ mRecurringTimer = new QTimer;
+ QObject::connect(mRecurringTimer, &QTimer::timeout, [=](){
+ if (!mRun || files->empty()) {
+ mRecurringTimer->stop();
+ return;
+ }
+ int currFrame = (mElapsedTimer->elapsed() / interval) % files->size();
+ QString filename = files->at(currFrame);
+ *ptrimagecounter += 1;
+ QString fullpath = path + QDir::separator() + filename;
+ QFile file(fullpath);
+ if (file.open(QIODevice::ReadOnly)) {
+ QByteArrayPtr data(new QByteArray(file.readAll()));
+ emit fileReady(getSession(), NetworkConnection::File(
+ NetworkConnection::FileType::IMAGE, *ptrimagecounter, data));
+ }
+ });
+ mRecurringTimer->setInterval(static_cast<double>(1000) / fps);
+ mRecurringTimer->start();
+ }
+}
+
+}
diff --git a/source/mockclient.h b/source/mockclient.h
new file mode 100644
index 0000000000000000000000000000000000000000..34e2d6beb9466d1005b15339c1a508cf7484c4a9
--- /dev/null
+++ b/source/mockclient.h
@@ -0,0 +1,56 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef MOCKCLIENT_H
+#define MOCKCLIENT_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QObject>
+
+class QTimer;
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class MRServer;
+
+/**
+ * @brief The MockClient class
+ *
+ * Acts as a client and reads images from the "Mock" directory
+ * and emits them as new File objects. Used for testing the
+ * ImageProcessor classes.
+ */
+class MockClient : public NetworkConnection {
+ Q_OBJECT
+
+public:
+ MockClient();
+ ~MockClient() override {}
+ quint16 getPort() override { return 4412; }
+ qint32 getSession() { return mSessionId; }
+ QString getHost() { return "0.0.0.0"; }
+ void stop() { mRun = false; }
+ void start(unsigned long delay, int fps, bool repeat, bool webcam, QString path="", bool sendCommands=true);
+
+private slots:
+ void startThread(unsigned long delay, int fps, bool repeat, bool webcam, QString path, bool sendCommands);
+
+signals:
+ void startSignal(unsigned long delay, int fps, bool repeat, bool webcam, QString path, bool sendCommands);
+
+private:
+ bool mRun;
+ qint32 mSessionId;
+ QTimer* mRecurringTimer;
+ QElapsedTimer *mElapsedTimer;
+};
+
+}
+
+#endif // MOCKCLIENT_H
diff --git a/source/mrserver.cpp b/source/mrserver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..fb98ba940ffdb7126e5e715f5ae4226c2c622e53
--- /dev/null
+++ b/source/mrserver.cpp
@@ -0,0 +1,796 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "mrserver.h"
+#include "cannyfilter.h"
+#include "echoimage.h"
+#include "imagewriter.h"
+#include "mockclient.h"
+#include "networkconnection.h"
+#include "tcpconnection.h"
+#include "udpbuilder.h"
+#include "orbslamprocesser.h"
+#include "udpconnection.h"
+#include "udpsender.h"
+#include "videoreceiver.h"
+#include "videotransmitter.h"
+
+#include <QDataStream>
+#include <QNetworkInterface>
+#include <QThread>
+#include <QSurfaceFormat>
+#include <QDateTime>
+#include <QJsonDocument>
+#include <QJsonObject>
+#include <QFileInfo>
+#include <QDir>
+#include <QElapsedTimer>
+
+namespace MRLeo {
+
+/**
+ * @brief The Session class
+ * Contains pointers to the session's instances.
+ */
+class Session {
+public:
+ Session()
+ : imageprocesser(nullptr),
+ videoreceiver(nullptr),
+ videotransmitter(nullptr) {}
+ ~Session() {
+ if (imageprocesser) {
+ imageprocesser->deleteLater();
+ imageprocesser = nullptr;
+ }
+ if (videoreceiver) {
+ videoreceiver->deleteLater();
+ videoreceiver = nullptr;
+ }
+ if (videotransmitter) {
+ videotransmitter->deleteLater();
+ videotransmitter = nullptr;
+ }
+ }
+ ImageProcesser *imageprocesser;
+ VideoReceiver *videoreceiver;
+ VideoTransmitter *videotransmitter;
+ qint32 sessionID;
+};
+
+/**
+ * @brief MRServer::MRServer
+ * @param tcpPort TCP port to listen to.
+ * @param udpPort UDP port to listen to.
+ */
+MRServer::MRServer()
+{
+ // Set up the default OpenGL surface format.
+ QSurfaceFormat format;
+ format.setDepthBufferSize(32);
+ format.setSamples(8);
+ QSurfaceFormat::setDefaultFormat(format);
+ mFileWriter = nullptr;
+ mBenchmarking = false;
+ mDisplayResult = false;
+ mReplaceVideoFeed = false;
+ mLogTime = false;
+ mJpegQualityLevel = 80;
+ mCvFramework = ORB_SLAM2;
+ mUptime = new QElapsedTimer();
+ mUptime->start();
+
+ qDebug() << " +------------------------------------";
+ qDebug() << " | MR-Leo";
+ qDebug() << " |";
+ foreach (const auto &netInterface, QNetworkInterface::allInterfaces()) {
+ QNetworkInterface::InterfaceFlags flags = netInterface.flags();
+ if ((flags & QNetworkInterface::IsRunning) &&
+ !(flags & QNetworkInterface::IsLoopBack)) {
+ foreach (const auto &address, netInterface.addressEntries()) {
+ if (address.ip().protocol() == QAbstractSocket::IPv4Protocol) {
+ qDebug() << " | IP:" << address.ip().toString().toLatin1().data();
+ }
+ }
+ }
+ }
+ qDebug() << " +------------------------------------";
+
+
+}
+
+/**
+ * @brief MRServer::~MRServer
+ */
+MRServer::~MRServer()
+{
+ qDeleteAll(sessions);
+ qDeleteAll(mMockClients);
+ if (mTcpCon) {
+ mTcpCon->deleteLater();
+ }
+ if (mUdpCon) {
+ mUdpCon->deleteLater();
+ }
+ if (mFileWriter) {
+ mFileWriter->deleteLater();
+ }
+#ifdef ENABLE_WIDGET_SUPPORT
+ qDeleteAll(mWindows);
+#endif
+ QMutableMapIterator<ORB_SLAM2::ORBVocabulary *, bool> vocPoolIt(mVocabularyPool);
+ while (vocPoolIt.hasNext()) {
+ vocPoolIt.next();
+ delete vocPoolIt.key();
+ }
+}
+
+/**
+ * @brief MRServer::loadVoc
+ * @param path
+ */
+void MRServer::loadVoc(QString path, int poolSize)
+{
+ fDebug << "Loading ORB-SLAM2's BoW vocabulary...";
+ mVocabularyPath = path;
+ for (int i = 0; i < poolSize; i++) {
+ ORB_SLAM2::ORBVocabulary* mpVocabulary = new ORB_SLAM2::ORBVocabulary();
+ mVocabularyPool.insert(mpVocabulary, false);
+ QFileInfo userVocabularyFile(path);
+ QFileInfo textVocabularyFile("ORBvoc.txt");
+ QFileInfo homeDirTextVocabularyFile(QDir::homePath() + QDir::separator() + "ORBvoc.txt");
+ if (userVocabularyFile.exists() && !path.isNull()) {
+ fDebug << "Loading text file from" << userVocabularyFile.absoluteFilePath();
+ mpVocabulary->loadFromTextFile(userVocabularyFile.absoluteFilePath().toStdString());
+ } else if (textVocabularyFile.exists()) {
+ fDebug << "Loading text file from" << textVocabularyFile.absoluteFilePath();
+ mpVocabulary->loadFromTextFile(textVocabularyFile.absoluteFilePath().toStdString());
+ } else if (homeDirTextVocabularyFile.exists()) {
+ fDebug << "Loading text file from" << homeDirTextVocabularyFile.absoluteFilePath();
+ mpVocabulary->loadFromTextFile(homeDirTextVocabularyFile.absoluteFilePath().toStdString());
+ }
+ if (mpVocabulary->empty()) {
+ fDebug << "Error: Could not load vocabulary.";
+ fDebug << "Place ORBvoc.txt in the executable's directory.";
+ fDebug << "ORBvoc.txt can be found in /externals/ORB_SLAM2/Vocabulary/ORBvoc.txt.tar.gz";
+ } else {
+ fDebug << "Vocabulary object added loaded to the pool.";
+ }
+ }
+}
+
+
+/**
+ * @brief MRServer::startServer
+ * @param tcpPort Port to listen for TCP connections on.
+ * @param udpPort Port to listen for UDP traffic on.
+ */
+void MRServer::startServer(quint16 tcpPort, quint16 udpPort)
+{
+ mTcpCon = new TcpConnection(this, tcpPort);
+ mUdpCon = new UdpConnection(this, udpPort);
+ QObject::connect(this, &MRServer::sendFile,
+ mUdpCon, &NetworkConnection::sendFile);
+ QObject::connect(this, &MRServer::sendFile,
+ mTcpCon, &NetworkConnection::sendFile);
+ QObject::connect(mTcpCon, &NetworkConnection::fileReady,
+ this, &MRServer::dataReceived);
+ QObject::connect(mUdpCon, &NetworkConnection::fileReady,
+ this, &MRServer::dataReceived);
+ QObject::connect(mTcpCon, &NetworkConnection::newSession,
+ this, &MRServer::newSession,
+ Qt::DirectConnection);
+ QObject::connect(mTcpCon, &NetworkConnection::sessionDestroyed,
+ this, &MRServer::removeSession);
+
+ qDebug() << " +------------------------------------";
+ qDebug() << " | Port (TCP):" << mTcpCon->getPort();
+ qDebug() << " | Port (UDP):" << mUdpCon->getPort();
+ qDebug() << " +------------------------------------";
+
+ if (mTcpCon->getPort()) {
+ fDebug << "Waiting for clients to connect.";
+ }
+}
+
+/**
+ * @brief MRServer::addFilewriter
+ */
+void MRServer::addFilewriter(QString path)
+{
+ if (mFileWriter) {
+ return;
+ }
+ mFileWriter = new ImageWriter(path);
+ if (!mFileWriter) {
+ return;
+ }
+ mFileWriter->setEnabled(true);
+ mFileWriter->moveToThread(new QThread());
+ mFileWriter->thread()->start();
+ QObject::connect(mUdpCon, &NetworkConnection::fileReady,
+ mFileWriter, &ImageWriter::writeImage);
+ QObject::connect(mTcpCon, &NetworkConnection::fileReady,
+ mFileWriter, &ImageWriter::writeImage);
+}
+
+/**
+ * @brief MRServer::newImage
+ * @param image Image to be displayed.
+ *
+ * Display the result image in a Qt GUI window.
+ */
+void MRServer::displayImage(qint32 session, quint32 frameid, QImage image)
+{
+ Q_UNUSED(session);
+ Q_UNUSED(frameid);
+ Q_UNUSED(image);
+#ifdef ENABLE_WIDGET_SUPPORT
+ if (mWindows.contains(session)) {
+ bool resizeToFit = false;
+ auto pixmap = mMockClients.isEmpty() ?
+ QPixmap::fromImage(image) :
+ QPixmap::fromImage(image.rgbSwapped());
+ auto window = mWindows.value(session);
+ if (resizeToFit) {
+ pixmap = pixmap.scaled(
+ window->width(), window->height(),
+ Qt::KeepAspectRatio);
+ }
+ window->setPixmap(pixmap);
+ }
+#endif
+}
+
+#ifdef ENABLE_WIDGET_SUPPORT
+void ClickableLabel::mousePressEvent(QMouseEvent *event) {
+ Q_UNUSED(event);
+ emit clicked();
+}
+#endif
+
+/**
+ * @brief MRServer::addMockClient
+ * @param useCamera
+ */
+void MRServer::addMockClient(bool useCamera, QString path)
+{
+ auto mockClient = new MockClient();
+ if (!mockClient) {
+ return;
+ }
+#ifdef ENABLE_WIDGET_SUPPORT
+ auto label = new ClickableLabel;
+ label->show();
+ label->resize(640, 480);
+ connect(label, &ClickableLabel::clicked, [=]() {
+ this->dataReceived(mockClient->getSession(), NetworkConnection::File(
+ NetworkConnection::FileType::TRIGGER_A, 1, nullptr));
+ });
+ mWindows.insert(mockClient->getSession(), label);
+ mDisplayResult = true;
+#endif
+ mMockClients.append(mockClient);
+ mockClient->moveToThread(new QThread());
+ mockClient->thread()->start();
+ QObject::connect(mockClient, &NetworkConnection::newSession,
+ this, &MRServer::newSession);
+ QObject::connect(mockClient, &NetworkConnection::fileReady,
+ this, &MRServer::dataReceived);
+ mockClient->start(100, 30, true, useCamera, path, true);
+}
+
+/**
+ * @brief MRServer::forceVideoInputFromCamera
+ * @param path
+ */
+void MRServer::forceVideoInputFromCamera(QString path)
+{
+ auto mockClient = new MockClient();
+ if (mockClient) {
+ mMockClients.append(mockClient);
+ mockClient->moveToThread(new QThread());
+ mockClient->thread()->start();
+ mockClient->start(0, 0, true, true, path, false);
+ mReplaceVideoFeed = true;
+ }
+}
+
+void MRServer::setBenchmarkingMode(bool enable)
+{
+ if (mBenchmarking) {
+ fDebug << "Benchmarking mode enabled.";
+ }
+ mBenchmarking = enable;
+}
+
+
+/**
+ * @brief MRServer::dataReceived
+ * @param sessionId Session id
+ * @param file NetworkConnection::File
+ *
+ * The TCP or UDP server has received a non-image file such as
+ * a JSON configuration or direct commands.
+ */
+void MRServer::dataReceived(qint32 sessionId, NetworkConnection::File file)
+{
+ mSessionsListmutex.lock();
+ Session *session = sessions.value(sessionId);
+ mSessionsListmutex.unlock();
+ if (session == nullptr) {
+ fDebug << sessionId << ": No session found";
+ return;
+ }
+ switch (file.type) {
+ case NetworkConnection::FileType::JSON: {
+ fDebug << sessionId << ": JSON:" << file.data->constData();
+ auto jsonObject = QJsonDocument::fromJson(file.data->constData()).object();
+ if (jsonObject.contains("TransportProtocol")) {
+ bool useTcp = (jsonObject["TransportProtocol"].toString() == "TCP");
+ bool useUdp = (jsonObject["TransportProtocol"].toString() == "UDP");
+ fDebug << sessionId << ": TransportProtocol:" << jsonObject["TransportProtocol"].toString();
+ mTcpCon->setSendImagesForSession(sessionId, useTcp);
+ mUdpCon->setSendImagesForSession(sessionId, useUdp);
+ }
+ if (jsonObject.contains("JpegStream")) {
+ bool sendmjpeg = jsonObject["JpegStream"].toBool();
+ fDebug << sessionId << ": JpegStream:" << sendmjpeg;
+ if (session->imageprocesser) {
+ session->imageprocesser->setEmitJPEG(sendmjpeg);
+ session->imageprocesser->setEmitQImage(!sendmjpeg);
+ }
+ }
+ if (jsonObject.contains("VideoBitrate")) {
+ int bitrate = jsonObject["VideoBitrate"].toInt();
+ fDebug << sessionId << ": VideoBitrate:" << bitrate;
+ if (session->videotransmitter) {
+ session->videotransmitter->setBitrate(bitrate);
+ }
+ }
+ if (jsonObject.contains("UserInteractionConfiguration")) {
+ fDebug << sessionId << ": UserInteractionConfiguration received";
+ if (session->imageprocesser) {
+ session->imageprocesser->setUserInteractionConfiguration(jsonObject);
+ }
+ }
+ if (jsonObject.contains("DebugMode")) {
+ bool debugmode = jsonObject["DebugMode"].toBool();
+ fDebug << sessionId << ": DebugMode:" << debugmode;
+ if (session->imageprocesser) {
+ session->imageprocesser->setDebugMode(debugmode);
+ }
+ }
+ if (jsonObject.contains("PacketSize")) {
+ qint32 packetSize = jsonObject["PacketSize"].toInt();
+ fDebug << sessionId << ": PacketSize:" << packetSize;
+ mUdpCon->setPacketSize(sessionId, packetSize);
+ }
+ if (jsonObject.contains("VideoReceiverPort") && jsonObject.contains("VideoReceiverFormat")) {
+ auto destPort = static_cast<quint16>(jsonObject["VideoReceiverPort"].toInt());
+ auto destFormatStr = jsonObject["VideoReceiverFormat"].toString();
+ fDebug << sessionId << ": VideoReceiverPort:" << destPort;
+ fDebug << sessionId << ": VideoReceiverFormat:" << destFormatStr;
+ VideoStreamer::Format format = VideoStreamer::Format::H264_UDP;
+ if (destFormatStr == "H264_UDP") {
+ format = VideoStreamer::Format::H264_UDP;
+ } else if (destFormatStr == "H264_TCP") {
+ format = VideoStreamer::Format::H264_TCP;
+ } else {
+ destPort = 0;
+ }
+ if (destPort > 0 && session->videotransmitter) {
+ session->videotransmitter->startStream(format, destPort);
+ }
+ }
+ if (jsonObject.contains("VideoTransmitterFormat")) {
+ auto format = VideoStreamer::Format::H264_UDP;
+ auto srcFormatStr = jsonObject["VideoTransmitterFormat"].toString();
+ fDebug << sessionId << ": VideoTransmitterFormat:" << srcFormatStr;
+ if (srcFormatStr == "H264_UDP") {
+ format = VideoStreamer::Format::H264_UDP;
+ } else if (srcFormatStr == "H264_TCP") {
+ format = VideoStreamer::Format::H264_TCP;
+ }
+ bool useJitterbuffer = false;
+ if (jsonObject.contains("VideoTransmitterUseJitterBuffer")) {
+ useJitterbuffer = jsonObject.value("VideoTransmitterUseJitterBuffer").toBool();
+ fDebug << sessionId << ": VideoTransmitterUseJitterBuffer:" << useJitterbuffer;
+ }
+ if (session->videoreceiver) {
+ session->videoreceiver->start(format, useJitterbuffer);
+ }
+ }
+ if (jsonObject.contains("MRFoundTime")) {
+ int time = jsonObject["MRFoundTime"].toInt();
+ fDebug << "MR Object found at time: " << time;
+ }
+ if (jsonObject.contains("Camera.width") &&
+ jsonObject.contains("Camera.height")) {
+ int width = jsonObject["Camera.width"].toInt();
+ int height = jsonObject["Camera.height"].toInt();
+ fDebug << sessionId << ": Camera.width:" << width;
+ fDebug << sessionId << ": Camera.height:" << height;
+ if (jsonObject.contains("Camera.fx")) {
+ fDebug << sessionId << ": Camera.fx:" << jsonObject["Camera.fx"].toDouble();
+ }
+ if (jsonObject.contains("Camera.fy")) {
+ fDebug << sessionId << ": Camera.fy:" << jsonObject["Camera.fy"].toDouble();
+ }
+ if (jsonObject.contains("Camera.cx")) {
+ fDebug << sessionId << ": Camera.cx:" << jsonObject["Camera.cx"].toDouble();
+ }
+ if (jsonObject.contains("Camera.cy")) {
+ fDebug << sessionId << ": Camera.cy:" << jsonObject["Camera.cy"].toDouble();
+ }
+ if (jsonObject.contains("Camera.k1")) {
+ fDebug << sessionId << ": Camera.k1:" << jsonObject["Camera.k1"].toDouble();
+ }
+ if (jsonObject.contains("Camera.k2")) {
+ fDebug << sessionId << ": Camera.k2:" << jsonObject["Camera.k2"].toDouble();
+ }
+ if (jsonObject.contains("Camera.p1")) {
+ fDebug << sessionId << ": Camera.p1:" << jsonObject["Camera.p1"].toDouble();
+ }
+ if (jsonObject.contains("Camera.p2")) {
+ fDebug << sessionId << ": Camera.p2:" << jsonObject["Camera.p2"].toDouble();
+ }
+ if (session->imageprocesser) {
+ session->imageprocesser->setConfig(jsonObject);
+ }
+ if (session->videotransmitter) {
+ session->videotransmitter->setImageSize(QSize(width, height));
+ }
+ }
+ break;
+ }
+ case NetworkConnection::FileType::TRIGGER_A:
+ fDebug << sessionId << ": Trigger Action A";
+ if (session->imageprocesser) {
+ session->imageprocesser->triggerActionA();
+ }
+ break;
+ case NetworkConnection::FileType::TRIGGER_B:
+ fDebug << sessionId << ": Trigger Action B";
+ if (session->imageprocesser) {
+ session->imageprocesser->triggerActionB();
+ }
+ break;
+ case NetworkConnection::FileType::PING: {
+ fDebug << sessionId << ": PING";
+ emit sendFile(sessionId, NetworkConnection::File(
+ NetworkConnection::FileType::PONG, 1, nullptr));
+ }
+ break;
+ case NetworkConnection::FileType::CALIBRATION: {
+ auto calibrationMode = (file.data.data()->length() == 1 &&
+ file.data.data()->data()[0] != 0);
+ fDebug << sessionId << ": Calibration mode:" << calibrationMode;
+ if (session->imageprocesser) {
+ session->imageprocesser->setCalibrateMode(calibrationMode);
+ }
+ }
+ }
+}
+
+
+/**
+ * @brief MRServer::getVocabulary
+ * @return Vocabulary object.
+ */
+ORB_SLAM2::ORBVocabulary * MRServer::getVocabulary()
+{
+ ORB_SLAM2::ORBVocabulary * retObject = nullptr;
+ mSessionsListmutex.lock();
+ QMapIterator<ORB_SLAM2::ORBVocabulary *, bool> vocPoolIt(mVocabularyPool);
+ while (vocPoolIt.hasNext()) {
+ vocPoolIt.next();
+ if (vocPoolIt.value() == false) {
+ retObject = vocPoolIt.key();
+ mVocabularyPool[vocPoolIt.key()] = true;
+ break;
+ }
+ }
+ mSessionsListmutex.unlock();
+ if (retObject == nullptr) {
+ loadVoc(mVocabularyPath, 1);
+ retObject = getVocabulary();
+ }
+ return retObject;
+}
+
+
+/**
+ * @brief MRServer::newSession
+ * @param sessionId The new client's session id.
+ * @param host Client host
+ * @param port Client source port.
+ *
+ * Set up a new session. Create and start the session specific
+ * class instances and threads. When that is done, inform the client
+ * about the values such as which UDP port to connect to.
+ */
+void MRServer::newSession(qint32 sessionId, QString host, quint16 port)
+{
+ fDebug << QString("MRServer::newSession: %1:%2").arg(host).arg(port);
+ mSessionsListmutex.lock();
+ auto session = new Session;
+ session->sessionID = sessionId;
+ sessions.insert(sessionId, session);
+ mSessionsListmutex.unlock();
+
+ mTcpCon->setSendImagesForSession(sessionId, true);
+ mUdpCon->setSendImagesForSession(sessionId, false);
+ mTcpCon->setLogTime(mLogTime, mUptime);
+ mUdpCon->setLogTime(mLogTime, mUptime);
+
+ ImageProcesser *imageprocesser = nullptr;
+ switch (mCvFramework) {
+ case CANNYFILTER:
+ imageprocesser = new CannyFilter(sessionId);
+ break;
+ case ECHOIMAGE:
+ imageprocesser = new EchoImage(sessionId, mBenchmarking);
+ break;
+ case ORB_SLAM2:
+ imageprocesser = new OrbSlamProcesser(sessionId, getVocabulary(), mBenchmarking, mLogTime, true);
+ break;
+ case ORB_SLAM2_NO_LC:
+ imageprocesser = new OrbSlamProcesser(sessionId, getVocabulary(), mBenchmarking, mLogTime, false);
+ break;
+ default:
+ return;
+ }
+
+ if (imageprocesser == nullptr) {
+ fDebug << "Could not create image processer.";
+ return;
+ }
+
+ if (mReplaceVideoFeed) {
+ imageprocesser->setAllowAllSources(true);
+ }
+ imageprocesser->setJpgegQualityLevel(mJpegQualityLevel);
+ session->imageprocesser = imageprocesser;
+ imageprocesser->setLogTime(mLogTime);
+ imageprocesser->setIdentifyColorFrame(mIdentifyColorFrame);
+ imageprocesser->moveToThread(new QThread(this));
+ imageprocesser->thread()->start();
+
+ auto videotransmitter = new VideoTransmitter(sessionId, host);
+ videotransmitter->setLogTime(mLogTime, mUptime);
+ videotransmitter->setBenchmarkingMode(mBenchmarking);
+ session->videotransmitter = videotransmitter;
+ videotransmitter->moveToThread(new QThread(this));
+ videotransmitter->thread()->start();
+ if (!mReplaceVideoFeed) {
+ auto videoreceiver = new VideoReceiver(sessionId, host);
+ videoreceiver->setLogTime(mLogTime, mUptime);
+ videoreceiver->setBenchmarkingMode(mBenchmarking);
+ session->videoreceiver = videoreceiver;
+ videoreceiver->moveToThread(new QThread(this));
+ videoreceiver->thread()->start();
+ QObject::connect(videoreceiver, &VideoReceiver::ready,
+ this, &MRServer::videoReceiverReady);
+ }
+
+ auto videoreceiver = session->videoreceiver;
+
+#ifdef ENABLE_WIDGET_SUPPORT
+ if (mDisplayResult && !mWindows.contains(sessionId)) {
+ auto outputdisplay = new ClickableLabel;
+ outputdisplay->show();
+ outputdisplay->resize(640, 480);
+ connect(outputdisplay, &ClickableLabel::clicked, [=]() {
+ this->dataReceived(sessionId, NetworkConnection::File(
+ NetworkConnection::FileType::TRIGGER_A, 1, nullptr));
+ });
+ mWindows.insert(sessionId, outputdisplay);
+ if (!mWindows.contains(sessionId + 1000)) {
+ auto inputdisplay = new QLabel;
+ inputdisplay->show();
+ inputdisplay->resize(640, 480);
+ mWindows.insert(sessionId + 1000, inputdisplay);
+ }
+ }
+#endif
+
+ // Receive data
+ NetworkConnection *currCon = mTcpCon;
+ for (int i = 0; i < (2 + mMockClients.size()); i++) {
+ QObject::connect(currCon, &NetworkConnection::fileReady,
+ imageprocesser, &ImageProcesser::addFileToProcessQueue,
+ Qt::DirectConnection);
+ QObject::connect(currCon, &NetworkConnection::fileReady,
+ imageprocesser, &ImageProcesser::processFile);
+ QObject::connect(currCon, &NetworkConnection::matReady,
+ imageprocesser, &ImageProcesser::addMatToProcessQueue,
+ Qt::DirectConnection);
+ QObject::connect(currCon, &NetworkConnection::matReady,
+ imageprocesser, &ImageProcesser::processMat);
+ // Send data
+ QObject::connect(imageprocesser, &ImageProcesser::sendFile,
+ currCon, &NetworkConnection::sendFileIfLatest,
+ Qt::DirectConnection);
+ // Add mock client if available.
+ if (i == 0) {
+ currCon = reinterpret_cast<NetworkConnection*>(mUdpCon);
+ } else if (mMockClients.size() >= i) {
+ currCon = reinterpret_cast<NetworkConnection*>(mMockClients.at(i - 1));
+ }
+ }
+ if (videoreceiver) {
+ QObject::connect(videoreceiver, &NetworkConnection::matReady,
+ imageprocesser, &ImageProcesser::addMatToProcessQueue,
+ Qt::DirectConnection);
+ QObject::connect(videoreceiver, &NetworkConnection::matReady,
+ imageprocesser, &ImageProcesser::processMat);
+ if (mFileWriter) {
+ QObject::connect(videoreceiver, &NetworkConnection::matReady,
+ mFileWriter, &ImageWriter::writeMat);
+ }
+ }
+ // Send graphics output
+ QObject::connect(imageprocesser, &ImageProcesser::sendQImage,
+ videotransmitter, &VideoTransmitter::addQImageToProcessQueue,
+ Qt::DirectConnection);
+ QObject::connect(imageprocesser, &ImageProcesser::sendQImage,
+ videotransmitter, &VideoTransmitter::processQImage);
+ if (mFileWriter) {
+ QObject::connect(imageprocesser, &ImageProcesser::sendFile,
+ mFileWriter, &ImageWriter::writeImage);
+ }
+ // GUI window output
+ if (mDisplayResult) {
+ QObject::connect(imageprocesser, &ImageProcesser::sendQImage,
+ this, &MRServer::displayImage);
+ QObject::connect(videoreceiver, &VideoReceiver::matReady,
+ [=](qint32 session, quint32 frameid, cvMatPtr image) {
+ this->displayImage(session + 1000, frameid, imageprocesser->qImageFromMat(*image));
+ });
+ }
+ QJsonObject retObject;
+ retObject.insert("UdpPort", mUdpCon->getPort());
+ retObject.insert("SessionId", sessionId);
+ QByteArrayPtr ret(new QByteArray(QJsonDocument(retObject).toJson()));
+ fDebug << sessionId << ": Connection JSON:" << ret->data();
+ emit sendFile(sessionId, NetworkConnection::File(
+ NetworkConnection::FileType::JSON, 1, ret));
+}
+
+
+double median(std::vector<int> &v)
+{
+ size_t n = v.size() / 2;
+ std::nth_element(v.begin(), v.begin()+n, v.end());
+ int vn = v[n];
+ if (v.size() % 2 == 1) {
+ return vn;
+ }
+ std::nth_element(v.begin(), v.begin()+n-1, v.end());
+ return 0.5 * (vn + v[n-1]);
+}
+
+/**
+ * @brief MRServer::removeSession
+ * @param sessionId
+ * Destroys a session with its associated class instances.
+ */
+void MRServer::removeSession(qint32 sessionId)
+{
+ fDebug << "MRServer::removeSession: id:" << sessionId;
+ mSessionsListmutex.lock();
+ auto session = sessions.take(sessionId);
+ if (mLogTime) {
+ fDebug << "+========================================+";
+ fDebug << "| STATISTICS |";
+ fDebug << "+========================================+";
+ auto imagesprocessortimes = session->imageprocesser->getProcessingTimes();
+ auto mjpegsenttimes = mTcpCon->getProcessingTimes(sessionId);
+ QMap<quint32, qint64> videosenttimes;
+ if (session->videotransmitter) {
+ videosenttimes = session->videotransmitter->getProcessingTimes();
+ }
+ auto videoarrivedtimes = session->videoreceiver->getProcessingTimes();
+ if (mjpegsenttimes.isEmpty()) {
+ mjpegsenttimes = mUdpCon->getProcessingTimes(sessionId);
+ }
+ int processedframes = 0;
+ quint64 totalprocessingtime = 0;
+ quint64 totalcvtime = 0;
+ std::vector<int> processingMedianVector;
+ std::vector<int> mrMedianVector;
+ qint64 timeFirst = std::numeric_limits<qint64>::max();
+ qint64 timeLast = std::numeric_limits<qint64>::min();
+ QMapIterator<quint32, qint64> arrivedIt(videoarrivedtimes);
+ qint64 lastArrivedTime = 0;
+ while (arrivedIt.hasNext()) {
+ arrivedIt.next();
+ if (imagesprocessortimes.contains(arrivedIt.key())) {
+ processedframes++;
+ int processingtime = 0;
+ if (videosenttimes.contains(arrivedIt.key())) {
+ processingtime = (videosenttimes.value(arrivedIt.key()) - arrivedIt.value()) / 1000000;
+ timeFirst = (videosenttimes.value(arrivedIt.key()) < timeFirst) ?
+ videosenttimes.value(arrivedIt.key()) : timeFirst;
+ timeLast = (videosenttimes.value(arrivedIt.key()) > timeLast) ?
+ videosenttimes.value(arrivedIt.key()) : timeLast;
+ } else if (mjpegsenttimes.contains(arrivedIt.key())) {
+ processingtime = (mjpegsenttimes.value(arrivedIt.key()) - arrivedIt.value()) / 1000000;
+ timeFirst = (mjpegsenttimes.value(arrivedIt.key()) < timeFirst) ?
+ mjpegsenttimes.value(arrivedIt.key()) : timeFirst;
+ timeLast = (mjpegsenttimes.value(arrivedIt.key()) > timeLast) ?
+ mjpegsenttimes.value(arrivedIt.key()) : timeLast;
+ }
+ int mrtime = imagesprocessortimes.value(arrivedIt.key());
+ totalcvtime += mrtime;
+ totalprocessingtime += processingtime;
+ processingMedianVector.push_back(processingtime);
+ mrMedianVector.push_back(mrtime);
+ fDebug << QString("Frame: %1 - Processing time (ms): %2 - MR time (ms): %3")
+ .arg(arrivedIt.key(), -4)
+ .arg(processingtime, -4)
+ .arg(mrtime, -4);
+ }
+ fDebug << QString("Frame arrival time: %1 Diff: %2 Dropped: %3")
+ .arg(qRound(((double) (arrivedIt.value())) / (1000 * 1000)))
+ .arg(qRound(((double) (arrivedIt.value() - lastArrivedTime)) / (1000 * 1000)))
+ .arg(imagesprocessortimes.contains(arrivedIt.key()) ? "No" : "Yes");
+ lastArrivedTime = arrivedIt.value();
+ }
+ fDebug << "---------------------------------------";
+ fDebug << "Frames arrived: " << videoarrivedtimes.size();
+ fDebug << "Frames processed: " << imagesprocessortimes.size();
+ fDebug << "Frames sent as MJPEG:" << mjpegsenttimes.size();
+ fDebug << "Frames sent as H.264:" << videosenttimes.size();
+ if (processedframes > 0) {
+ fDebug << "Percentage skipped: " << 100 *
+ (1 - (double)imagesprocessortimes.size() /
+ (videoarrivedtimes.empty() ? 0 : videoarrivedtimes.size()));
+ fDebug << "Average Complete Time:" << (totalprocessingtime / processedframes);
+ fDebug << "Average MR time: " << (totalcvtime / processedframes);
+ fDebug << "Median Complete time: " << median(processingMedianVector);
+ fDebug << "Median MR time: " << median(mrMedianVector);
+ fDebug << "Total time: " << ((timeLast - timeFirst) / 1000000) << "ms";
+ fDebug << "Average fps: " <<
+ ((double) processedframes / ((timeLast - timeFirst) / 1000000000));
+ }
+ fDebug << "=======================================";
+
+ }
+ mSessionsListmutex.unlock();
+ delete session;
+}
+
+/**
+ * @brief MRServer::videoReceiverReady
+ * @param sessionId Session id
+ * @param format The video format the receiver has been configured to.
+ * @param port Server port the video receiver listens to.
+ *
+ * Listener for when the video receiver has been configured and is ready.
+ * The port is for the open video socket the client can connect to.
+ */
+void MRServer::videoReceiverReady(qint32 sessionId, VideoStreamer::Format format, quint16 port)
+{
+ QString formatString;
+ if (format == VideoStreamer::Format::H264_UDP) {
+ formatString = "H264_UDP";
+ } else if (format == VideoStreamer::Format::H264_TCP) {
+ formatString = "H264_TCP";
+ }
+ fDebug << QString("VideoReceiver Ready: session=%1, format=%2, port=%3")
+ .arg(sessionId).arg(formatString).arg(port);
+ QJsonObject retObject;
+ retObject.insert("VideoReceiverFormat", formatString);
+ retObject.insert("VideoReceiverPort", port);
+ emit sendFile(sessionId, NetworkConnection::File(
+ NetworkConnection::FileType::JSON, 1,
+ QByteArrayPtr(new QByteArray(QJsonDocument(retObject).toJson()))));
+}
+
+}
diff --git a/source/mrserver.h b/source/mrserver.h
new file mode 100644
index 0000000000000000000000000000000000000000..a389f66585120c5d7ada34b543bc8ffdd8791b6d
--- /dev/null
+++ b/source/mrserver.h
@@ -0,0 +1,130 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef UDPServer_H
+#define UDPServer_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QMutex>
+#include <QUdpSocket>
+#include <QImage>
+#include <QJsonObject>
+#include "ORBVocabulary.h"
+#include "videoreceiver.h"
+#ifdef ENABLE_WIDGET_SUPPORT
+#include <QLabel>
+#endif
+
+class QLabel;
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class UdpSender;
+class ImageWriter;
+class MockClient;
+class ImageProcesser;
+class VideoReceiver;
+class VideoTransmitter;
+class Session;
+class TcpConnection;
+class UdpConnection;
+
+/**
+ * @brief The MRServer class
+ *
+ * The overarching class for the server.
+ */
+class MRServer : public QObject {
+ Q_OBJECT
+
+public:
+ enum CV_FRAMEWORKS {
+ CANNYFILTER=0,
+ ECHOIMAGE=1,
+ ORB_SLAM2=2,
+ ORB_SLAM2_NO_LC=3
+ };
+
+ MRServer();
+ ~MRServer();
+ void addMockClient(bool useWebcam, QString path="");
+ void addFilewriter(QString path="");
+ void forceVideoInputFromCamera(QString path);
+
+signals:
+ void startMock(unsigned long delay, unsigned long interval, bool repeat,
+ bool webcam, QString path, bool sendCommands);
+ void sendFile(qint32 session, NetworkConnection::File file);
+
+public slots:
+ void dataReceived(qint32 session, NetworkConnection::File file);
+ void displayImage(qint32 session, quint32 frameid, QImage image);
+ void newSession(qint32 session, QString host, quint16 port);
+ void removeSession(qint32 session);
+ void videoReceiverReady(qint32 session, VideoStreamer::Format format, quint16 port);
+ void setDisplayResults(bool enable) { mDisplayResult = enable; }
+ void setBenchmarkingMode(bool enable);
+ void setLogTime(bool enable) { mLogTime = enable; }
+ void setIdentifyColorFrame(bool enable) { mIdentifyColorFrame = enable; }
+ void startServer(quint16 tcpPort, quint16 udpPort);
+ void loadVoc(QString path, int poolSize);
+ void setJpegQualityLevel(int level) { mJpegQualityLevel = level; }
+ void setMixedRealityFramework(CV_FRAMEWORKS framework) {
+ mCvFramework = framework; }
+
+private:
+ ORB_SLAM2::ORBVocabulary * getVocabulary();
+ CV_FRAMEWORKS mCvFramework;
+ TcpConnection *mTcpCon;
+ UdpConnection *mUdpCon;
+ ImageWriter *mFileWriter;
+ QList<MockClient *> mMockClients;
+ QMap<qint32, Session *> sessions;
+ QMutex mSessionsListmutex;
+ QMap<ORB_SLAM2::ORBVocabulary *, bool> mVocabularyPool;
+ QString mVocabularyPath;
+ QMap<qint32, QLabel *> mWindows;
+ bool mDisplayResult;
+ bool mBenchmarking;
+ bool mReplaceVideoFeed;
+ bool mLogTime;
+ QElapsedTimer* mUptime;
+ bool mIdentifyColorFrame;
+ int mJpegQualityLevel;
+};
+
+/**
+ * @brief Class for a clickable QLabel to display images.
+ */
+class ClickableLabel
+#ifdef ENABLE_WIDGET_SUPPORT
+ : public QLabel
+#else
+ : public QObject
+#endif
+{
+ Q_OBJECT
+public:
+#ifdef ENABLE_WIDGET_SUPPORT
+ ClickableLabel(QWidget *p=0)
+ : QLabel(p) {}
+ virtual ~ClickableLabel() {}
+#endif
+signals:
+ void clicked();
+#ifdef ENABLE_WIDGET_SUPPORT
+protected:
+ void mousePressEvent(QMouseEvent *event);
+#endif
+};
+
+
+}
+
+#endif // UDPServer_H
diff --git a/source/networkconnection.cpp b/source/networkconnection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1362faadaac2da4d432c97b5e508140a66143e2a
--- /dev/null
+++ b/source/networkconnection.cpp
@@ -0,0 +1,57 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "networkconnection.h"
+#include <mutex>
+
+namespace MRLeo {
+
+/**
+ * @brief NetworkConnection::setSendImagesForSession
+ * @param session
+ * @param enabled
+ */
+void NetworkConnection::setSendImagesForSession(qint32 session, bool enabled)
+{
+ mSendImagesForSessionListMutex.lock();
+ mSendImagesForSessionList.insert(session, enabled);
+ mSendImagesForSessionListMutex.unlock();
+}
+
+/**
+ * @brief NetworkConnection::sendImagesForSession
+ * @param sessionId
+ * @return
+ */
+bool NetworkConnection::sendImagesForSession(qint32 sessionId)
+{
+ mSendImagesForSessionListMutex.lock();
+ bool retval = mSendImagesForSessionList.value(sessionId, false);
+ mSendImagesForSessionListMutex.unlock();
+ return retval;
+}
+
+/**
+ * @brief NetworkConnection::getProcessingTimes
+ * @return
+ */
+QMap<quint32, qint64> NetworkConnection::getProcessingTimes(qint32 session)
+{
+ mTimeLogsMutex.lock();
+ auto timelogs = mTimeLogs.value(session, nullptr);
+ mTimeLogsMutex.unlock();
+ QMap<quint32, qint64> retMap;
+ if (timelogs) {
+ retMap = *timelogs;
+ retMap.detach();
+ }
+ retMap.remove(retMap.lastKey());
+ return retMap;
+}
+
+
+}
diff --git a/source/networkconnection.h b/source/networkconnection.h
new file mode 100644
index 0000000000000000000000000000000000000000..933760669bd9d5e1e8fcc9c8a835235b66443348
--- /dev/null
+++ b/source/networkconnection.h
@@ -0,0 +1,88 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef NETWORKCONNECTION_H
+#define NETWORKCONNECTION_H
+
+#include "global.h"
+#include <QDebug>
+#include <QMutex>
+#include <QJsonObject>
+
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class NetworkConnection : public QObject {
+ Q_OBJECT
+
+public:
+ virtual quint16 getPort() = 0;
+
+ class FileType {
+ public:
+ static const qint16 NONE = 0;
+ static const qint16 CONNECTION = 1;
+ static const qint16 JSON = 2;
+ static const qint16 IMAGE = 3;
+ static const qint16 IMAGE_WITH_METADATA = 4;
+ static const qint16 CALIBRATION = 5;
+ static const qint16 TRIGGER_A = 6;
+ static const qint16 TRIGGER_B = 7;
+ static const qint16 PING = 21;
+ static const qint16 PONG = 22;
+ };
+
+ class File {
+ public:
+ File(qint16 _type = 0, quint32 _id = 0,
+ QByteArrayPtr _data = nullptr, qint32 _metadata = 0)
+ : id(_id), type(_type), data(_data), metadata(_metadata) {}
+ ~File() {}
+ File(const File &rhs) :
+ id(rhs.id), type(rhs.type),
+ data(rhs.data), metadata(rhs.metadata) {}
+ quint32 id;
+ qint16 type;
+ QByteArrayPtr data;
+ qint32 metadata;
+ };
+
+ virtual QMap<quint32, qint64> getProcessingTimes(qint32 session);
+
+signals:
+ void fileReady(qint32 session, File file);
+ void matReady(qint32 session, quint32 frameid, cvMatPtr image);
+ void newSession(qint32 session, QString host, quint16 port);
+ void sessionDestroyed(qint32);
+
+public slots:
+ virtual void sendFileIfLatest(qint32 /*session*/, File /*file*/) {}
+ virtual void sendFile(qint32 /*session*/, File /*file*/) {}
+ virtual void setSendImagesForSession(qint32 sessionId, bool enabled);
+ virtual bool sendImagesForSession(qint32 sessionId);
+ virtual void setLogTime(bool enable, QElapsedTimer* timer=nullptr) {
+ mLogTime = enable; mUptime=timer; }
+
+protected:
+ virtual void bytesWritten(qint32, qint64) {}
+
+ QMap<qint32, qint64> mSendBufferLevels;
+ QMap<qint32, File> mSendBufferFiles;
+ QMap<qint32, bool> mSendImagesForSessionList;
+ QMutex mSendBufferFilesMutex;
+ QMutex mSendImagesForSessionListMutex;
+
+ bool mLogTime = false;
+ QElapsedTimer* mUptime;
+ QMutex mTimeLogsMutex;
+ QMap<qint32, QMap<quint32, qint64>*> mTimeLogs;
+};
+
+}
+
+#endif // NETWORKCONNECTION_H
diff --git a/source/orbslamprocesser.cpp b/source/orbslamprocesser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..da5f7fc4865a55af03b6b32900c16152179172b2
--- /dev/null
+++ b/source/orbslamprocesser.cpp
@@ -0,0 +1,357 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "orbslamprocesser.h"
+#include <QDebug>
+#include <QImage>
+#include <QtMath>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgcodecs.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/ximgproc.hpp>
+#include "System.h"
+#include <QJsonDocument>
+#include "ViewerAR.h"
+#include <QDir>
+#include <QThread>
+#include <QElapsedTimer>
+
+namespace MRLeo {
+
+/**
+ * @brief OrbSlamProcesser::OrbSlamProcesser
+ * @param session session id
+ * @param voc ORBVocabulary file with Bag of Words for image parsing.
+ */
+OrbSlamProcesser::OrbSlamProcesser(
+ qint32 session, ORB_SLAM2::ORBVocabulary *voc,
+ bool benchmarking, bool logTime, bool loopClosing)
+{
+ mSession = session;
+ mVocabulary = voc;
+ mSLAM = nullptr;
+ mLogTime = logTime;
+ mViewerAR = nullptr;
+ mCalibrateMode = false;
+ mLoopClosing = loopClosing;
+ mBenchmarking = benchmarking;
+ mRunning = true;
+}
+
+/**
+ * @brief OrbSlamProcesser::~OrbSlamProcesser
+ */
+OrbSlamProcesser::~OrbSlamProcesser()
+{
+ mRunning = false;
+ if (mSLAM) {
+ fDebug << "Shutting down SLAM";
+ mSLAM->Shutdown();
+ }
+ if (mViewerAR) {
+ fDebug << "Shutting down Graphics Module";
+ mViewerAR->Stop();
+ }
+}
+
+/**
+ * @brief OrbSlamProcesser::setCalibrateMode
+ * @param setEnabled
+ */
+void OrbSlamProcesser::setCalibrateMode(bool setEnabled)
+{
+ if (mCalibrateMode != setEnabled) {
+ mCalibrateMode = setEnabled;
+ if (!mCalibrateMode) {
+ calibrateCamera();
+ } else {
+ mImgSizeCV = cv::Size();
+ }
+ }
+}
+
+/**
+ * @brief OrbSlamProcessor::setUserInteractionConfiguration
+ * @param config
+ */
+void OrbSlamProcesser::setUserInteractionConfiguration(QJsonObject config)
+{
+ if (config.contains("3DObjectType")) {
+ m3DObjectType = config.value("3DObjectType").toString();
+ if (mViewerAR) {
+ mViewerAR->set3DObjectType(m3DObjectType);
+ }
+ }
+}
+
+/**
+ * @brief OrbSlamProcesser::setConfig
+ * @param calibration
+ *
+ * Sets up ORB_SLAM2, the rendering threads and calibration data.
+ */
+void OrbSlamProcesser::setConfig(QJsonObject calibration)
+{
+ if (mSLAM != nullptr) {
+ return;
+ }
+ // Create SLAM system. It initializes all system threads and gets ready to
+ // process frames.
+ if (!calibration.contains("Camera.width") ||
+ !calibration.contains("Camera.height")) {
+ fDebug << "Config data does not contain width or height.";
+ return;
+ }
+ fDebug << "Width: " << calibration.value("Camera.width").toInt();
+ fDebug << "Height: " << calibration.value("Camera.height").toInt();
+
+ QSize imageSize = QSize(calibration.value("Camera.width").toInt(),
+ calibration.value("Camera.height").toInt());
+ if (imageSize != mImgSize) {
+ mViewerAR = new ViewerAR(imageSize.width(), imageSize.height(), mBenchmarking, mLogTime);
+ mViewerAR->set3DObjectType(m3DObjectType);
+ mSLAM = new ORB_SLAM2::System(
+ mVocabulary,
+ calibration,
+ ORB_SLAM2::System::MONOCULAR,
+ false,
+ mLoopClosing);
+
+ double fps = calibration.value("Camera.fps").toDouble(30);
+ mViewerAR->setSLAM(mSLAM);
+ mViewerAR->setFPS(fps);
+
+ float fx = static_cast<float>(calibration.value("Camera.fx").toDouble(517.306408));
+ float fy = static_cast<float>(calibration.value("Camera.fy").toDouble(516.469215));
+ float cx = static_cast<float>(calibration.value("Camera.cx").toDouble(318.643040));
+ float cy = static_cast<float>(calibration.value("Camera.cy").toDouble(255.313989));
+
+ mViewerAR->setCameraCalibration(fx,fy,cx,cy);
+
+ mK = cv::Mat::eye(3,3,CV_32F);
+ mK.at<float>(0,0) = fx;
+ mK.at<float>(1,1) = fy;
+ mK.at<float>(0,2) = cx;
+ mK.at<float>(1,2) = cy;
+
+ mDistCoef = cv::Mat::zeros(4, 1, CV_32F);
+ mDistCoef.at<float>(0) = static_cast<float>(calibration.value("Camera.k1").toDouble(0.262383));
+ mDistCoef.at<float>(1) = static_cast<float>(calibration.value("Camera.k2").toDouble(-0.953104));
+ mDistCoef.at<float>(2) = static_cast<float>(calibration.value("Camera.p1").toDouble(-0.005358));
+ mDistCoef.at<float>(3) = static_cast<float>(calibration.value("Camera.p2").toDouble(0.002628));
+ const float k3 = static_cast<float>(calibration.value("Camera.k3").toDouble(1.163314));
+ if (k3 != 0) {
+ mDistCoef.resize(5);
+ mDistCoef.at<float>(4) = k3;
+ }
+
+ mViewerARthread = new std::thread(&ViewerAR::Run, mViewerAR);
+
+ QObject::connect(mViewerAR, &ViewerAR::newImageReady,
+ [=](quint32 frameid, QImage img, int metadata) {
+ if (mRunning && mLogTime) {
+ mProcessingFinishedTimes.insert(frameid, mUptime->nsecsElapsed());
+ }
+ if (mRunning && mEmitJPEG) {
+ emit sendFile(
+ mSession, NetworkConnection::File(
+ (mBenchmarking ?
+ NetworkConnection::FileType::IMAGE_WITH_METADATA :
+ NetworkConnection::FileType::IMAGE),
+ frameid, jpegFromQImage(img, mBenchmarking, metadata)));
+ }
+ if (mRunning && mEmitQImage) {
+ emit sendQImage(mSession, frameid, img);
+ }
+ });
+ }
+ mImgSize = imageSize;
+}
+
+/**
+ * @brief OrbSlamProcesser::setDebugMode
+ * @param enable Enabled or disabled.
+ *
+ * Set the system in debug mode to retrieve more real-time debugging data.
+ */
+void OrbSlamProcesser::setDebugMode(bool enable)
+{
+ mDebugMode = enable;
+}
+
+
+
+
+/**
+ * @brief OrbSlamProcesser::process
+ * @param session Session id
+ * @param mat OpenCV Mat
+ * @param frameid Image id
+ *
+ * If the instance is in camera calibration mode, the images sent to the internal
+ * functions wrapping OpenCV's chessboard camera calibration functions.
+ * If it's in normal mode, it's sent to the
+ * ORB_SLAM2 SLAM system, to be rendered using the ViewerAR Mixed Reality class.
+ */
+void OrbSlamProcesser::process(qint32 session, quint32 frameid, cvMatPtr mat)
+{
+ Q_UNUSED(session);
+ if (mCalibrateMode) {
+ auto outimg = processImageForCalibration(*mat);
+ cv::Mat dstImage;
+ cvtColor(outimg, dstImage, cv::COLOR_BGR2RGB);
+ if (mEmitJPEG) {
+ emit sendFile(mSession, NetworkConnection::File(
+ NetworkConnection::FileType::IMAGE,
+ frameid, jpegFromMat(dstImage)));
+ }
+ if (mEmitQImage) {
+ emit sendQImage(mSession, frameid, qImageFromMat(outimg));
+ }
+ } else if (mSLAM) {
+ bool colorFrame = false;
+ if (mIdentifyColorFrame) {
+ colorFrame = true;
+ for (int i = 0; i < 10; i++) {
+ unsigned char * p = mat->ptr(2, 20 * i + 20);
+ if (p[0] < 230 || p[1] > 20 || p[2] < 230) {
+ colorFrame = false;
+ break;
+ }
+ }
+ }
+ cv::Mat Tcw = mSLAM->TrackMonocular(mat->clone(), static_cast<double>(frameid) / 30);
+ int state = mSLAM->GetTrackingState();
+ vector<ORB_SLAM2::MapPoint*> vMPs = mSLAM->GetTrackedMapPoints();
+ vector<cv::KeyPoint> vKeys = mSLAM->GetTrackedKeyPointsUn();
+ if (mTriggeredA) {
+ // Add a 3D object.
+ mViewerAR->add3DObject();
+ mTriggeredA = false;
+ }
+ if (mTriggeredB) {
+ // Remove the MR 3D object.
+ mViewerAR->remove3DObject();
+ mTriggeredB = false;
+ }
+ if (mTriggeredC) {
+ // Reset localization.
+ mSLAM->Reset();
+ mTriggeredC = false;
+ }
+ mViewerAR->setImagePose(frameid, colorFrame, mat->clone(), Tcw, state, vKeys, vMPs);
+ }
+}
+
+/**
+ * @brief OrbSlamProcesser::processImageForCalibration
+ * @param image
+ * @return OpenCV Mat with chessboard pattern highlighted if found.
+ *
+ * If action is triggered the image chessboard data is
+ * added to the calibration vectors.
+ */
+cv::Mat OrbSlamProcesser::processImageForCalibration(cv::Mat image) {
+ /*
+ Problem : Advanced Lane Finding
+ A C++ translation of Project 4 - Advanced Lane Finding
+ Course: Udacity Self-Driving Car Nanodegree
+ Author : Dat Nguyen
+ Date : Feb 24, 2016
+ */
+ const int CHESS_ROWS = 9;
+ const int CHESS_COLS = 6;
+ if (mImgSizeCV.area() != 0 && image.size() != mImgSizeCV) {
+ fDebug << QString("Size differs. old=(%1x%2), new=(%3x%4)")
+ .arg(mImgSizeCV.width).arg(mImgSizeCV.height)
+ .arg(image.size().width).arg(image.size().height);
+ mArrObjPoints = std::vector<objPoints>();
+ mArrImgPoints = std::vector<imgPoints>();
+ mOutCamMatrix = cv::Mat(3, 3, CV_32FC1);
+ mOutDistCoeffs = cv::Mat();
+ }
+ if (!image.empty()) {
+ // Size of the chess board
+ cv::Size board_sz = cv::Size(CHESS_ROWS, CHESS_COLS);
+ // Hold corner values returned from findChessCorners
+ imgPoints corners;
+ // Generate object points
+ objPoints corners_3d;
+ for (int i = 0; i < CHESS_COLS * CHESS_ROWS; i++) {
+ corners_3d.push_back(cv::Point3f(i / CHESS_ROWS, i % CHESS_ROWS, 0.0f));
+ }
+ // Find Chess corners
+ if (cv::findChessboardCorners(
+ image, board_sz, corners,
+ CV_CALIB_CB_ADAPTIVE_THRESH |
+ CV_CALIB_CB_FAST_CHECK |
+ CV_CALIB_CB_NORMALIZE_IMAGE)) {
+ cv::Mat gray;
+ cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY);
+ cv::cornerSubPix(gray, corners,
+ cv::Size(11, 11), cv::Size(-1, -1),
+ cv::TermCriteria(CV_TERMCRIT_EPS |
+ CV_TERMCRIT_ITER,
+ 30, 0.1));
+ if (mTriggeredA) {
+ mTriggeredA = false;
+ fDebug << QString("Added image points for image size %1x%2.")
+ .arg(image.size().width).arg(image.size().height);
+ mArrImgPoints.push_back(corners);
+ mArrObjPoints.push_back(corners_3d);
+ }
+ cv::drawChessboardCorners(image, board_sz, corners, true);
+ cv::circle(image, cv::Point(image.size().width, image.size().height),
+ 20, cv::Scalar(0.5, 0.5, 0.5, 0.5));
+ }
+ mImgSizeCV = image.size();
+ }
+ return image;
+}
+
+/**
+ * @brief OrbSlamProcesser::calibrateCamera
+ */
+void OrbSlamProcesser::calibrateCamera()
+{
+ if (mArrObjPoints.empty() || mArrImgPoints.empty()) {
+ return;
+ }
+ /*
+ Problem : Advanced Lane Finding
+ A C++ translation of Project 4 - Advanced Lane Finding
+ Course: Udacity Self-Driving Car Nanodegree
+ Author : Dat Nguyen
+ Date : Feb 24, 2016
+ */
+ std::vector<cv::Mat> rotationVectors;
+ std::vector<cv::Mat> translationVectors;
+ mOutCamMatrix.ptr<float>(0)[0] = 1;
+ mOutCamMatrix.ptr<float>(1)[1] = 1;
+ cv::calibrateCamera(mArrObjPoints, mArrImgPoints, mImgSizeCV,
+ mOutCamMatrix, mOutDistCoeffs, rotationVectors, translationVectors);
+ QJsonObject recordObject;
+ recordObject.insert("Camera.width", mImgSizeCV.width);
+ recordObject.insert("Camera.height", mImgSizeCV.height);
+ recordObject.insert("Camera.fx", mOutCamMatrix.at<double>(0, 0));
+ recordObject.insert("Camera.fy", mOutCamMatrix.at<double>(1, 1));
+ recordObject.insert("Camera.cx", mOutCamMatrix.at<double>(0, 2));
+ recordObject.insert("Camera.cy", mOutCamMatrix.at<double>(1, 2));
+ recordObject.insert("Camera.k1", mOutDistCoeffs.at<double>(0));
+ recordObject.insert("Camera.k2", mOutDistCoeffs.at<double>(1));
+ recordObject.insert("Camera.p1", mOutDistCoeffs.at<double>(2));
+ recordObject.insert("Camera.p2", mOutDistCoeffs.at<double>(3));
+ recordObject.insert("Camera.k3", (mOutDistCoeffs.size().height > 4) ?
+ mOutDistCoeffs.at<double>(4) : 0);
+ setConfig(recordObject);
+ QByteArrayPtr ret(new QByteArray(QJsonDocument(recordObject).toJson()));
+ fDebug << "Calibration: " << ret->data();
+ emit sendFile(mSession, NetworkConnection::File(
+ NetworkConnection::FileType::JSON, 1, ret));
+}
+
+}
diff --git a/source/orbslamprocesser.h b/source/orbslamprocesser.h
new file mode 100644
index 0000000000000000000000000000000000000000..ada938bc6ab3dd12553978958127809b03dbe629
--- /dev/null
+++ b/source/orbslamprocesser.h
@@ -0,0 +1,88 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef ORBSLAMPROCESSER_H
+#define ORBSLAMPROCESSER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QImage>
+#include <QMutex>
+#include <QObject>
+#include <QJsonObject>
+#include <opencv/cv.hpp>
+#include <opencv/cxcore.hpp>
+#include "imageprocesser.h"
+#include "ORBVocabulary.h"
+
+namespace ORB_SLAM2 {
+class System;
+}
+
+namespace std {
+class thread;
+}
+
+namespace MRLeo {
+
+class ViewerAR;
+
+/**
+ * @brief The OrbSlamProcesser class
+ *
+ * Class to manage Mixed Reality.
+ * Contains a SLAM image analyzer (ORB_SLAM2) and a 3D renderer for point
+ * clouds and mixed reality visual objects.
+ * Can be set in camera calibration mode to retrieve the camera properties for
+ * better 3D localization results.
+ */
+class OrbSlamProcesser : public ImageProcesser {
+ Q_OBJECT
+
+ typedef std::vector<cv::Point3f> objPoints; // object points (3D) for one image
+ typedef std::vector<cv::Point2f> imgPoints; // image point (2D) for one image
+
+public:
+ explicit OrbSlamProcesser(qint32 session, ORB_SLAM2::ORBVocabulary *voc,
+ bool benchmarking, bool logTimes, bool loopClosing);
+ ~OrbSlamProcesser() override;
+ void setDebugMode(bool enable) override;
+ ORB_SLAM2::ORBVocabulary * getVocabulary() { return mVocabulary; }
+
+
+public slots:
+ void setConfig(QJsonObject calibration) override;
+ void setUserInteractionConfiguration(QJsonObject config) override;
+ void setCalibrateMode(bool enabled) override;
+ void calibrateCamera() override;
+
+protected:
+ void process(qint32 session, quint32 frameid, cvMatPtr mat) override;
+
+private:
+ bool mCalibrateMode;
+ cv::Mat processImageForCalibration(cv::Mat image);
+ std::vector<objPoints> mArrObjPoints; //array of object points for an array of images
+ std::vector<imgPoints> mArrImgPoints; //array of images points for an array of iamges
+ cv::Mat mOutCamMatrix = cv::Mat(3, 3, CV_32FC1);
+ cv::Mat mOutDistCoeffs; // This is what we need to undistort an image
+ cv::Size mImgSizeCV;
+ QSize mImgSize;
+ ORB_SLAM2::ORBVocabulary *mVocabulary;
+ ORB_SLAM2::System *mSLAM;
+ bool mLogTime;
+ cv::Mat mK;
+ cv::Mat mDistCoef;
+ ViewerAR *mViewerAR;
+ QString m3DObjectType;
+ std::thread* mViewerARthread;
+ bool mLoopClosing;
+};
+
+}
+
+#endif // ORBSLAMPROCESSER_H
diff --git a/source/tcpbuilder.cpp b/source/tcpbuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f6f6859b27fd44037d9221f138a0544941ba5a74
--- /dev/null
+++ b/source/tcpbuilder.cpp
@@ -0,0 +1,85 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "tcpbuilder.h"
+#include <QDataStream>
+#include <QDebug>
+#include <QFile>
+#include <QtMath>
+
+namespace MRLeo {
+
+/**
+ * @brief TcpBuilder::TcpBuilder
+ * @param session
+ */
+TcpBuilder::TcpBuilder(qint32 session)
+{
+ mSession = session;
+ mNumReceivedBytes = 0;
+ mData = QByteArrayPtr(new QByteArray);
+ mFileTransferSize = -1;
+ mBufferSent = true;
+ mBufferInitialized = false;
+}
+
+/**
+ * @brief TcpBuilder::readData
+ * @param session Session id.
+ * @param data New data in this packet
+ *
+ * Appends the new TCP packet and parses the data.
+ */
+void TcpBuilder::readData(qint32 session, QByteArrayPtr data)
+{
+ if (session != mSession) {
+ // The received signal was not for this session. Ignore it.
+ return;
+ }
+ if (!data.isNull()) {
+ // Append the received byte to concatenated byte array.
+ mData->append(*data);
+ mNumReceivedBytes = mData->size();
+ }
+ if (mBufferInitialized &&
+ mNumReceivedBytes >= mFileTransferSize &&
+ !mBufferSent) {
+ // We have received all the bytes for this object as
+ // defined in its header. Create a new byte array with all
+ // the content and send it out.
+ QByteArrayPtr retArray(new QByteArray(*mData));
+ if (retArray->size() > mFileTransferSize) {
+ retArray->truncate(mFileTransferSize);
+ }
+ mData->remove(0, mFileTransferSize);
+ emit fileReady(mSession, NetworkConnection::File(
+ mFileTransferType, mFileTransferId, retArray));
+ mNumReceivedBytes = 0;
+ mFileTransferSize = -1;
+ mBufferInitialized = false;
+ mBufferSent = true;
+ }
+ if (!mBufferInitialized && mData->size() >= 10) {
+ // We have received a new header. Parse it and store its content.
+ QDataStream dstream(mData.data(), QIODevice::OpenModeFlag::ReadOnly);
+ dstream.setByteOrder(QDataStream::BigEndian);
+ dstream >> mFileTransferSize;
+ dstream >> mFileTransferType;
+ dstream >> mFileTransferId;
+ mData->remove(0, 10);
+ mNumReceivedBytes = mData->size();
+ mBufferInitialized = true;
+ mBufferSent = false;
+ if (mNumReceivedBytes >= mFileTransferSize) {
+ // All the content of the new file is already in the stored byte array.
+ // Rerun the function so that the file can be sent immediately.
+ readData(mSession, nullptr);
+ }
+ }
+}
+
+}
diff --git a/source/tcpbuilder.h b/source/tcpbuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..410a8a8e32407009ac1b279c627c0c9120b71b39
--- /dev/null
+++ b/source/tcpbuilder.h
@@ -0,0 +1,49 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef TCPBUILDER_H
+#define TCPBUILDER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QObject>
+
+namespace MRLeo {
+
+/**
+ * @brief The TcpBuilder class
+ *
+ * Concatenates the TCP byte stream for a single session and parses it. Objects in the stream (images, json text, control messages)
+ * are packaged as NetworkConnection::File objects and sent out to listeners using Qt signal fileReady().
+ */
+class TcpBuilder : public QObject {
+ Q_OBJECT
+
+public:
+ TcpBuilder(qint32 session);
+
+public slots:
+ void readData(qint32 session, QByteArrayPtr data);
+
+signals:
+ void fileReady(qint32 session, NetworkConnection::File file);
+
+private:
+ explicit TcpBuilder();
+ qint32 mSession;
+ qint32 mNumReceivedBytes;
+ QByteArrayPtr mData;
+ qint32 mFileTransferSize;
+ qint32 mFileTransferId;
+ qint16 mFileTransferType;
+ bool mBufferInitialized;
+ bool mBufferSent;
+};
+
+}
+
+#endif // TCPBUILDER_H
diff --git a/source/tcpconnection.cpp b/source/tcpconnection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..b357ae30bd0801c68a00618278778bf128b97007
--- /dev/null
+++ b/source/tcpconnection.cpp
@@ -0,0 +1,276 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "tcpconnection.h"
+#include "cannyfilter.h"
+#include "mrserver.h"
+#include "tcpbuilder.h"
+#include "udpsender.h"
+#include <QTcpServer>
+#include <QTcpSocket>
+#include <QThread>
+#include <QDateTime>
+#include <QElapsedTimer>
+
+namespace MRLeo {
+
+/**
+ * @brief TcpConnection::TcpConnection
+ * @param parent The owner.
+ * @param port Port to listen to. Use 0 if up to system to decide which.
+ *
+ * Starts the TCP server. If port is set to 0 the system will select an
+ * available one. Status can be checked with getPort(), where 0 means
+ * that the serve initialization wasn't successful (the port was in use).
+ */
+TcpConnection::TcpConnection(MRServer *parent, quint16 port)
+{
+ this->mMRServer = parent;
+ mServer = new QTcpServer(this);
+ bool portBound = mServer->listen(QHostAddress::Any, port);
+ if (portBound) {
+ mPort = mServer->serverPort();
+ } else {
+ mPort = 0;
+ }
+ QObject::connect(mServer, &QTcpServer::newConnection,
+ this, &TcpConnection::newConnection);
+ QObject::connect(this, &TcpConnection::sendFileNow,
+ this, &TcpConnection::sendFile);
+ // Used to generate session id.
+}
+
+/**
+ * @brief TcpConnection::~TcpConnection
+ * Close all sockets and TCP listeners.
+ */
+TcpConnection::~TcpConnection()
+{
+ auto begin = mTcpBuilders.begin();
+ while (begin != mTcpBuilders.end()) {
+ begin.value()->deleteLater();
+ begin++;
+ }
+ mServer->close();
+}
+
+/**
+ * @brief TcpConnection::sendFileIfLatest
+ * @param session
+ * @param file
+ */
+void TcpConnection::sendFileIfLatest(qint32 session, File file)
+{
+ if ((file.type == NetworkConnection::FileType::IMAGE ||
+ file.type == NetworkConnection::FileType::IMAGE_WITH_METADATA) &&
+ !sendImagesForSession(session)) {
+ return;
+ }
+ mSendBufferFilesMutex.lock();
+ if (mSendBufferFiles.contains(session)) {
+ fDebug << "New file replaces old file in buffer.";
+ }
+ mSendBufferFiles.insert(session, file);
+ mSendBufferFilesMutex.unlock();
+ bytesWritten(session, 0); // Check if can send.
+}
+
+/**
+ * @brief TcpConnection::sendFile
+ * @param session The session id for the connection.
+ * @param file NetworkConnection::File to be sent.
+ *
+ * Sends a NetworkConnection::File to the socket for the specified session.
+ * If the file's an image and this instance's setSendImagesForSession() has been set
+ * to false for this session id the call will be ignored.
+ */
+void TcpConnection::sendFile(qint32 session, File file)
+{
+ if (file.type == NetworkConnection::FileType::PONG ||
+ ((file.type == NetworkConnection::FileType::IMAGE ||
+ file.type == NetworkConnection::FileType::IMAGE_WITH_METADATA) &&
+ !sendImagesForSession(session))) {
+ return;
+ }
+ if (file.type != NetworkConnection::FileType::IMAGE &&
+ file.type != NetworkConnection::FileType::IMAGE_WITH_METADATA) {
+ //fDebug << QString("Sending (session=%1, type=%2, length=%3)")
+ // .arg(session).arg(file.type).arg(file.data->length());
+ }
+ mSessionMutex.lock();
+ auto sendsock = mSockets.value(session, nullptr);
+ mSessionMutex.unlock();
+ if (sendsock == nullptr || !sendsock->isOpen()) {
+ fDebug << "No valid open socket";
+ return;
+ }
+ mSendBufferLevelsMutex.lock();
+ QByteArray datagram;
+ // Create a header for the file. Must follow this format and be
+ // big endian to be parsable by the Java clients.
+ QDataStream dstream(&datagram, QIODevice::OpenModeFlag::WriteOnly);
+ dstream.setByteOrder(QDataStream::BigEndian);
+ dstream << static_cast<qint32>(file.data->size());
+ dstream << static_cast<qint16>(file.type);
+ dstream << static_cast<qint32>(file.id);
+ // Add the header and file lengths to the buffer level, so that we
+ // can see when the file has been sent by listening to bytesWritten().
+ auto level = mSendBufferLevels.value(session);
+ level += sendsock->write(datagram);
+ level += sendsock->write(*file.data);
+ mSendBufferLevels.insert(session, level);
+ mSendBufferLevelsMutex.unlock();
+ if (mLogTime && mUptime &&
+ (file.type == NetworkConnection::FileType::IMAGE ||
+ file.type == NetworkConnection::FileType::IMAGE_WITH_METADATA)) {
+ mTimeLogsMutex.lock();
+ auto timelogs = mTimeLogs.value(session, nullptr);
+ mTimeLogsMutex.unlock();
+ if (timelogs) {
+ timelogs->insert(file.id, mUptime->nsecsElapsed());
+ }
+ }
+}
+
+/**
+ * @brief TcpConnection::newConnection
+ *
+ * Called by the underlying system when a connection has been made.
+ * Creates class instances that will parse incoming data and send files
+ * for the new session.
+ */
+void TcpConnection::newConnection()
+{
+ while (mServer->hasPendingConnections()) {
+ auto client = mServer->nextPendingConnection();
+ QString clientHost = client->peerAddress().toString();
+ quint16 clientPort = client->peerPort();
+ fDebug << QString("New connection: Sender ip: %1, sender port %2")
+ .arg(clientHost).arg(clientPort);
+ mSessionMutex.lock();
+ qint32 sessionId = 0;
+ // Generate a new unique session id. Randomize id until unique.
+ while (true) {
+ sessionId = static_cast<qint32>(qrand());
+ if (sessionId < 0) {
+ // Id must be > 0.
+ sessionId = 0 - sessionId;
+ }
+ if (!mSockets.contains(sessionId)) {
+ fDebug << "New session id: " << sessionId;
+ break;
+ }
+ }
+ mSockets.insert(sessionId, client);
+ mSessionMutex.unlock();
+ mTimeLogsMutex.lock();
+ mTimeLogs.insert(sessionId, new QMap<quint32, qint64>());
+ mTimeLogsMutex.unlock();
+ auto tcpbuilder = new TcpBuilder(sessionId);
+ mTcpBuilders.insert(sessionId, tcpbuilder);
+ // Announce that a new session has been set up so that other
+ // classes like videoreceiver etc. can be set up.
+ emit newSession(sessionId, clientHost, clientPort);
+ // Run the TCP message parser in a separate thread.
+ tcpbuilder->moveToThread(new QThread(this));
+ tcpbuilder->thread()->start();
+
+ QObject::connect(client, &QTcpSocket::readyRead,
+ [=]() { this->readyRead(client); });
+ QObject::connect(client, &QAbstractSocket::disconnected,
+ client, &QObject::deleteLater);
+ QObject::connect(client, &QAbstractSocket::disconnected,
+ [=]() { this->socketDisconnected(client); });
+ QObject::connect(this, &TcpConnection::dataAvailable,
+ tcpbuilder, &TcpBuilder::readData);
+ QObject::connect(tcpbuilder, &TcpBuilder::fileReady,
+ [=](qint32 sessionId, File file) {
+ emit fileReady(sessionId, file);
+ });
+ QObject::connect(client, &QTcpSocket::disconnected,
+ [=]() {
+ mSessionMutex.lock();
+ fDebug << "TCP: Socket Disconnected";
+ mSockets.remove(sessionId);
+ mSessionMutex.unlock();
+ });
+ QObject::connect(client, &QTcpSocket::bytesWritten,
+ [=](qint64 bytes) {
+ bytesWritten(sessionId, bytes);
+ });
+ // For debugging reasons this has been disabled.
+ /*
+ QObject::connect(client, &QAbstractSocket::disconnected,
+ client, &QObject::deleteLater);
+ QObject::connect(client, &QAbstractSocket::disconnected,
+ tcpbuilder, &QObject::deleteLater);
+ QObject::connect(client, &QAbstractSocket::disconnected,
+ client, &QObject::deleteLater);
+ */
+ }
+}
+
+/**
+ * @brief TcpConnection::bytesWritten
+ * @param session Session id for socket.
+ * @param bytesWritten Bytes sent since the last call to this callback.
+ *
+ * Callback function for socket usage for seeing if we have sent all data in the
+ * pipeline and a new file can be sent.
+ * If the pipeline is empty, a new file is picked from the send buffer and sent.
+ * Used to control files to be sent with sendFilesIfLatest().
+ */
+void TcpConnection::bytesWritten(qint32 session, qint64 bytesWritten)
+{
+ auto level = mSendBufferLevels.value(session);
+ level -= bytesWritten;
+ mSendBufferLevelsMutex.lock();
+ mSendBufferLevels.insert(session, level);
+ mSendBufferLevelsMutex.unlock();
+ if (level == 0) {
+ mSendBufferFilesMutex.lock();
+ File file = mSendBufferFiles.take(session);
+ mSendBufferFilesMutex.unlock();
+ if (!file.data.isNull()) {
+ emit sendFileNow(session, file);
+ }
+ }
+}
+
+/**
+ * @brief TcpConnection::readyRead
+ * @param client Socket in use.
+ *
+ * Called by QTcpSocket when it has received new data. Socket is mapped to a
+ * session id and data is packaged into a byte array and sent to the TcpBuilder
+ * function to be parsed and rebuilt.
+ */
+void TcpConnection::readyRead(QTcpSocket *client)
+{
+ mSessionMutex.lock();
+ auto session = mSockets.key(client, 0);
+ mSessionMutex.unlock();
+ QByteArrayPtr bufferPtr(new QByteArray(client->readAll()));
+ emit dataAvailable(session, bufferPtr);
+}
+
+/**
+ * @brief TcpConnection::disconnected
+ * @param client Socket that was disconnected.
+ *
+ * Called by QTcpSocket when the client has disconnected.
+ */
+void TcpConnection::socketDisconnected(QTcpSocket *client)
+{
+ mSessionMutex.lock();
+ auto session = mSockets.key(client, 0);
+ emit sessionDestroyed(session);
+ mSessionMutex.unlock();
+}
+
+
+}
diff --git a/source/tcpconnection.h b/source/tcpconnection.h
new file mode 100644
index 0000000000000000000000000000000000000000..7be2696ed37383f638fed568e40a50e6aa8a9b3e
--- /dev/null
+++ b/source/tcpconnection.h
@@ -0,0 +1,71 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef TCPCONNECTION_H
+#define TCPCONNECTION_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QMap>
+#include <QObject>
+
+class QTcpSocket;
+class QTcpServer;
+
+namespace MRLeo {
+
+class UdpSender;
+class MRServer;
+class TcpBuilder;
+class ImageProcesser;
+
+/**
+ * @brief The TcpConnection class
+ *
+ * A TCP server. Manages TCP client sockets, receiving and sending files
+ * and TCP related threads.
+ * Data sent to the sockets must follow the mrleo network communication format
+ * for header parsing to work.
+ * Uses QTcpServer as the underlying layer for communication with the system platform.
+ */
+class TcpConnection : public NetworkConnection {
+ Q_OBJECT
+
+public:
+ TcpConnection(MRServer *parent, quint16 port);
+ ~TcpConnection() override;
+ quint16 getPort() override { return mPort; }
+
+public slots:
+ void newConnection();
+ void sendFile(qint32 sessionId, NetworkConnection::File file) override;
+ void sendFileIfLatest(qint32 session, File file) override;
+
+signals:
+ void sendFileNow(qint32 session, NetworkConnection::File file);
+ void dataAvailable(qint32, QByteArrayPtr);
+
+private slots:
+ void socketDisconnected(QTcpSocket *client);
+ void readyRead(QTcpSocket *socket);
+
+protected:
+ void bytesWritten(qint32 session, qint64 bytesWritten) override;
+
+private:
+ MRServer *mMRServer;
+ QTcpServer *mServer;
+ QMap<qint32, QTcpSocket*> mSockets;
+ QMap<qint32, TcpBuilder*> mTcpBuilders;
+ quint16 mPort = 0;
+ QMutex mSendBufferLevelsMutex;
+ QMutex mSessionMutex;
+};
+
+}
+
+#endif // TCPCONNECTION_H
diff --git a/source/udpbuilder.cpp b/source/udpbuilder.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c99dea715b39e179ab5e6019ca450b9732ea404a
--- /dev/null
+++ b/source/udpbuilder.cpp
@@ -0,0 +1,111 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "udpbuilder.h"
+#include <QDataStream>
+#include <QDebug>
+#include <QFile>
+#include <QThread>
+
+namespace MRLeo {
+
+/**
+ * @brief UdpBuilder::UdpBuilder
+ * @param host Sender port.
+ * @param port Sender port.
+ */
+UdpBuilder::UdpBuilder(QString host, quint16 port)
+{
+ mHost = host;
+ mPort = port;
+ mNumReceivedBytes = 0;
+ mCurrentFileId = -1;
+ mData = QByteArrayPtr(new QByteArray);
+ mCurrentType = 0;
+ mCurrentSize = 0;
+ mSession = 0;
+ mSent = true;
+}
+
+/**
+ * @brief UdpBuilder::readData
+ * @param host Sender host.
+ * @param port Sender port.
+ * @param data The packet including header and data.
+ *
+ * New data received from the network layer.
+ */
+void UdpBuilder::readData(QString host, quint16 port, QByteArrayPtr data)
+{
+ if (host != mHost || port != mPort) {
+ // The data is not for this session.
+ return;
+ }
+
+ // Parse the datagram header
+ QDataStream dstream(data.data(), QIODevice::OpenModeFlag::ReadOnly);
+ dstream.setByteOrder(QDataStream::BigEndian);
+ qint16 type, packetsize;
+ qint32 fileid, offset, totalsize;
+ dstream >> type;
+ dstream >> fileid;
+ dstream >> totalsize;
+ dstream >> offset;
+ dstream >> packetsize;
+
+ if (fileid < mCurrentFileId) {
+ // The packet file id is for a file already passed along.
+ // Discard late-arriving packets.
+ return;
+ }
+
+ if (!mSent && fileid > mCurrentFileId) {
+ // While we haven't received 100% of this file we have now
+ // received a packet with a new file. Send out the old received data so
+ // that we can start with the new file.
+ mData->truncate(mCurrentSize);
+ emit fileReady(mSession, NetworkConnection::File(
+ mCurrentType, mCurrentFileId, mData));
+ mSent = true;
+ }
+
+ if (mSent == true) {
+ // Start with a new file. Set up a new file session
+ // with the data from the header.
+ mSent = false;
+ mData = QByteArrayPtr(new QByteArray);
+ mData->resize(totalsize);
+ mCurrentSize = totalsize;
+ mCurrentType = type;
+ mNumReceivedBytes = 0;
+ mCurrentFileId = fileid;
+ }
+
+ int thisPacketSize = data->length() - HEADER_SIZE;
+ mNumReceivedBytes += thisPacketSize;
+
+ if (packetsize != thisPacketSize) {
+ // The datagram does not contain all the data it reports that it contains.
+ fDebug << QString("UdpBuilder: ERROR: Packet sizes incorrect %1 %2")
+ .arg(packetsize).arg(thisPacketSize);
+ }
+ if (offset > totalsize) {
+ // The datagram wants to be inserted in a strange position.
+ fDebug << "UdpBuilder: ERROR: Offset too large: " << offset;
+ }
+
+ mData->insert(offset, data->mid(HEADER_SIZE, packetsize));
+ if (!mSent && mNumReceivedBytes >= mCurrentSize) {
+ // We have received 100% of all packets for this file.
+ mData->truncate(mCurrentSize);
+ emit fileReady(mSession, NetworkConnection::File(
+ mCurrentType, mCurrentFileId, mData));
+ mSent = true;
+ }
+}
+
+}
diff --git a/source/udpbuilder.h b/source/udpbuilder.h
new file mode 100644
index 0000000000000000000000000000000000000000..171fd5be58e7c4b9c8304daffa968cc947ee4dd9
--- /dev/null
+++ b/source/udpbuilder.h
@@ -0,0 +1,59 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef UDPBUILDER_H
+#define UDPBUILDER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QHostAddress>
+#include <QObject>
+
+namespace MRLeo {
+
+/**
+ * @brief The UdpBuilder class
+ *
+ * Parse the UDP datagram stream for a client session and sends out
+ * file objects found in it.
+ * Implements the mrleo network protocol to overcome issues related
+ * to UDP packet loss and packet corruption.
+ */
+class UdpBuilder : public QObject {
+ Q_OBJECT
+
+public:
+ UdpBuilder(QString host, quint16 port);
+ ~UdpBuilder() {}
+ void setSession(qint32 session) {
+ mSession = session;
+ }
+
+public slots:
+ void readData(QString host, quint16 port, QByteArrayPtr data);
+
+signals:
+ void fileReady(qint32 session, NetworkConnection::File file);
+
+private:
+ explicit UdpBuilder();
+
+ QString mHost;
+ quint16 mPort;
+ qint32 mSession;
+ QByteArrayPtr mData;
+ int mNumReceivedBytes;
+ int mCurrentFileId;
+ qint16 mCurrentType;
+ int mCurrentSize;
+ bool mSent;
+ static const int HEADER_SIZE = 16;
+};
+
+}
+
+#endif // UDPBUILDER_H
diff --git a/source/udpconnection.cpp b/source/udpconnection.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..4edf774ea56bcb856e6ab3b13697853a17c3bc09
--- /dev/null
+++ b/source/udpconnection.cpp
@@ -0,0 +1,224 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "udpconnection.h"
+#include "mrserver.h"
+#include "udpbuilder.h"
+#include "udpsender.h"
+#include <tuple>
+#include <QThread>
+#include <QUdpSocket>
+#include <QDataStream>
+
+namespace MRLeo {
+
+/**
+ * @brief UdpConnection::UdpConnection
+ * @param parent The owner.
+ * @param port Port to listen to.
+ *
+ * Starts the UDP server listener.
+ * Status can be checked with getPort(), where 0 means
+ * that the serve initialization wasn't successful (the port was in use).
+ */
+UdpConnection::UdpConnection(MRServer *parent, quint16 port)
+{
+ this->mMRServer = parent;
+ mRecvsock = new QUdpSocket(this);
+ bool bound = mRecvsock->bind(QHostAddress::Any, port);
+ mPort = bound ? port : 0;
+ QObject::connect(mRecvsock, &QUdpSocket::readyRead,
+ this, &UdpConnection::readyRead);
+ // Create a sender instance and move it to a new thread.
+ mUdpSender = new UdpSender();
+ mUdpSender->moveToThread(new QThread(this));
+ mUdpSender->thread()->start();
+ QObject::connect(this, &UdpConnection::fileToSendUdp,
+ mUdpSender, &UdpSender::sendFileUdp);
+}
+
+/**
+ * @brief UdpConnection::~UdpConnection
+ *
+ * Close receiver and sender threads.
+ */
+UdpConnection::~UdpConnection()
+{
+ for (auto builder : mUdpBuilders) {
+ builder->thread()->deleteLater();
+ }
+ mUdpSender->thread()->deleteLater();
+ mRecvsock->close();
+}
+
+/**
+ * @brief UdpConnection::getProcessingTimes
+ * @param session
+ * @return QMap with frame id and nanosecond timestamp.
+ */
+QMap<quint32, qint64> UdpConnection::getProcessingTimes(qint32 session)
+{
+ return mUdpSender->getProcessingTimes(session);
+}
+
+/**
+ * @brief UdpConnection::setLogTime
+ * @param enable
+ * @param timer
+ */
+void UdpConnection::setLogTime(bool enable, QElapsedTimer* timer) {
+ mLogTime = enable;
+ mUptime = timer;
+ mUdpSender->setLogTime(enable, timer);
+}
+
+/**
+ * @brief UdpConnection::setPacketSize
+ * @param session Session id
+ * @param packetsize Packet size for this session. [100-2000]
+ *
+ * Sets the packet size to be used when sending files to
+ * the specified session.
+ * Packet sizes should not be too big, to prevent the packet from
+ * being broken up by the underlying network layers, or too small to prevent
+ * noise or packet being received in the wrong order.
+ * Recommended values are 500-800 bytes.
+ */
+void UdpConnection::setPacketSize(qint32 session, qint32 packetsize)
+{
+ mSessionsMutex.lock();
+ if (mSessions.contains(session)) {
+ auto obj = mSessions.value(session);
+ mSessions.insert(session, std::make_tuple(std::get<0>(obj), std::get<1>(obj), packetsize));
+ } else {
+ mSessions.insert(session, std::make_tuple("", 0, packetsize));
+ }
+ mSessionsMutex.unlock();
+}
+
+/**
+ * @brief UdpConnection::sendFile
+ * @param session The session id for the connection.
+ * @param file NetworkConnection::File to be sent.
+ *
+ * Sends a NetworkConnection::File to the host and port for the specified session.
+ * If the file's an image and this instance's setSendImagesForSession() has
+ * been set to false for this session id the call will be ignored.
+ */
+void UdpConnection::sendFile(qint32 session, File file)
+{
+ if (file.type != NetworkConnection::FileType::PONG &&
+ (!(file.type == NetworkConnection::FileType::IMAGE ||
+ file.type == NetworkConnection::FileType::IMAGE_WITH_METADATA) ||
+ !sendImagesForSession(session))) {
+ return;
+ }
+ mSessionsMutex.lock();
+ bool contains = mSessions.contains(session);
+ auto dest = mSessions.value(session);
+ mSessionsMutex.unlock();
+ if (!contains) {
+ fDebug << "Can not find session id=" << session;
+ return;
+ }
+ fDebug << QString("sendFile: type=%1, length=%2")
+ .arg(file.type).arg((file.data != nullptr) ? file.data->size() : 0);
+ // Read target IP, target port, session packet size and pass it to the UDP sender
+ emit fileToSendUdp(session, std::get<0>(dest), std::get<1>(dest), std::get<2>(dest), file);
+}
+
+/**
+ * @brief UdpConnection::sendFileIfLatest
+ * @param session The session id for the connection.
+ * @param file NetworkConnection::File to be sent.
+ *
+ * Sends a NetworkConnection::File to the host and port for the specified session.
+ * If the file's an image and this instance's setSendImagesForSession() has
+ * been set to false for this session id the call will be ignored.
+ * Before the socket is ready and the file is sent, any future calls to
+ * this function will replace this file with the new one.
+ */
+void UdpConnection::sendFileIfLatest(qint32 session, File file)
+{
+ if (!(file.type == NetworkConnection::FileType::IMAGE ||
+ file.type == NetworkConnection::FileType::IMAGE_WITH_METADATA) ||
+ !sendImagesForSession(session)) {
+ return;
+ }
+ mSessionsMutex.lock();
+ bool contains = mSessions.contains(session);
+ auto dest = mSessions.value(session);
+ mSessionsMutex.unlock();
+ if (!contains) {
+ fDebug << "Can not find session id=" << session;
+ return;
+ }
+ mUdpSender->sendFileIfLatest(session, file);
+ file.data = nullptr;
+ emit fileToSendUdp(session, std::get<0>(dest), std::get<1>(dest), std::get<2>(dest), file);
+}
+
+/**
+ * @brief UdpConnection::readyRead
+ *
+ * Called by the system network layer when a new UDP datagram (packet)
+ * has been received on the server port.
+ */
+void UdpConnection::readyRead()
+{
+ while (mRecvsock->hasPendingDatagrams()) {
+ QByteArrayPtr bufferPtr(new QByteArray);
+ bufferPtr->resize(static_cast<int>(mRecvsock->pendingDatagramSize()));
+ QHostAddress host;
+ quint16 senderPort;
+ mRecvsock->readDatagram(bufferPtr->data(), bufferPtr->size(),
+ &host, &senderPort);
+ QString senderHost = host.toString();
+ if (!mUdpBuilders.contains(std::make_pair(senderHost, senderPort))) {
+ // This client host and client port has not been seen before.
+ // Create a new UDP session.
+ fDebug << "New UDP connection";
+ auto udpbuilder = new UdpBuilder(senderHost, senderPort);
+ // Send all received data to the UDP stream parser.
+ QObject::connect(this, &UdpConnection::dataAvailable,
+ udpbuilder, &UdpBuilder::readData);
+ mUdpBuilders.insert(std::make_pair(senderHost, senderPort), udpbuilder);
+ // A new file object has been received from the UDP stream parser.
+ QObject::connect(udpbuilder, &UdpBuilder::fileReady,
+ [=](qint32 session, File file) {
+ if (file.type == NetworkConnection::FileType::CONNECTION) {
+ // The client has sent a session ID. We now know which TCP session
+ // this UDP session is bound to.
+ QDataStream dstream(file.data.data(), QIODevice::OpenModeFlag::ReadOnly);
+ dstream.setByteOrder(QDataStream::BigEndian);
+ qint32 session;
+ dstream >> session;
+ fDebug << "Register TCP session id:" << session;
+ udpbuilder->setSession(session);
+ mSessionsMutex.lock();
+ if (mSessions.contains(session)) {
+ // Packet size has already been defined for this session. Reuse the old session.
+ auto obj = mSessions.value(session);
+ mSessions.insert(session, std::make_tuple(senderHost, senderPort, std::get<2>(obj)));
+ } else {
+ // Create new entry in the session list.
+ mSessions.insert(session, std::make_tuple(senderHost, senderPort, mDefaultPacketSize));
+ }
+ mSessionsMutex.unlock();
+ } else {
+ emit fileReady(session, file);
+ }
+ });
+ // Create a new thread and move the UDP stream parser to that one.
+ udpbuilder->moveToThread(new QThread(this));
+ udpbuilder->thread()->start();
+ }
+ emit dataAvailable(senderHost, senderPort, bufferPtr);
+ }
+}
+
+}
diff --git a/source/udpconnection.h b/source/udpconnection.h
new file mode 100644
index 0000000000000000000000000000000000000000..65b9a5dfca5e3d433d78f350e8270148e25e4d63
--- /dev/null
+++ b/source/udpconnection.h
@@ -0,0 +1,69 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef UDPCONNECTION_H
+#define UDPCONNECTION_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <tuple>
+#include <QMap>
+#include <QObject>
+
+class QUdpSocket;
+
+namespace MRLeo {
+
+class MRServer;
+class UdpSender;
+class UdpBuilder;
+
+/**
+ * @brief The UdpConnection class
+ *
+ * A UDP server. Manages UDP client sessions, receiving and sending files
+ * and UDP related threads.
+ * Data sent to the server port must follow the MR-Leo network
+ * communication format for header parsing to work.
+ * Uses QUdpSocket as the underlying layer to the system network platform.
+ */
+class UdpConnection : public NetworkConnection {
+ Q_OBJECT
+
+public:
+ UdpConnection(MRServer *parent, quint16 port);
+ ~UdpConnection() override;
+ quint16 getPort() override { return mPort; }
+ QMap<quint32, qint64> getProcessingTimes(qint32 session) override;
+
+signals:
+ void dataAvailable(QString, quint16, QByteArrayPtr);
+ void fileToSendUdp(qint32 session, QString host, quint16 port,
+ qint32 packetsize, File file);
+
+public slots:
+ void readyRead();
+ void setPacketSize(qint32 session, qint32 packetsize);
+ void sendFile(qint32 session, File file) override;
+ void sendFileIfLatest(qint32 session, File file) override;
+ void setLogTime(bool enable, QElapsedTimer* timer) override;
+
+private:
+ QUdpSocket *mRecvsock;
+ UdpSender *mUdpSender;
+ MRServer *mMRServer;
+ QMap<std::pair<QString, quint16>, UdpBuilder *> mUdpBuilders;
+ QMap<std::pair<QString, quint16>, QByteArrayDataPtr> data;
+ QMutex mSessionsMutex;
+ QMap<qint32, std::tuple<QString, quint16, quint16>> mSessions;
+ quint16 mPort = 0;
+ qint32 mDefaultPacketSize;
+};
+
+}
+
+#endif // UDPCONNECTION_H
diff --git a/source/udpsender.cpp b/source/udpsender.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e1bb1f2c92bf1a16b079d44ca06b6494935781ca
--- /dev/null
+++ b/source/udpsender.cpp
@@ -0,0 +1,112 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "udpsender.h"
+#include <QDataStream>
+#include <QUdpSocket>
+#include <QtMath>
+#include <QElapsedTimer>
+#include <mutex>
+
+namespace MRLeo {
+
+/**
+ * @brief UdpSender::UdpSender
+ */
+UdpSender::UdpSender()
+{
+ sendsock = new QUdpSocket(this);
+ sendsock->bind(0);
+}
+
+/**
+ * @brief UdpSender::~UdpSender
+ */
+UdpSender::~UdpSender()
+{
+ delete sendsock;
+}
+
+/**
+ * @brief UdpSender::sendFileIfLatest
+ * @param session
+ * @param file
+ */
+void UdpSender::sendFileIfLatest(qint32 session, File file)
+{
+ mSendBufferFilesMutex.lock();
+ if (mSendBufferFiles.contains(session)) {
+ fDebug << "New file replaces old file in buffer.";
+ }
+ mSendBufferFiles.insert(session, file);
+ mSendBufferFilesMutex.unlock();
+}
+
+/**
+ * @brief UdpSender::sendFileUdp
+ * @param session Session ID
+ * @param host Target host IP number.
+ * @param port Target port.
+ * @param packetsize Packet size to use.
+ * @param file The file being sent.
+ *
+ * Sends a file to to session using [(file size) / (packet size)] number of UDP datagrams.
+ */
+void UdpSender::sendFileUdp(qint32 session, QString host, quint16 port, qint32 packetsize, File file)
+{
+ sendmutex.lock();
+ mSendBufferFilesMutex.lock();
+ if (mSendBufferFiles.contains(session)) {
+ file = mSendBufferFiles.take(session);
+ }
+ if (mLogTime && mUptime) {
+ mTimeLogsMutex.lock();
+ if (!mTimeLogs.contains(session)) {
+ mTimeLogs.insert(session, new QMap<quint32, qint64>());
+ }
+ auto timelogs = mTimeLogs.value(session, nullptr);
+ if (timelogs) {
+ timelogs->insert(file.id, mUptime->nsecsElapsed());
+ }
+ mTimeLogsMutex.unlock();
+ }
+
+ mSendBufferFilesMutex.unlock();
+ // Take the latest file in the pipeline. As this function
+ // fDebug << QString("Sending (session=%1, host=%2, port=%3, type=%4, length=%5)")
+ // .arg(session).arg(host).arg(port).arg(file.type)
+ // .arg((file.data != nullptr) ? file.data->length() : 0);
+
+ qint32 offset = 0;
+ qint32 totalbytes = (file.data != nullptr) ? file.data->length() : 0;
+ QHostAddress hostAddress(host);
+ while (true) {
+ // There are still data to be sent. Create a new packet and send it.
+ qint32 currpacketsize = qMin(packetsize, (totalbytes - offset));
+ QByteArray datagram;
+ // Create a header.
+ QDataStream dstream(&datagram, QIODevice::OpenModeFlag::WriteOnly);
+ dstream.setByteOrder(QDataStream::BigEndian);
+ dstream << static_cast<qint16>(file.type);
+ dstream << static_cast<qint32>(file.id);
+ dstream << static_cast<qint32>(totalbytes);
+ dstream << static_cast<qint32>(offset);
+ dstream << static_cast<qint16>(currpacketsize);
+ if (file.data != nullptr) {
+ datagram.insert(16, file.data->data() + offset, currpacketsize);
+ }
+ offset += currpacketsize;
+ sendsock->writeDatagram(datagram, hostAddress, port);
+ if (offset >= totalbytes) {
+ break;
+ }
+ }
+ emit fileSent();
+ sendmutex.unlock();
+}
+
+}
diff --git a/source/udpsender.h b/source/udpsender.h
new file mode 100644
index 0000000000000000000000000000000000000000..ca0fe61b6ac13aed4b893729cc6ac81b54d5062a
--- /dev/null
+++ b/source/udpsender.h
@@ -0,0 +1,64 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef UDPSENDER_H
+#define UDPSENDER_H
+
+#include "global.h"
+#include "networkconnection.h"
+#include <QHostAddress>
+#include <QMutex>
+#include <QObject>
+
+class QUdpSocket;
+
+namespace MRLeo {
+
+class UdpConnection;
+
+/**
+ * @brief The UdpSender class
+ *
+ * To be used by UdpConnection to Send Data.
+ * Currently only supports having files sent with sendFileIfLatest, i.e. files sent
+ * to this instance might be dropped before they are sent to the network layer.
+ * The class is optimized to transmit real time data like video where latency is
+ * most important and the second-latest file has little value.
+ */
+class UdpSender : public NetworkConnection {
+ Q_OBJECT
+
+ // Class only to be used by UdpConnection.
+ friend class UdpConnection;
+
+public:
+ virtual ~UdpSender() override;
+
+public slots:
+ void sendFileIfLatest(qint32 session, File file) override;
+
+signals:
+ void fileSent();
+
+private:
+ explicit UdpSender();
+ quint16 getPort() override { return 513; }
+
+private slots:
+ void sendFileUdp(qint32 session, QString host, quint16 port, qint32 packetsize, File file);
+ void sendFile(qint32, File) override {
+ // Only sendFileIfLatest is suported at the moment.
+ }
+
+private:
+ QMutex sendmutex;
+ QUdpSocket *sendsock;
+};
+
+}
+
+#endif // UDPSENDER_H
diff --git a/source/videoreceiver.cpp b/source/videoreceiver.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..40f5d58800518c2238d13e53686887bb72b426a0
--- /dev/null
+++ b/source/videoreceiver.cpp
@@ -0,0 +1,523 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include <gst/app/app.h>
+#include "videoreceiver.h"
+#include <QDebug>
+#include <QObject>
+#include <QThread>
+#include <QDateTime>
+#include <QElapsedTimer>
+#include <QDataStream>
+#include <QNetworkInterface>
+#include <QDebug>
+#include <QFile>
+#include <vector>
+#include <stdio.h>
+#include <QTimer>
+#include <QMetaEnum>
+#include <opencv2/opencv.hpp>
+
+#define WRITE_LOG_TRIGGER_TIME 63
+
+namespace MRLeo {
+
+
+class GStreamerReceiverContext {
+public:
+ GStreamerReceiverContext(GStreamerReceiver *_parent) {
+ parent = _parent;
+ reachedEndOfStream = false;
+ running = false;
+ timer = nullptr;
+ frames_sent = 0;
+ }
+ // Share frame between main loop and gstreamer callback
+ cvMatPtr latestFrame;
+ bool reachedEndOfStream;
+ bool running;
+ QElapsedTimer *timer;
+ QMap<int, int> frames_processed_per_time_slice;
+ GMainLoop *mainLoop;
+ GStreamerReceiver *parent;
+ quint32 frames_sent;
+};
+
+/**
+ * @brief VideoReceiver::VideoReceiver
+ * @param session Session id
+ * @param senderHost The host name of the stream sender.
+ */
+VideoReceiver::VideoReceiver(qint32 session, QString senderHost)
+{
+ mSession = session;
+ mSenderHost = senderHost;
+ mPort = 0;
+ mFormat = UNDEFINED;
+ mGStreamerReceiver = new GStreamerReceiver(this);
+ mGStreamerReceiver->moveToThread(new QThread());
+ mGStreamerReceiver->thread()->start();
+
+ mWriteLogTimer = new QTimer(this);
+ connect(mWriteLogTimer, &QTimer::timeout,
+ [=]() {
+ if (getBenchmarkingMode()) {
+ mGStreamerReceiver->writeLog();
+ }
+ });
+ mWriteLogTimer->start(WRITE_LOG_TRIGGER_TIME * 1000);
+
+ QObject::connect(mGStreamerReceiver, &GStreamerReceiver::ready,
+ [=](VideoStreamer::Format format, quint16 port) {
+ mPort = port;
+ mFormat = format;
+ emit ready(mSession, format, port);
+ });
+ QObject::connect(this, &VideoReceiver::startReceiver,
+ mGStreamerReceiver, &GStreamerReceiver::start);
+}
+
+/**
+ * @brief VideoTransmitter::getProcessingTimes
+ * @return
+ */
+QMap<quint32, qint64> VideoReceiver::getProcessingTimes() const
+{
+ auto retMap = mFramesArrivedNsec;
+ retMap.detach();
+ return retMap;
+}
+
+/**
+ * @brief VideoReceiver::newMat
+ * @param image
+ * @param frameid
+ */
+void VideoReceiver::newMat(quint32 frameid, cvMatPtr image)
+{
+ if (mLogTime && mUptime) {
+ mFramesArrivedNsec.insert(frameid, mUptime->nsecsElapsed());
+ }
+ emit matReady(mSession, frameid, image);
+}
+
+/**
+ * @brief VideoReceiver::~VideoReceiver
+ */
+VideoReceiver::~VideoReceiver()
+{
+ if (mGStreamerReceiver) {
+ mGStreamerReceiver->stop();
+ }
+ mWriteLogTimer->stop();
+ delete mWriteLogTimer;
+ if (getBenchmarkingMode()) {
+ mGStreamerReceiver->writeLog();
+ }
+}
+
+/**
+ * @brief VideoReceiver::start
+ * @param format Stream format to start.
+ * Starts the receiver in its thread.
+ */
+void VideoReceiver::start(Format format, bool userJitterbuffer)
+{
+ if (mFormat != format) {
+ mFormat = format;
+ mGStreamerReceiver->stop();
+ emit startReceiver(format, mSenderHost, userJitterbuffer);
+ }
+}
+
+/**
+ * @brief new_preroll_callback
+ * Unused GStreamer callback function.
+ */
+GstFlowReturn new_preroll_callback(GstAppSink * /*appsink*/, gpointer /*ctx*/)
+{
+ fDebug << "new_preroll";
+ return GST_FLOW_OK;
+}
+
+/**
+ * @brief eos_stream_callback
+ * @return
+ */
+void eos_stream_callback(GstAppSink * /*appsink*/, gpointer /*ctx*/)
+{
+ fDebug << "eos_stream";
+}
+
+/**
+ * @brief frame_received_callback
+ * @param appsink Active GStreamer appsink
+ * @param data The GStreamer context.
+ *
+ * GStreamer has received a new frame (image). Copy it into the
+ * appropriate receiver's context so that it can be sent to the
+ * listeners.
+ */
+GstFlowReturn frame_received_callback(GstAppSink *appsink, gpointer data)
+{
+ auto ctx = reinterpret_cast<GStreamerReceiverContext *>(data);
+ if (!ctx) {
+ fDebug << "Error: frame_received_callback could not convert data.";
+ return GST_FLOW_ERROR;
+ }
+ // Get caps and frame
+ auto sample = gst_app_sink_pull_sample(appsink);
+ auto caps = gst_sample_get_caps(sample);
+ auto buffer = gst_sample_get_buffer(sample);
+ auto structure = gst_caps_get_structure(caps, 0);
+ int width = g_value_get_int(gst_structure_get_value(structure, "width"));
+ int height = g_value_get_int(gst_structure_get_value(structure, "height"));
+ // Get frame data
+ GstMapInfo map;
+ gst_buffer_map(buffer, &map, GST_MAP_READ);
+ // Convert gstreamer data to OpenCV Mat
+ auto currFrame = cvMatPtr(new cv::Mat(cv::Size(width, height), CV_8UC3));
+ memcpy(currFrame->data, map.data, width * height * 3);
+
+ gst_buffer_unmap(buffer, &map);
+ gst_sample_unref(sample);
+ ctx->frames_sent++;
+ if (ctx->timer == nullptr) {
+ ctx->timer = new QElapsedTimer;
+ ctx->timer->start();
+ }
+ auto time_slice = ctx->timer->elapsed() / 1000;
+ ctx->frames_processed_per_time_slice[time_slice] += 1;
+ ctx->parent->parent()->newMat(ctx->frames_sent, currFrame);
+ return GST_FLOW_OK;
+}
+
+/**
+ * @brief gst_bus_callbacks
+ * GStreamer callback
+ */
+static gboolean gst_bus_callbacks(GstBus*, GstMessage *message, gpointer data)
+{
+ fDebug << "gst_bus_callbacks";
+ auto ctx = reinterpret_cast<GStreamerReceiverContext *>(data);
+ if (!ctx) {
+ fDebug << "Error: gst_bus_callbacks could not convert data.";
+ return true;
+ }
+ switch (static_cast<GstMessage *>(message)->type) {
+ case GST_MESSAGE_ERROR: {
+ GError *err;
+ gchar *debug;
+ gst_message_parse_error(message, &err, &debug);
+ fDebug << "Error: " << err->message;
+ g_error_free(err);
+ g_free(debug);
+ break;
+ }
+ case GST_MESSAGE_WARNING: {
+ GError *err;
+ gchar *debug;
+ gst_message_parse_warning(message, &err, &debug);
+ fDebug << "Warning: " << err->message;
+ g_error_free(err);
+ g_free(debug);
+ break;
+ }
+ case GST_MESSAGE_INFO: {
+ GError *err;
+ gchar *debug;
+ gst_message_parse_info(message, &err, &debug);
+ fDebug << "Info: " << err->message;
+ g_error_free(err);
+ g_free(debug);
+ break;
+ }
+ case GST_MESSAGE_EOS:
+ ctx->reachedEndOfStream = true;
+ fDebug << "End of stream";
+ break;
+ case GST_MESSAGE_UNKNOWN:
+ fDebug << "GST_MESSAGE_UNKNOWN";
+ break;
+ case GST_MESSAGE_TAG:
+ fDebug << "GST_MESSAGE_TAG";
+ break;
+ case GST_MESSAGE_BUFFERING:
+ fDebug << "GST_MESSAGE_BUFFERING";
+ break;
+ case GST_MESSAGE_STATE_CHANGED:
+ fDebug << "GST_MESSAGE_STATE_CHANGED";
+ break;
+ case GST_MESSAGE_STATE_DIRTY:
+ fDebug << "GST_MESSAGE_STATE_DIRTY";
+ break;
+ case GST_MESSAGE_STEP_DONE:
+ fDebug << "GST_MESSAGE_STEP_DONE";
+ break;
+ case GST_MESSAGE_CLOCK_PROVIDE:
+ fDebug << "GST_MESSAGE_CLOCK_PROVIDE";
+ break;
+ case GST_MESSAGE_CLOCK_LOST:
+ fDebug << "GST_MESSAGE_CLOCK_LOST";
+ break;
+ case GST_MESSAGE_NEW_CLOCK:
+ fDebug << "GST_MESSAGE_NEW_CLOCK";
+ break;
+ case GST_MESSAGE_STRUCTURE_CHANGE:
+ fDebug << "GST_MESSAGE_STRUCTURE_CHANGE";
+ break;
+ case GST_MESSAGE_STREAM_STATUS:
+ fDebug << "GST_MESSAGE_STREAM_STATUS";
+ break;
+ case GST_MESSAGE_APPLICATION:
+ fDebug << "GST_MESSAGE_APPLICATION";
+ break;
+ case GST_MESSAGE_ELEMENT:
+ fDebug << "GST_MESSAGE_ELEMENT";
+ break;
+ case GST_MESSAGE_SEGMENT_START:
+ fDebug << "GST_MESSAGE_SEGMENT_START";
+ break;
+ case GST_MESSAGE_SEGMENT_DONE:
+ fDebug << "GST_MESSAGE_SEGMENT_DONE";
+ break;
+ case GST_MESSAGE_DURATION_CHANGED:
+ fDebug << "GST_MESSAGE_DURATION_CHANGED";
+ break;
+ case GST_MESSAGE_LATENCY:
+ fDebug << "GST_MESSAGE_LATENCY";
+ break;
+ case GST_MESSAGE_ASYNC_START:
+ fDebug << "GST_MESSAGE_ASYNC_START";
+ break;
+ case GST_MESSAGE_ASYNC_DONE:
+ fDebug << "GST_MESSAGE_ASYNC_DONE";
+ break;
+ case GST_MESSAGE_REQUEST_STATE:
+ fDebug << "GST_MESSAGE_REQUEST_STATE";
+ break;
+ case GST_MESSAGE_STEP_START:
+ fDebug << "GST_MESSAGE_STEP_START";
+ break;
+ case GST_MESSAGE_QOS:
+ fDebug << "GST_MESSAGE_QOS";
+ break;
+ case GST_MESSAGE_PROGRESS:
+ fDebug << "GST_MESSAGE_PROGRESS";
+ break;
+ case GST_MESSAGE_TOC:
+ fDebug << "GST_MESSAGE_TOC";
+ break;
+ case GST_MESSAGE_RESET_TIME:
+ fDebug << "GST_MESSAGE_RESET_TIME";
+ break;
+ case GST_MESSAGE_STREAM_START:
+ fDebug << "GST_MESSAGE_STREAM_START";
+ break;
+ case GST_MESSAGE_NEED_CONTEXT:
+ fDebug << "GST_MESSAGE_NEED_CONTEXT";
+ break;
+ case GST_MESSAGE_HAVE_CONTEXT:
+ fDebug << "GST_MESSAGE_HAVE_CONTEXT";
+ break;
+ case GST_MESSAGE_REDIRECT:
+ fDebug << "GST_MESSAGE_REDIRECT";
+ break;
+ case GST_MESSAGE_ANY:
+ fDebug << "GST_MESSAGE_ANY";
+ break;
+ default:
+ fDebug << "Message type unknown";
+ break;
+ }
+ return true;
+}
+
+/**
+ * @brief GStreamerReceiver::GStreamerReceiver
+ */
+GStreamerReceiver::GStreamerReceiver(VideoReceiver *parent)
+{
+ mParent = parent;
+ mCtx = new GStreamerReceiverContext(this);
+ mLogWritten = false;
+}
+
+/**
+ * @brief GStreamerReceiver::~GStreamerReceiver
+ */
+GStreamerReceiver::~GStreamerReceiver()
+{
+ delete mCtx;
+}
+
+/**
+ * @brief GStreamerReceiver::writeLog
+ */
+void GStreamerReceiver::writeLog()
+{
+ if (mLogWritten) {
+ return;
+ }
+ mLogWritten = true;
+ QString formatString;
+ auto format = parent()->getFormat();
+ if (format == VideoStreamer::Format::H264_UDP) {
+ formatString = "H264_UDP";
+ } else if (format == VideoStreamer::Format::H264_TCP) {
+ formatString = "H264_TCP";
+ }
+ QString filename = QString("receiver_fps_%1_%2.csv")
+ .arg(formatString).arg(QTime::currentTime().toString());
+ filename.replace(QString(":"), QString(""));
+
+ QFile file(filename);
+ if (file.open(QIODevice::WriteOnly | QIODevice::Append | QFile::Text)) {
+ file.seek(file.size());
+ QTextStream out(&file);
+ for (int i = 0; i < WRITE_LOG_TRIGGER_TIME; i++) {
+ out << i;
+ out << ", ";
+ out << mCtx->frames_processed_per_time_slice.value(i, 0);
+ out << "\n";
+ }
+ file.close();
+ }
+}
+
+/**
+ * @brief GStreamerReceiver::start
+ * @param format Stream format to receive.
+ * @param host Host to receive camera stream from.
+ *
+ * Starts the stream. This function will not return until the receiver has stopped,
+ * so call it from another thread using a slot. When the receiver has started it will
+ * emit the signal ready() with the port number the client can connect to.
+ */
+void GStreamerReceiver::start(VideoStreamer::Format format, QString destHost, bool jitterbuffer)
+{
+ gst_init(nullptr, nullptr);
+ destHost.replace("::ffff:", "", Qt::CaseInsensitive);
+ mCtx->running = true;
+ mCtx->reachedEndOfStream = false;
+ QString pipelineCmd;
+
+ QString thisHost;
+
+ if (format == VideoStreamer::Format::H264_TCP) {
+ foreach (const auto &netInterface, QNetworkInterface::allInterfaces()) {
+ QNetworkInterface::InterfaceFlags flags = netInterface.flags();
+ if (thisHost.isEmpty() &&
+ (flags & QNetworkInterface::IsRunning) &&
+ !(flags & QNetworkInterface::IsLoopBack)) {
+ foreach (const auto &address, netInterface.addressEntries()) {
+ if (address.ip().protocol() == QAbstractSocket::IPv4Protocol) {
+ thisHost = address.ip().toString().toLatin1().data();
+ break;
+ }
+ }
+ }
+ }
+ }
+
+ switch (format) {
+ case VideoStreamer::Format::H264_UDP:
+ pipelineCmd = QString(
+ "udpsrc name=insrc port=0 ! "
+ "application/x-rtp,payload=96,encoding-name=H264 ! "
+ "%1 "
+ "rtph264depay ! "
+ "h264parse ! "
+ "avdec_h264 output-corrupt=false ! "
+ "videoconvert ! "
+ "video/x-raw,format=(string)RGB ! "
+ "videoconvert ! "
+ "appsink name=sink emit-signals=true "
+ "max-buffers=1 drop=true"
+ ).arg(jitterbuffer ? "rtpjitterbuffer !" : "");
+ break;
+ case VideoStreamer::Format::H264_TCP:
+ pipelineCmd = QString(
+ "tcpserversrc name=insrc port=0 host=%1 ! "
+ "tsdemux ! "
+ "h264parse ! "
+ "avdec_h264 output-corrupt=false ! "
+ "videoconvert ! "
+ "video/x-raw,format=(string)RGB ! "
+ "videoconvert ! "
+ "appsink name=sink emit-signals=true "
+ "max-buffers=1 drop=true"
+ ).arg(thisHost);
+ break;
+ default:
+ fDebug << "Format not set";
+ return;
+ }
+
+ fDebug << pipelineCmd.toLocal8Bit().constData();
+
+ GError *error = nullptr;
+ auto pipeline = gst_parse_launch(pipelineCmd.toLocal8Bit().constData(), &error);
+
+ if (error) {
+ g_print("could not construct pipeline: %s\n", error->message);
+ g_error_free(error);
+ return;
+ }
+
+ auto insrc = gst_bin_get_by_name(reinterpret_cast<GstBin *>(pipeline), "insrc");
+ auto sink = gst_bin_get_by_name(reinterpret_cast<GstBin *>(pipeline), "sink");
+ gst_app_sink_set_emit_signals(reinterpret_cast<GstAppSink *>(sink), true);
+ GstAppSinkCallbacks callbacks = {
+ eos_stream_callback, new_preroll_callback, frame_received_callback, {nullptr}};
+ gst_app_sink_set_callbacks(reinterpret_cast<GstAppSink *>(sink), &callbacks,
+ mCtx, nullptr);
+
+ // Declare bus
+ auto bus = gst_pipeline_get_bus(reinterpret_cast<GstPipeline *>(pipeline));
+ gst_bus_add_watch(bus, gst_bus_callbacks, mCtx);
+ gst_object_unref(bus);
+
+ gst_element_set_state(pipeline, GST_STATE_PAUSED);
+ gint port;
+ g_object_get(insrc, "port", &port, nullptr);
+ if (!port) {
+ g_object_get(insrc, "current-port", &port, nullptr);
+ }
+ gst_element_set_state(pipeline, GST_STATE_PLAYING);
+ emit ready(format, static_cast<quint16>(port));
+
+ fDebug << "Port: " << port;
+
+ mCtx->mainLoop = g_main_loop_new(nullptr, 0);
+ g_main_loop_run(mCtx->mainLoop);
+
+ fDebug << "Loop finished";
+
+ gst_element_set_state(pipeline, GST_STATE_NULL);
+ gst_object_unref(insrc);
+ gst_object_unref(sink);
+ gst_object_unref(pipeline);
+}
+
+/**
+ * @brief GStreamerReceiver::stop
+ */
+void GStreamerReceiver::stop()
+{
+ if (mCtx->running) {
+ fDebug << "Stopping GStreamerReceiver";
+ mCtx->running = false;
+ if (mCtx->mainLoop != nullptr) {
+ g_main_quit(mCtx->mainLoop);
+ }
+ mCtx->mainLoop = nullptr;
+ }
+}
+
+
+}
diff --git a/source/videoreceiver.h b/source/videoreceiver.h
new file mode 100644
index 0000000000000000000000000000000000000000..e17be16cbbb5fc268802bec38f6ba3f20a555552
--- /dev/null
+++ b/source/videoreceiver.h
@@ -0,0 +1,79 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef VIDEORECEIVER_H
+#define VIDEORECEIVER_H
+
+#include <QObject>
+#include <QWaitCondition>
+#include "global.h"
+#include "networkconnection.h"
+#include "videostreamer.h"
+
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class GStreamerReceiver;
+class GStreamerReceiverContext;
+
+class VideoReceiver : public VideoStreamer {
+ Q_OBJECT
+
+public:
+ explicit VideoReceiver(qint32 session, QString senderip);
+ ~VideoReceiver() override;
+ quint16 getPort() override { return mPort; }
+ Format getFormat() { return mFormat; }
+ QMap<quint32, qint64> getProcessingTimes() const;
+
+public slots:
+ void start(Format format, bool useJitterbuffer);
+ void newMat(quint32 id, cvMatPtr image);
+ void setLogTime(bool enable, QElapsedTimer* timer=nullptr) {
+ mLogTime = enable; mUptime=timer; }
+
+signals:
+ void startReceiver(Format format, QString senderip, bool useJitterbuffer);
+ void ready(qint32 session, VideoStreamer::Format format, quint16 port);
+
+private:
+ GStreamerReceiver *mGStreamerReceiver;
+ QString mSenderHost;
+ qint32 mSession;
+ Format mFormat;
+ quint16 mPort;
+ QTimer *mWriteLogTimer;
+ QElapsedTimer* mUptime;
+ bool mLogTime = false;
+ QMap<quint32, qint64> mFramesArrivedNsec;
+};
+
+class GStreamerReceiver : public QObject {
+ Q_OBJECT
+
+public:
+ explicit GStreamerReceiver(VideoReceiver *parent);
+ ~GStreamerReceiver();
+ void stop();
+public slots:
+ void start(VideoStreamer::Format format, QString host, bool useJitterbuffer);
+ void writeLog();
+signals:
+ void ready(VideoStreamer::Format format, quint16 port);
+
+public:
+ VideoReceiver *parent() { return mParent; }
+private:
+ GStreamerReceiverContext *mCtx;
+ VideoReceiver *mParent;
+ bool mLogWritten;
+};
+
+}
+
+#endif // VIDEORECEIVER_H
diff --git a/source/videostreamer.h b/source/videostreamer.h
new file mode 100644
index 0000000000000000000000000000000000000000..eaf06a7dd493f8469954c331f4fb87bd587cc0cc
--- /dev/null
+++ b/source/videostreamer.h
@@ -0,0 +1,26 @@
+#ifndef VIDEO_H
+#define VIDEO_H
+
+#include <QObject>
+#include "networkconnection.h"
+
+namespace MRLeo {
+
+class VideoStreamer : public NetworkConnection {
+public:
+ enum Format {
+ UNDEFINED,
+ H264_UDP,
+ H264_TCP
+ };
+ Q_ENUM(Format)
+ virtual ~VideoStreamer() {}
+ void setBenchmarkingMode(bool enabled) { mBenchmarking = enabled;}
+ bool getBenchmarkingMode() { return mBenchmarking; }
+private:
+ bool mBenchmarking = false;
+};
+
+}
+
+#endif // VIDEO_H
diff --git a/source/videotransmitter.cpp b/source/videotransmitter.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..76eb5fd3d894dcf0ae523cec203f13825b246cc1
--- /dev/null
+++ b/source/videotransmitter.cpp
@@ -0,0 +1,455 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#include "videotransmitter.h"
+#include <QDebug>
+#include <QThread>
+#include <gst/app/app.h>
+#include <gst/gstutils.h>
+#include <mutex>
+#include <QFile>
+#include <QMetaEnum>
+#include <QTimer>
+#include <QTime>
+
+#define WRITE_LOG_TRIGGER_TIME 63
+
+namespace MRLeo {
+
+class GStreamerTransmitterContext {
+public:
+ GStreamerTransmitterContext(GstAppSrc *_appsrc) {
+ initialized = false;
+ sourceid = 0;
+ appsrc = _appsrc;
+ mainloop = nullptr;
+ encoder = nullptr;
+ if (_appsrc != nullptr) {
+ mainloop = g_main_loop_new(nullptr, FALSE);
+ }
+ }
+ GstAppSrc *appsrc;
+ GMainLoop *mainloop;
+ GstElement* encoder;
+ uint sourceid;
+ quint16 destPort;
+ bool initialized;
+ char _a, _b, _c;
+ QSize videoSize;
+ QMap<int, int> frames_processed_per_time_slice;
+};
+
+
+
+
+/**
+ * @brief VideoTransmitter::VideoTransmitter
+ * @param session Session id.
+ * @param destHost Host to send the stream to.
+ */
+VideoTransmitter::VideoTransmitter(qint32 session, QString destHost)
+{
+ mTimer = nullptr;
+ mDestPort = 0;
+ mFormat = UNDEFINED;
+ mSession = session;
+ mDestHost = destHost;
+ mBitrate = 5000;
+ mStreamContext = new GStreamerTransmitterContext(nullptr);
+ mGStreamerTransmitter = new GStreamerTransmitter();
+ mGStreamerTransmitter->moveToThread(new QThread());
+ mGStreamerTransmitter->thread()->start();
+ mUptime = nullptr;
+ mLogWritten = false;
+
+ mWriteLogTimer = new QTimer(this);
+ connect(mWriteLogTimer, &QTimer::timeout,
+ [=]() {
+ if (getBenchmarkingMode()) {
+ writeLog();
+ }
+ });
+ mWriteLogTimer->start(WRITE_LOG_TRIGGER_TIME * 1000);
+
+ QObject::connect(this, &VideoTransmitter::startTransmitter,
+ mGStreamerTransmitter, &GStreamerTransmitter::start);
+ QObject::connect(mGStreamerTransmitter, &GStreamerTransmitter::started,
+ [=](GStreamerTransmitterContext *ctx) { mStreamContext = ctx; });
+ init();
+}
+
+/**
+ * @brief VideoTransmitter::~VideoTransmitter
+ */
+VideoTransmitter::~VideoTransmitter()
+{
+ mWriteLogTimer->stop();
+ delete mWriteLogTimer;
+ if (getBenchmarkingMode()) {
+ writeLog();
+ }
+ if (mStreamContext && mStreamContext->mainloop != nullptr && g_main_loop_is_running(mStreamContext->mainloop)) {
+ g_main_loop_quit(mStreamContext->mainloop);
+ }
+}
+
+/**
+ * @brief VideoTransmitter::writeLog
+ */
+void VideoTransmitter::writeLog()
+{
+ if (mLogWritten) {
+ return;
+ }
+ mLogWritten = true;
+ QString formatString;
+ if (mFormat == VideoStreamer::Format::H264_UDP) {
+ formatString = "H264_UDP";
+ } else if (mFormat == VideoStreamer::Format::H264_TCP) {
+ formatString = "H264_TCP";
+ }
+ QString filename = QString("transmitter_fps_%1_%2.csv")
+ .arg(formatString).arg(QTime::currentTime().toString());
+ filename.replace(QString(":"), QString(""));
+
+ QFile file(filename);
+ if (file.open(QIODevice::WriteOnly | QIODevice::Append | QFile::Text)) {
+ file.seek(file.size());
+ QTextStream out(&file);
+ for (int i = 0; i < WRITE_LOG_TRIGGER_TIME; i++) {
+ out << i;
+ out << ", ";
+ out << mStreamContext->frames_processed_per_time_slice.value(i, 0);
+ out << "\n";
+ }
+ file.close();
+ }
+}
+
+/**
+ * @brief VideoTransmitter::startStream
+ * @param format Stream format to use.
+ * @param destPort Port to use at the destination server.
+ */
+void VideoTransmitter::startStream(Format format, quint16 destPort)
+{
+ if (mDestPort != destPort || mFormat != format) {
+ mFormat = format;
+ mDestPort = destPort;
+ init();
+ }
+}
+
+/**
+ * @brief VideoTransmitter::setImageSize
+ * @param size Video image resolution width and height.
+ */
+void VideoTransmitter::setImageSize(QSize size)
+{
+ if (mImageSize != size) {
+ mImageSize = size;
+ init();
+ }
+}
+
+/**
+ * @brief VideoTransmitter::setBitrate
+ * @param bitrate Bitrate in for output stream in kbit/s.
+ */
+void VideoTransmitter::setBitrate(int bitrate)
+{
+ mBitrate = bitrate;
+ if (mStreamContext && mStreamContext->encoder) {
+ g_object_set(
+ reinterpret_cast<GObject *>(mStreamContext->encoder),
+ "bitrate", bitrate,
+ nullptr);
+ }
+}
+
+/**
+ * @brief VideoTransmitter::init
+ */
+void VideoTransmitter::init()
+{
+ if (mDestPort == 0 || mImageSize.isEmpty()) {
+ return;
+ }
+ if (mStreamContext != nullptr) {
+ if (mStreamContext->videoSize == mImageSize &&
+ mStreamContext->destPort == mDestPort) {
+ return;
+ }
+ mStreamContext->initialized = false;
+ if (mStreamContext->mainloop != nullptr) {
+ g_main_loop_quit(mStreamContext->mainloop);
+ }
+ }
+ emit startTransmitter(mDestHost, mDestPort, mFormat, mImageSize, mBitrate);
+}
+
+/**
+ * @brief VideoTransmitter::addQImageToProcessQueue
+ * @param session Session id.
+ * @param image New video frame to add to the streamer's buffer.
+ */
+void VideoTransmitter::addQImageToProcessQueue(qint32 session, quint32 frameid, QImage image)
+{
+ if (!mStreamContext || !mStreamContext->initialized || session != mSession) {
+ return;
+ }
+ Q_UNUSED(frameid);
+ mMutex.lock();
+ if (mValidImage) {
+ fDebug << "addQImageToProcessQueue: Buffer not empty. Replace image.";
+ }
+ mImage = image;
+ mValidImage = true;
+ mMutex.unlock();
+}
+
+/**
+ * @brief VideoTransmitter::processQImage
+ * @param session Session id.
+ * @param image New video frame to encode and transmit.
+ *
+ * Sends the image to GStreamer's inbox, to be encoded and transmitted using
+ * the configured streaming format.
+ */
+void VideoTransmitter::processQImage(qint32 session, quint32 frameid, QImage image)
+{
+ if (!mStreamContext || !mStreamContext->initialized || session != mSession) {
+ return;
+ }
+ Q_UNUSED(frameid);
+ bool isValid = false;
+ mMutex.lock();
+ image = mImage;
+ isValid = mValidImage;
+ mValidImage = false;
+ mMutex.unlock();
+ if (!isValid) {
+ fDebug << "Frame skipped. Inflow faster than they could be processed.";
+ return;
+ }
+ if (mStreamContext->videoSize != image.size()) {
+ fDebug << QString("Image is wrong size. Should be (%1, %2)")
+ .arg(mStreamContext->videoSize.width())
+ .arg(mStreamContext->videoSize.height());
+ return;
+ }
+ if (mTimer == nullptr) {
+ mTimer = new QElapsedTimer;
+ mTimer->start();
+ }
+ auto buffer = gst_buffer_new_and_alloc(static_cast<gsize>(image.byteCount()));
+ gst_buffer_fill(buffer, 0, image.constBits(), static_cast<gsize>(image.byteCount()));
+ gst_buffer_set_flags(buffer, GST_BUFFER_FLAG_LIVE);
+ buffer->pts = static_cast<GstClockTime>(mTimer->nsecsElapsed());
+ auto result = gst_app_src_push_buffer(mStreamContext->appsrc, buffer);
+
+ auto time_slice = mTimer->elapsed() / 1000;
+ mStreamContext->frames_processed_per_time_slice[time_slice] += 1;
+ if (mLogTime && mUptime) {
+ mFramesProcessedNsec.insert(frameid, mUptime->nsecsElapsed());
+ }
+ if (result != GST_FLOW_OK) {
+ fDebug << "Failure when pushing image" << frameid;
+ }
+}
+
+/**
+ * @brief VideoTransmitter::getProcessingTimes
+ * @return
+ */
+QMap<quint32, qint64> VideoTransmitter::getProcessingTimes() const
+{
+ auto retMap = mFramesProcessedNsec;
+ retMap.detach();
+ return retMap;
+}
+
+/**
+ * @brief gstreamer_error_callback
+ * @param msg
+ * @param data
+ */
+static void gstreamer_error_callback(GstBus *, GstMessage *msg, GStreamerTransmitterContext *data)
+{
+ Q_UNUSED(data);
+ GError *err;
+ gchar *debug_info;
+ gst_message_parse_error(msg, &err, &debug_info);
+ fDebug << QString("Error received from element: %1, %2")
+ .arg(msg->src->name, err->message);
+ fDebug << "Debugging information: " << (debug_info ? debug_info : "none");
+ g_clear_error(&err);
+ g_free(debug_info);
+}
+
+/**
+ * @brief gstreamer_qos_callback
+ * @param msg
+ * @param data
+ */
+static void gstreamer_qos_callback(GstBus *, GstMessage *msg, GStreamerTransmitterContext *data)
+{
+ Q_UNUSED(data);
+ GError *err;
+ gchar *debug_info;
+ gst_message_parse_error(msg, &err, &debug_info);
+ fDebug << QString("QOS received from element: %1, %2")
+ .arg(msg->src->name, err->message);
+ fDebug << "Debugging information: " << (debug_info ? debug_info : "none");
+ g_clear_error(&err);
+ g_free(debug_info);
+}
+
+/**
+ * @brief GStreamerTransmitter::start
+ * @param destHost Destination host.
+ * @param destPort Destination port.
+ * @param format Streamer format.
+ * @param size Video frame resolution.
+ */
+void GStreamerTransmitter::start(QString destHost, quint16 destPort,
+ VideoStreamer::Format format,
+ QSize size, int bitrate)
+{
+ gst_init(nullptr, nullptr);
+
+ fDebug << QString("start: destination host=%1, port=%2, size=(%3, %4), video=(%5, bitrate=%6)")
+ .arg(destHost).arg(destPort).arg(size.width()).arg(size.height())
+ .arg(format).arg(bitrate);
+
+ auto appsrc = gst_element_factory_make("appsrc", "appsrc");
+ auto videoconvert = gst_element_factory_make("videoconvert", "videoconvert");
+ auto x264enc = gst_element_factory_make("x264enc", "x264enc");
+ auto tcpclientsink = gst_element_factory_make("tcpclientsink", "tcpclientsink");
+ auto rtph264pay = gst_element_factory_make("rtph264pay", "rtph264pay");
+ auto udpsink = gst_element_factory_make("udpsink", "udpsink");
+ auto pipeline = gst_pipeline_new("pipeline");
+ auto mpegtsmux = gst_element_factory_make("mpegtsmux", "mpegtsmux");
+
+ g_object_set
+ (reinterpret_cast<GObject *>(appsrc),
+ "stream-type", 0,
+ "is-live", TRUE,
+ "do-timestamp", TRUE,
+ "format", GST_FORMAT_TIME,
+ "caps", gst_caps_new_simple(
+ "video/x-raw",
+ "format", G_TYPE_STRING, "RGB",
+ "width", G_TYPE_INT, size.width(),
+ "height", G_TYPE_INT, size.height(),
+ "framerate", GST_TYPE_FRACTION, 30, 1,
+ nullptr),
+ nullptr);
+
+ g_object_set(
+ reinterpret_cast<GObject *>(x264enc),
+ "speed-preset", 1, // ultrafast
+ "bitrate", bitrate, // Bitrate in kbit/sec. [1,2048000] Default: 2048
+ //"rc-lookahead", 1,
+ "tune", 4, // zerolatency
+ //"insert-vui", TRUE,
+ nullptr);
+
+ if (format == VideoStreamer::Format::H264_UDP) {
+ g_object_set(reinterpret_cast<GObject *>(x264enc),
+ "key-int-max", 1, nullptr);
+ }
+ g_object_set(
+ reinterpret_cast<GObject *>(rtph264pay),
+ "config-interval", 1,
+ "pt", 96,
+ nullptr);
+
+ destHost.replace("::ffff:", "", Qt::CaseInsensitive);
+
+ g_object_set(
+ reinterpret_cast<GObject *>(udpsink),
+ "host", destHost.toLocal8Bit().constData(),
+ "port", destPort,
+ "async", TRUE,
+ nullptr);
+
+ g_object_set(
+ reinterpret_cast<GObject *>(tcpclientsink),
+ "host", destHost.toLocal8Bit().constData(),
+ "port", destPort,
+ "async", TRUE,
+ nullptr);
+
+ gboolean result = 0;
+
+ switch (format) {
+ case VideoStreamer::Format::H264_UDP:
+ fDebug << "H264_UDP";
+ gst_bin_add_many(
+ reinterpret_cast<GstBin *>(pipeline),
+ appsrc, videoconvert, x264enc, rtph264pay, udpsink, nullptr);
+ result = gst_element_link_many(
+ appsrc,
+ videoconvert,
+ x264enc,
+ rtph264pay,
+ udpsink,
+ nullptr);
+ break;
+ case VideoStreamer::Format::H264_TCP:
+ fDebug << "H264_TCP";
+ gst_bin_add_many(
+ reinterpret_cast<GstBin *>(pipeline),
+ appsrc, videoconvert, x264enc, mpegtsmux, tcpclientsink, nullptr);
+ result = gst_element_link_many(
+ appsrc,
+ videoconvert,
+ x264enc,
+ mpegtsmux,
+ tcpclientsink,
+ nullptr);
+ break;
+ default:
+ fDebug << "Format not set";
+ return;
+ }
+
+ if (!result) {
+ fDebug << "Unable to initialize GST";
+ return;
+ }
+
+ auto ctx = new GStreamerTransmitterContext(
+ reinterpret_cast<GstAppSrc *>(appsrc));
+
+ ctx->destPort = destPort;
+ ctx->videoSize = size;
+
+ auto status = gst_element_set_state(pipeline, GST_STATE_PLAYING);
+ fDebug << "Status:" << status;
+ ctx->initialized = true;
+
+ auto bus = gst_pipeline_get_bus(reinterpret_cast<GstPipeline *>(pipeline));
+ gst_bus_add_signal_watch(bus);
+ g_signal_connect(bus, "message::error", G_CALLBACK(gstreamer_error_callback), ctx);
+ g_signal_connect(bus, "message::qos", G_CALLBACK(gstreamer_qos_callback), ctx);
+ gst_object_unref(bus);
+
+ emit started(ctx);
+
+ g_main_loop_run(ctx->mainloop);
+ fDebug << "finished transmitter";
+ gst_element_set_state(pipeline, GST_STATE_NULL);
+ if (ctx->mainloop != nullptr) {
+ g_free(ctx->mainloop);
+ }
+ ctx->mainloop = nullptr;
+ g_free(ctx);
+}
+
+}
diff --git a/source/videotransmitter.h b/source/videotransmitter.h
new file mode 100644
index 0000000000000000000000000000000000000000..652eccf9adb08d5db16182031b15dd02463a54f4
--- /dev/null
+++ b/source/videotransmitter.h
@@ -0,0 +1,92 @@
+/**
+ * Part of MR-Leo.
+ * Linköping University
+ *
+ * Author: Johan Lindqvist (johli392@student.liu.se)
+ */
+
+#ifndef VIDEOTRANSMITTER_H
+#define VIDEOTRANSMITTER_H
+
+#include <QElapsedTimer>
+#include <QObject>
+#include <QMutex>
+#include "global.h"
+#include "videostreamer.h"
+
+class QElapsedTimer;
+
+namespace MRLeo {
+
+class GStreamerTransmitter;
+class GStreamerTransmitterContext;
+
+/**
+ * @brief The VideoTransmitter class
+ */
+class VideoTransmitter : public VideoStreamer {
+ Q_OBJECT
+
+public:
+ explicit VideoTransmitter(qint32 session, QString desthost);
+ virtual ~VideoTransmitter() override;
+ void startStream(VideoStreamer::Format format, quint16 port);
+ void setImageSize(QSize size);
+ quint16 getPort() override { return mDestPort; }
+ void setBitrate(int bitrate);
+ QMap<quint32, qint64> getProcessingTimes() const;
+
+public slots:
+ void addQImageToProcessQueue(qint32 , quint32, QImage);
+ void processQImage(qint32, quint32, QImage);
+ void writeLog();
+ void setLogTime(bool enable, QElapsedTimer* timer=nullptr) {
+ mLogTime = enable; mUptime=timer; }
+
+signals:
+ void startTransmitter(QString, quint16, VideoStreamer::Format,
+ QSize size, int bitrate);
+
+private:
+ void init();
+ GStreamerTransmitter *mGStreamerTransmitter;
+ QElapsedTimer *mTimer;
+ GStreamerTransmitterContext *mStreamContext;
+ QString mDestHost;
+ qint32 mSession;
+ quint16 mDestPort;
+ QImage mImage;
+ bool mValidImage;
+ QMutex mMutex;
+ Format mFormat;
+ QSize mImageSize;
+ int mBitrate;
+ QTimer *mWriteLogTimer;
+ QElapsedTimer* mUptime;
+ bool mLogTime = false;
+ QMap<quint32, qint64> mFramesProcessedNsec;
+ bool mLogWritten;
+
+};
+
+class GStreamerTransmitter : public QObject {
+ Q_OBJECT
+
+public:
+ explicit GStreamerTransmitter() {}
+ ~GStreamerTransmitter() {}
+
+public slots:
+ void start(QString dest, quint16 port, VideoStreamer::Format format,
+ QSize size, int bitrate);
+
+signals:
+ void started(GStreamerTransmitterContext *);
+
+private:
+ QString mClientHost;
+};
+
+}
+
+#endif // VIDEOTRANSMITTER_H