tango: fixed api19 build with latest master

* Added Kinect for Azure menu option

* Reintegrated Kinect for Azure initialization code, fixed previous GUI commit

* Added ifdefs to fix build

* Added preliminary K4A device support

* Edits to K4A camera implementation

* K4A minor initialization fix

* Typo

* Another initialization fix

* Update CameraRGBD tool to display K4A camera output

* K4A added model parameter so we can create maps

* Added combo boxes for K4A options

* Functionalized combo boxes for K4A options

* Finished adding K4A preferences

.appveyor.yml

@@ -0,0 +1,137 @@
+
+branches:
+  only:
+    - master
+    - devel
+
+os: Visual Studio 2015
+
+clone_folder: c:\projects\rtabmap
+
+platform: x64
+configuration: Release
+
+init:
+  - cmake --version
+  - call "C:\Program Files\Microsoft SDKs\Windows\v7.1\Bin\SetEnv.cmd" /x64
+  - call "C:\Program Files (x86)\Microsoft Visual Studio 14.0\VC\vcvarsall.bat" x86_amd64
+
+install:
+  # Qt
+  - set QTDIR=C:\Qt\5.10.1\msvc2015_64
+  # make sure Qt bin path is before cmake bin path to avoid copying qt5 dlls from cmake before qt installation
+  - set PATH=%QTDIR%\bin;%PATH%
+  # Boost
+  - set PATH=%PATH%;C:\Libraries\boost_1_62_0\lib64-msvc-14.0
+  # Openni2
+  - ps: wget 'https://dl.dropboxusercontent.com/s/d98jv79l6oy9fxz/OpenNI2.exe?dl=0' -outfile OpenNI2.exe
+  - cmd: OpenNI2.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenNI2:"
+  - ps: "ls \"C:/Program Files/OpenNI2\""
+  - set PATH=%PATH%;C:\Program Files\OpenNI2\Redist
+  - set OPENNI2_INCLUDE64=C:\Program Files\OpenNI2\Include
+  - set OPENNI2_LIB64=C:\Program Files\OpenNI2\Lib
+  - set OPENNI2_REDIST64=C:\Program Files\OpenNI2\Redist
+  # OpenCV
+  #- ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/3.3.1/opencv-3.3.1-vc14.exe' -outfile opencv-3.3.1-vc14.exe
+  #- cmd: opencv-3.3.1-vc14.exe -o"C:\Program Files" -y
+  - ps: wget 'http://kent.dl.sourceforge.net/project/opencvlibrary/opencv-win/2.4.13/opencv-2.4.13.6-vc14.exe' -outfile opencv-2.4.13.6-vc14.exe
+  - cmd: opencv-2.4.13.6-vc14.exe -o"C:\Program Files" -y
+  - ECHO "Installed OpenCV:"
+  - ps: "ls \"C:/Program Files/opencv/build\""
+  - set PATH=%PATH%;C:\Program Files\opencv\build\x64\vc14\bin
+  # VTK (including QVTK)
+  - ps: wget 'https://dl.dropboxusercontent.com/s/1l33b5l3f3y52gf/VTK-6_3-msvc140.exe?dl=0' -outfile VTK-6_3.exe
+  - cmd: VTK-6_3.exe -o"C:\Program Files" -y
+  - ECHO "Installed VTK:"
+  - ps: "ls \"C:/Program Files/VTK\""
+  - set PATH=%PATH%;C:\Program Files\VTK\bin
+  # QHull 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/9widnk9msdsh2b8/Qhull-msvc140.exe?dl=0' -outfile Qhull.exe
+  - cmd: Qhull.exe -o"C:\Program Files" -y
+  - ECHO "Installed QHull:"
+  - ps: "ls \"C:/Program Files/Qhull\""
+  - set PATH=%PATH%;C:\Program Files\Qhull\bin
+  # FLANN 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/7k58jbmqa51sxmh/FLANN-msvc140.exe?dl=0' -outfile FLANN.exe
+  - cmd: FLANN.exe -o"C:\Program Files" -y
+  - ECHO "Installed FLANN:"
+  - ps: "ls \"C:/Program Files/FLANN\""
+  - set PATH=%PATH%;C:\Program Files\FLANN\bin
+  # Eigen 
+  - ps: wget 'https://dl.dropboxusercontent.com/s/3v6i9i8dxj4o8ji/Eigen.exe?dl=0' -outfile Eigen.exe
+  - cmd: Eigen.exe -o"C:\Program Files" -y
+  - ECHO "Installed Eigen:"
+  - ps: "ls \"C:/Program Files/Eigen\""
+  # PCL
+  - ps: wget 'https://dl.dropboxusercontent.com/s/2iayr4lyqa50i9j/PCL_181_August2018_x64_vc14.exe?dl=0' -outfile PCL_1.8.1.exe
+  - cmd: PCL_1.8.1.exe -o"C:\Program Files" -y
+  - ECHO "Installed PCL:"
+  - ps: "ls \"C:/Program Files/PCL\""
+  - set PATH=%PATH%;C:\Program Files\PCL\bin
+  # zlib
+  - ps: wget 'https://docs.google.com/uc?authuser=0&id=0B46akLGdg-uaYm9MTTI4MUtUcmc&export=download' -outfile zlib-1.2.8-vc2010-x64.zip
+  - ps: Expand-Archive zlib-1.2.8-vc2010-x64.zip -DestinationPath 'C:\Program Files'
+  - ECHO "Installed zlib:"
+  - ps: "ls \"C:/Program Files/zlib\""
+  - set PATH=%PATH%;C:\Program Files\zlib\bin
+  # g2o
+  - ps: wget 'https://dl.dropboxusercontent.com/s/ht74s5pa21wokzw/g2o.exe?dl=0' -outfile g2o.exe
+  - cmd: g2o.exe -o"C:\Program Files" -y
+  - ECHO "Installed g2o:"
+  - ps: "ls \"C:/Program Files/g2o\""
+  - set PATH=%PATH%;C:\Program Files\g2o\bin
+  # GTSAM
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0fpr6r4cgsqmvhf/GTSAM-4_0_0_alpha2-msvc140.exe?dl=0' -outfile GTSAM.exe
+  - cmd: GTSAM.exe -o"C:\Program Files" -y
+  - ECHO "Installed GTSAM:"
+  - ps: "ls \"C:/Program Files/GTSAM\""
+  - set PATH=%PATH%;C:\Program Files\GTSAM\bin
+  # OctoMap
+  - ps: wget 'https://dl.dropboxusercontent.com/s/6jpxu0nm8ne6e54/octomap_x64_vc14.exe?dl=0' -outfile octomap.exe
+  - cmd: octomap.exe -o"C:\Program Files" -y
+  - ECHO "Installed OctoMap:"
+  - ps: "ls \"C:/Program Files/octomap-distribution\""
+  - set PATH=%PATH%;C:\Program Files\octomap-distribution\bin
+  # CPU-TSDF
+  - ps: wget 'https://dl.dropboxusercontent.com/s/mgges9va1uzxr0q/cpu_tsdf_sept2015_x64_vc14.exe?dl=0' -outfile cpu_tsdf.exe
+  - cmd: cpu_tsdf.exe -o"C:\Program Files" -y
+  - ECHO "Installed CPU-TSDF:"
+  - ps: "ls \"C:/Program Files/cpu_tsdf\""
+  - set PATH=%PATH%;C:\Program Files\cpu_tsdf\bin
+  # Open Chisel
+  - ps: wget 'https://dl.dropboxusercontent.com/s/0aaphcde4acrinm/open_chisel_x64_vc14.exe?dl=0' -outfile open_chisel.exe
+  - cmd: open_chisel.exe -o"C:\Program Files" -y
+  - ECHO "Installed Open Chisel:"
+  - ps: "ls \"C:/Program Files/open_chisel\""
+  - set PATH=%PATH%;C:\Program Files\open_chisel\bin
+  # cvsba
+  - ps: wget 'https://dl.dropboxusercontent.com/s/4ey8ergerx46zvj/cvsba_x64_vc14.exe?dl=0' -outfile cvsba.exe
+  - cmd: cvsba.exe -o"C:\Program Files" -y
+  - ECHO "Installed cvsba:"
+  - ps: "ls \"C:/Program Files/cvsba\""
+  - set PATH=%PATH%;C:\Program Files\cvsba\bin
+  - ps: wget 'https://dl.dropboxusercontent.com/s/22qfvftwj6zq8tj/yaml-cpp_x64_vc14.exe?dl=0' -outfile yaml-cpp.exe
+  - cmd: yaml-cpp.exe -o"C:\Program Files" -y
+  - ECHO "Installed yaml-cpp:"
+  - ps: "ls \"C:/Program Files/yaml-cpp\""
+  
+before_build:
+  - cd c:\projects\rtabmap\build
+  - ECHO %PROGRAMFILES%
+  - ECHO %PATH%
+  - cmake -G "Visual Studio 14 2015 Win64" -DOpenCV_DIR="C:\Program Files\opencv\build" -DPCL_DIR="C:\Program Files\PCL\cmake" -DCPUTSDF_DIR="C:\Program Files\cpu_tsdf\share\cpu_tsdf" -Dcvsba_DIR="C:\Program Files\cvsba\lib\cmake" -Dyaml-cpp_DIR="C:\Program Files\yaml-cpp\CMake" -DBUILD_AS_BUNDLE=ON ..
+  
+after_build :
+  - cmake --build . --config Release --target package
+
+artifacts:
+  - path: build\RTABMap-*
+  
+notifications:
+  - provider: Email
+    to:
+      - matlabbe@gmail.com
+    on_build_success: false
+    on_build_failure: false
+    on_build_status_changed: true

.gitignore

@@ -1,6 +1,9 @@
 /lib
 .DS_Store
 .settings/language.settings.xml
+.idea/
+.vscode
+cmake-build-debug/
 app/android/.classpath
 app/android/.project
 app/android/AndroidManifest.xml

.travis.yml

@@ -1,30 +1,72 @@
 sudo: true
-dist: trusty
 language: cpp
+group: deprecated-2017Q3
 
 compiler:
   - gcc
-  - clang
 
-addons:
+matrix:
-  apt:
+  include:
-    packages:
-      - cmake
-      - libopencv-dev
-      - libqt4-dev
-      - libsqlite3-dev
 
-install:
+    - dist: trusty
-    - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
+      install:
-    - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty main" > /etc/apt/sources.list.d/ros-latest.list'
-    - sudo apt-get update
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
-    - sudo apt-get -y install libpcl-1.7-all libfreenect-dev ros-indigo-libg2o ros-indigo-octomap libopenni2-dev
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-indigo-rtabmap-ros
+        - sudo apt-get -y remove ros-indigo-rtabmap
 
-script: 
+      script: 
-    - source /opt/ros/indigo/setup.bash
+        - source /opt/ros/indigo/setup.bash
-    - mkdir -p build && cd build
+        - mkdir -p build && cd build
-    - cmake .. 
+        - cmake .. 
-    - make
+        - make
+
+    - dist: xenial
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu xenial main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-kinetic-rtabmap-ros
+        - sudo apt-get -y remove ros-kinetic-rtabmap
+
+      script: 
+        - source /opt/ros/kinetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+
+    - dist: bionic
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu bionic main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-melodic-rtabmap-ros
+        - sudo apt-get -y remove ros-melodic-rtabmap
+
+      script: 
+        - source /opt/ros/melodic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
+        
+    - dist: focal
+      install:
+        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu focal main" > /etc/apt/sources.list.d/ros-latest.list'
+        - wget http://packages.ros.org/ros.key -O - | sudo apt-key add -
+        - sudo apt-get update
+        - sudo apt-get update && sudo apt-get install dpkg
+        - sudo apt-get -y install ros-noetic-rtabmap-ros
+        - sudo apt-get -y remove ros-noetic-rtabmap
+
+      script: 
+        - source /opt/ros/noetic/setup.bash
+        - mkdir -p build && cd build
+        - cmake .. 
+        - make
 
 notifications:
   email:

README.md

@@ -1,8 +1,27 @@
-rtabmap [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap)
+rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
 =======
 
+[![RTAB-Map Logo](https://raw.githubusercontent.com/introlab/rtabmap/master/guilib/src/images/RTAB-Map100.png)](http://introlab.github.io/rtabmap)
+
+[![Release][release-image]][releases]
+[![License][license-image]][license]
+Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
+
+[release-image]: https://img.shields.io/badge/release-0.20.2-green.svg?style=flat
+[releases]: https://github.com/introlab/rtabmap/releases
+
+[license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat
+[license]: https://github.com/introlab/rtabmap/blob/master/LICENSE
+
 RTAB-Map library and standalone application.
 
 For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
 
 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
+
+### Acknowledgements
+This project is supported by [IntRoLab - Intelligent / Interactive / Integrated / Interdisciplinary Robot Lab](https://introlab.3it.usherbrooke.ca/), Sherbrooke, Québec, Canada.
+
+<a href="https://introlab.3it.usherbrooke.ca/">
+<img src="https://github.com/introlab/16SoundsUSB/blob/master/images/IntRoLab.png" alt="IntRoLab" height="100">
+</a>

RTABMapConfig.cmake.in

@@ -1,4 +1,8 @@
 # - Config file for the RTABMap package
+# Components:
+#  core    (required)
+#  gui     (optional)
+#  utilite (required)
 # It defines the following variables
 #  RTABMap_INCLUDE_DIRS - include directories for RTABMap
 #  RTABMap_LIBRARIES    - libraries to link against
@@ -42,8 +46,18 @@ ENDIF()
 
 set(RTABMap_LIBRARIES ${RTABMap_CORE} ${RTABMap_UTILITE})
 
+list(LENGTH RTABMap_FIND_COMPONENTS RTABMap_FIND_COMPONENTS_LENGTH)
+set(WITH_GUI ON)
+if(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+   list (FIND RTABMap_FIND_COMPONENTS "gui" _index)
+   if (${_index} EQUAL -1)
+      set(WITH_GUI OFF)
+   endif()
+endif(${RTABMap_FIND_COMPONENTS_LENGTH} GREATER 0)
+
+
 #gui lib (OFF if RTAB-Map is not built with Qt)
-if(@CONF_WITH_GUI@)
+if(@CONF_WITH_GUI@ AND ${WITH_GUI})
    find_library(RTABMap_GUI_RELEASE NAMES rtabmap_gui NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    find_library(RTABMap_GUI_DEBUG NAMES rtabmap_guid NO_DEFAULT_PATH HINTS @CONF_LIB_DIR@)
    
@@ -59,13 +73,15 @@ if(@CONF_WITH_GUI@)
    ENDIF()
    
    set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} ${RTABMap_GUI})
-endif(@CONF_WITH_GUI@)
+elseif(${WITH_GUI})
+   MESSAGE(WARNING "Asked for \"gui\" module but RTABMap hasn't been built with gui support.")
+endif()
 
 # Dependencies
-if(@CONF_VTK_QT@)
+if(@CONF_VTK_QT@ AND ${WITH_GUI})
    find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
-endif(@CONF_VTK_QT@)
+endif(@CONF_VTK_QT@ AND ${WITH_GUI})
-set(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} @CONF_DEPENDENCIES@)
+SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
 
 #backward compatibilities
 set(RTABMAP_CORE ${RTABMap_CORE})
@@ -74,3 +90,7 @@ if(RTABMap_GUI)
    set(RTABMAP_GUI ${RTABMap_GUI})
    set(RTABMAP_QT_VERSION @CONF_QT_VERSION@)
 endif(RTABMap_GUI)
+
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(RTABMap DEFAULT_MSG RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS)
+mark_as_advanced(RTABMap_LIBRARIES RTABMap_INCLUDE_DIRS RTABMap_LIBRARY_DIRS)

Version.h.in

@@ -40,20 +40,42 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @NONFREE@#define RTABMAP_NONFREE
 @TORO@#define RTABMAP_TORO
 @G2O@#define RTABMAP_G2O
+@G2O_CPP_CONF@#define RTABMAP_G2O_CPP11
 @GTSAM@#define RTABMAP_GTSAM
+@CERES@#define RTABMAP_CERES
 @VERTIGO@#define RTABMAP_VERTIGO
-@OPENCV3@#define RTABMAP_OPENCV3
 @OPENNI2@#define RTABMAP_OPENNI2
 @FREENECT@#define RTABMAP_FREENECT
 @FREENECT2@#define RTABMAP_FREENECT2
+@K4W2@#define RTABMAP_K4W2
+@K4A@#define RTABMAP_K4A
 @CVSBA@#define RTABMAP_CVSBA
+@POINTMATCHER@#define RTABMAP_POINTMATCHER
+@FASTCV@#define RTABMAP_FASTCV
+@LOAM@#define RTABMAP_LOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED
 @REALSENSE@#define RTABMAP_REALSENSE
 @REALSENSESLAM@#define RTABMAP_REALSENSE_SLAM
+@REALSENSE2@#define RTABMAP_REALSENSE2
+@MYNTEYE@#define RTABMAP_MYNTEYE
 @OCTOMAP@#define RTABMAP_OCTOMAP
 @CPUTSDF@#define RTABMAP_CPUTSDF
+@ALICE_VISION@#define RTABMAP_ALICE_VISION
+@OPENCHISEL@#define RTABMAP_OPENCHISEL
+@FOVIS@#define RTABMAP_FOVIS
+@VISO2@#define RTABMAP_VISO2
+@DVO@#define RTABMAP_DVO
+@OKVIS@#define RTABMAP_OKVIS
+@MSCKF_VIO@#define RTABMAP_MSCKF_VIO
+@VINS@#define RTABMAP_VINS
+@ORB_SLAM2@#define RTABMAP_ORB_SLAM2
+@ORB_OCTREE@#define RTABMAP_ORB_OCTREE
+@SUPERPOINT_TORCH@#define RTABMAP_SUPERPOINT_TORCH
+@PYMATCHER@#define RTABMAP_PYMATCHER
+@MADGWICK@#define RTABMAP_MADGWICK
+
 
 #endif /* VERSION_H_ */
 

app/android/.gitignore

@@ -1 +1,2 @@
 /bin/
+/gen/

app/android/AndroidManifest.xml.in

@@ -2,20 +2,21 @@
 <!-- BEGIN_INCLUDE(manifest) -->
 <manifest xmlns:android="http://schemas.android.com/apk/res/android"
         package="com.introlab.rtabmap"
-        android:versionCode="50"
+        android:versionCode="72"
         android:versionName="@RTABMAP_VERSION@">
 
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.READ_FRAME_BUFFER" />
-    <uses-permission android:name="android.permission.ACCESS_SURFACE_FLINGER" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
+    <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />
+    <uses-permission android:name="android.permission.ACCESS_WIFI_STATE" />
+    <uses-feature android:name="android.hardware.location.gps" />
     <uses-feature android:glEsVersion="0x00020000" />
 
-    <!-- This is the platform API where NativeActivity was introduced. -->
+    <!-- This is the platform API where depth16 support in android was introduced. -->
-    <uses-sdk android:minSdkVersion="19" />
+    <uses-sdk android:minSdkVersion="@ANDROID_NATIVE_API_LEVEL@" />
 
     <!-- This .apk has no Java code itself, so set hasCode to false. -->
     <application 
@@ -23,7 +24,9 @@
         android:icon="@drawable/ic_launcher"
         android:debuggable="@ANDROID_DEBUGGABLE@">
         
-        <uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
+        <uses-library android:name="com.projecttango.libtango_device2" android:required="false" />
+        <meta-data android:name="com.google.ar.core" android:value="optional" />
+        <meta-data android:name="com.huawei.ar.engine" android:value="optional" />
 
         <!-- Our activity is the built-in NativeActivity framework class.
              This will take care of integrating with our NDK code. -->
@@ -31,7 +34,8 @@
                 android:label="@string/app_name"
                 android:launchMode="singleTask"
 		android:screenOrientation="fullSensor"
-		android:configChanges="orientation|screenSize|keyboardHidden">
+		android:configChanges="orientation|screenSize|keyboardHidden"
+		android:theme="@style/ThemeApp">
             <!-- Tell NativeActivity the name of our .so -->
             <meta-data android:name="android.app.lib_name"
                     android:value="NativeRTABMap" />
@@ -43,6 +47,27 @@
 
         <activity android:name="SettingsActivity" android:label="@string/settings" android:screenOrientation="fullSensor"/>
         <activity android:name="SketchfabActivity" android:label="@string/sketchfab" android:screenOrientation="fullSensor"/>
+    
+        <meta-data
+            android:name="com.google.ar.core.min_apk_version"
+            android:value="191106000" /> <!-- This activity is critical for installing ARCore when it is not already present. -->
+        <activity
+            android:name="com.google.ar.core.InstallActivity"
+            android:configChanges="keyboardHidden|orientation|screenSize"
+            android:excludeFromRecents="true"
+            android:exported="false"
+            android:launchMode="singleTop"
+            android:theme="@style/ThemeApp" />
+            
+        <provider
+            android:name="android.support.v4.content.FileProvider"
+            android:authorities="com.introlab.rtabmap.provider"
+            android:exported="false"
+            android:grantUriPermissions="true">
+            <meta-data
+                android:name="android.support.FILE_PROVIDER_PATHS"
+                android:resource="@xml/provider_paths"/>
+        </provider>
     </application>
 
 </manifest>

app/android/CMakeLists.txt

@@ -1,10 +1,95 @@
 
+option(WITH_TANGO         "Include Tango support"                ON)
+option(WITH_ARCORE        "Include ARCore support"               ON)
+option(WITH_ARENGINE      "Include AREngine support"             ON)
 option(DISABLE_LOG        "Disable Android logging (should be true in release)"    ON)
+option(DEPTH_TEST         "Enable depth test on ARCore"    OFF)
+
+# Google Tango needs access to system shared 
+# libraries (e.g. libbinder.so) that are not accessible 
+# with android >=24
+IF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    FIND_PACKAGE(Tango QUIET)
+    IF(Tango_FOUND)
+       MESSAGE(STATUS "Found Tango: ${Tango_INCLUDE_DIRS}")
+    ENDIF(Tango_FOUND)
+ENDIF(WITH_TANGO AND ${ANDROID_NATIVE_API_LEVEL} LESS 24)
+
+IF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+    FIND_PACKAGE(ARCore QUIET)
+    IF(ARCore_FOUND)
+       MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+    ENDIF(ARCore_FOUND)
+ENDIF(WITH_ARCORE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 22)
+
+IF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    FIND_PACKAGE(AREngine QUIET)
+    IF(AREngine_FOUND)
+       MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+    ENDIF(AREngine_FOUND)
+ENDIF(WITH_ARENGINE AND ${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+
+IF(NOT Tango_FOUND)
+   SET(TANGO "//")
+ENDIF(NOT Tango_FOUND)
+IF(NOT ARCore_FOUND)
+   SET(ARCORE "//")
+ENDIF(NOT ARCore_FOUND)
+IF(NOT AREngine_FOUND)
+   SET(ARENGINE "//")
+ENDIF(NOT AREngine_FOUND)
+
+CONFIGURE_FILE(CameraAvailability.h.in ${CMAKE_CURRENT_SOURCE_DIR}/jni/CameraAvailability.h)
+
+
 IF(DISABLE_LOG)
   ADD_DEFINITIONS(-DDISABLE_LOG)
 ENDIF(DISABLE_LOG)
+IF(DEPTH_TEST)
+  ADD_DEFINITIONS(-DDEPTH_TEST)
+ENDIF(DEPTH_TEST)
+
+MESSAGE(STATUS "--------------------------------------------")
+MESSAGE(STATUS "Android build info:")
+MESSAGE(STATUS "  DISABLE_LOG    = ${DISABLE_LOG}")
+MESSAGE(STATUS "  DEPTH_TEST     = ${DEPTH_TEST}")
+IF(Tango_FOUND)
+  MESSAGE(STATUS "  With Tango     = YES")
+ELSEIF(NOT WITH_TANGO)
+  MESSAGE(STATUS "  With Tango     = NO (WITH_TANGO=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} GREATER 23)
+    MESSAGE(STATUS "  With Tango     = NO (ANDROID_NATIVE_API_LEVEL should be <= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With Tango     = NO (tango not found)")
+  ENDIF()
+ENDIF()
+IF(ARCore_FOUND)
+  MESSAGE(STATUS "  With ARCore    = YES")
+ELSEIF(NOT WITH_ARCORE)
+  MESSAGE(STATUS "  With ARCore    = NO (WITH_ARCORE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 23)
+    MESSAGE(STATUS "  With ARCore    = NO (ANDROID_NATIVE_API_LEVEL should be >= 23)")
+  ELSE()
+    MESSAGE(STATUS "  With ARCore    = NO (ARCore not found)")
+  ENDIF()
+ENDIF()
+IF(AREngine_FOUND)
+  MESSAGE(STATUS "  With AREngine  = YES")
+ELSEIF(NOT WITH_ARENGINE)
+  MESSAGE(STATUS "  With AREngine  = NO (WITH_ARENGINE=OFF)")
+ELSE()
+  IF(${ANDROID_NATIVE_API_LEVEL} LESS 24)
+    MESSAGE(STATUS "  With AREngine  = NO (ANDROID_NATIVE_API_LEVEL should be >= 24)")
+  ELSE()
+    MESSAGE(STATUS "  With AREngine  = NO (AREngine not found)")
+  ENDIF()
+ENDIF()
+MESSAGE(STATUS "  ANDROID_NATIVE_API_LEVEL       = ${ANDROID_NATIVE_API_LEVEL}")
+MESSAGE(STATUS "  ANDROID_COMPILER_FLAGS_RELEASE = ${ANDROID_COMPILER_FLAGS_RELEASE}")
+MESSAGE(STATUS "  ANDROID_TOOLCHAIN_PREFIX       = ${ANDROID_TOOLCHAIN_PREFIX}")
 
-MESSAGE(STATUS "DISABLE_LOG = ${DISABLE_LOG}")
 
 IF(DISABLE_LOG)
    SET(ANDROID_DEBUGGABLE false)
@@ -29,7 +114,8 @@ endif()
 
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml.in"
-  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml")
+  "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"
+  @ONLY)
   
 configure_file(
   "${CMAKE_CURRENT_SOURCE_DIR}/AndroidManifest.xml"

app/android/CameraAvailability.h.in

@@ -0,0 +1,39 @@
+/*
+Copyright (c) 2010-2014, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 UNIVERTY DE SHERBROOKE 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 CAMERAAVAILABILITY_H_
+#define CAMERAAVAILABILITY_H_
+
+// This is auto-generated!
+
+@TANGO@#define RTABMAP_TANGO
+@ARCORE@#define RTABMAP_ARCORE
+@ARENGINE@#define RTABMAP_ARENGINE
+
+
+#endif /* CAMERAAVAILABILITY_H_ */
+

app/android/custom_rules.xml

@@ -6,8 +6,8 @@
 		<copy todir="${native.libs.dir}/${android.abi}">
 			<fileset dir="${srcdir}/jni/third-party/lib" includes="*.so"/>
 		</copy>
-                <copy todir="${native.libs.dir}">
+		<copy todir="${native.libs.dir}">
-                        <fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
+			<fileset dir="${srcdir}/jni/third-party/lib" includes="${android.abi}/*.so"/>
-                </copy>
+		</copy>
 	</target>
 </project>

app/android/info.txt.in

@@ -1,7 +1,7 @@
 <h3>Real-Time Appearance-Based Mapping</h3>
 Version @RTABMAP_VERSION@<br>
 Author: Mathieu Labb&eacute;<br>
-Copyright 2016<br>
+Copyright 2016-2020<br>
 IntRoLab - Universit&eacute; de Sherbrooke<br>
 <b>http://introlab.github.io/rtabmap</b><br><br>
 

app/android/jni/.gitignore

@@ -0,0 +1 @@
+CameraAvailability.h

app/android/jni/CMakeLists.txt

@@ -1,6 +1,4 @@
 
-find_package(Tango REQUIRED)
-
 SET(INCLUDE_DIRS
     ${CMAKE_CURRENT_SOURCE_DIR}
     ${CMAKE_CURRENT_SOURCE_DIR}/tango-gl/include
@@ -9,27 +7,22 @@ SET(INCLUDE_DIRS
 	${PROJECT_SOURCE_DIR}/utilite/include
     ${OpenCV_INCLUDE_DIRS}
     ${PCL_INCLUDE_DIRS}
-    ${Tango_INCLUDE_DIRS}
     "${ANDROID_NDK}/platforms/android-${ANDROID_NATIVE_API_LEVEL}/arch-${ANDROID_ARCH_NAME}/usr/include"
 )
 
 SET(LIBRARIES
 	${OpenCV_LIBRARIES} 
 	${PCL_LIBRARIES}
-	${Tango_LIBRARIES}
 )
 
-add_definitions(${PCL_DEFINITIONS})
-
-INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
-
 set(sources
   jni_interface.cpp
-  CameraTango.cpp
+  CameraMobile.cpp
   RTABMapApp.cpp
   scene.cpp
   point_cloud_drawable.cpp
   graph_drawable.cpp
+  background_renderer.cc
   tango-gl/axis.cpp 
   tango-gl/camera.cpp 
   tango-gl/conversions.cpp 
@@ -44,6 +37,82 @@ set(sources
   tango-gl/util.cpp
 )
 
+IF(OPENMP_FOUND)
+file(COPY ${OpenMP_CXX_LIBRARIES}
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+ENDIF(OPENMP_FOUND)
+
+IF(Tango_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraTango.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${Tango_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${Tango_LIBRARIES}
+  )
+  
+  file(COPY ${Tango_support_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}) 
+ENDIF(Tango_FOUND)
+
+IF(ARCore_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraARCore.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${ARCore_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${ARCore_LIBRARIES}
+  )
+ 
+  file(COPY ${ARCore_c_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${ARCore_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(ARCore_FOUND)
+
+IF(AREngine_FOUND)
+
+  SET(sources
+    ${sources}
+    CameraAREngine.cpp
+  )
+  SET(INCLUDE_DIRS
+    ${INCLUDE_DIRS}
+    ${AREngine_INCLUDE_DIRS}
+  )
+  SET(LIBRARIES
+    ${LIBRARIES}
+    ${AREngine_LIBRARIES}
+    camera2ndk
+    mediandk
+  )
+ 
+  file(COPY ${AREngine_impl_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_jni_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+  file(COPY ${AREngine_ndk_LIBRARY} 
+       DESTINATION ${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME})
+    
+ENDIF(AREngine_FOUND)
+
+add_definitions(${PCL_DEFINITIONS})
+
+INCLUDE_DIRECTORIES(${INCLUDE_DIRS})
+
 add_library(NativeRTABMap SHARED ${sources})        
 target_link_libraries(NativeRTABMap ${LIBRARIES}
   android
@@ -58,3 +127,10 @@ set_target_properties(NativeRTABMap PROPERTIES
          LIBRARY_OUTPUT_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
          LIBRARY_OUTPUT_DIRECTORY_DEBUG "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}"
          LIBRARY_OUTPUT_DIRECTORY_RELEASE "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}")
+         
+IF(ANDROID_NATIVE_API_LEVEL GREATER 22)
+add_custom_command(TARGET NativeRTABMap POST_BUILD
+    COMMAND "${ANDROID_TOOLCHAIN_PREFIX}strip" -g -S -d --strip-debug --verbose
+    "${CMAKE_CURRENT_BINARY_DIR}/../libs/${ANDROID_NDK_ABI_NAME}/libNativeRTABMap.so"
+    COMMENT "Strip debug symbols done on final binary.")
+ENDIF(ANDROID_NATIVE_API_LEVEL GREATER 22)

app/android/jni/CameraARCore.cpp

@@ -0,0 +1,688 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 "CameraARCore.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+//////////////////////////////
+// CameraARCore
+//////////////////////////////
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false),
+	textureId_(9999),
+	uvs_initialized_(false),
+	updateOcclusionImage_(false),
+	depthFromMotion_(depthFromMotion)
+{
+}
+
+CameraARCore::~CameraARCore() {
+	// Disconnect ARCore service
+	close();
+
+	if(textureId_ != 9999)
+	{
+		glDeleteTextures(1, &textureId_);
+		textureId_ = 9999;
+	}
+}
+
+
+struct CameraConfig {
+  int32_t width = 0;
+  int32_t height = 0;
+  std::string config_label;
+  ArCameraConfig* config = nullptr;
+};
+
+void getCameraConfigLowestAndHighestResolutions(
+		std::vector<CameraConfig> & camera_configs,
+    CameraConfig** lowest_resolution_config,
+    CameraConfig** highest_resolution_config) {
+  if (camera_configs.empty()) {
+    return;
+  }
+
+  int low_resolution_config_idx = 0;
+  int high_resolution_config_idx = 0;
+  int32_t smallest_height = camera_configs[0].height;
+  int32_t largest_height = camera_configs[0].height;
+
+  for (int i = 1; i < camera_configs.size(); ++i) {
+    int32_t image_height = camera_configs[i].height;
+    if (image_height < smallest_height) {
+      smallest_height = image_height;
+      low_resolution_config_idx = i;
+    } else if (image_height > largest_height) {
+      largest_height = image_height;
+      high_resolution_config_idx = i;
+    }
+  }
+
+  if (low_resolution_config_idx == high_resolution_config_idx) {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+  } else {
+    *lowest_resolution_config = &camera_configs[low_resolution_config_idx];
+    *highest_resolution_config = &camera_configs[high_resolution_config_idx];
+  }
+}
+
+void copyCameraConfig(
+    const ArSession* ar_session, const ArCameraConfigList* all_configs,
+    int index, int num_configs, CameraConfig* camera_config) {
+  if (camera_config != nullptr && index >= 0 && index < num_configs) {
+    ArCameraConfig_create(ar_session, &camera_config->config);
+    ArCameraConfigList_getItem(ar_session, all_configs, index,
+                               camera_config->config);
+    ArCameraConfig_getImageDimensions(ar_session, camera_config->config,
+                                      &camera_config->width,
+                                      &camera_config->height);
+    camera_config->config_label = "(" + std::to_string(camera_config->width) +
+                                  "x" + std::to_string(camera_config->height) +
+                                  ")";
+  }
+}
+
+void destroyCameraConfigs(std::vector<CameraConfig> & camera_configs) {
+  for (int i = 0; i < camera_configs.size(); ++i) {
+    if (camera_configs[i].config != nullptr) {
+      ArCameraConfig_destroy(camera_configs[i].config);
+    }
+  }
+}
+
+std::string CameraARCore::getSerial() const
+{
+	return "ARCore";
+}
+
+bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	UScopeMutex lock(arSessionMutex_);
+
+	ArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	ArCoreApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case AR_INSTALL_STATUS_INSTALLED:
+			break;
+		case AR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
+	UASSERT(arSession_);
+
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+	ArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
+	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
+
+	ArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	ArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_);
+	UASSERT(arCameraIntrinsics_);
+
+	ArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	ArCameraConfigList* all_camera_configs = nullptr;
+	int32_t num_configs = 0;
+	ArCameraConfigList_create(arSession_, &all_camera_configs);
+	// Create filter first to get both 30 and 60 fps.
+	ArCameraConfigFilter* camera_config_filter = nullptr;
+	ArCameraConfigFilter_create(arSession_, &camera_config_filter);
+	ArCameraConfigFilter_setTargetFps(arSession_, camera_config_filter, AR_CAMERA_CONFIG_TARGET_FPS_30 | AR_CAMERA_CONFIG_TARGET_FPS_60);
+	ArSession_getSupportedCameraConfigsWithFilter(arSession_, camera_config_filter, all_camera_configs);
+	ArCameraConfigList_getSize(arSession_, all_camera_configs, &num_configs);
+
+	if (num_configs < 1) {
+		UERROR("No camera config found");
+		close();
+		return false;
+	}
+
+	std::vector<CameraConfig> camera_configs;
+	CameraConfig* cpu_low_resolution_camera_config_ptr = nullptr;
+	CameraConfig* cpu_high_resolution_camera_config_ptr = nullptr;
+	camera_configs.resize(num_configs);
+	for (int i = 0; i < num_configs; ++i) {
+		copyCameraConfig(arSession_, all_camera_configs, i, num_configs,
+						 &camera_configs[i]);
+	}
+	// Determine the highest and lowest CPU resolutions.
+	cpu_low_resolution_camera_config_ptr = nullptr;
+	cpu_high_resolution_camera_config_ptr = nullptr;
+	getCameraConfigLowestAndHighestResolutions(
+			camera_configs,
+			&cpu_low_resolution_camera_config_ptr,
+			&cpu_high_resolution_camera_config_ptr);
+
+	// Cleanup the list obtained as it is safe to destroy the list as camera
+	// config instances were explicitly created and copied. Refer to the
+	// previous comment.
+	ArCameraConfigList_destroy(all_camera_configs);
+	ArSession_setCameraConfig(arSession_, cpu_low_resolution_camera_config_ptr->config);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	ArConfig_setUpdateMode(arSession_, arConfig_, AR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraARCore::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if(arSession_!= nullptr)
+	{
+		ArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		ArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		ArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		ArCameraIntrinsics_destroy(arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		ArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+	occlusionImage_ = cv::Mat();
+}
+
+LaserScan CameraARCore::scanFromPointCloudData(
+		const float * pointCloudData,
+		int points,
+		const Transform & pose,
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		std::vector<cv::KeyPoint> * kpts,
+		std::vector<cv::Point3f> * kpts3D)
+{
+	if(pointCloudData && points>0)
+	{
+		cv::Mat scanData(1, points, CV_32FC4);
+		float * ptr = scanData.ptr<float>();
+		for(unsigned int i=0;i<points; ++i)
+		{
+			cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
+			pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+			ptr[i*4] = pt.x;
+			ptr[i*4 + 1] = pt.y;
+			ptr[i*4 + 2] = pt.z;
+
+			//get color from rgb image
+			cv::Point3f org= pt;
+			pt = util3d::transformPoint(pt, opticalRotationInv);
+			int u,v;
+			model.reproject(pt.x, pt.y, pt.z, u, v);
+			unsigned char r=255,g=255,b=255;
+			if(model.inFrame(u, v))
+			{
+				b=rgb.at<cv::Vec3b>(v,u).val[0];
+				g=rgb.at<cv::Vec3b>(v,u).val[1];
+				r=rgb.at<cv::Vec3b>(v,u).val[2];
+				if(kpts)
+					kpts->push_back(cv::KeyPoint(u,v,3));
+				if(kpts3D)
+					kpts3D->push_back(org);
+			}
+			*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+
+			//confidence
+			//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+		}
+		return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+	}
+	return LaserScan();
+}
+
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraARCore::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return data;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+
+		// Get calibration parameters
+		float fx,fy, cx, cy;
+		int32_t width, height;
+		ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
+#ifndef DISABLE_LOG
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#endif
+
+		if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
+		{
+			model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(width, height));
+
+			ArPointCloud * pointCloud = nullptr;
+			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
+
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+			ArImage * image = nullptr;
+			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			if(status == AR_SUCCESS)
+			{
+				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				{
+					LOGD("Acquire depth image!");
+					ArImage * depthImage = nullptr;
+					ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+					ArImageFormat format;
+					ArImage_getFormat(arSession_, depthImage, &format);
+					if(format == AR_IMAGE_FORMAT_DEPTH16)
+					{
+						LOGD("Depth format detected!");
+						int planeCount;
+						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+						LOGD("planeCount=%d", planeCount);
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						const uint8_t *data = nullptr;
+						int len = 0;
+						int stride;
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
+						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);
+
+						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+					}
+					ArImage_release(depthImage);
+				}
+
+				int64_t timestamp_ns;
+				ArImageFormat format;
+				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
+				ArImage_getFormat(arSession_, image, &format);
+				if(format == AR_IMAGE_FORMAT_YUV_420_888)
+				{
+#ifndef DISABLE_LOG
+					int32_t num_planes;
+					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
+					for(int i=0;i<num_planes; ++i)
+					{
+						int32_t pixel_stride;
+						int32_t row_stride;
+						ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
+						ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
+						LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
+					}
+#endif
+					const uint8_t * plane_data;
+					const uint8_t * plane_uv_data;
+					int32_t data_length;
+					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
+					int32_t uv_data_length;
+					ArImage_getPlaneData(arSession_, image, 2, &plane_uv_data, &uv_data_length);
+
+					if(plane_data != nullptr && data_length == height*width)
+					{
+						double stamp = double(timestamp_ns)/10e8;
+#ifndef DISABLE_LOG
+						LOGI("data_length=%d stamp=%f", data_length, stamp);
+#endif
+						cv::Mat rgb;
+						if((long)plane_uv_data-(long)plane_data != data_length)
+						{
+							// The uv-plane is not concatenated to y plane in memory, so concatenate them
+							cv::Mat yuv(height+height/2, width, CV_8UC1);
+							memcpy(yuv.data, plane_data, data_length);
+							memcpy(yuv.data+data_length, plane_uv_data, height/2*width);
+							cv::cvtColor(yuv, rgb, CV_YUV2BGR_NV21);
+						}
+						else
+						{
+							cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
+						}
+
+						std::vector<cv::KeyPoint> kpts;
+						std::vector<cv::Point3f> kpts3;
+						LaserScan scan;
+						if(pointCloud)
+						{
+							int32_t points = 0;
+							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
+							const float * pointCloudData = 0;
+							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+#ifndef DISABLE_LOG
+							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+#endif
+							if(pointCloudData && points>0)
+							{
+								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
+							}
+						}
+						else
+						{
+							LOGI("pointCloud empty");
+						}
+
+						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
+						data.setFeatures(kpts,  kpts3, cv::Mat());
+					}
+				}
+				else
+				{
+					LOGE("CameraARCore: cannot convert image format %d", format);
+				}
+			}
+			else
+			{
+				LOGE("CameraARCore: failed to get rgb image (status=%d)", (int)status);
+			}
+
+			ArImage_release(image);
+			ArPointCloud_release(pointCloud);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraARCore: Pose is null");
+	}
+	else
+	{
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraARCore::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	if(!arSession_)
+	{
+		return;
+	}
+
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
+	// Update session to get current frame and render camera background.
+	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
+		return;
+	}
+
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
+	ArCamera* ar_camera;
+	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	ArTrackingState camera_tracking_state;
+	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		ArCamera_getPose(arSession_, ar_camera, arPose_);
+		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported && updateOcclusionImage_)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+			}
+			ArImage_release(depthImage);
+		}
+	}
+
+	ArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraARCore.h

@@ -0,0 +1,115 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 CAMERAARCORE_H_
+#define CAMERAARCORE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
+
+#include <arcore_c_api.h>
+#include <camera/NdkCameraDevice.h>
+#include <camera/NdkCameraManager.h>
+#include <media/NdkImageReader.h>
+#include <android/native_window.h>
+
+namespace rtabmap {
+
+class CameraARCore : public CameraMobile {
+public:
+	static LaserScan scanFromPointCloudData(
+			const float * pointCloudData,
+			int points,
+			const Transform & pose,
+			const CameraModel & model,
+			const cv::Mat & rgb,
+			std::vector<cv::KeyPoint> * kpts = 0,
+			std::vector<cv::Point3f> * kpts3D = 0);
+
+public:
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
+	virtual ~CameraARCore();
+
+	bool uvsInitialized() const {return uvs_initialized_;}
+	const float* uvsTransformed() const {return transformed_uvs_;}
+	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
+
+	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
+	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}
+
+	void imageCallback(AImageReader *reader);
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	ArSession* arSession_ = nullptr;
+	ArConfig* arConfig_ = nullptr;
+	ArFrame* arFrame_ = nullptr;
+	ArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	ArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
+	bool uvs_initialized_ = false;
+	glm::mat4 viewMatrix_;
+	glm::mat4 projectionMatrix_;
+
+	bool updateOcclusionImage_;
+	cv::Mat occlusionImage_;
+	CameraModel occlusionModel_;
+	bool depthFromMotion_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARCORE_H_ */

app/android/jni/CameraAREngine.cpp

@@ -0,0 +1,351 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 "CameraAREngine.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+#include <media/NdkImage.h>
+
+namespace rtabmap {
+
+
+//////////////////////////////
+// CameraAREngine
+//////////////////////////////
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing):
+	CameraMobile(smoothing),
+	env_(env),
+	context_(context),
+	activity_(activity),
+	arInstallRequested_(false)
+{
+	glGenTextures(1, &textureId_);
+}
+
+CameraAREngine::~CameraAREngine() {
+	// Disconnect ARCore service
+	close();
+
+	glDeleteTextures(1, &textureId_);
+}
+
+std::string CameraAREngine::getSerial() const
+{
+	return "AREngine";
+}
+
+bool CameraAREngine::init(const std::string & calibrationFolder, const std::string & cameraName)
+{
+	close();
+
+	UScopeMutex lock(arSessionMutex_);
+
+	HwArInstallStatus install_status;
+	// If install was not yet requested, that means that we are resuming the
+	// activity first time because of explicit user interaction (such as
+	// launching the application)
+	bool user_requested_install = !arInstallRequested_;
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	HwArEnginesApk_requestInstall(env_, activity_, user_requested_install, &install_status);
+
+	switch (install_status)
+	{
+		case HWAR_INSTALL_STATUS_INSTALLED:
+			break;
+		case HWAR_INSTALL_STATUS_INSTALL_REQUESTED:
+			arInstallRequested_ = true;
+			return false;
+	}
+
+	// === ATTENTION!  ATTENTION!  ATTENTION! ===
+	// This method can and will fail in user-facing situations.  Your
+	// application must handle these cases at least somewhat gracefully.  See
+	// HelloAR Java sample code for reasonable behavior.
+	UASSERT(HwArSession_create(env_, context_, &arSession_) == HWAR_SUCCESS);
+	UASSERT(arSession_);
+
+	HwArConfig_create(arSession_, &arConfig_);
+	UASSERT(arConfig_);
+
+	HwArConfig_setFocusMode(arSession_, arConfig_, HWAR_FOCUS_MODE_FIXED);
+	UASSERT(HwArSession_configure(arSession_, arConfig_) == HWAR_SUCCESS);
+
+	HwArFrame_create(arSession_, &arFrame_);
+	UASSERT(arFrame_);
+
+	HwArCameraIntrinsics_create(arSession_, &arCameraIntrinsics_); // May fail?!
+	//UASSERT(arCameraIntrinsics_);
+
+	HwArPose_create(arSession_, nullptr, &arPose_);
+	UASSERT(arPose_);
+
+	/// Sets the behavior of @ref ArSession_update(). See
+	/// ::ArUpdateMode for available options.
+	HwArConfig_setUpdateMode(arSession_, arConfig_, HWAR_UPDATE_MODE_BLOCKING);
+
+	deviceTColorCamera_ = opticalRotation;
+
+	// Required as ArSession_update does some off-screen OpenGL stuff...
+	HwArSession_setCameraTextureName(arSession_, textureId_);
+
+	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
+	{
+		UERROR("Cannot resume camera!");
+		// In a rare case (such as another camera app launching) the camera may be
+		// given to a different app and so may not be available to this app. Handle
+		// this properly and recreate the session at the next iteration.
+		close();
+		return false;
+	}
+
+	return true;
+}
+
+void CameraAREngine::close()
+{
+	UScopeMutex lock(arSessionMutex_);
+	if (arCameraIntrinsics_ != nullptr)
+	{
+		HwArCameraIntrinsics_destroy(arSession_, arCameraIntrinsics_);
+	}
+	arCameraIntrinsics_ = nullptr;
+
+	if(arSession_!= nullptr)
+	{
+		HwArSession_destroy(arSession_);
+	}
+	arSession_ = nullptr;
+
+	if(arConfig_!= nullptr)
+	{
+		HwArConfig_destroy(arConfig_);
+	}
+	arConfig_ = nullptr;
+
+	if (arFrame_ != nullptr)
+	{
+		HwArFrame_destroy(arFrame_);
+	}
+	arFrame_ = nullptr;
+
+	if (arPose_ != nullptr)
+	{
+		HwArPose_destroy(arPose_);
+	}
+	arPose_ = nullptr;
+
+	CameraMobile::close();
+}
+
+SensorData CameraAREngine::captureImage(CameraInfo * info)
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return data;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraAREngine::captureImage() ArSession_update error");
+		return data;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+
+		// Get calibration parameters
+		// FIXME: Hard-coded as getting intrinsics with the api fails
+		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
+		int32_t camWidth=640, camHeight=480;
+		//HwArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+		//HwArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+		//HwArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+		//HwArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &camWidth, &camHeight);
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, camWidth, camHeight);
+
+		if(fx > 0 && fy > 0 && camWidth > 0 && camHeight > 0 && cx > 0 && cy > 0)
+		{
+			//ArPointCloud * point_cloud;
+			//ArFrame_acquirePointCloud(ar_session_, ar_frame_, &point_cloud);
+
+			HwArImage * image = nullptr;
+			HwArImage * depthImage = nullptr;
+			HwArStatus statusRgb = HwArFrame_acquireCameraImage(arSession_, arFrame_, &image);
+			HwArStatus statusDepth = HwArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+			if(statusRgb == HWAR_SUCCESS && statusDepth == HWAR_SUCCESS)
+			{
+				int64_t timestamp_ns;
+				HwArFrame_getTimestamp(arSession_, arFrame_, &timestamp_ns);
+
+				int planeCount;
+				uint8_t *imageData = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				const AImage* ndkImageRGB;
+				HwArImage_getNdkImage(image, &ndkImageRGB);
+
+				AImage_getNumberOfPlanes(ndkImageRGB, &planeCount);
+				AImage_getWidth(ndkImageRGB, &width);
+				AImage_getHeight(ndkImageRGB, &height);
+				AImage_getPlaneRowStride(ndkImageRGB, 0, &stride);
+				AImage_getPlaneData(ndkImageRGB, 0, &imageData, &len);
+				LOGI("RGB: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputRGB;
+				if(imageData != nullptr && len>0)
+				{
+					cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)imageData), outputRGB, CV_YUV2BGR_NV21);
+				}
+
+				//Depth
+				const AImage* ndkImageDepth;
+				HwArImage_getNdkImage(depthImage, &ndkImageDepth);
+				AImage_getNumberOfPlanes(ndkImageDepth, &planeCount);
+				AImage_getWidth(ndkImageDepth, &width);
+				AImage_getHeight(ndkImageDepth, &height);
+				AImage_getPlaneRowStride(ndkImageDepth, 0, &stride);
+				AImage_getPlaneData(ndkImageDepth, 0, &imageData, &len);
+				LOGI("Depth: width=%d, height=%d, bytes=%d stride=%d planeCount=%d", width, height, len, stride, planeCount);
+
+				cv::Mat outputDepth(height, width, CV_16UC1);
+				uint16_t *dataShort = (uint16_t *)imageData;
+				for (int y = 0; y < outputDepth.rows; ++y)
+				{
+					for (int x = 0; x < outputDepth.cols; ++x)
+					{
+						uint16_t depthSample = dataShort[y*outputDepth.cols + x];
+						uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
+						outputDepth.at<uint16_t>(y,x) = depthRange;
+					}
+				}
+
+				if(!outputRGB.empty() && !outputDepth.empty())
+				{
+					double stamp = double(timestamp_ns)/10e8;
+					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
+					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+				}
+			}
+			else
+			{
+				LOGE("CameraAREngine: failed to get rgb image (status=%d %d)", (int)statusRgb, (int)statusDepth);
+			}
+
+			HwArImage_release(image);
+			HwArImage_release(depthImage);
+		}
+		else
+		{
+			LOGE("Invalid intrinsics!");
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+
+	if(pose.isNull())
+	{
+		LOGE("CameraAREngine: Pose is null");
+	}
+	else
+	{
+		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+		this->poseReceived(pose);
+		info->odomPose = pose;
+	}
+	return data;
+
+}
+
+void CameraAREngine::capturePoseOnly()
+{
+	UScopeMutex lock(arSessionMutex_);
+	//LOGI("Capturing image...");
+
+	SensorData data;
+	if(!arSession_)
+	{
+		return;
+	}
+
+	// Update session to get current frame and render camera background.
+	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
+		LOGE("CameraARCore::captureImage() ArSession_update error");
+		return;
+	}
+
+	HwArCamera* ar_camera;
+	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
+
+	HwArTrackingState camera_tracking_state;
+	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
+
+	Transform pose;
+	CameraModel model;
+	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
+	{
+		// pose in OpenGL coordinates
+		float pose_raw[7];
+		HwArCamera_getPose(arSession_, ar_camera, arPose_);
+		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		if(!pose.isNull())
+		{
+			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+			this->poseReceived(pose);
+		}
+	}
+
+	HwArCamera_release(ar_camera);
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraAREngine.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 CAMERAARENGINE_H_
+#define CAMERAARENGINE_H_
+
+#include "CameraMobile.h"
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+
+#include <huawei_arengine_interface.h>
+
+namespace rtabmap {
+
+class CameraAREngine : public CameraMobile {
+public:
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
+	virtual ~CameraAREngine();
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // close Tango connection
+	virtual std::string getSerial() const;
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly();
+
+private:
+	rtabmap::Transform getPoseAtTimestamp(double timestamp);
+
+private:
+	void * env_;
+	void * context_;
+	void * activity_;
+	HwArSession* arSession_ = nullptr;
+	HwArConfig* arConfig_ = nullptr;
+	HwArFrame* arFrame_ = nullptr;
+	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
+	HwArPose * arPose_ = nullptr;
+	bool arInstallRequested_;
+	GLuint textureId_;
+	UMutex arSessionMutex_;
+
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERAARENGINE_H_ */

app/android/jni/CameraMobile.cpp

@@ -0,0 +1,341 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 "CameraMobile.h"
+#include "util.h"
+#include "rtabmap/utilite/ULogger.h"
+#include "rtabmap/core/util3d_transforms.h"
+#include "rtabmap/core/OdometryEvent.h"
+#include "rtabmap/core/util2d.h"
+
+namespace rtabmap {
+
+#define nullptr 0
+
+//////////////////////////////
+// CameraMobile
+//////////////////////////////
+const float CameraMobile::bilateralFilteringSigmaS = 2.0f;
+const float CameraMobile::bilateralFilteringSigmaR = 0.075f;
+
+const rtabmap::Transform CameraMobile::opticalRotation = Transform(
+		0.0f,  0.0f,  1.0f, 0.0f,
+	   -1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f);
+const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
+		0.0f, -1.0f,  0.0f, 0.0f,
+	    0.0f,  0.0f, -1.0f, 0.0f,
+		1.0f,  0.0f,  0.0f, 0.0f);
+
+CameraMobile::CameraMobile(bool smoothing) :
+		Camera(10),
+		deviceTColorCamera_(Transform::getIdentity()),
+		spinOncePreviousStamp_(0.0),
+		previousStamp_(0.0),
+		stampEpochOffset_(0.0),
+		smoothing_(smoothing),
+		colorCameraToDisplayRotation_(ROTATION_0),
+		originUpdate_(false)
+{
+}
+
+CameraMobile::~CameraMobile() {
+	// Disconnect camera service
+	close();
+}
+
+bool CameraMobile::init(const std::string &, const std::string &)
+{
+	deviceTColorCamera_ = opticalRotation;
+	return true;
+}
+
+void CameraMobile::close()
+{
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	originOffset_ = Transform();
+	originUpdate_ = false;
+	pose_ = Transform();
+	data_ = SensorData();
+}
+
+void CameraMobile::resetOrigin()
+{
+	originUpdate_ = true;
+}
+
+void CameraMobile::poseReceived(const Transform & pose)
+{
+	if(!pose.isNull())
+	{
+		// send pose of the camera (without optical rotation)
+		Transform p = pose*deviceTColorCamera_;
+		if(originUpdate_)
+		{
+			originOffset_ = p.translation().inverse();
+			originUpdate_ = false;
+		}
+
+		if(!originOffset_.isNull())
+		{
+			this->post(new PoseEvent(originOffset_*p));
+		}
+		else
+		{
+			this->post(new PoseEvent(p));
+		}
+	}
+}
+
+bool CameraMobile::isCalibrated() const
+{
+	return model_.isValidForProjection();
+}
+
+void CameraMobile::setGPS(const GPS & gps)
+{
+	lastKnownGPS_ = gps;
+}
+
+void CameraMobile::setData(const SensorData & data, const Transform & pose)
+{
+	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	data_ = data;
+	pose_ = pose;
+}
+
+void CameraMobile::addEnvSensor(int type, float value)
+{
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::spinOnce()
+{
+	if(!this->isRunning())
+	{
+		bool ignoreFrame = false;
+		float rate = 10.0f; // maximum 10 FPS for image data
+		double now = UTimer::now();
+		if(rate>0.0f)
+		{
+			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
+				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
+			{
+				ignoreFrame = true;
+			}
+		}
+
+		if(!ignoreFrame)
+		{
+			spinOnceFrameRateTimer_.start();
+			spinOncePreviousStamp_ = now;
+			mainLoop();
+		}
+		else
+		{
+			// just send pose
+			capturePoseOnly();
+		}
+	}
+}
+
+void CameraMobile::mainLoopBegin()
+{
+	double t = cameraStartedTime_.elapsed();
+	if(t < 5.0)
+	{
+		uSleep((5.0-t)*1000); // just to make sure that the camera is started
+	}
+}
+
+void CameraMobile::mainLoop()
+{
+	CameraInfo info;
+	SensorData data = this->captureImage(&info);
+
+	if(data.isValid() && !info.odomPose.isNull())
+	{
+		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		{
+			data.setGPS(lastKnownGPS_);
+		}
+		else if(lastKnownGPS_.stamp()>0.0)
+		{
+			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+		}
+
+		if(lastEnvSensors_.size())
+		{
+			data.setEnvSensors(lastEnvSensors_);
+			lastEnvSensors_.clear();
+		}
+
+		if(smoothing_ && !data.depthRaw().empty())
+		{
+			//UTimer t;
+			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			//LOGD("Bilateral filtering, time=%fs", t.ticks());
+		}
+
+		// Rotate image depending on the camera orientation
+		if(colorCameraToDisplayRotation_ == ROTATION_90)
+		{
+			UDEBUG("ROTATION_90");
+			cv::Mat rgb, depth;
+			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::transpose(rgb,rgbt);
+			rgb = rgbt;
+			cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
+			cv::flip(data.depthRaw(),depth,1);
+			cv::transpose(depth,deptht);
+			depth = deptht;
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_180)
+		{
+			UDEBUG("ROTATION_180");
+			cv::Mat rgb, depth;
+			cv::flip(data.imageRaw(),rgb,1);
+			cv::flip(rgb,rgb,0);
+			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(depth,depth,0);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageWidth(), model.imageHeight());
+			model = CameraModel(
+					model.fx(),
+					model.fy(),
+					model.cx()>0?model.imageWidth()-model.cx():0,
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+		else if(colorCameraToDisplayRotation_ == ROTATION_270)
+		{
+			UDEBUG("ROTATION_270");
+			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
+			cv::transpose(data.imageRaw(),rgb);
+			cv::flip(rgb,rgb,1);
+			cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
+			cv::transpose(data.depthOrRightRaw(),depth);
+			cv::flip(depth,depth,1);
+			CameraModel model = data.cameraModels()[0];
+			cv::Size sizet(model.imageHeight(), model.imageWidth());
+			model = CameraModel(
+					model.fy(),
+					model.fx(),
+					model.cy()>0?model.imageHeight()-model.cy():0,
+					model.cx(),
+					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
+			model.setImageSize(sizet);
+			data.setRGBDImage(rgb, depth, model);
+
+			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			for(size_t i=0; i<keypoints.size(); ++i)
+			{
+				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+			}
+			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+		}
+
+		rtabmap::Transform pose = info.odomPose;
+		data.setGroundTruth(Transform());
+
+		// convert stamp to epoch
+		bool firstFrame = previousPose_.isNull();
+		if(firstFrame)
+		{
+			stampEpochOffset_ = UTimer::now()-data.stamp();
+		}
+		data.setStamp(stampEpochOffset_ + data.stamp());
+		OdometryInfo info;
+		if(!firstFrame)
+		{
+			info.interval = data.stamp()-previousStamp_;
+			info.transform = previousPose_.inverse() * pose;
+		}
+		// linear cov = 0.0001
+		info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
+		if(!firstFrame)
+		{
+			// angular cov = 0.000001
+			info.reg.covariance.at<double>(3,3) *= 0.01;
+			info.reg.covariance.at<double>(4,4) *= 0.01;
+			info.reg.covariance.at<double>(5,5) *= 0.01;
+		}
+		LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
+		this->post(new OdometryEvent(data, pose, info));
+		previousPose_ = pose;
+		previousStamp_ = data.stamp();
+	}
+	else if(!this->isKilled())
+	{
+		LOGW("Odometry lost");
+		this->post(new OdometryEvent());
+	}
+}
+
+SensorData CameraMobile::captureImage(CameraInfo * info)
+{
+	if(info)
+	{
+		info->odomPose = pose_;
+	}
+	return data_;
+}
+
+} /* namespace rtabmap */

app/android/jni/CameraMobile.h

@@ -0,0 +1,134 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 CAMERAMOBILE_H_
+#define CAMERAMOBILE_H_
+
+#include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
+#include <rtabmap/utilite/UMutex.h>
+#include <rtabmap/utilite/USemaphore.h>
+#include <rtabmap/utilite/UEventsSender.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEvent.h>
+#include <rtabmap/utilite/UTimer.h>
+#include <boost/thread/mutex.hpp>
+#include "util.h"
+
+namespace rtabmap {
+
+class CameraInfoEvent: public UEvent
+{
+public:
+	CameraInfoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
+	virtual std::string getClassName() const {return "CameraInfoEvent";}
+	int type() const {return type_;}
+	const std::string & key() const {return key_;}
+	const std::string & value() const {return value_;}
+
+private:
+	int type_;
+	std::string key_;
+	std::string value_;
+
+};
+
+class PoseEvent: public UEvent
+{
+public:
+	PoseEvent(const Transform & pose) : pose_(pose) {}
+	virtual std::string getClassName() const {return "PoseEvent";}
+	const Transform & pose() const {return pose_;}
+
+private:
+	Transform pose_;
+};
+
+class CameraMobile : public Camera, public UThread, public UEventsSender {
+public:
+	static const float bilateralFilteringSigmaS;
+	static const float bilateralFilteringSigmaR;
+
+	static const rtabmap::Transform opticalRotation;
+	static const rtabmap::Transform opticalRotationInv;
+
+public:
+	CameraMobile(bool smoothing = false);
+	virtual ~CameraMobile();
+
+	// abstract functions
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	virtual void close(); // inherited classes should call its parent in their close().
+	virtual std::string getSerial() const {return "CameraMobile";}
+
+	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
+	void resetOrigin();
+	virtual bool isCalibrated() const;
+
+	void poseReceived(const Transform & pose); // in rtabmap frame
+
+	const CameraModel & getCameraModel() const {return model_;}
+	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
+	void setSmoothing(bool enabled) {smoothing_ = enabled;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	void setGPS(const GPS & gps);
+	void addEnvSensor(int type, float value);
+	void setData(const SensorData & data, const Transform & pose);
+
+	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
+
+protected:
+	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual void capturePoseOnly() {}
+
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+
+protected:
+	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
+	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
+	UTimer spinOnceFrameRateTimer_;
+	double spinOncePreviousStamp_;
+
+private:
+	Transform previousPose_;
+	double previousStamp_;
+	UTimer cameraStartedTime_;
+	double stampEpochOffset_;
+	bool smoothing_;
+	ScreenRotation colorCameraToDisplayRotation_;
+	GPS lastKnownGPS_;
+	EnvSensors lastEnvSensors_;
+	Transform originOffset_;
+	bool originUpdate_;
+
+	SensorData data_;
+	Transform pose_;
+};
+
+} /* namespace rtabmap */
+#endif /* CAMERATANGO_H_ */

app/android/jni/CameraTango.cpp

@@ -46,7 +46,7 @@ const int scanDownsampling = 1;
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(point_cloud->num_points>0)
+	if(app->isRunning() && point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,31 +55,34 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	cv::Mat tangoImage;
+	if(app->isRunning())
-	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
 	{
-		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+		cv::Mat tangoImage;
-	}
+		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+		{
-	{
+			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-	else if(color->format == 35)
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+		}
-	}
+		else if(color->format == 35)
-	else
+		{
-	{
+			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		LOGE("Not supported color format : %d.", color->format);
+		}
-	}
+		else
+		{
+			LOGE("Not supported color format : %d.", color->format);
+		}
 
-	if(!tangoImage.empty())
+		if(!tangoImage.empty())
-	{
+		{
-		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+		}
 	}
 }
 
@@ -88,7 +91,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 	if(pose->status_code == TANGO_POSE_VALID)
 	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-		app->poseReceived(app->tangoPoseToTransform(pose));
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
 	}
 }
 
@@ -101,22 +104,15 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event)
 //////////////////////////////
 // CameraTango
 //////////////////////////////
-const float CameraTango::bilateralFilteringSigmaS = 2.0f;
-const float CameraTango::bilateralFilteringSigmaR = 0.075f;
-
 CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing) :
-		Camera(0),
+		CameraMobile(smoothing),
 		tango_config_(0),
-		firstFrame_(true),
-		stampEpochOffset_(0.0),
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		smoothing_(smoothing),
 		cloudStamp_(0),
 		tangoColorType_(0),
-		tangoColorStamp_(0),
+		tangoColorStamp_(0)
-		colorCameraToDisplayRotation_(ROTATION_0)
 {
 	UASSERT(decimation >= 1);
 }
@@ -189,7 +185,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
-	TangoSupport_initializeLibrary();
+	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
 	// Connect to Tango
 	LOGI("NativeRTABMap: Setup tango config");
@@ -346,6 +342,7 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 			pose_data.orientation[1],
 			pose_data.orientation[2],
 			pose_data.orientation[3]);
+	deviceTColorCamera_ = rtabmap_world_T_opengl_world * deviceTColorCamera_;
 
 	// camera intrinsic
 	TangoCameraIntrinsics color_camera_intrinsics;
@@ -404,18 +401,14 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 	model_ = CameraModel(colorCamera_?"color":"fisheye",
 			cv::Size(color_camera_intrinsics.width, color_camera_intrinsics.height),
 			K, D, R, P,
-			tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_); // device to camera optical rotation in rtabmap frame
+			deviceTColorCamera_);
 
 	if(!colorCamera_)
 	{
 		initFisheyeRectificationMap(model_, fisheyeRectifyMapX_, fisheyeRectifyMapY_);
 	}
 
-	LOGI("deviceTColorCameraTango  =%s", deviceTColorCamera_.prettyPrint().c_str());
+	LOGI("deviceTColorCameraRtabmap  =%s", deviceTColorCamera_.prettyPrint().c_str());
-	LOGI("deviceTColorCameraRtabmap=%s", (tango_device_T_rtabmap_device.inverse()*deviceTColorCamera_).prettyPrint().c_str());
-
-	cameraStartedTime_.restart();
-
 	return true;
 }
 
@@ -425,11 +418,14 @@ void CameraTango::close()
 	{
 		TangoConfig_free(tango_config_);
 		tango_config_ = nullptr;
+		LOGI("TangoService_disconnect()");
 		TangoService_disconnect();
+		LOGI("TangoService_disconnect() done.");
 	}
-	firstFrame_ = true;
 	fisheyeRectifyMapX_ = cv::Mat();
 	fisheyeRectifyMapY_ = cv::Mat();
+
+	CameraMobile::close();
 }
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
@@ -484,27 +480,9 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	}
 }
 
-static rtabmap::Transform opticalRotation(
-								1.0f,  0.0f,  0.0f, 0.0f,
-							    0.0f, -1.0f,  0.0f, 0.0f,
-								0.0f,  0.0f, -1.0f, 0.0f);
-void CameraTango::poseReceived(const Transform & pose)
-{
-	if(!pose.isNull() && pose.getNormSquared() < 100000)
-	{
-		// send pose of the camera (without optical rotation), not the device
-		this->post(new PoseEvent(pose*deviceTColorCamera_*opticalRotation));
-	}
-}
-
 void CameraTango::tangoEventReceived(int type, const char * key, const char * value)
 {
-	this->post(new CameraTangoEvent(type, key, value));
+	this->post(new CameraInfoEvent(type, key, value));
-}
-
-bool CameraTango::isCalibrated() const
-{
-	return model_.isValidForProjection();
 }
 
 std::string CameraTango::getSerial() const
@@ -555,7 +533,7 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	else
 	{
 
-		pose = tangoPoseToTransform(&pose_start_service_T_device);
+		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
 	}
 
 	return pose;
@@ -571,7 +549,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		if(this->isRunning())
 		{
 			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
-			this->post(new CameraTangoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
 
 			boost::mutex::scoped_lock  lock(dataMutex_);
 			if(!cloud_.empty() && !tangoColor_.empty())
@@ -698,6 +676,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 			CameraModel depthModel = model_.scaled(1.0f/float(depthSizeDec));
 			std::vector<cv::Point3f> scanData(rawScanPublished_?cloud.total():0);
 			int oi=0;
+			int closePoints = 0;
+			float closeROI[4];
+			closeROI[0] = depth.cols/4;
+			closeROI[1] = 3*(depth.cols/4);
+			closeROI[2] = depth.rows/4;
+			closeROI[3] = 3*(depth.rows/4);
+			unsigned short minDepthValue=10000;
 			for(unsigned int i=0; i<cloud.total(); ++i)
 			{
 				float * p = cloud.ptr<float>(0,i);
@@ -716,6 +701,17 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				pixel_y_h = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy() + 0.5f);
 				unsigned short depth_value(pt.z * 1000.0f);
 
+				if(pixel_x_l>=closeROI[0] && pixel_x_l<closeROI[1] &&
+				   pixel_y_l>closeROI[2] && pixel_y_l<closeROI[3] &&
+				   depth_value < 600)
+				{
+					++closePoints;
+					if(depth_value < minDepthValue)
+					{
+						minDepthValue = depth_value;
+					}
+				}
+
 				bool pixelSet = false;
 				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
 				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
@@ -745,6 +741,11 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 				}
 			}
 
+			if(closePoints > 100)
+			{
+				this->post(new CameraInfoEvent(0, "TooClose", ""));
+			}
+
 			if(oi)
 			{
 				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
@@ -760,82 +761,32 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 		{
 			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
 
-			Transform poseDevice = getPoseAtTimestamp(rgbStamp);
+			Transform odom = getPoseAtTimestamp(rgbStamp);
 
 			//LOGD("Local    = %s", model.localTransform().prettyPrint().c_str());
 			//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 			//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
-			//Rotate in RTAB-Map's coordinate
+			// adjust origin
-			Transform odom = rtabmap_world_T_tango_world * poseDevice * tango_device_T_rtabmap_device;
+			if(!getOriginOffset().isNull())
+			{
+				odom = getOriginOffset() * odom;
+			}
 
 			//LOGD("rtabmap  = %s", odom.prettyPrint().c_str());
 			//LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str());
 
 			Transform scanLocalTransform = model.localTransform();
 
-			// Rotate image depending on the camera orientation
-			if(colorCameraToDisplayRotation_ == ROTATION_90)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::flip(rgb,rgb,1);
-				cv::transpose(rgb,rgbt);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::flip(depth,depth,1);
-				cv::transpose(depth,deptht);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy(), model.cx()>0?model.imageWidth()-model.cx():0, model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_180)
-			{
-				cv::flip(rgb,rgb,1);
-				cv::flip(rgb,rgb,0);
-				cv::flip(depth,depth,1);
-				cv::flip(depth,depth,0);
-				cv::Size sizet(model.imageWidth(), model.imageHeight());
-				model = CameraModel(
-						model.fx(),
-						model.fy(),
-						model.cx()>0?model.imageWidth()-model.cx():0,
-						model.cy()>0?model.imageHeight()-model.cy():0,
-						model.localTransform()*rtabmap::Transform(0,0,0,0,0,1.57079632679489661923132169163975144*2.0));
-				model.setImageSize(sizet);
-			}
-			else if(colorCameraToDisplayRotation_ == ROTATION_270)
-			{
-				cv::Mat rgbt(rgb.cols, rgb.rows, rgb.type());
-				cv::transpose(rgb,rgbt);
-				cv::flip(rgbt,rgbt,1);
-				rgb = rgbt;
-				cv::Mat deptht(depth.cols, depth.rows, depth.type());
-				cv::transpose(depth,deptht);
-				cv::flip(deptht,deptht,1);
-				depth = deptht;
-				cv::Size sizet(model.imageHeight(), model.imageWidth());
-				model = CameraModel(model.fy(), model.fx(), model.cy()>0?model.imageHeight()-model.cy():0, model.cx(), model.localTransform()*rtabmap::Transform(0,0,0,0,0,-1.57079632679489661923132169163975144));
-				model.setImageSize(sizet);
-			}
-
-			if(smoothing_)
-			{
-				//UTimer t;
-				depth = rtabmap::util2d::fastBilateralFiltering(depth, bilateralFilteringSigmaS, bilateralFilteringSigmaR);
-				data.setDepthOrRightRaw(depth);
-				//LOGD("Bilateral filtering, time=%fs", t.ticks());
-			}
-
 			if(rawScanPublished_)
 			{
-				data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+				data = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
 			else
 			{
 				data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 			}
-			data.setGroundTruth(odom);
+			info->odomPose = odom;
 		}
 		else
 		{
@@ -846,46 +797,4 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 
 }
 
-void CameraTango::mainLoopBegin()
-{
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
-}
-
-void CameraTango::mainLoop()
-{
-	if(tango_config_)
-	{
-		SensorData data = this->captureImage();
-
-		if(!data.groundTruth().isNull())
-		{
-			rtabmap::Transform pose = data.groundTruth();
-			data.setGroundTruth(Transform());
-			// convert stamp to epoch
-			if(firstFrame_)
-			{
-				stampEpochOffset_ = UTimer::now()-data.stamp();
-			}
-			data.setStamp(stampEpochOffset_ + data.stamp());
-			LOGI("Publish odometry message (variance=%f)", firstFrame_?9999:0.000001);
-			this->post(new OdometryEvent(data, pose, firstFrame_?9999:0.000001, firstFrame_?9999:0.000001));
-			firstFrame_ = false;
-		}
-		else if(!this->isKilled())
-		{
-			LOGW("Odometry lost");
-			this->post(new OdometryEvent());
-		}
-	}
-	else
-	{
-		UERROR("Camera not initialized, cannot start thread.");
-		this->kill();
-	}
-}
-
 } /* namespace rtabmap */

app/android/jni/CameraTango.h

@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CAMERATANGO_H_
 #define CAMERATANGO_H_
 
+#include "CameraMobile.h"
 #include <rtabmap/core/Camera.h>
+#include <rtabmap/core/GeodeticCoords.h>
 #include <rtabmap/utilite/UMutex.h>
 #include <rtabmap/utilite/USemaphore.h>
 #include <rtabmap/utilite/UEventsSender.h>
@@ -36,63 +38,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <tango_client_api.h>
 #include <tango_support_api.h>
 
-class TangoPoseData;
-
 namespace rtabmap {
 
-class PoseEvent: public UEvent
+class CameraTango : public CameraMobile {
-{
-public:
-	PoseEvent(const Transform & pose) : pose_(pose) {}
-	virtual std::string getClassName() const {return "PoseEvent";}
-	const Transform & pose() const {return pose_;}
-
-private:
-	Transform pose_;
-};
-
-class CameraTangoEvent: public UEvent
-{
-public:
-	CameraTangoEvent(int type, const std::string & key, const std::string & value) : type_(type), key_(key), value_(value) {}
-	virtual std::string getClassName() const {return "CameraTangoEvent";}
-	int type() const {return type_;}
-	const std::string & key() const {return key_;}
-	const std::string & value() const {return value_;}
-
-private:
-	int type_;
-	std::string key_;
-	std::string value_;
-
-};
-
-class CameraTango : public Camera, public UThread, public UEventsSender {
-public:
-	static const float bilateralFilteringSigmaS;
-	static const float bilateralFilteringSigmaR;
-
 public:
 	CameraTango(bool colorCamera, int decimation, bool publishRawScan, bool smoothing);
 	virtual ~CameraTango();
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void close(); // close Tango connection
+	virtual void close(); // close Tango connection
-	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
-	const CameraModel & getCameraModel() const {return model_;}
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
 	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
-	void setSmoothing(bool enabled) {smoothing_ = enabled;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}
-	void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 
 	void cloudReceived(const cv::Mat & cloud, double timestamp);
 	void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp);
-	void poseReceived(const Transform & pose);
 	void tangoEventReceived(int type, const char * key, const char * value);
 
 protected:
@@ -101,18 +66,11 @@ protected:
 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
 
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-
 private:
 	void * tango_config_;
-	bool firstFrame_;
-	UTimer cameraStartedTime_;
-	double stampEpochOffset_;
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	bool smoothing_;
 	cv::Mat cloud_;
 	double cloudStamp_;
 	cv::Mat tangoColor_;
@@ -120,9 +78,6 @@ private:
 	double tangoColorStamp_;
 	boost::mutex dataMutex_;
 	USemaphore dataReady_;
-	CameraModel model_;
-	Transform deviceTColorCamera_;
-	TangoSupportRotation colorCameraToDisplayRotation_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 };

app/android/jni/ProgressionStatus.h

@@ -70,7 +70,7 @@ public:
 			increment();
 		}
 
-		return !isCanceled();
+		return ProgressState::callback(msg);
 	}
 	virtual ~ProgressionStatus(){}
 

app/android/jni/RTABMapApp.h

@@ -31,11 +31,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <jni.h>
 #include <memory>
 
-#include <tango_client_api.h>  // NOLINT
 #include <tango-gl/util.h>
 
 #include "scene.h"
-#include "CameraTango.h"
+#include "CameraMobile.h"
 #include "util.h"
 #include "ProgressionStatus.h"
 
@@ -45,43 +44,20 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>
 
+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
  public:
   // Constructor and deconstructor.
-  RTABMapApp();
+  RTABMapApp(JNIEnv* env, jobject caller_activity);
   ~RTABMapApp();
 
-  void onCreate(JNIEnv* env, jobject caller_activity);
-
   void setScreenRotation(int displayRotation, int cameraRotation);
 
-  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize);
+  int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
 
-  bool onTangoServiceConnected(JNIEnv* env, jobject iBinder);
+  bool isBuiltWith(int cameraDriver) const;
-
+  bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
-  // Explicitly reset motion tracking and restart the pipeline.
-  // Note that this will cause motion tracking to re-initialize.
-  void TangoResetMotionTracking();
-
-  // Tango Service point cloud callback function for depth data. Called when new
-  // new point cloud data is available from the Tango Service.
-  //
-  // @param pose: The current point cloud returned by the service,
-  //              caller allocated.
-  void onPointCloudAvailable(const TangoXYZij* xyz_ij);
-
-  // Tango service pose callback function for pose data. Called when new
-  // information about device pose is available from the Tango Service.
-  //
-  // @param pose: The current pose returned by the service, caller allocated.
-  void onPoseAvailable(const TangoPoseData* pose);
-
-  // Tango service event callback function for event data. Called when new events
-  // are available from the Tango Service.
-  //
-  // @param event: Tango event, caller allocated.
-  void onTangoEventAvailable(const TangoEvent* event);
 
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
@@ -92,8 +68,7 @@ class RTABMapApp : public UEventsHandler {
   // Main render loop.
   int Render();
 
-  // Release all non-OpenGL allocated resources.
+  void stopCamera();
-  void onPause();
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
@@ -119,8 +94,12 @@ class RTABMapApp : public UEventsHandler {
   void setOdomCloudShown(bool shown);
   void setMeshRendering(bool enabled, bool withTexture);
   void setPointSize(float value);
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float value);
   void setLighting(bool enabled);
   void setBackfaceCulling(bool enabled);
+  void setWireframe(bool enabled);
   void setLocalizationMode(bool enabled);
   void setTrajectoryMode(bool enabled);
   void setGraphOptimization(bool enabled);
@@ -131,6 +110,7 @@ class RTABMapApp : public UEventsHandler {
   void setCameraColor(bool enabled);
   void setFullResolution(bool enabled);
   void setSmoothing(bool enabled);
+  void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
@@ -143,19 +123,17 @@ class RTABMapApp : public UEventsHandler {
   void setRenderingTextureDecimation(int value);
   void setBackgroundColor(float gray);
   int setMappingParameter(const std::string & key, const std::string & value);
+  void setGPS(const rtabmap::GPS & gps);
+  void addEnvSensor(int type, float value);
 
-  void resetMapping();
   void save(const std::string & databasePath);
-  cv::Mat mergeTextures(pcl::TextureMesh & mesh,
-		  int textureSize,
-		  const std::vector<std::map<int, pcl::PointXY> > & vertexToPixels) const;
   void cancelProcessing();
   bool exportMesh(
-		  const std::string & filePath,
 		  float cloudVoxelSize,
 		  bool regenerateCloud,
 		  bool meshing,
 		  int textureSize,
+		  int textureCount,
 		  int normalK,
 		  bool optimized,
 		  float optimizedVoxelSize,
@@ -163,28 +141,41 @@ class RTABMapApp : public UEventsHandler {
 		  int optimizedMaxPolygons,
 		  float optimizedColorRadius,
 		  bool optimizedCleanWhitePolygons,
-		  bool optimizedColorWhitePolygons,
+		  int optimizedMinClusterSize,
 		  float optimizedMaxTextureDistance,
 		  int optimizedMinTextureClusterSize,
 		  bool blockRendering);
   bool postExportation(bool visualize);
+  bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
+  void postCameraPoseEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw);
+
+  void postOdometryEvent(
+  		float x, float y, float z, float qx, float qy, float qz, float qw,
+  		float fx, float fy, float cx, float cy,
+  		double stamp,
+		void * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		float * points, int pointsLen);
+
  protected:
   virtual bool handleEvent(UEvent * event);
 
  private:
   rtabmap::ParametersMap getRtabmapParameters();
-  bool smoothMesh(int id, Mesh & mesh);
+  bool smoothMesh(int id, rtabmap::Mesh & mesh);
   void gainCompensation(bool full = false);
   std::vector<pcl::Vertices> filterOrganizedPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
   std::vector<pcl::Vertices> filterPolygons(const std::vector<pcl::Vertices> & polygons, int cloudSize) const;
 
  private:
-  rtabmap::CameraTango * camera_;
+  int cameraDriver_;
+  rtabmap::CameraMobile * camera_;
   rtabmap::RtabmapThread * rtabmapThread_;
   rtabmap::Rtabmap * rtabmap_;
-  LogHandler * logHandler_;
+  rtabmap::LogHandler * logHandler_;
 
   bool odomCloudShown_;
   bool graphOptimization_;
@@ -193,6 +184,7 @@ class RTABMapApp : public UEventsHandler {
   bool trajectoryMode_;
   bool rawScanSaved_;
   bool smoothing_;
+  bool depthFromMotion_;
   bool cameraColor_;
   bool fullResolution_;
   bool appendMode_;
@@ -208,7 +200,6 @@ class RTABMapApp : public UEventsHandler {
 
   rtabmap::ParametersMap mappingParameters_;
 
-  bool paused_;
   bool dataRecorderMode_;
   bool clearSceneOnNextRender_;
   bool openingDatabase_;
@@ -217,6 +208,7 @@ class RTABMapApp : public UEventsHandler {
   bool filterPolygonsOnNextRender_;
   int gainCompensationOnNextRender_;
   bool bilateralFilteringOnNextRender_;
+  bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
   int meshDecimation_;
   int totalPoints_;
@@ -224,14 +216,15 @@ class RTABMapApp : public UEventsHandler {
   int lastDrawnCloudsCount_;
   float renderingTime_;
   double lastPostRenderEventTime_;
-  long processMemoryUsedBytes;
+  double lastPoseEventTime_;
-  long processGPUMemoryUsedBytes;
   std::map<std::string, float> bufferedStatsData_;
 
   bool visualizingMesh_;
   bool exportedMeshUpdated_;
-  pcl::TextureMesh::Ptr exportedMesh_;
+  pcl::TextureMesh::Ptr optMesh_;
-  cv::Mat exportedTexture_;
+  cv::Mat optTexture_;
+  int optRefId_;
+  rtabmap::Transform * optRefPose_; // App crashes when loading native library if not dynamic
 
   // main_scene_ includes all drawable object for visualizing Tango device's
   // movement and point cloud.
@@ -243,13 +236,16 @@ class RTABMapApp : public UEventsHandler {
 
 	rtabmap::Transform mapToOdom_;
 
+	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
 	boost::mutex odomMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;
 
-	std::map<int, Mesh> createdMeshes_;
+	USemaphore screenshotReady_;
+
+	std::map<int, rtabmap::Mesh> createdMeshes_;
 	std::map<int, rtabmap::Transform> rawPoses_;
 
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;

app/android/jni/background_renderer.cc

@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShader =
+	"#extension GL_OES_EGL_image_external : require\n"
+
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+	"}\n";
+
+}  // namespace
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+{
+  texture_id_ = textureId;
+
+  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  glUseProgram(shader_program_);
+  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+  glUseProgram(0);
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  glUseProgram(shader_program_);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+
+  glEnableVertexAttribArray(attribute_vertices_);
+  glEnableVertexAttribArray(attribute_uvs_);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attribute_uvs_);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+

app/android/jni/background_renderer.h

@@ -0,0 +1,58 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#include <cstdlib>
+
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer() = default;
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs);
+
+ private:
+
+  GLuint shader_program_;
+  GLuint texture_id_;
+
+  GLuint attribute_vertices_;
+  GLuint attribute_uvs_;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

app/android/jni/point_cloud_drawable.cpp

@@ -318,7 +318,8 @@ PointCloudDrawable::PointCloudDrawable(
 }
 
 PointCloudDrawable::PointCloudDrawable(
-		const Mesh & mesh) :
+		const rtabmap::Mesh & mesh,
+		bool createWireframe) :
 				vertex_buffers_(0),
 				textures_(0),
 				nPoints_(0),
@@ -330,7 +331,7 @@ PointCloudDrawable::PointCloudDrawable(
 				gainG_(1.0f),
 				gainB_(1.0f)
 {
-	updateMesh(mesh);
+	updateMesh(mesh, createWireframe);
 }
 
 PointCloudDrawable::~PointCloudDrawable()
@@ -351,22 +352,33 @@ PointCloudDrawable::~PointCloudDrawable()
 	}
 }
 
-void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes)
+void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
 	LOGD("Update polygons");
 	polygons_.clear();
+	polygonLines_.clear();
+	polygonsLowRes_.clear();
+	polygonLinesLowRes_.clear();
 	if(polygons.size() && organizedToDenseIndices_.size())
 	{
 		unsigned int polygonSize = polygons[0].vertices.size();
 		UASSERT(polygonSize == 3);
 		polygons_.resize(polygons.size() * polygonSize);
+		if(createWireframe)
+			polygonLines_.resize(polygons_.size()*2);
 		int oi = 0;
+		int li = 0;
 		for(unsigned int i=0; i<polygons.size(); ++i)
 		{
 			UASSERT(polygons[i].vertices.size() == polygonSize);
 			for(unsigned int j=0; j<polygonSize; ++j)
 			{
 				polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+				if(createWireframe)
+				{
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
+					polygonLines_[li++] = organizedToDenseIndices_.at(polygons[i].vertices[(j+1) % polygonSize]);
+				}
 			}
 		}
 
@@ -375,13 +387,21 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
 			unsigned int polygonSize = polygonsLowRes[0].vertices.size();
 			UASSERT(polygonSize == 3);
 			polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
+			if(createWireframe)
+				polygonLinesLowRes_.resize(polygonsLowRes_.size()*2);
 			int oi = 0;
+			int li = 0;
 			for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
 			{
 				UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
 				for(unsigned int j=0; j<polygonSize; ++j)
 				{
 					polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+					if(createWireframe)
+					{
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
+						polygonLinesLowRes_[li++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[(j+1)%polygonSize]);
+					}
 				}
 			}
 		}
@@ -505,7 +525,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
 	nPoints_ = totalPoints;
 }
 
-void PointCloudDrawable::updateMesh(const Mesh & mesh)
+void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWireframe)
 {
 	UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
 	nPoints_ = 0;
@@ -797,7 +817,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
 
 	if(polygons_.size() != polygons.size())
 	{
-		updatePolygons(polygons, polygonsLowRes);
+		updatePolygons(polygons, polygonsLowRes, createWireframe);
 	}
 
 	if(!pose_.isNull())
@@ -857,7 +877,8 @@ void PointCloudDrawable::Render(
 		int screenHeight,
 		float nearClipPlane,
 	    float farClipPlane,
-		bool packDepthToColorChannel) const
+		bool packDepthToColorChannel,
+		bool wireFrame) const
 {
 	if(vertex_buffers_ && nPoints_ && visible_ && !shaderPrograms_.empty())
 	{
@@ -1021,22 +1042,38 @@ void PointCloudDrawable::Render(
 		{
 			if(distanceToCameraSqr<16.0f || polygonsLowRes_.empty())
 			{
-				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
-				glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
 			}
 		}
 		else if(meshRendering && polygons_.size())
 		{
 			if(distanceToCameraSqr<50.0f || polygonsLowRes_.empty())
 			{
-				glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
+				wireFrame = wireFrame && polygonLines_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLines_.size(), GL_UNSIGNED_INT, polygonLines_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
 			}
 			else
 			{
-				glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
+				wireFrame = wireFrame && polygonLinesLowRes_.size();
+				if(wireFrame)
+					glDrawElements(GL_LINES, polygonLinesLowRes_.size(), GL_UNSIGNED_INT, polygonLinesLowRes_.data());
+				else
+					glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
 			}
 		}
 		else if(!verticesLowRes_.empty())

app/android/jni/point_cloud_drawable.h

@@ -55,12 +55,13 @@ private:
 		  float gainG = 1.0f,
 		  float gainB = 1.0f);
   PointCloudDrawable(
-    	  const Mesh & mesh);
+    	  const rtabmap::Mesh & mesh,
+		  bool createWireframe = false);
   virtual ~PointCloudDrawable();
 
-  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
+  void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>(), bool createWireframe = false);
   void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices);
-  void updateMesh(const Mesh & mesh);
+  void updateMesh(const rtabmap::Mesh & mesh, bool createWireframe = false);
   void setPose(const rtabmap::Transform & pose);
   void setVisible(bool visible) {visible_=visible;}
   void setGains(float gainR, float gainG, float gainB) {gainR_ = gainR; gainG_ = gainG; gainB_ = gainB;}
@@ -69,6 +70,7 @@ private:
   bool isVisible() const {return visible_;}
   bool hasMesh() const {return polygons_.size()!=0;}
   bool hasTexture() const {return textures_ != 0;}
+  float getMinHeight() const {return minHeight_;}
   const pcl::PointXYZ & aabbMinModel() const {return aabbMinModel_;}
   const pcl::PointXYZ & aabbMaxModel() const {return aabbMaxModel_;}
   const pcl::PointXYZ & aabbMinWorld() const {return aabbMinWorld_;}
@@ -93,7 +95,8 @@ private:
 		  int screenHeight = 0,    // nonnull if depthTexture>0
 		  float nearClipPlane = 0, // nonnull if depthTexture>0
 		  float farClipPlane = 0,  // nonnull if depthTexture>0
-		  bool packDepthToColorChannel = false) const;
+		  bool packDepthToColorChannel = false,
+		  bool wireFrame = false) const;
 
  private:
   template<class PointT>
@@ -114,6 +117,8 @@ private:
   GLuint textures_;
   std::vector<GLuint> polygons_;
   std::vector<GLuint> polygonsLowRes_;
+  std::vector<GLuint> polygonLines_;
+  std::vector<GLuint> polygonLinesLowRes_;
   std::vector<GLuint> verticesLowRes_;
   std::vector<GLuint> verticesLowLowRes_;
   int nPoints_;
@@ -122,6 +127,7 @@ private:
   bool visible_;
   bool hasNormals_;
   std::vector<unsigned int> organizedToDenseIndices_;
+  float minHeight_; // odom frame
 
   float gainR_;
   float gainG_;

app/android/jni/scene.cpp

@@ -16,12 +16,16 @@
 
 #include <tango-gl/conversions.h>
 #include <tango-gl/gesture_camera.h>
+#include <tango-gl/util.h>
 
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <rtabmap/core/util3d_filtering.h>
+#include <rtabmap/core/util3d_transforms.h>
+#include <rtabmap/core/util3d_surface.h>
 #include <pcl/common/transforms.h>
+#include <pcl/common/common.h>
 
 #include <glm/gtx/transform.hpp>
 
@@ -32,7 +36,7 @@
 // add an offset in z to our origin. We'll set this offset to 1.3 meters based
 // on the average height of a human standing with a Tango device. This allows us
 // to place a grid roughly on the ground for most users.
-const glm::vec3 kHeightOffset = glm::vec3(0.0f, 1.3f, 0.0f);
+const glm::vec3 kHeightOffset = glm::vec3(0.0f, -1.3f, 0.0f);
 
 // Color of the motion tracking trajectory.
 const tango_gl::Color kTraceColor(0.66f, 0.66f, 0.66f);
@@ -65,6 +69,7 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -75,7 +80,8 @@ Scene::Scene() :
 		graphVisible_(true),
 		gridVisible_(true),
 		traceVisible_(true),
-		color_camera_to_display_rotation_(ROTATION_0),
+		frustumVisible_(true),
+		color_camera_to_display_rotation_(rtabmap::ROTATION_0),
 		currentPose_(0),
 		graph_shader_program_(0),
 		blending_(true),
@@ -86,6 +92,7 @@ Scene::Scene() :
 		boundingBoxRendering_(false),
 		lighting_(false),
 		backfaceCulling_(true),
+		wireFrame_(false),
 		r_(0.0f),
 		g_(0.0f),
 		b_(0.0f),
@@ -97,12 +104,13 @@ Scene::Scene() :
 {
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
-	      tango_gl::GestureCamera::kFirstPerson);
+	      tango_gl::GestureCamera::kThirdPersonFollow);
 }
 
 Scene::~Scene() {
 	DeleteResources();
 	delete gesture_camera_;
+	delete currentPose_;
 }
 
 //Should only be called in OpenGL thread!
@@ -121,7 +129,6 @@ void Scene::InitGLContent()
 	trace_ = new tango_gl::Trace();
 	grid_ = new tango_gl::Grid();
 	box_ = new BoundingBoxDrawable();
-	currentPose_ = new rtabmap::Transform();
 
 
 	axis_->SetScale(glm::vec3(0.5f,0.5f,0.5f));
@@ -129,7 +136,7 @@ void Scene::InitGLContent()
 	trace_->ClearVertexArray();
 	trace_->SetColor(kTraceColor);
 	grid_->SetColor(kGridColor);
-	grid_->SetPosition(-kHeightOffset);
+	grid_->SetPosition(kHeightOffset);
 	box_->SetShader();
 	box_->SetColor(1,0,0);
 
@@ -153,8 +160,9 @@ void Scene::DeleteResources() {
 		delete frustum_;
 		delete trace_;
 		delete grid_;
-		delete currentPose_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -183,6 +191,10 @@ void Scene::clear()
 	{
 		delete iter->second;
 	}
+	for(std::map<int, tango_gl::Axis*>::iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		delete iter->second;
+	}
 	if(trace_)
 	{
 		trace_->ClearVertexArray();
@@ -193,6 +205,11 @@ void Scene::clear()
 		graph_ = 0;
 	}
 	pointClouds_.clear();
+	markers_.clear();
+	if(grid_)
+	{
+		grid_->SetPosition(kHeightOffset);
+	}
 }
 
 //Should only be called in OpenGL thread!
@@ -201,7 +218,7 @@ void Scene::SetupViewPort(int w, int h) {
 		LOGE("Setup graphic height not valid");
 	}
 	UASSERT(gesture_camera_ != 0);
-	gesture_camera_->SetAspectRatio(static_cast<float>(w) / static_cast<float>(h));
+	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
 	glViewport(0, 0, w, h);
 	if(screenWidth_ != w || fboId_ == 0)
 	{
@@ -350,9 +367,13 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
 	UASSERT(gesture_camera_ != 0);
 
+	if(currentPose_ == 0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z());
 	Eigen::Quaternionf quat = currentPose_->getQuaternionf();
 	glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
@@ -377,6 +398,17 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
+	bool renderBackgroundCamera =
+			background_renderer_ &&
+			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+			uvsTransformed;
+	if(renderBackgroundCamera)
+	{
+		projectionMatrix = arProjectionMatrix;
+		viewMatrix = arViewMatrix;
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -426,7 +458,7 @@ int Scene::Render() {
 
 	UTimer timer;
 
-	bool onlineBlending = blending_ && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -436,11 +468,19 @@ int Scene::Render() {
 		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-		// Draw scene
+		if(renderBackgroundCamera)
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
 		{
-			// set large distance to cam to use low res polygons for fast processing
+			PointCloudDrawable drawable(occlusionMesh);
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+		}
+		else
+		{
+			// Draw scene
+			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+			{
+				// set large distance to cam to use low res polygons for fast processing
+				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			}
 		}
 
 		// back to normal window-system-provided framebuffer
@@ -477,9 +517,18 @@ int Scene::Render() {
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+	if(renderBackgroundCamera)
+	{
+		background_renderer_->Draw(uvsTransformed);
+
+		//To debug occlusion image:
+		//PointCloudDrawable drawable(occlusionMesh);
+		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+	}
+
 	if(!currentPose_->isNull())
 	{
-		if (gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
+		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
 		{
 			frustum_->SetPosition(position);
 			frustum_->SetRotation(rotation);
@@ -488,8 +537,13 @@ int Scene::Render() {
 			frustum_->SetScale(kFrustumScale);
 			frustum_->Render(projectionMatrix, viewMatrix);
 
-			axis_->SetPosition(position);
+			rtabmap::Transform cameraFrame = *currentPose_*rtabmap::optical_T_opengl*rtabmap::CameraMobile::opticalRotationInv;
-			axis_->SetRotation(rotation);
+			glm::vec3 positionCamera(cameraFrame.x(), cameraFrame.y(), cameraFrame.z());
+			Eigen::Quaternionf quatCamera = cameraFrame.getQuaternionf();
+			glm::quat rotationCamera(quatCamera.w(), quatCamera.x(), quatCamera.y(), quatCamera.z());
+
+			axis_->SetPosition(positionCamera);
+			axis_->SetRotation(rotationCamera);
 			axis_->Render(projectionMatrix, viewMatrix);
 		}
 
@@ -498,11 +552,11 @@ int Scene::Render() {
 		{
 			trace_->Render(projectionMatrix, viewMatrix);
 		}
+	}
 
-		if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
-		{
+	{
-			grid_->Render(projectionMatrix, viewMatrix);
+		grid_->Render(projectionMatrix, viewMatrix);
-		}
 	}
 
 	if(graphVisible_ && graph_)
@@ -533,7 +587,7 @@ int Scene::Render() {
 				cloud->getPose().z() - openglCamera.z());
 		float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
 
-		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane());
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}
 
 	if(onlineBlending)
@@ -542,6 +596,12 @@ int Scene::Render() {
 		glDepthMask(GL_TRUE);
 	}
 
+	//draw markers on foreground
+	for(std::map<int, tango_gl::Axis*>::const_iterator iter=markers_.begin(); iter!=markers_.end(); ++iter)
+	{
+		iter->second->Render(projectionMatrix, viewMatrix);
+	}
+
 	return (int)cloudsToDraw.size();
 }
 
@@ -551,19 +611,39 @@ void Scene::SetCameraType(tango_gl::GestureCamera::CameraType camera_type) {
 
 void Scene::SetCameraPose(const rtabmap::Transform & pose)
 {
-	UASSERT(currentPose_ != 0);
 	UASSERT(!pose.isNull());
+	if(currentPose_ ==0)
+	{
+		currentPose_ = new rtabmap::Transform(0,0,0,0,0,-M_PI/2.0f);
+	}
 	*currentPose_ = pose;
 }
 
+void Scene::setFOV(float angle)
+{
+	gesture_camera_->SetFieldOfView(angle);
+}
+void Scene::setOrthoCropFactor(float value)
+{
+	gesture_camera_->SetOrthoCropFactor(value);
+}
+void Scene::setGridRotation(float angleDeg)
+{
+	float angleRad = angleDeg * DEGREE_2_RADIANS;
+	if(grid_)
+	{
+		glm::quat rot = glm::rotate(glm::quat(1,0,0,0), angleRad, glm::vec3(0, 1, 0));
+		grid_->SetRotation(rot);
+	}
+}
+
 rtabmap::Transform Scene::GetOpenGLCameraPose(float * fov) const
 {
 	if(fov)
 	{
 		*fov = gesture_camera_->getFOV();
 	}
-	return glmToTransform(gesture_camera_->GetTransformationMatrix());
+	return rtabmap::glmToTransform(gesture_camera_->GetTransformationMatrix());
-
 }
 
 void Scene::OnTouchEvent(int touch_count,
@@ -621,7 +701,59 @@ void Scene::setTraceVisible(bool visible)
 	traceVisible_ = visible;
 }
 
+void Scene::setFrustumVisible(bool visible)
+{
+	frustumVisible_ = visible;
+}
+
 //Should only be called in OpenGL thread!
+void Scene::addMarker(
+		int id,
+		const rtabmap::Transform & pose)
+{
+	LOGI("add marker %d", id);
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter == markers_.end())
+	{
+		//create
+		tango_gl::Axis * drawable = new tango_gl::Axis();
+		drawable->SetScale(glm::vec3(0.05f,0.05f,0.05f));
+		drawable->SetLineWidth(5);
+		markers_.insert(std::make_pair(id, drawable));
+	}
+	setMarkerPose(id, pose);
+}
+void Scene::setMarkerPose(int id, const rtabmap::Transform & pose)
+{
+	UASSERT(!pose.isNull());
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		glm::vec3 position(pose.x(), pose.y(), pose.z());
+		Eigen::Quaternionf quat = pose.getQuaternionf();
+		glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z());
+		iter->second->SetPosition(position);
+		iter->second->SetRotation(rotation);
+	}
+}
+bool Scene::hasMarker(int id) const
+{
+	return markers_.find(id) != markers_.end();
+}
+void Scene::removeMarker(int id)
+{
+	std::map<int, tango_gl::Axis*>::iterator iter=markers_.find(id);
+	if(iter != markers_.end())
+	{
+		delete iter->second;
+		markers_.erase(iter);
+	}
+}
+std::set<int> Scene::getAddedMarkers() const
+{
+	return uKeysSet(markers_);
+}
+
 void Scene::addCloud(
 		int id,
 		const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
@@ -644,8 +776,9 @@ void Scene::addCloud(
 
 void Scene::addMesh(
 		int id,
-		const Mesh & mesh,
+		const rtabmap::Mesh & mesh,
-		const rtabmap::Transform & pose)
+		const rtabmap::Transform & pose,
+		bool createWireframe)
 {
 	LOGI("add mesh %d", id);
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
@@ -656,9 +789,61 @@ void Scene::addMesh(
 	}
 
 	//create
-	PointCloudDrawable * drawable = new PointCloudDrawable(mesh);
+	PointCloudDrawable * drawable = new PointCloudDrawable(mesh, createWireframe);
 	drawable->setPose(pose);
 	pointClouds_.insert(std::make_pair(id, drawable));
+
+	if(!mesh.pose.isNull() && mesh.cloud->size() && (!mesh.cloud->isOrganized() || mesh.indices->size()))
+	{
+		UTimer time;
+		float height = 0.0f;
+		Eigen::Affine3f affinePose = mesh.pose.toEigen3f();
+		if(mesh.polygons.size())
+		{
+			for(unsigned int i=0; i<mesh.polygons.size(); ++i)
+			{
+				for(unsigned int j=0; j<mesh.polygons[i].vertices.size(); ++j)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.polygons[i].vertices[j]), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+		else
+		{
+			if(mesh.cloud->isOrganized())
+			{
+				for(unsigned int i=0; i<mesh.indices->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(mesh.indices->at(i)), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+			else
+			{
+				for(unsigned int i=0; i<mesh.cloud->size(); ++i)
+				{
+					pcl::PointXYZRGB pt = pcl::transformPoint(mesh.cloud->at(i), affinePose);
+					if(pt.z < height)
+					{
+						height = pt.z;
+					}
+				}
+			}
+		}
+
+		if(grid_->GetPosition().y == kHeightOffset.y || grid_->GetPosition().y > height)
+		{
+			grid_->SetPosition(glm::vec3(0,height,0));
+		}
+		LOGD("compute min height %f s", time.ticks());
+	}
 }
 
 
@@ -710,7 +895,7 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
 	}
 }
 
-void Scene::updateMesh(int id, const Mesh & mesh)
+void Scene::updateMesh(int id, const rtabmap::Mesh & mesh)
 {
 	std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
 	if(iter != pointClouds_.end())

app/android/jni/scene.h

@@ -21,7 +21,7 @@
 #include <memory>
 #include <set>
 
-#include <tango_client_api.h>  // NOLINT
+#include "CameraMobile.h"
 #include <tango-gl/axis.h>
 #include <tango-gl/camera.h>
 #include <tango-gl/color.h>
@@ -38,6 +38,7 @@
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
 #include <bounding_box_drawable.h>
+#include <background_renderer.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -57,8 +58,11 @@ class Scene {
 
   // Setup GL view port.
   void SetupViewPort(int w, int h);
+  int getViewPortWidth() const {return screenWidth_;}
+  int getViewPortHeight() const {return screenHeight_;}
 
-  void setScreenRotation(TangoSupportRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
+  rtabmap::ScreenRotation getScreenRotation() const {return color_camera_to_display_rotation_;}
+  void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation) {color_camera_to_display_rotation_ = colorCameraToDisplayRotation;}
 
   void clear(); // removed all point clouds
 
@@ -68,15 +72,16 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
   // @param: camera_type, camera type includes first person, third person and
   //         top down
   void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}
 
-  void SetCameraPose(const rtabmap::Transform & pose);
+  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
   rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}
   rtabmap::Transform GetOpenGLCameraPose(float * fov = 0) const;
 
@@ -99,6 +104,13 @@ class Scene {
   void setGraphVisible(bool visible);
   void setGridVisible(bool visible);
   void setTraceVisible(bool visible);
+  void setFrustumVisible(bool visible);
+
+  void addMarker(int id, const rtabmap::Transform & pose);
+  void setMarkerPose(int id, const rtabmap::Transform & pose);
+  bool hasMarker(int id) const;
+  void removeMarker(int id);
+  std::set<int> getAddedMarkers() const;
 
   void addCloud(
   		  int id,
@@ -107,8 +119,9 @@ class Scene {
   		  const rtabmap::Transform & pose);
   void addMesh(
   		int id,
-  		const Mesh & mesh,
+  		const rtabmap::Mesh & mesh,
-  		const rtabmap::Transform & pose);
+  		const rtabmap::Transform & pose,
+		bool createWireframe = false);
 
   void setCloudPose(int id, const rtabmap::Transform & pose);
   void setCloudVisible(int id, bool visible);
@@ -117,15 +130,19 @@ class Scene {
   bool hasTexture(int id) const;
   std::set<int> getAddedClouds() const;
   void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
-  void updateMesh(int id, const Mesh & mesh);
+  void updateMesh(int id, const rtabmap::Mesh & mesh);
   void updateGains(int id, float gainR, float gainG, float gainB);
 
   void setBlending(bool enabled) {blending_ = enabled;}
   void setMapRendering(bool enabled) {mapRendering_ = enabled;}
   void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
   void setPointSize(float size) {pointSize_ = size;}
+  void setFOV(float angle);
+  void setOrthoCropFactor(float value);
+  void setGridRotation(float angleDeg);
   void setLighting(bool enabled) {lighting_ = enabled;}
   void setBackfaceCulling(bool enabled) {backfaceCulling_ = enabled;}
+  void setWireframe(bool enabled) {wireFrame_ = enabled;}
   void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f
   void setGridColor(float r, float g, float b);
 
@@ -137,6 +154,8 @@ class Scene {
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
+  BackgroundRenderer * background_renderer_;
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;
@@ -159,8 +178,11 @@ class Scene {
   bool graphVisible_;
   bool gridVisible_;
   bool traceVisible_;
+  bool frustumVisible_;
 
-  TangoSupportRotation color_camera_to_display_rotation_;
+  std::map<int, tango_gl::Axis*> markers_;
+
+  rtabmap::ScreenRotation color_camera_to_display_rotation_;
 
   std::map<int, PointCloudDrawable*> pointClouds_;
 
@@ -177,6 +199,7 @@ class Scene {
   bool boundingBoxRendering_;
   bool lighting_;
   bool backfaceCulling_;
+  bool wireFrame_;
   float r_;
   float g_;
   float b_;

app/android/jni/tango-gl/camera.cpp

@@ -22,8 +22,13 @@ namespace tango_gl {
 Camera::Camera() {
   field_of_view_ = 45.0f * DEGREE_2_RADIANS;
   aspect_ratio_ = 4.0f / 3.0f;
+  width_ = 800.0f;
+  height_ = 600.0f;
   near_clip_plane_ = 0.2f;
   far_clip_plane_ = 1000.0f;
+  ortho_ = false;
+  orthoScale_ = 2.0f;
+  orthoCropFactor_ = -1.0f;
 }
 
 glm::mat4 Camera::GetViewMatrix() {
@@ -31,12 +36,17 @@ glm::mat4 Camera::GetViewMatrix() {
 }
 
 glm::mat4 Camera::GetProjectionMatrix() {
-  return glm::perspective(field_of_view_, aspect_ratio_,
+	if(ortho_)
-                          near_clip_plane_, far_clip_plane_);
+	{
+		return glm::ortho(-orthoScale_*aspect_ratio_, orthoScale_*aspect_ratio_, -orthoScale_, orthoScale_, orthoScale_ + orthoCropFactor_, far_clip_plane_);
+	}
+  return glm::perspective(field_of_view_, aspect_ratio_, near_clip_plane_, far_clip_plane_);
 }
 
-void Camera::SetAspectRatio(float aspect_ratio) {
+void Camera::SetWindowSize(float width, float height) {
-  aspect_ratio_ = aspect_ratio;
+	width_ = width;
+	height_ = height;
+  aspect_ratio_ = width/height;
 }
 
 void Camera::SetFieldOfView(float fov) {

app/android/jni/tango-gl/frustum.cpp

@@ -18,11 +18,14 @@
 namespace tango_gl {
 
 static const float float_vertices[] = {
-    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  1.0f,
+    0.0f,  0.0f,  0.0f,  -1.0f, 1.0f,  -1.0f,
-    1.0f,  -1.0f, 0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f, 0.0f,  0.0f,
+	0.0f,  0.0f,  0.0f,  1.0f, 1.0f,  -1.0f,
-    0.0f,  1.0f,  -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+	0.0f,  0.0f,  0.0f,  -1.0f, -1.0f, -1.0f,
-    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f, 1.0f,  -1.0f, -1.0f, -1.0f,
+	0.0f,  0.0f,  0.0f,  1.0f,  -1.0f, -1.0f,
-    -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
+	-1.0f, 1.0f,  -1.0f, 1.0f,  1.0f,  -1.0f,
+    1.0f,  1.0f,  -1.0f, 1.0f,  -1.0f, -1.0f,
+	1.0f,  -1.0f, -1.0f, -1.0f, -1.0f, -1.0f,
+	-1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  -1.0f};
 
 Frustum::Frustum() : Line(3.0f, GL_LINES) {
   SetShader();

app/android/jni/tango-gl/gesture_camera.cpp

@@ -62,68 +62,83 @@ GestureCamera::GestureCamera() :
 GestureCamera::~GestureCamera() { delete cam_parent_transform_; }
 
 void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
-                                 float y0, float x1, float y1) {
+		float y0, float x1, float y1) {
-  if (camera_type_ == kFirstPerson) {
-    return;
-  }
 
-  if (touch_count == 1) {
+	if (camera_type_!=kFirstPerson && touch_count == 1) {
-    switch (event) {
+		switch (event) {
-      case kTouch0Down: {
+		case kTouch0Down: {
-        cam_start_angle_ = cam_cur_angle_;
+			cam_start_angle_ = cam_cur_angle_;
 
-        touch0_start_position_.x = x0;
+			touch0_start_position_.x = x0;
-        touch0_start_position_.y = y0;
+			touch0_start_position_.y = y0;
-        break;
+			break;
-      }
+		}
-      case kTouchMove: {
+		case kTouchMove: {
-        float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
+			glm::vec2 offset;
-        float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
 
-        cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
+			float rotation_x = (touch0_start_position_.y - y0) * kRotationSpeed;
-        cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
+			float rotation_y = (touch0_start_position_.x - x0) * kRotationSpeed;
-        StartCameraToCurrentTransform();
-        break;
-      }
-      default: { break; }
-    }
-  }
-  if (touch_count == 2) {
-    switch (event) {
-      case kTouch1Down: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
-        cam_start_dist_ = GetPosition().z;
 
-        // center touch
+			if(camera_type_!=kTopOrtho)
-        touch0_start_position_.x = (x0+x1)/2.0f;
+				cam_cur_angle_.x = cam_start_angle_.x + rotation_x;
-        touch0_start_position_.y = (y0+y1)/2.0f;
+			cam_cur_angle_.y = cam_start_angle_.y + rotation_y;
-        break;
-      }
-      case kTouchMove: {
-        float abs_x = x0 - x1;
-        float abs_y = y0 - y1;
-        float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
 
-        cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+			StartCameraToCurrentTransform();
-                                  kCamViewMinDist, kCamViewMaxDist);
 
-        glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+			break;
-        glm::vec2 offset;
+		}
-        offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+		default: { break; }
-        offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+		}
-        touch0_start_position_ = touch_center_position;
+	}
+	if (touch_count == 2) {
+		switch (event) {
+		case kTouch1Down: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			start_touch_dist_ = std::sqrt(abs_x * abs_x + abs_y * abs_y);
+			cam_start_dist_ = GetPosition().z;
+			cam_start_fov_ = this->getFOV();
 
-        StartCameraToCurrentTransform();
+			// center touch
+			touch0_start_position_.x = (x0+x1)/2.0f;
+			touch0_start_position_.y = (y0+y1)/2.0f;
+			break;
+		}
+		case kTouchMove: {
+			float abs_x = x0 - x1;
+			float abs_y = y0 - y1;
+			float dist = start_touch_dist_ - std::sqrt(abs_x * abs_x + abs_y * abs_y);
 
-        anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			if(camera_type_ == kFirstPerson)
+			{
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+			}
+			else
+			{
+				cam_cur_dist_ = tango_gl::util::Clamp(cam_start_dist_ + dist * kZoomSpeed,
+								kCamViewMinDist, kCamViewMaxDist);
 
-        break;
+				this->SetOrthoMode(camera_type_ == kTopOrtho);
-      }
+				if(camera_type_ == kTopOrtho)
-      default: { break; }
+				{
-    }
+					this->SetOrthoScale(cam_cur_dist_);
-  }
+				}
+
+				glm::vec2 touch_center_position((x0+x1)/2.0f, (y0+y1)/2.0f);
+				glm::vec2 offset;
+				offset.x = (touch_center_position.x - touch0_start_position_.x) * kMoveSpeed;
+				offset.y = (touch_center_position.y - touch0_start_position_.y) * kMoveSpeed;
+				touch0_start_position_ = touch_center_position;
+
+				StartCameraToCurrentTransform();
+
+				anchor_offset_ += glm::rotate(cam_parent_transform_->GetRotation(), glm::vec3(-offset.x, offset.y, 0));
+			}
+			break;
+		}
+		default: { break; }
+		}
+	}
 }
 
 Segment GestureCamera::GetSegmentFromTouch(float normalized_x,
@@ -164,6 +179,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
   camera_type_ = camera_index;
   switch (camera_index) {
     case kFirstPerson:
+    	SetOrthoMode(false);
       SetFieldOfView(kLowFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
@@ -177,20 +193,22 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       break;
     case kThirdPerson:
     case kThirdPersonFollow:
-      SetFieldOfView(kHighFov);
+    	SetOrthoMode(false);
+    	SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
-      cam_cur_angle_.x = -M_PI / 6.0f;
+      cam_cur_angle_.x = -M_PI / 12.0f;
-      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 4.0f;
+      cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
     case kTopDown:
-      SetFieldOfView(kHighFov);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+      SetOrthoMode(false);
+      SetFieldOfView(kHighFov);
       cam_cur_dist_ = kTopDownCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = -M_PI / 2.0f;
@@ -198,6 +216,19 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       cam_cur_target_rot_ = glm::quat(1,0,0,0);
       StartCameraToCurrentTransform();
       break;
+    case kTopOrtho:
+	  SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
+	  SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
+	  SetOrthoMode(true);
+	  SetOrthoScale(kTopDownCameraDist);
+	  SetOrthoCropFactor(-1.0f);
+	  cam_cur_dist_ = kTopDownCameraDist;
+	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
+	  cam_cur_angle_.x = -M_PI / 2.0f;
+	  cam_cur_angle_.y = 0.0f;
+	  cam_cur_target_rot_ = glm::quat(1,0,0,0);
+	  StartCameraToCurrentTransform();
+	  break;
     default:
       break;
   }

app/android/jni/tango-gl/grid.cpp

@@ -31,13 +31,17 @@ Grid::Grid(float density, int qx, int qy) : Line(1.0f, GL_LINES) {
 
   // Horizontal line.
   for (int i = 0; i < (qy + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width, 0.0f, -height + i * density));
+	  for (int j = 0; j < (qx + 1); j++) {
-    vec_vertices_.push_back(glm::vec3(width, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width + j*density, 0.0f, -height + i * density));
+		vec_vertices_.push_back(glm::vec3(-width+ + (j+1)*density, 0.0f, -height + i * density));
+	  }
   }
 
   for (int i = 0; i < (qx + 1); i++) {
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height));
+	  for (int j = 0; j < (qy + 1); j++) {
-    vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, height));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + j*density));
+		vec_vertices_.push_back(glm::vec3(-width + i * density, 0.0f, -height + (j+1)*density));
+	  }
   }
 }
 }  // namespace tango_gl

app/android/jni/tango-gl/include/tango-gl/camera.h

@@ -27,8 +27,11 @@ class Camera : public Transform {
   Camera& operator=(const Camera&) = delete;
   ~Camera();
 
-  void SetAspectRatio(const float aspect_ratio);
+  void SetWindowSize(const float width, const float height);
   void SetFieldOfView(const float fov);
+  void SetOrthoMode(bool enabled) {ortho_ = enabled;}
+  void SetOrthoScale(float scale) {orthoScale_ = scale;}
+  void SetOrthoCropFactor(float value) {orthoCropFactor_ = value;}
   void SetNearFarClipPlanes(const float near, const float far);
 
   glm::mat4 GetViewMatrix();
@@ -55,7 +58,12 @@ class Camera : public Transform {
  protected:
   float field_of_view_;
   float aspect_ratio_;
+  float width_;
+  float height_;
   float near_clip_plane_, far_clip_plane_;
+  bool ortho_;
+  float orthoScale_;
+  float orthoCropFactor_;
 };
 }  // namespace tango_gl
 #endif  // TANGO_GL_CAMERA_H_

app/android/jni/tango-gl/include/tango-gl/gesture_camera.h

@@ -29,7 +29,8 @@ class GestureCamera : public Camera {
     kFirstPerson = 0,
     kThirdPersonFollow = 1,
     kTopDown = 2,
-    kThirdPerson = 3
+	kTopOrtho = 3,
+    kThirdPerson = 4
   };
 
   enum TouchEvent {
@@ -80,6 +81,7 @@ class GestureCamera : public Camera {
   glm::quat cam_cur_target_rot_;
 
   float cam_start_dist_;
+  float cam_start_fov_;
   float cam_cur_dist_;
   glm::vec3 anchor_offset_;
 

app/android/jni/tango-gl/include/tango-gl/util.h

@@ -25,7 +25,6 @@
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
-#include <tango_support_api.h>
 
 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -44,6 +43,7 @@
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
 #endif
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
+#define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)
 
 #ifndef M_PI
 #define M_PI 3.1415926f
@@ -84,32 +84,6 @@ namespace util {
 
   glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec);
 
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: integer value of display orientation, values available
-  // are 0, 1, 2 ,3. Followed by Android display orientation standard:
-  // https://developer.android.com/reference/android/view/Display.html#getRotation()
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      int display_rotation, int color_camera_rotation);
-
-  // Get the Android rotation integer value from color camera to display.
-  // This function is used to compute the orientation difference to handle
-  // the portrait and landscape mode for color camera display.
-  //
-  // @param display: the device display orientation.
-  // @param color_camera: integer value of color camera oreintation, values
-  // available are 0, 90, 180, 270. Followed by Android camera orientation
-  // standard:
-  // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
-  TangoSupportRotation GetAndroidRotationFromColorCameraToDisplay(
-      TangoSupportRotation display_rotation, int color_camera_rotation);
-
 }  // namespace util
 }  // namespace tango_gl
 #endif  // TANGO_GL_RENDERER_GL_UTIL

app/android/jni/tango-gl/util.cpp

@@ -238,23 +238,4 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) {
   return glm::vec3(mat * glm::vec4(vec, 1.0f));
 }
 
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    int display_rotation, int color_camera_rotation) {
-  TangoSupportRotation r =
-      static_cast<TangoSupportRotation>(display_rotation);
-  return util::GetAndroidRotationFromColorCameraToDisplay(
-      r, color_camera_rotation);
-}
-
-TangoSupportRotation util::GetAndroidRotationFromColorCameraToDisplay(
-    TangoSupportRotation display_rotation, int color_camera_rotation) {
-  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
-
-  int ret = static_cast<int>(display_rotation) - color_camera_n;
-  if (ret < 0) {
-    ret += 4;
-  }
-  return static_cast<TangoSupportRotation>(ret % 4);
-}
-
 }  // namespace tango_gl

app/android/jni/util.h

@@ -39,13 +39,15 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/Vertices.h>
 #include <pcl/pcl_base.h>
 
+namespace rtabmap {
+
 class LogHandler : public UEventsHandler
 {
 public:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kWarning);
+		ULogger::setLevel(ULogger::kDebug);
-		ULogger::setEventLevel(ULogger::kWarning);
+		ULogger::setEventLevel(ULogger::kDebug);
 		ULogger::setPrintThreadId(true);
 
 		registerToEventsManager();
@@ -58,19 +60,23 @@ protected:
 			ULogEvent * logEvent = (ULogEvent*)event;
 			if(logEvent->getCode() == ULogger::kDebug)
 			{
-				LOGD(logEvent->getMsg().c_str());
+				LOGD("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kInfo)
 			{
-				LOGI(logEvent->getMsg().c_str());
+				LOGI("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() == ULogger::kWarning)
 			{
-				LOGW(logEvent->getMsg().c_str());
+				LOGW("%s", logEvent->getMsg().c_str());
 			}
 			else if(logEvent->getCode() >= ULogger::kError)
 			{
-				LOGE(logEvent->getMsg().c_str());
+				LOGE("%s", logEvent->getMsg().c_str());
+			}
+			else if(logEvent->getCode() >= ULogger::kFatal)
+			{
+				LOGF("%s", logEvent->getMsg().c_str());
 			}
 
 		}
@@ -78,6 +84,11 @@ protected:
 	}
 };
 
+static const rtabmap::Transform optical_T_opengl(
+		1.0f,  0.0f,  0.0f, 0.0f,
+		0.0f, -1.0f,  0.0f, 0.0f,
+		0.0f,  0.0f, -1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_tango_world(
 		1.0f,  0.0f,  0.0f, 0.0f,
 		0.0f,  0.0f,  1.0f, 0.0f,
@@ -88,20 +99,25 @@ static const rtabmap::Transform rtabmap_world_T_tango_world(
 	    -1.0f, 0.0f, 0.0f, 0.0f,
 		 0.0f, 0.0f, 1.0f, 0.0f);
 
-static const rtabmap::Transform tango_device_T_rtabmap_device(
+static const rtabmap::Transform tango_device_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 	     0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);
 
+static const rtabmap::Transform tango_world_T_rtabmap_world(
+		 0.0f, -1.0f, 0.0f, 0.0f,
+	     1.0f, 0.0f, 0.0f, 0.0f,
+		 0.0f, 0.0f, 1.0f, 0.0f);
+
 static const rtabmap::Transform opengl_world_T_rtabmap_world(
 		 0.0f, -1.0f, 0.0f, 0.0f,
 		 0.0f,  0.0f, 1.0f, 0.0f,
 		-1.0f,  0.0f, 0.0f, 0.0f);
 
-static const rtabmap::Transform rtabmap_device_T_opengl_device(
+static const rtabmap::Transform rtabmap_world_T_opengl_world(
-		 0.0f, 0.0f, -1.0f, 0.0f,
+		 0.0f,  0.0f,-1.0f, 0.0f,
-		-1.0f, 0.0f,  0.0f, 0.0f,
+		-1.0f,  0.0f, 0.0f, 0.0f,
-		 0.0f, 1.0f,  0.0f, 0.0f);
+		 0.0f,  1.0f, 0.0f, 0.0f);
 
 inline glm::mat4 glmFromTransform(const rtabmap::Transform & transform)
 {
@@ -173,4 +189,81 @@ public:
 	cv::Mat texture;
 };
 
+typedef enum {
+  /// Not apply any rotation.
+  ROTATION_IGNORED = -1,
+
+  /// 0 degree rotation (natural orientation)
+  ROTATION_0 = 0,
+
+  /// 90 degree rotation.
+  ROTATION_90 = 1,
+
+  /// 180 degree rotation.
+  ROTATION_180 = 2,
+
+  /// 270 degree rotation.
+  ROTATION_270 = 3
+} ScreenRotation;
+
+inline int NormalizedColorCameraRotation(int camera_rotation) {
+  int camera_n = 0;
+  switch (camera_rotation) {
+    case 90:
+      camera_n = 1;
+      break;
+    case 180:
+      camera_n = 2;
+      break;
+    case 270:
+      camera_n = 3;
+      break;
+    default:
+      camera_n = 0;
+      break;
+  }
+  return camera_n;
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: the device display orientation.
+// @param color_camera: integer value of color camera oreintation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+		ScreenRotation display_rotation, int color_camera_rotation) {
+  int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation);
+
+  int ret = static_cast<int>(display_rotation) - color_camera_n;
+  if (ret < 0) {
+    ret += 4;
+  }
+  return static_cast<ScreenRotation>(ret % 4);
+}
+
+// Get the Android rotation integer value from color camera to display.
+// This function is used to compute the orientation difference to handle
+// the portrait and landscape mode for color camera display.
+//
+// @param display: integer value of display orientation, values available
+// are 0, 1, 2 ,3. Followed by Android display orientation standard:
+// https://developer.android.com/reference/android/view/Display.html#getRotation()
+// @param color_camera: integer value of color camera orientation, values
+// available are 0, 90, 180, 270. Followed by Android camera orientation
+// standard:
+// https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation
+inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
+    int display_rotation, int color_camera_rotation) {
+	ScreenRotation r =
+      static_cast<ScreenRotation>(display_rotation);
+  return GetAndroidRotationFromColorCameraToDisplay(
+      r, color_camera_rotation);
+}
+
+}
+
 #endif /* UTIL_H_ */

app/android/libs/.gitignore

@@ -0,0 +1 @@
+*.jar

app/android/res/drawable/custom_seekbar.xml

@@ -0,0 +1,12 @@
+<?xml version="1.0" encoding="utf-8"?>
+<layer-list xmlns:android="http://schemas.android.com/apk/res/android" >
+    <item>
+        <shape>
+            <gradient
+                android:endColor="#00ffffff"
+                android:startColor="#ff686868"
+                android:useLevel="false" />
+        </shape>
+    </item>
+
+</layer-list>

app/android/res/layout/activity_rtabmap.xml

@@ -15,13 +15,51 @@
 -->
 <RelativeLayout xmlns:android="http://schemas.android.com/apk/res/android"
     android:layout_width="wrap_content"
-    android:layout_height="wrap_content" >
+    android:layout_height="wrap_content">
 
     <android.opengl.GLSurfaceView
         android:id="@+id/gl_surface_view"
         android:layout_width="fill_parent"
         android:layout_height="fill_parent"
-        android:layout_gravity="top" />
+        android:layout_gravity="top"/>
+    
+    <RelativeLayout 
+        android:layout_width="wrap_content"
+        android:layout_height="match_parent"
+        android:fitsSystemWindows="true">
+        
+    <ImageButton
+        android:id="@+id/start_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/start96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
+
+    <ImageButton
+        android:id="@+id/stop_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentBottom="true"
+        android:layout_centerHorizontal="true"
+        android:src="@drawable/stop96"
+        android:background="@null"
+        android:layout_marginBottom="10dp" />
+    
+    <ToggleButton
+        android:id="@+id/wireframe_button"
+        android:layout_width="100dp"
+        android:layout_height="wrap_content"
+        android:layout_above="@+id/light_button"
+        android:layout_alignLeft="@+id/light_button"
+        android:layout_alignParentRight="true"
+        android:layout_marginBottom="5dp"
+        android:layout_marginRight="5dp"
+        android:paddingRight="5dp"
+        android:textOff="@string/wireframe"
+        android:textOn="@string/wireframe" />
     
     <ToggleButton
         android:id="@+id/light_button"
@@ -30,7 +68,7 @@
         android:layout_above="@+id/backface_button"
         android:layout_alignLeft="@+id/backface_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="15dp"
+        android:layout_marginBottom="5dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/light_off"
@@ -40,87 +78,93 @@
         android:id="@+id/backface_button"
         android:layout_width="100dp"
         android:layout_height="wrap_content"
-        android:layout_above="@+id/first_person_button"
+        android:layout_above="@+id/camera_button"
-        android:layout_alignLeft="@+id/first_person_button"
+        android:layout_alignRight="@+id/camera_button"
         android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
+        android:layout_marginBottom="15dp"
         android:layout_marginRight="5dp"
         android:paddingRight="5dp"
         android:textOff="@string/backface_off"
         android:textOn="@string/backface_on" />
-      
+ 
-    <ToggleButton
+    <com.introlab.rtabmap.NDSpinner
-        android:id="@+id/first_person_button"
+        android:id="@+id/camera_button"
-        android:layout_width="100dp"
+        android:layout_width="140dp"
-        android:layout_height="wrap_content"
+        android:layout_height="40dp"
-        android:layout_above="@+id/third_person_button"
-        android:layout_alignLeft="@+id/third_person_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/first_person"
-        android:textOn="@string/first_person" />
-    
-    <ToggleButton
-        android:id="@+id/third_person_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_above="@+id/top_down_button"
-        android:layout_alignParentRight="true"
-        android:layout_marginBottom="5dp"
-        android:layout_marginRight="5dp"
-        android:paddingRight="5dp"
-        android:textOff="@string/third_person"
-        android:textOn="@string/third_person" />
-
-    <ToggleButton
-        android:id="@+id/top_down_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
         android:layout_alignParentBottom="true"
         android:layout_alignParentRight="true"
         android:layout_marginRight="5dp"
+        android:layout_marginBottom="10dp"
         android:paddingRight="5dp"
-        android:textOff="@string/top_down"
+        android:paddingBottom="10dp"
-        android:textOn="@string/top_down" />
+        android:text="@string/camera_button"
+        android:spinnerMode="dropdown"/>
 
-    <ToggleButton
-        android:id="@+id/pause_button"
-        android:layout_width="100dp"
-        android:layout_height="wrap_content"
-        android:layout_alignLeft="@+id/first_person_button"
-        android:layout_alignParentTop="true"
-        android:layout_marginTop="61dp"
-        android:layout_alignParentRight="true"
-        android:paddingRight="5dp"
-        android:textOff="@string/pause"
-        android:textOn="@string/resume" />
 
     <Button
         android:id="@+id/button_shareToSketchfab"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
+        android:layout_marginRight="5dp"
-        android:layout_alignRight="@+id/pause_button"
+        android:layout_marginTop="10dp"
+        android:layout_alignParentRight="true"
+        android:layout_below="@+id/pause_button"
         android:text="@string/share_to_sketchfab" />
 
     <Button
         android:id="@+id/button_saveOnDevice"
         android:layout_width="wrap_content"
         android:layout_height="wrap_content"
-        android:layout_alignParentTop="true"
+        android:layout_alignRight="@+id/button_shareToSketchfab"
-        android:layout_toLeftOf="@+id/button_shareToSketchfab"
+        android:layout_below="@+id/button_shareToSketchfab"
         android:text="@string/save_to_file" />
 
     <Button
         android:id="@+id/close_visualization_button"
         android:layout_width="200dp"
         android:layout_height="wrap_content"
-        android:layout_alignBaseline="@+id/top_down_button"
+        android:layout_above="@+id/camera_button"
-        android:layout_alignBottom="@+id/top_down_button"
         android:layout_centerHorizontal="true"
-        android:paddingLeft="5dp"
         android:text="@string/close_visualization" />
-        
+
+    <SeekBar
+        android:id="@+id/seekBar_ortho_cut"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:rotation="270"
+        android:layout_above="@+id/light_button"
+        android:layout_alignParentLeft="true"
+        android:layout_gravity="center"
+        android:layout_marginLeft="-50dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+    
+    <SeekBar
+        android:id="@+id/seekBar_grid"
+        android:layout_width="200dp"
+        android:layout_height="wrap_content"
+        android:layout_alignParentLeft="true"
+        android:layout_alignBottom="@+id/camera_button"
+        android:layout_marginLeft="35dp"
+        android:paddingBottom="10dp"
+        android:progressDrawable="@drawable/custom_seekbar" />
+
+    <Button
+        android:id="@+id/button_library"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_centerHorizontal="true"
+        android:layout_gravity="top"
+        android:layout_marginTop="100dp"
+        android:text="@string/library" />
+
+    <Button
+        android:id="@+id/button_new_scan"
+        android:layout_width="match_parent"
+        android:layout_height="100dp"
+        android:layout_alignLeft="@+id/button_library"
+        android:layout_below="@+id/button_library"
+        android:layout_marginTop="20dp"
+        android:text="@string/new_scan" />
+
+</RelativeLayout>        
 </RelativeLayout>

app/android/res/layout/activity_settings.xml

@@ -1,5 +1,13 @@
 <PreferenceScreen xmlns:android="http://schemas.android.com/apk/res/android">
     
+    <ListPreference 
+            android:key="@string/pref_key_camera_driver"
+    		android:title="@string/pref_title_camera_driver"
+    		android:summary="@string/pref_summary_camera_driver"
+   			android:entries="@array/pref_camera_driver_keys"
+   			android:entryValues="@array/pref_camera_driver_values"
+   			android:defaultValue="@string/pref_default_camera_driver"/>
+        
     <PreferenceCategory
         android:title="@string/pref_title_rendering">
         <ListPreference 
@@ -58,12 +66,12 @@
    			android:entries="@array/pref_background_color_keys"
    			android:entryValues="@array/pref_background_color_values"
    			android:defaultValue="@string/pref_default_background_color"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
             android:key="@string/pref_key_blending"
             android:title="@string/pref_title_blending"
             android:summary="@string/pref_summary_blending"
             android:defaultValue="@string/pref_default_blending"/>
-        <SwitchPreference
+        <com.introlab.rtabmap.CustomSwitchPreference
             android:key="@string/pref_key_nodes_filtering"
             android:title="@string/pref_title_nodes_filtering"
             android:summary="@string/pref_summary_nodes_filtering"
@@ -78,23 +86,28 @@
             android:title="@string/pref_title_mapping_sub"
             android:summary="@string/pref_summary_mapping"
             android:persistent="false">
-        
+                    
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_append"
 	            android:title="@string/pref_title_append"
 	            android:summary="@string/pref_summary_append"
 	            android:defaultValue="@string/pref_default_append"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_resolution"
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
-		    <SwitchPreference
+		     <com.introlab.rtabmap.CustomSwitchPreference
+                android:key="@string/pref_key_depth_from_motion"
+                android:title="@string/pref_title_depth_from_motion"
+                android:summary="@string/pref_summary_depth_from_motion"
+                android:defaultValue="@string/pref_default_depth_from_motion"/>
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"
 		        android:summary="@string/pref_summary_smoothing"
 		        android:defaultValue="@string/pref_default_smoothing"/>
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_fisheye"
 		        android:title="@string/pref_title_fisheye"
 		        android:summary="@string/pref_summary_fisheye"
@@ -109,6 +122,13 @@
 		            android:entries="@array/pref_update_rate_keys"
 		   			android:entryValues="@array/pref_update_rate_values"
 		   			android:defaultValue="@string/pref_default_update_rate"/>
+		        <ListPreference
+		            android:key="@string/pref_key_max_speed"
+		            android:title="@string/pref_title_max_speed"
+		            android:summary="@string/pref_summary_max_speed"
+		            android:entries="@array/pref_max_speed_keys"
+		   			android:entryValues="@array/pref_max_speed_values"
+		   			android:defaultValue="@string/pref_default_max_speed"/>
 		        <ListPreference 
 		            android:key="@string/pref_key_time_thr"
 		    		android:title="@string/pref_title_time_thr"
@@ -179,25 +199,49 @@
 		   			android:entries="@array/pref_optimizer_keys"
 		   			android:entryValues="@array/pref_optimizer_values"
 		   			android:defaultValue="@string/pref_default_optimizer"/>
-		        <SwitchPreference 
+		        <com.introlab.rtabmap.CustomSwitchPreference 
 		            android:key="@string/pref_key_optimize_end"
 		    		android:title="@string/pref_title_optimize_end"
 		    		android:summary="@string/pref_summary_optimize_end"
 		   			android:defaultValue="@string/pref_default_optimize_end"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection"
+		    		android:title="@string/pref_title_marker_detection"
+		    		android:summary="@string/pref_summary_marker_detection"
+		   			android:entries="@array/pref_marker_detection_keys"
+		   			android:entryValues="@array/pref_marker_detection_values"
+		   			android:defaultValue="@string/pref_default_marker_detection"/>
+		   		<ListPreference 
+		            android:key="@string/pref_key_marker_detection_depth_error"
+		    		android:title="@string/pref_title_marker_detection_depth_error"
+		    		android:summary="@string/pref_summary_marker_detection_depth_error"
+		   			android:entries="@array/pref_marker_detection_depth_error_keys"
+		   			android:entryValues="@array/pref_marker_detection_depth_error_values"
+		   			android:defaultValue="@string/pref_default_marker_detection_depth_error"/>
 		    </PreferenceCategory>
 	        <PreferenceCategory
 	        	android:title="@string/pref_title_mapping_database">
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_keep_all_db"
 		            android:title="@string/pref_title_keep_all_db"
 		            android:summary="@string/pref_summary_keep_all_db"
 		            android:defaultValue="@string/pref_default_keep_all_db"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_raw_scan_saved"
 		            android:title="@string/pref_title_raw_scan_saved"
 		            android:summary="@string/pref_summary_raw_scan_saved"
 		            android:defaultValue="@string/pref_default_raw_scan_saved"/>
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_gps_saved"
+		            android:title="@string/pref_title_gps_saved"
+		            android:summary="@string/pref_summary_gps_saved"
+		            android:defaultValue="@string/pref_default_gps_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
+		            android:key="@string/pref_key_env_sensors_saved"
+		            android:title="@string/pref_title_env_sensors_saved"
+		            android:summary="@string/pref_summary_env_sensors_saved"
+		            android:defaultValue="@string/pref_default_env_sensors_saved"/>
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_db_in_memory"
 		            android:title="@string/pref_title_db_in_memory"
 		            android:summary="@string/pref_summary_db_in_memory"
@@ -227,6 +271,14 @@
 	   			android:entryValues="@array/pref_texture_size_values"
 	   			android:defaultValue="@string/pref_default_texture_size"/>
 	        
+	        <ListPreference 
+	            android:key="@string/pref_key_texture_count"
+	    		android:title="@string/pref_title_texture_count"
+	    		android:summary="@string/pref_summary_texture_count"
+	   			android:entries="@array/pref_texture_count_keys"
+	   			android:entryValues="@array/pref_texture_count_values"
+	   			android:defaultValue="@string/pref_default_texture_count"/>
+	        
 	        <ListPreference 
 	            android:key="@string/pref_key_normal_k"
 	    		android:title="@string/pref_title_normal_k"
@@ -251,7 +303,7 @@
 	   			android:entryValues="@array/pref_min_texture_cluster_size_values"
 	   			android:defaultValue="@string/pref_default_min_texture_cluster_size"/>
 	        
-		    <SwitchPreference
+		    <com.introlab.rtabmap.CustomSwitchPreference
 	            android:key="@string/pref_key_block_render"
 	            android:title="@string/pref_title_block_render"
 	            android:summary="@string/pref_summary_block_render"
@@ -278,16 +330,23 @@
 		   			android:entryValues="@array/pref_opt_color_radius_values"
 		   			android:defaultValue="@string/pref_default_opt_color_radius"/>
 		        
-		        <SwitchPreference
+		        <com.introlab.rtabmap.CustomSwitchPreference
 		            android:key="@string/pref_key_opt_clean_white"
 		            android:title="@string/pref_title_opt_clean_white"
 		            android:summary="@string/pref_summary_opt_clean_white"
 		            android:defaultValue="@string/pref_default_opt_clean_white"/>
 		        
+		        <ListPreference 
+		            android:key="@string/pref_key_opt_min_cluster_size"
+		    		android:title="@string/pref_title_opt_min_cluster_size"
+		    		android:summary="@string/pref_summary_opt_min_cluster_size"
+		   			android:entries="@array/pref_opt_min_cluster_size_keys"
+		   			android:entryValues="@array/pref_opt_min_cluster_size_values"
+		   			android:defaultValue="@string/pref_default_opt_min_cluster_size"/>
+		        
 		       </PreferenceCategory>
 	 	</PreferenceScreen>
-        
+	 	<ListPreference 
-        <ListPreference 
             android:key="@string/pref_key_gain_max_radius"
     		android:title="@string/pref_title_gain_max_radius"
     		android:summary="@string/pref_summary_gain_max_radius"
@@ -301,7 +360,20 @@
    			android:entries="@array/pref_cluster_ratio_keys"
    			android:entryValues="@array/pref_cluster_ratio_values"
    			android:defaultValue="@string/pref_default_cluster_ratio"/>
-        
+        <com.introlab.rtabmap.CustomSwitchPreference 
+            android:key="@string/pref_key_notification_sound"
+    		android:title="@string/pref_title_notification_sound"
+    		android:summary="@string/pref_summary_notification_sound"
+   			android:defaultValue="@string/pref_default_notification_sound"/>
+    </PreferenceCategory>
+	<PreferenceCategory
+        android:title="@string/pref_title_presets">
+        <Preference android:title="@string/pref_title_open_button"
+	                android:key="@string/pref_key_open_button"/>
+        <Preference android:title="@string/pref_title_save_button"
+	                android:key="@string/pref_key_save_button"/>
+        <Preference android:title="@string/pref_title_remove_button"
+	                android:key="@string/pref_key_remove_button"/>
 	    <Preference android:title="@string/pref_title_reset_button"
 	                android:key="@string/pref_key_reset_button"/>
     </PreferenceCategory>

app/android/res/layout/activity_sketchfab.xml

@@ -62,20 +62,11 @@
         android:checked="true"
         android:text="Draft Mode" />
 
-    <LinearLayout
+    <Button
+        android:id="@+id/button_ok"
         android:layout_width="match_parent"
-        android:layout_height="0dip"
+        android:layout_height="wrap_content"
-        android:layout_weight="1"
+        android:text="Upload" />
-        android:gravity="right|bottom"
-        android:orientation="horizontal" >
-
-        <Button
-            android:id="@+id/button_ok"
-            android:layout_width="wrap_content"
-            android:layout_height="wrap_content"
-            android:layout_weight="0.06"
-            android:text="Upload" />
-    </LinearLayout>
 
 </LinearLayout>
 </ScrollView>

app/android/res/layout/database_list.xml

@@ -0,0 +1,22 @@
+<?xml version="1.0" encoding="utf-8"?>
+<LinearLayout xmlns:android="http://schemas.android.com/apk/res/android"
+android:layout_width="match_parent"
+android:layout_height="wrap_content"
+android:background="#fff">
+
+<ImageView
+android:id="@+id/imageView"
+android:layout_width="@dimen/image_width"
+android:layout_height="@dimen/image_width"
+android:layout_marginLeft="10dp"
+android:padding="5dp"
+android:src="@drawable/ic_launcher" />
+
+<TextView
+android:id="@+id/textView"
+android:layout_width="wrap_content"
+android:layout_height="wrap_content"
+android:layout_gravity="center"
+android:text="Demo"
+android:textColor="#000" />
+</LinearLayout>

app/android/res/menu/optionmenu.xml

@@ -7,9 +7,12 @@
 		  <item android:id="@+id/mesh" android:title="Mesh" />
 		  <item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
 	   </group>
-	   <item android:id="@+id/save" android:title="Save" android:showAsAction="ifRoom"/>
+	   
+       <item android:id="@+id/new_scan" android:title="New Scan"/>
+       <item android:id="@+id/open" android:title="Library" android:showAsAction="ifRoom"/>
+	   <item android:id="@+id/save" android:title="Save"/>
                      
-       <item android:id="@+id/export" android:showAsAction="ifRoom" android:title="Export">
+       <item android:id="@+id/export" android:title="Export">
           <menu>
               <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
                   <menu>
@@ -26,7 +29,7 @@
           </menu>
        </item>
        
-        <item android:id="@+id/post_processing" android:title="Optimize" android:showAsAction="ifRoom">
+        <item android:id="@+id/post_processing" android:title="Optimize">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
              <item android:id="@+id/post_processing_advanced" android:title="Advanced..." >
@@ -43,8 +46,6 @@
           </menu>
        </item>
        
-       <item android:id="@+id/open" android:title="Open" android:showAsAction="ifRoom"/>
-       
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">
@@ -68,7 +69,7 @@
 	       </menu>
 	   </item>
 	   <item android:id="@+id/settings" android:title="Settings"/>
-       <item android:id="@+id/reset" android:title="Reset"/>
+       <item android:id="@+id/resume" android:title="Resume"/>
        <item android:id="@+id/about" android:title="About"/>
     </group>
 </menu>

app/android/res/values/arrays.xml

@@ -0,0 +1,9 @@
+
+    <resources>
+        <string-array name="camera_view_array">
+        <item>First View</item>
+        <item>Third-P. View</item>
+        <item>Top View</item>
+        <item>Ortho View</item>
+    </string-array>
+    </resources>

app/android/res/values/dimens.xml

@@ -0,0 +1,3 @@
+<resources>
+    <dimen name="image_width">150dp</dimen>
+</resources>

app/android/res/values/strings.xml

@@ -8,15 +8,16 @@
     <string name="sketchfab">Upload to Sketchfab&#8230;</string>
     <string name="status">"Status: "</string>
     <string name="words">"Words: "</string>
-    <string name="first_person">First</string>
+    <string name="camera_button">First View</string>
-    <string name="third_person">Third</string>
+    <string name="library">Library</string>
-    <string name="top_down">Top</string>
+    <string name="new_scan">New Scan</string>
-    <string name="pause">Pause</string>
+    <string name="pause">Stop</string>
-    <string name="resume">Resume</string>
+    <string name="resume">Start</string>
     <string name="backface_on">Backface</string>
     <string name="backface_off">Backface</string>
     <string name="light_on">Lighting</string>
     <string name="light_off">Lighting</string>
+    <string name="wireframe">Wireframe</string>
     <string name="close_visualization">Close Visualization</string>
     <string name="save_to_file">Export to File&#8230;</string>
     <string name="share_to_sketchfab">Share to Sketchfab&#8230;</string>
@@ -35,12 +36,19 @@
     <string name="memory">"Used Memory (MB): "</string>
     <string name="hypothesis">"Hypothesis (%): "</string>
     <string name="fps">"FPS (rendering): "</string>
-
+    <string name="distance">"Distance travelled: "</string>
+    <string name="gps">"GPS (long,lat,alt,bearing,err): "</string>
+    <string name="env_sensors">"Sensors: "</string>
+    <string name="time">"Time: "</string>
+    
     <!-- Preference keys: BEGIN -->
     <string name="pref_key_tags">pref_key_tags</string>
     <string name="pref_default_tags">rtabmap 3dscan tango</string>
     <string name="pref_key_rendering">pref_key_rendering</string>
     <string name="pref_default_rendering">2</string>
+    <string name="pref_key_open_button">pref_key_open_button</string>
+    <string name="pref_key_save_button">pref_key_save_button</string>
+    <string name="pref_key_remove_button">pref_key_remove_button</string>
     <string name="pref_key_reset_button">pref_key_reset_button</string>
     <string name="pref_key_density">pref_key_density</string>
     <string name="pref_default_density">1</string>
@@ -49,7 +57,7 @@
     <string name="pref_key_depth">pref_key_depth</string>
     <string name="pref_default_depth">2.5</string>
     <string name="pref_key_point_size">pref_key_point_size</string>
-    <string name="pref_default_point_size">5</string>
+    <string name="pref_default_point_size">10</string>
     <string name="pref_key_angle">pref_key_angle</string>
     <string name="pref_default_angle">20</string>
     <string name="pref_key_triangle">pref_key_triangle</string>
@@ -57,7 +65,7 @@
     <string name="pref_key_rendering_texture_decimation">pref_key_rendering_texture_decimation</string>
     <string name="pref_default_rendering_texture_decimation">4</string>
     <string name="pref_key_blending">pref_key_blending</string>
-    <string name="pref_default_blending">true</string>
+    <string name="pref_default_blending">false</string>
     <string name="pref_key_background_color">pref_key_background_color</string>
     <string name="pref_default_background_color">0.2</string>
     <string name="pref_key_nodes_filtering">pref_key_nodes_filtering</string>
@@ -71,8 +79,14 @@
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
+    <string name="pref_key_camera_driver">pref_key_camera_driver</string>
+    <string name="pref_default_camera_driver">0</string>
+    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
+    <string name="pref_default_depth_from_motion">false</string>
     <string name="pref_key_update_rate">pref_key_update_rate</string>
     <string name="pref_default_update_rate">1</string>
+    <string name="pref_key_max_speed">pref_key_max_speed</string>
+    <string name="pref_default_max_speed">0</string>
     <string name="pref_key_time_thr">pref_key_time_thr</string>
     <string name="pref_default_time_thr">1000</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
@@ -84,7 +98,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">0.1</string>
+    <string name="pref_default_opt_error">3</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -95,17 +109,27 @@
     <string name="pref_default_optimizer">2</string>
     <string name="pref_key_optimize_end">pref_key_optimize_end</string>
     <string name="pref_default_optimize_end">true</string>
+    <string name="pref_key_marker_detection">pref_key_marker_detection</string>
+    <string name="pref_default_marker_detection">-1</string>
+    <string name="pref_key_marker_detection_depth_error">pref_key_marker_detection_depth_error</string>
+    <string name="pref_default_marker_detection_depth_error">0.1</string>
     <string name="pref_key_keep_all_db">pref_key_keep_all_db</string>
     <string name="pref_default_keep_all_db">true</string>
     <string name="pref_key_raw_scan_saved">pref_key_raw_scan_saved</string>
     <string name="pref_default_raw_scan_saved">false</string>
+    <string name="pref_key_gps_saved">pref_key_gps_saved</string>
+    <string name="pref_default_gps_saved">false</string>
+    <string name="pref_key_env_sensors_saved">pref_key_env_sensors_saved</string>
+    <string name="pref_default_env_sensors_saved">false</string>
     <string name="pref_key_db_in_memory">pref_key_db_in_memory</string>
-    <string name="pref_default_db_in_memory">true</string>
+    <string name="pref_default_db_in_memory">false</string>
     
     <string name="pref_key_cloud_voxel">pref_key_cloud_voxel</string>
     <string name="pref_default_cloud_voxel">0.01</string>
     <string name="pref_key_texture_size">pref_key_texture_size</string>
     <string name="pref_default_texture_size">4096</string>
+    <string name="pref_key_texture_count">pref_key_texture_count</string>
+    <string name="pref_default_texture_count">1</string>
     <string name="pref_key_normal_k">pref_key_normal_k</string>
     <string name="pref_default_normal_k">18</string>
     <string name="pref_key_max_texture_distance">pref_key_max_texture_distance</string>
@@ -120,10 +144,14 @@
     <string name="pref_default_opt_color_radius">0.05</string>
     <string name="pref_key_opt_clean_white">pref_key_opt_clean_white</string>
     <string name="pref_default_opt_clean_white">true</string>
+    <string name="pref_key_opt_min_cluster_size">pref_key_opt_min_cluster_size</string>
+    <string name="pref_default_opt_min_cluster_size">0</string>
     <string name="pref_key_gain_max_radius">pref_key_gain_max_radius</string>
     <string name="pref_default_gain_max_radius">0.02</string>
     <string name="pref_key_cluster_ratio">pref_key_cluster_ratio</string>
     <string name="pref_default_cluster_ratio">0.05</string>
+    <string name="pref_key_notification_sound">pref_key_notification_sound</string>
+    <string name="pref_default_notification_sound">true</string>
     <!-- Preference keys: END -->
 
     <string name="pref_title_rendering">Rendering</string>
@@ -295,6 +323,10 @@
     <string name="pref_summary_mapping">Advanced mapping parameters for fine tuning.</string>
     <string name="pref_title_mapping_core">Core</string>
     <string name="pref_title_mapping_database">Database</string>
+    <string name="pref_title_camera_driver">Camera Driver</string>
+    <string name="pref_summary_camera_driver">AR sdk use for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
+    <string name="pref_title_depth_from_motion">Depth From Motion</string>
+    <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>
@@ -302,9 +334,11 @@
     <string name="pref_title_smoothing">Smoothing</string>
     <string name="pref_summary_smoothing">Smooth the point clouds.</string>
     <string name="pref_title_fisheye">Fish Eye Camera</string>
-    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. Cannot be used on Yellowstone tablet.</string>
+    <string name="pref_summary_fisheye">Use fish eye camera instead of the color camera. May not work on some devices.</string>
     <string name="pref_title_update_rate">Update Rate</string>
     <string name="pref_summary_update_rate">Rate at which a new node is added to map.</string>
+    <string name="pref_title_max_speed">Maximum Motion Speed</string>
+    <string name="pref_summary_max_speed">Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
     <string name="pref_title_time_thr">Time Limit</string>
     <string name="pref_summary_time_thr">Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates.</string>
     <string name="pref_title_mem_thr">Memory Limit</string>
@@ -316,7 +350,7 @@
     <string name="pref_title_min_inliers">Min Inliers</string>
     <string name="pref_summary_min_inliers">Minimum visual inliers to accept a loop closure.</string>
     <string name="pref_title_opt_error">Max Optimization Error</string>
-    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this threshold.</string>
+    <string name="pref_summary_opt_error">Reject any loop closures causing error corrections in the map higher than this factor of the link\'s variance.</string>
     <string name="pref_title_features_voc">Max Features Extracted (Vocabulary)</string>
     <string name="pref_summary_features_voc">Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
     <string name="pref_title_features">Max Features Extracted (Loop Closure)</string>
@@ -327,13 +361,33 @@
     <string name="pref_summary_optimizer">Graph optimization approach.</string>
     <string name="pref_title_optimize_end">Optimization from Graph End</string>
     <string name="pref_summary_optimize_end">The map\'s graph is optimized from the last node. The map is moved when a loop closure happens instead of jumping the current pose back to localized area. </string>
+    <string name="pref_title_marker_detection">ArUco Marker Detection</string>
+    <string name="pref_summary_marker_detection">ArUco markers can be detected for localization and graph optimization.</string>
+    <string name="pref_title_marker_detection_depth_error">Marker Depth Error Estimation</string>
+    <string name="pref_summary_marker_detection_depth_error">Size of markers are automatically initialized on the first marker seen. All markers should have the same size. This value is the maximum depth error to do the initialization to get accurate size of the tag. The lower it is, the more perpendicular the camera should be from the marker to do initialization, but size estimated would be more accurate.</string>
     <string name="pref_title_keep_all_db">Save All Frames in Database</string>
     <string name="pref_summary_keep_all_db">Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
     <string name="pref_title_raw_scan_saved">Save Raw Scan</string>
     <string name="pref_summary_raw_scan_saved">Save raw point clouds in database.</string>
+    <string name="pref_title_gps_saved">Save GPS</string>
+    <string name="pref_summary_gps_saved">Save GPS to database.</string>
+    <string name="pref_title_env_sensors_saved">Save Environmental Sensors</string>
+    <string name="pref_summary_env_sensors_saved">Save Wifi strength, temperature, air pressure, light intensity and relative humidity to database.</string>
     <string name="pref_title_db_in_memory">Database In Memory</string>
     <string name="pref_summary_db_in_memory">The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened.</string>
 
+    <string-array name="pref_camera_driver_keys">
+        <item>"Google Tango NDK"</item>
+        <item>"ARCore NDK"</item>
+        <item>"AREngine NDK"</item>
+        <item>"ARCore Java"</item>
+    </string-array>
+    <string-array name="pref_camera_driver_values">
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+    </string-array>
     <string-array name="pref_update_rate_keys">
         <item>"Max"</item>
         <item>"5 Hz"</item>
@@ -352,6 +406,20 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_max_speed_keys">
+        <item>"No Limit"</item>
+        <item>"High"</item>
+        <item>"Medium"</item>
+        <item>"Low"</item>
+        <item>"Very Low"</item>
+    </string-array>
+    <string-array name="pref_max_speed_values">
+        <item>"0"</item>
+        <item>"0.4"</item>
+        <item>"0.3"</item>
+        <item>"0.2"</item>
+        <item>"0.1"</item>
+    </string-array>
     <string-array name="pref_time_thr_keys">
         <item>"No Limit"</item>
         <item>"1500 ms"</item>
@@ -461,25 +529,29 @@
         <item>"10"</item>
     </string-array>
     <string-array name="pref_opt_error_keys">
-        <item>"1.0 m"</item>
+        <item>"10x"</item>
-        <item>"0.5 m"</item>
+        <item>"9x"</item>
-        <item>"0.35 m"</item>
+        <item>"8x"</item>
-        <item>"0.2 m"</item>
+        <item>"7x"</item>
-        <item>"0.1 m"</item>
+        <item>"6x"</item>
-        <item>"0.05 m"</item>
+        <item>"5x"</item>
-        <item>"0.025 m"</item>
+        <item>"4x"</item>
-        <item>"0.01 m"</item>
+        <item>"3x"</item>
+        <item>"2x"</item>
+        <item>"1x"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_error_values">
-        <item>"1.0"</item>
+        <item>"10"</item>
-        <item>"0.5"</item>
+        <item>"9"</item>
-        <item>"0.35"</item>
+        <item>"8"</item>
-        <item>"0.2"</item>
+        <item>"7"</item>
-        <item>"0.1"</item>
+        <item>"6"</item>
-        <item>"0.05"</item>
+        <item>"5"</item>
-        <item>"0.025"</item>
+        <item>"4"</item>
-        <item>"0.01"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
         <item>"0"</item>
     </string-array>
     <string-array name="pref_features_voc_keys">
@@ -554,6 +626,91 @@
         <item>"1"</item>
         <item>"0"</item>
     </string-array>
+    <string-array name="pref_marker_detection_keys">
+        <item>"Disabled"</item>
+        <item>"4X4_50"</item>
+        <item>"4X4_100"</item>
+        <item>"4X4_250"</item>
+        <item>"4X4_1000"</item>
+        <item>"5X5_50"</item>
+        <item>"5X5_100"</item>
+        <item>"5X5_250"</item>
+        <item>"5X5_1000"</item>
+        <item>"6X6_50"</item>
+        <item>"6X6_100"</item>
+        <item>"6X6_250"</item>
+        <item>"6X6_1000"</item>
+        <item>"7X7_50"</item>
+        <item>"7X7_100"</item>
+        <item>"7X7_250"</item>
+        <item>"7X7_1000"</item>
+        <item>"ARUCO_ORIGINAL"</item>
+        <item>"APRILTAG_16h5"</item>
+        <item>"APRILTAG_25h9"</item>
+        <item>"APRILTAG_36h10"</item>
+        <item>"APRILTAG_36h11"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+        <item>"1"</item>
+        <item>"2"</item>
+        <item>"3"</item>
+        <item>"4"</item>
+        <item>"5"</item>
+        <item>"6"</item>
+        <item>"7"</item>
+        <item>"8"</item>
+        <item>"9"</item>
+        <item>"10"</item>
+        <item>"11"</item>
+        <item>"12"</item>
+        <item>"13"</item>
+        <item>"14"</item>
+        <item>"15"</item>
+        <item>"16"</item>
+        <item>"17"</item>
+        <item>"18"</item>
+        <item>"19"</item>
+        <item>"20"</item>
+    </string-array>
+    
+    <string-array name="pref_marker_detection_depth_error_keys">
+        <item>"1 cm"</item>
+        <item>"2 cm"</item>
+        <item>"3 cm"</item>
+        <item>"4 cm"</item>
+        <item>"5 cm"</item>
+        <item>"6 cm"</item>
+        <item>"7 cm"</item>
+        <item>"8 cm"</item>
+        <item>"9 cm"</item>
+        <item>"10 cm"</item>
+        <item>"15 cm"</item>
+        <item>"20 cm"</item>
+        <item>"30 cm"</item>
+        <item>"40 cm"</item>
+        <item>"50 cm"</item>
+        <item>"100 cm"</item>
+    </string-array>
+    <string-array name="pref_marker_detection_depth_error_values">
+        <item>"0.01"</item>
+        <item>"0.02"</item>
+        <item>"0.03"</item>
+        <item>"0.04"</item>
+        <item>"0.05"</item>
+        <item>"0.06"</item>
+        <item>"0.07"</item>
+        <item>"0.08"</item>
+        <item>"0.09"</item>
+        <item>"0.1"</item>
+        <item>"0.15"</item>
+        <item>"0.20"</item>
+        <item>"0.30"</item>
+        <item>"0.40"</item>
+        <item>"0.50"</item>
+        <item>"1"</item>
+    </string-array>
 
     <string name="pref_title_export_sub">Exporting&#8230;</string>
     <string name="pref_title_export">Exporting</string>
@@ -562,6 +719,8 @@
     <string name="pref_summary_cloud_voxel">If you don\'t need a very precise point cloud, you can set this to reduce the output point cloud size. This is also used for optimized mesh.</string>
     <string name="pref_title_texture_size">Texture Size</string>
     <string name="pref_summary_texture_size">If the map is large, you may want to increase this to maximize the texture resolution.</string>
+    <string name="pref_title_texture_count">Maximum Output Textures</string>
+    <string name="pref_summary_texture_count">Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
     <string name="pref_title_normal_k">Normal K</string>
     <string name="pref_summary_normal_k">K-nearest neighbors used for normal computation when a mesh is created.</string>
     <string name="pref_title_max_texture_distance">Max Texture Distance</string>
@@ -603,6 +762,26 @@
         <item>"2048"</item>
         <item>"1024"</item>
     </string-array>
+    <string-array name="pref_texture_count_keys">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
+    <string-array name="pref_texture_count_values">
+        <item>"8"</item>
+        <item>"7"</item>
+        <item>"6"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+    </string-array>
     <string-array name="pref_normal_k_values">
         <item>"30"</item>
         <item>"24"</item>
@@ -631,22 +810,22 @@
         <item>"2"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_keys">
-        <item>"Disabled"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_min_texture_cluster_size_values">
-        <item>"0"</item>
         <item>"1000"</item>
         <item>"500"</item>
         <item>"200"</item>
         <item>"100"</item>
         <item>"50"</item>
         <item>"10"</item>
+        <item>"0"</item>
     </string-array>
     
     <string name="pref_title_optimized">Optimized</string>
@@ -658,7 +837,9 @@
     <string name="pref_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When exporting with texture, if Clean Mesh is also enabled, this will limit the number of polygons textured in holes.</string>
     <string name="pref_title_opt_clean_white">Clean Mesh</string>
     <string name="pref_summary_opt_clean_white">Clean mesh from textureless or colorless reconstructed polygons.</string>
-
+	<string name="pref_summary_opt_min_cluster_size">This can be used to filter polygons before texturing.</string>
+    <string name="pref_title_opt_min_cluster_size">Polygon Filtering</string>
+    
     <string-array name="pref_opt_depth_keys">
         <item>"Auto"</item>
         <item>"12"</item>
@@ -707,12 +888,22 @@
         <item>"0.01"</item>
         <item>"-1"</item>
     </string-array>
+    <string-array name="pref_opt_min_cluster_size_keys">
+        <item>"Only biggest cluster kept"</item>
+        <item>"Keep all polygons"</item>
+    </string-array>
+    <string-array name="pref_opt_min_cluster_size_values">
+        <item>"-1"</item>
+        <item>"0"</item>
+    </string-array>
 
     <string name="pref_title_general">General</string>
     <string name="pref_title_gain_max_radius">Color Correction Radius</string>
     <string name="pref_summary_gain_max_radius">Radius used to find pixel correspondences for Adjust Colors optimization.</string>
     <string name="pref_title_cluster_ratio">Noise Filtering Ratio</string>
     <string name="pref_summary_cluster_ratio">Polygon clusters with size smaller than this ratio of the largest cluster are removed by the Noise Filtering optimization.</string>
+    <string name="pref_title_notification_sound">Notification Sound</string>
+    <string name="pref_summary_notification_sound">After saving database or preparing data to export, a notification sound is played.</string>
 
     <string-array name="pref_gain_max_radius_keys">
         <item>"0.3 m"</item>
@@ -763,6 +954,10 @@
         <item>"0.01"</item>
     </string-array>
 
+    <string name="pref_title_presets">Presets</string>
+    <string name="pref_title_open_button">Open</string>
+    <string name="pref_title_save_button">Save</string>
+    <string name="pref_title_remove_button">Remove</string>
     <string name="pref_title_reset_button">Restore All Default Settings</string>
     <string name="title_activity_sketchfab">SketchfabActivity</string>
     <string name="hello_world">Hello world!</string>

app/android/res/values/styles.xml

@@ -0,0 +1,10 @@
+<resources>
+    <style name="ThemeActionBar" parent="@android:style/Widget.DeviceDefault.ActionBar.Solid">
+        <item name="android:background">#20000000</item>
+    </style>
+
+    <style name="ThemeApp" parent="@android:style/Theme.DeviceDefault">
+        <item name="android:actionBarStyle">@style/ThemeActionBar</item>
+        <item name="android:windowActionBarOverlay">true</item>
+    </style>
+</resources>

app/android/res/xml/provider_paths.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="utf-8"?>
+<paths xmlns:android="http://schemas.android.com/apk/res/android">
+    <external-path name="external_files" path="."/>
+</paths>

app/android/src/com/introlab/rtabmap/CustomSwitchPreference.java

@@ -0,0 +1,82 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.preference.SwitchPreference;
+import android.util.AttributeSet;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.Switch;
+
+/**
+ * 
+ * @author mathieu
+ * Bug fix of switch preferences changing states 
+ * when scrolling on Jelly Bean: 
+ * https://issuetracker.google.com/issues/36941388#comment4
+ */
+
+public class CustomSwitchPreference extends SwitchPreference { 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     * @param defStyle Theme attribute defining the default style options 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs, int defStyle) { 
+        super(context, attrs, defStyle); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with the given style options.
+     * 
+     * @param context The Context that will style this preference 
+     * @param attrs Style attributes that differ from the default 
+     */ 
+    public CustomSwitchPreference(Context context, AttributeSet attrs) { 
+        super(context, attrs); 
+    } 
+
+    /** 
+     * Construct a new SwitchPreference with default style options. 
+     * 
+     * @param context The Context that will style this preference 
+     */ 
+    public CustomSwitchPreference(Context context) { 
+        super(context, null); 
+    } 
+
+    @Override 
+    protected void onBindView(View view) { 
+        // Clean listener before invoke SwitchPreference.onBindView 
+        ViewGroup viewGroup= (ViewGroup)view; 
+        clearListenerInViewGroup(viewGroup); 
+        super.onBindView(view); 
+    } 
+
+    /** 
+     * Clear listener in Switch for specify ViewGroup. 
+     * 
+     * @param viewGroup The ViewGroup that will need to clear the listener. 
+     */ 
+    private void clearListenerInViewGroup(ViewGroup viewGroup) { 
+        if (null == viewGroup) { 
+            return; 
+        } 
+
+        int count = viewGroup.getChildCount(); 
+        for(int n = 0; n < count; ++n) { 
+            View childView = viewGroup.getChildAt(n); 
+            if(childView instanceof Switch) { 
+                final Switch switchView = (Switch) childView; 
+                switchView.setOnCheckedChangeListener(null); 
+                return; 
+            } else if (childView instanceof ViewGroup){ 
+                ViewGroup childGroup = (ViewGroup)childView; 
+                clearListenerInViewGroup(childGroup); 
+            } 
+        } 
+    } 
+
+}

app/android/src/com/introlab/rtabmap/DatabaseListArrayAdapter.java

@@ -0,0 +1,102 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.database.Cursor;
+import android.database.sqlite.SQLiteDatabase;
+import android.graphics.Bitmap;
+import android.graphics.BitmapFactory;
+import android.graphics.drawable.Drawable;
+import android.util.Log;
+import android.view.LayoutInflater;
+import android.view.View;
+import android.view.ViewGroup;
+import android.widget.BaseAdapter;
+import android.widget.ImageView;
+import android.widget.LinearLayout;
+import android.widget.SimpleAdapter;
+
+import java.io.ByteArrayInputStream;
+import java.io.InputStream;
+import java.util.ArrayList;
+import java.util.HashMap;
+
+public class DatabaseListArrayAdapter extends SimpleAdapter {
+	LayoutInflater inflater;
+	Context context;
+	ArrayList<HashMap<String, String>> arrayList;
+	int imageWidth;
+
+	public DatabaseListArrayAdapter(Context context, ArrayList<HashMap<String, String>> data, int resource, String[] from, int[] to) {
+		super(context, data, resource, from, to);
+		this.context = context;
+		this.arrayList = data;
+		this.imageWidth = (int)context.getResources().getDimension(R.dimen.image_width);
+		inflater.from(context);
+	}
+
+	@Override
+	public View getView(final int position, View convertView, ViewGroup parent) {
+		View view = super.getView(position, convertView, parent);
+		ImageView imageView = (ImageView) view.findViewById(R.id.imageView);
+		
+		boolean imageSet = false;
+		String path = this.arrayList.get(position).get("path");
+		if(!path.isEmpty())
+		{
+			SQLiteDatabase db = null;
+			try {
+				db = SQLiteDatabase.openDatabase(path, null, SQLiteDatabase.OPEN_READONLY);
+				
+				// get version
+				Cursor c1 = db.rawQuery("SELECT version FROM Admin", null);    
+		        if(c1.moveToFirst()) {
+		        	String version = c1.getString(c1.getColumnIndex("version"));
+		        	Log.i(RTABMapActivity.TAG, "Version="+version);
+		        	if(Util.versionCompare(version, "0.12.0") >= 0) {
+		        		Cursor c2 = db.rawQuery("SELECT preview_image FROM Admin WHERE preview_image is not null", null); 
+		        		if(c2.moveToFirst()) {
+		        			Log.i(RTABMapActivity.TAG, "Found image preview for db " + path);
+
+		        			byte[] bytes = c2.getBlob(c2.getColumnIndex("preview_image"));
+		        			ByteArrayInputStream inputStream = new ByteArrayInputStream(bytes);
+		        			Bitmap bitmap = BitmapFactory.decodeStream(inputStream);
+		        			imageView.setImageBitmap(bitmap);
+		        			imageSet = true;
+		        		}
+		        		else {
+		        			Log.i(RTABMapActivity.TAG, "Not found image preview for db " + path);
+		        		}
+		        	}
+		        	else {
+		        		Log.i(RTABMapActivity.TAG, "Too old database for preview image, path = " + path);
+		        	}
+		        }
+		        else {
+		        	Log.e(RTABMapActivity.TAG, "Failed getting version from database");
+		        }
+	
+	        } catch (Exception e) {
+	            Log.e(RTABMapActivity.TAG, e.getMessage());
+	        }
+			finally {
+				if(db != null && db.isOpen()) {
+	            	db.close();
+	            }
+			}
+		}
+		else
+		{
+			Log.e(RTABMapActivity.TAG, "Database path empty for item " + position);
+		}
+		
+		if(!imageSet)
+		{
+			Drawable myDrawable = context.getResources().getDrawable(R.drawable.ic_launcher);
+			imageView.setImageDrawable(myDrawable);
+		}
+		LinearLayout.LayoutParams layoutParams = new LinearLayout.LayoutParams(imageWidth,imageWidth/(!imageSet?2:1));
+		imageView.setLayoutParams(layoutParams);
+		return view;
+	}
+
+}

app/android/src/com/introlab/rtabmap/NDSpinner.java

@@ -0,0 +1,43 @@
+package com.introlab.rtabmap;
+
+import android.content.Context;
+import android.util.AttributeSet;
+import android.widget.Spinner;
+
+
+/** Spinner extension that calls onItemSelected even when the selection is the same as its previous value 
+Author: Mattia Ruggiero 
+https://stackoverflow.com/questions/5335306/how-can-i-get-an-event-in-android-spinner-when-the-current-selected-item-is-sele
+*/
+public class NDSpinner extends Spinner {
+
+    public NDSpinner(Context context)
+    { super(context); }
+
+    public NDSpinner(Context context, AttributeSet attrs)
+    { super(context, attrs); }
+
+    public NDSpinner(Context context, AttributeSet attrs, int defStyle)
+    { super(context, attrs, defStyle); }
+
+    @Override 
+    public void setSelection(int position, boolean animate) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position, animate);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            //getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    } 
+
+    @Override
+    public void setSelection(int position) {
+        boolean sameSelected = position == getSelectedItemPosition();
+        super.setSelection(position);
+        if (sameSelected) {
+            // Spinner does not call the OnItemSelectedListener if the same item is selected, so do it manually now
+            getOnItemSelectedListener().onItemSelected(this, getSelectedView(), position, getSelectedItemId());
+        }
+    }
+
+}

app/android/src/com/introlab/rtabmap/PermissionHelper.java

@@ -0,0 +1,79 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+package com.introlab.rtabmap;
+
+import android.Manifest;
+
+import android.app.Activity;
+import android.content.Intent;
+import android.content.pm.PackageManager;
+import android.net.Uri;
+import android.provider.Settings;
+import android.support.v4.app.ActivityCompat;
+import android.support.v4.content.ContextCompat;
+
+/** Helper to ask camera permission. */
+public class PermissionHelper {
+	public static final int CAMERA_CODE = 0;
+	public static final int READ_EXTERNAL_STORAGE_CODE = 1;
+	public static final int WRITE_EXTERNAL_STORAGE_CODE = 2;
+	public static final int INTERNET_CODE = 3;
+	public static final int ACCESS_NETWORK_STATE_CODE = 4;
+	public static final int ACCESS_FINE_LOCATION_CODE = 5;
+	public static final int ACCESS_WIFI_STATE_CODE = 6;
+
+	/** Check to see we have the necessary permissions for this app. */
+	public static boolean hasPermission(Activity activity, String permission) {
+		return ContextCompat.checkSelfPermission(activity, permission) == PackageManager.PERMISSION_GRANTED;
+	}
+
+	/** Check to see we have the necessary permissions for this app, and ask for them if we don't. */
+	public static void requestPermission(Activity activity, String permission) {
+		int requestCode = -1;
+		if (permission == Manifest.permission.CAMERA) {
+			requestCode = CAMERA_CODE;
+		} else if(permission == Manifest.permission.READ_EXTERNAL_STORAGE) {
+			requestCode = READ_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.WRITE_EXTERNAL_STORAGE) {
+			requestCode = WRITE_EXTERNAL_STORAGE_CODE;
+		} else if(permission == Manifest.permission.INTERNET) {
+			requestCode = INTERNET_CODE;
+		} else if(permission == Manifest.permission.ACCESS_NETWORK_STATE) {
+			requestCode = ACCESS_NETWORK_STATE_CODE;
+		} else if(permission == Manifest.permission.ACCESS_FINE_LOCATION) {
+			requestCode = ACCESS_FINE_LOCATION_CODE;
+		} else if(permission == Manifest.permission.ACCESS_WIFI_STATE) {
+			requestCode = ACCESS_WIFI_STATE_CODE;
+		}
+		if(requestCode >=0)
+		{
+			ActivityCompat.requestPermissions(
+					activity, new String[] {permission}, requestCode);
+		}
+	}
+
+	/** Check to see if we need to show the rationale for this permission. */
+	public static boolean shouldShowRequestPermissionRationale(Activity activity, String permission) {
+		return ActivityCompat.shouldShowRequestPermissionRationale(activity, permission);
+	}
+
+	/** Launch Application Setting to grant permission. */
+	public static void launchPermissionSettings(Activity activity) {
+		Intent intent = new Intent();
+		intent.setAction(Settings.ACTION_APPLICATION_DETAILS_SETTINGS);
+		intent.setData(Uri.fromParts("package", activity.getPackageName(), null));
+		activity.startActivity(intent);
+	}
+}

app/android/src/com/introlab/rtabmap/RTABMapLib.java

@@ -1,5 +1,10 @@
 
 package com.introlab.rtabmap;
+import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
+
+import android.app.Activity;
+import android.content.Context;
 import android.os.IBinder;
 import android.view.KeyEvent;
 import android.util.Log;
@@ -15,7 +20,7 @@ public class RTABMapLib
         // the correct library first.
         if (TangoInitializationHelper.loadTangoSharedLibrary() ==
                 TangoInitializationHelper.ARCH_ERROR) {
-            Log.e(RTABMapActivity.class.getSimpleName(), "ERROR! Unable to load libtango_client_api.so!");
+            Log.w(RTABMapActivity.class.getSimpleName(), "WArning! Unable to load libtango_client_api.so! This can be safely ignored if RTAB-Map NDK is not build with tango support.");
         }
         System.loadLibrary("NativeRTABMap");
     }
@@ -23,80 +28,90 @@ public class RTABMapLib
  // Initialize the Tango Service, this function starts the communication
     // between the application and Tango Service.
     // The activity object is used for checking if the API version is outdated.
-    public static native void onCreate(RTABMapActivity activity);
+    public static native long createNativeApplication(RTABMapActivity activity);
     
-    public static native void setScreenRotation(int displayRotation, int cameraRotation);
+    public static native void destroyNativeApplication(long nativeApplication);
     
-    public static native int openDatabase(String databasePath, boolean databaseInMemory, boolean optimize);
+    public static native void setScreenRotation(long nativeApplication, int displayRotation, int cameraRotation);
     
-    /*
+    public static native int openDatabase(long nativeApplication, String databasePath, boolean databaseInMemory, boolean optimize);
-     * Called when the Tango service is connected.
+    public static native int openDatabase2(long nativeApplication, String databaseSource, String databasePath, boolean databaseInMemory, boolean optimize);
-     *
+    
-     * @param binder The native binder object.
+    public static native boolean isBuiltWith(long nativeApplication, int cameraDriver);
-     */
+    public static native boolean startCamera(long nativeApplication, IBinder binder, Context context, Activity activity, int driver);
-    public static native boolean onTangoServiceConnected(IBinder binder);
+    public static native void stopCamera(long nativeApplication);
-
-    // Release all non OpenGl resources that are allocated from the program.
-    public static native void onPause();
 
     // Allocate OpenGL resources for rendering.
-    public static native void initGlContent();
+    public static native void initGlContent(long nativeApplication);
 
     // Setup the view port width and height.
-    public static native void setupGraphic(int width, int height);
+    public static native void setupGraphic(long nativeApplication, int width, int height);
 
     // Main render loop.
-    public static native int render();
+    public static native int render(long nativeApplication);
 
     // Set the render camera's viewing angle:
     //   first person, third person, or top down.
-    public static native void setCamera(int cameraIndex);
+    public static native void setCamera(long nativeApplication, int cameraIndex);
     
     // Pass touch events to the native layer.
-    public static native void onTouchEvent(int touchCount, int event0,
+    public static native void onTouchEvent(long nativeApplication, int touchCount, int event0,
                                            float x0, float y0, float x1, float y1);
     
+    public static native void setPausedMapping(long nativeApplication, boolean paused);
+    public static native void setOnlineBlending(long nativeApplication, boolean enabled);
+    public static native void setMapCloudShown(long nativeApplication, boolean shown);
+    public static native void setOdomCloudShown(long nativeApplication, boolean shown);
+    public static native void setMeshRendering(long nativeApplication, boolean enabled, boolean withTexture);
+    public static native void setLocalizationMode(long nativeApplication, boolean enabled);
+    public static native void setTrajectoryMode(long nativeApplication, boolean enabled);
+    public static native void setGraphOptimization(long nativeApplication, boolean enabled);
+    public static native void setNodesFiltering(long nativeApplication, boolean enabled);
+    public static native void setGraphVisible(long nativeApplication, boolean visible);
+    public static native void setGridVisible(long nativeApplication, boolean visible);
+    public static native void setRawScanSaved(long nativeApplication, boolean enabled);
+    public static native void setFullResolution(long nativeApplication, boolean enabled);
+    public static native void setSmoothing(long nativeApplication, boolean enabled);
+    public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
+    public static native void setCameraColor(long nativeApplication, boolean enabled);
+    public static native void setAppendMode(long nativeApplication, boolean enabled);
+    public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
+    public static native void setMaxCloudDepth(long nativeApplication, float value);
+    public static native void setMinCloudDepth(long nativeApplication, float value);
+    public static native void setPointSize(long nativeApplication, float value);
+    public static native void setFOV(long nativeApplication, float value);
+    public static native void setOrthoCropFactor(long nativeApplication, float value);
+    public static native void setGridRotation(long nativeApplication, float value);
+    public static native void setLighting(long nativeApplication, boolean enabled);
+    public static native void setBackfaceCulling(long nativeApplication, boolean enabled);
+    public static native void setWireframe(long nativeApplication, boolean enabled);
+    public static native void setCloudDensityLevel(long nativeApplication, int value);
+    public static native void setMeshAngleTolerance(long nativeApplication, float value);
+    public static native void setMeshTriangleSize(long nativeApplication, int value);
+    public static native void setClusterRatio(long nativeApplication, float value);
+    public static native void setMaxGainRadius(long nativeApplication, float value);
+    public static native void setRenderingTextureDecimation(long nativeApplication, int value);
+    public static native void setBackgroundColor(long nativeApplication, float gray);
+    public static native int setMappingParameter(long nativeApplication, String key, String value);
+    public static native void setGPS(
+    		long nativeApplication, 
+			double stamp,
+			double longitude, 
+			double latitude, 
+			double altitude,  
+			double accuracy,
+			double bearing);
+    public static native void addEnvSensor(long nativeApplication, int type, float value);
 
-    public static native void setPausedMapping(boolean paused);
+    public static native void save(long nativeApplication, String outputDatabasePath);
-    public static native void setOnlineBlending(boolean enabled);
+    public static native void cancelProcessing(long nativeApplication);
-    public static native void setMapCloudShown(boolean shown);
-    public static native void setOdomCloudShown(boolean shown);
-    public static native void setMeshRendering(boolean enabled, boolean withTexture);
-    public static native void setLocalizationMode(boolean enabled);
-    public static native void setTrajectoryMode(boolean enabled);
-    public static native void setGraphOptimization(boolean enabled);
-    public static native void setNodesFiltering(boolean enabled);
-    public static native void setGraphVisible(boolean visible);
-    public static native void setGridVisible(boolean visible);
-    public static native void setRawScanSaved(boolean enabled);
-    public static native void setFullResolution(boolean enabled);
-    public static native void setSmoothing(boolean enabled);
-    public static native void setCameraColor(boolean enabled);
-    public static native void setAppendMode(boolean enabled);
-    public static native void setDataRecorderMode(boolean enabled);
-    public static native void setMaxCloudDepth(float value);
-    public static native void setMinCloudDepth(float value);
-    public static native void setPointSize(float value);
-    public static native void setLighting(boolean enabled);
-    public static native void setBackfaceCulling(boolean enabled);
-    public static native void setCloudDensityLevel(int value);
-    public static native void setMeshAngleTolerance(float value);
-    public static native void setMeshTriangleSize(int value);
-    public static native void setClusterRatio(float value);
-    public static native void setMaxGainRadius(float value);
-    public static native void setRenderingTextureDecimation(int value);
-    public static native void setBackgroundColor(float gray);
-    public static native int setMappingParameter(String key, String value);
-
-    public static native void resetMapping();
-    public static native void save(String outputDatabasePath);
-    public static native void cancelProcessing();
     public static native boolean exportMesh(
-    		String filePath,
+    		long nativeApplication, 
     		float cloudVoxelSize,
     		boolean regenerateCloud,
     		boolean meshing,
     		int textureSize,
+    		int textureCount,
     		int normalK,
     		boolean optimized,
     		float optimizedVoxelSize,
@@ -104,18 +119,28 @@ public class RTABMapLib
     		int optimizedMaxPolygons,
     		float optimizedColorRadius,
     		boolean optimizedCleanWhitePolygons,
-    		boolean optimizedColorWhitePolygons,
+    		int optimizedMinClusterSize,
     		float optimizedMaxTextureDistance,
     		int optimizedMinTextureClusterSize,
     		boolean blockRendering);
-    public static native boolean postExportation(boolean visualize);
+    public static native boolean writeExportedMesh(long nativeApplication, String directory, String name);
-    public static native int postProcessing(int approach);
+    public static native boolean postExportation(long nativeApplication, boolean visualize);
+    public static native int postProcessing(long nativeApplication, int approach);
     
-    public static native String getStatus();
+    public static native String getStatus(long nativeApplication);
-    public static native int getTotalNodes();
+    public static native int getTotalNodes(long nativeApplication);
-    public static native int getTotalWords();
+    public static native int getTotalWords(long nativeApplication);
-    public static native int getTotalPoints();
+    public static native int getTotalPoints(long nativeApplication);
-    public static native float getUpdateTime();
+    public static native float getUpdateTime(long nativeApplication);
-    public static native int getLoopClosureId();
+    public static native int getLoopClosureId(long nativeApplication);
+    
+    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw);
+    public static native void postOdometryEvent(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float fx, float fy, float cx, float cy,
+    		double stamp, 
+    		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+    		FloatBuffer points, int pointsLen);
     
 }

app/android/src/com/introlab/rtabmap/Renderer.java

@@ -42,6 +42,8 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private TextManager mTextManager = null;
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
+	private int mOffset = 0;
+	//private ARCoreSharedCamera mCamera = null;
 	
 	private Vector<TextObject> mTexts;
 
@@ -65,88 +67,114 @@ public class Renderer implements GLSurfaceView.Renderer {
 	{
 		mToast = toast;
 	}
+	
+	public void setOffset(int offset)
+	{
+		mOffset = offset;
+	}
+	
+	//public void setCamera(ARCoreSharedCamera camera)
+	//{
+	//	mCamera = camera;
+	//}
 
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
 
-		try
+		synchronized (this) {
-		{
+			if(mActivity.nativeApplication != 0)
-			final int value = RTABMapLib.render();
-
-			if(mTextManager!=null)
 			{
-				if(mTextChanged)
+				try
 				{
-					mTextChanged = false;
+				//	if(mCamera!=null)
-					Vector<TextObject> txtcollection = new Vector<TextObject>();
+				//	{
+				//		mCamera.updateGL();
+				//	}
 					
-					mTextLock.lock();
+					final int value = RTABMapLib.render(mActivity.nativeApplication);
-				    try {
+		
-				    	if(mTexts.size() > 0)
+					if(mTextManager!=null)
-				    	{
-				    		txtcollection.addAll(mTexts);
-				    	}
-				    } finally {
-				    	mTextLock.unlock();
-				    }
-				    
-					// Prepare the text for rendering
-					mTextManager.PrepareDraw(txtcollection);
-				}
-				
-				mTextManager.Draw(mtrxProjectionAndView);
-			}
-
-			if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
-						mProgressDialog.dismiss();
-					}
-				});
-			}
-			if(value==-1)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-			else if(value==-2)
-			{
-				mActivity.runOnUiThread(new Runnable() {
-					public void run() {
-						if(mToast!=null)
-						{
-							mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
-						}
-					}
-				});
-			}
-		}
-		catch(final Exception e)
-		{
-			mActivity.runOnUiThread(new Runnable() {
-				public void run() {
-					if(mToast!=null)
 					{
-						mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+						if(mTextChanged)
+						{
+							mTextChanged = false;
+							Vector<TextObject> txtcollection = new Vector<TextObject>();
+							
+							mTextLock.lock();
+						    try {
+						    	if(mTexts.size() > 0)
+						    	{
+						    		txtcollection.addAll(mTexts);
+						    	}
+						    } finally {
+						    	mTextLock.unlock();
+						    }
+						    
+							// Prepare the text for rendering
+							mTextManager.PrepareDraw(txtcollection);
+						}
+						
+						float[] mvp = new float[16];
+						Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
+						mTextManager.Draw(mvp);
+					}
+		
+					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(!RTABMapActivity.DISABLE_LOG) Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value));
+								mProgressDialog.dismiss();
+								mActivity.resetNoTouchTimer();
+							}
+						});
+					}
+					if(value==-1)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
+					}
+					else if(value==-2)
+					{
+						mActivity.runOnUiThread(new Runnable() {
+							public void run() {
+								if(mToast!=null)
+								{
+									mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+								}
+							}
+						});
 					}
 				}
-
+				catch(final Exception e)
-			});
+				{
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+							}
+						}
+		
+					});
+				}
+			}
 		}
 	}
 
 	// Called when the surface size changes.
 	public void onSurfaceChanged(GL10 useGLES20instead, int width, int height) {
 		
-		RTABMapLib.setupGraphic(width, height);
+		if(mActivity.nativeApplication!=0)
+		{
+			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
+		}
 		
 		mSurfaceHeight = (float)height;
 
@@ -171,7 +199,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 	// Called when the surface is created or recreated.
 	public void onSurfaceCreated(GL10 useGLES20instead, EGLConfig config) {
 
-		RTABMapLib.initGlContent();
+		if(mActivity.nativeApplication != 0)
+		{
+			RTABMapLib.initGlContent(mActivity.nativeApplication);
+		}
 		
 		// Create our text manager
 		mTextManager = new TextManager(mActivity);

app/android/src/com/introlab/rtabmap/SettingsActivity.java

@@ -1,33 +1,218 @@
 package com.introlab.rtabmap;
 
-import android.app.Activity;
+import java.io.File;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Iterator;
+import java.util.Map.Entry;
+
+import android.Manifest;
+import android.app.AlertDialog;
+import android.content.DialogInterface;
 import android.content.SharedPreferences;
 import android.content.SharedPreferences.OnSharedPreferenceChangeListener;
+import android.content.pm.PackageManager;
 import android.os.Bundle;
+import android.preference.CheckBoxPreference;
 import android.preference.ListPreference;
 import android.preference.Preference;
 import android.preference.PreferenceActivity;
-import android.preference.PreferenceManager;
+import android.text.InputType;
+import android.view.WindowManager;
+import android.view.inputmethod.EditorInfo;
+import android.widget.EditText;
+import android.widget.Toast;
 
 public class SettingsActivity extends PreferenceActivity implements OnSharedPreferenceChangeListener {
 	
+	private SettingsActivity getActivity() {return this;}
+	
     @Override
     public void onCreate(Bundle savedInstanceState) {
         super.onCreate(savedInstanceState);
         addPreferencesFromResource(R.layout.activity_settings);
         
-        Preference button = findPreference(getString(R.string.pref_key_reset_button));
+        Preference buttonReset = findPreference(getString(R.string.pref_key_reset_button));
-        button.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        buttonReset.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
         	@Override
         	public boolean onPreferenceClick(Preference preference) {   
         		getPreferenceScreen().getSharedPreferences().edit().clear().commit();
-        		
+        	
                 recreate();
         		
         		return true;
         	}
         });
         
+        Preference buttonOpen = findPreference(getString(R.string.pref_key_open_button));
+        buttonOpen.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Choose Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						//sp1 is the shared pref to copy to
+        						SharedPreferences.Editor ed = getPreferenceScreen().getSharedPreferences().edit(); 
+        						SharedPreferences sp = getActivity().getSharedPreferences(files[which], MODE_PRIVATE); //The shared preferences to copy from
+        						//Cycle through all the entries in the sp
+        						for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+        						 Object v = entry.getValue(); 
+        						 String key = entry.getKey();
+        						 //Now we just figure out what type it is, so we can copy it.
+        						 // Note that i am using Boolean and Integer instead of boolean and int.
+        						 // That's because the Entry class can only hold objects and int and boolean are primatives.
+        						 if(v instanceof Boolean) 
+        						 // Also note that i have to cast the object to a Boolean 
+        						 // and then use .booleanValue to get the boolean
+        						    ed.putBoolean(key, ((Boolean)v).booleanValue());
+        						 else if(v instanceof Float)
+        						    ed.putFloat(key, ((Float)v).floatValue());
+        						 else if(v instanceof Integer)
+        						    ed.putInt(key, ((Integer)v).intValue());
+        						 else if(v instanceof Long)
+        						    ed.putLong(key, ((Long)v).longValue());
+        						 else if(v instanceof String)
+        						    ed.putString(key, ((String)v));         
+        						}
+        						ed.commit(); //save it.
+        						recreate();
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonSave = findPreference(getString(R.string.pref_key_save_button));
+        buttonSave.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+    			builder.setTitle("Save Presets:");
+    			final EditText input = new EditText(getActivity());
+    			input.setInputType(InputType.TYPE_CLASS_TEXT); 
+    			input.setImeOptions(EditorInfo.IME_FLAG_NO_EXTRACT_UI);
+    			builder.setView(input);
+    			builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
+    				@Override
+    				public void onClick(DialogInterface dialog, int which)
+    				{
+    					final String fileName = input.getText().toString();  
+    					dialog.dismiss();
+    					if(!fileName.isEmpty())
+    					{
+    						File newFile = new File(getApplicationInfo().dataDir + "/shared_prefs/" + fileName + ".xml");
+    						if(newFile.exists())
+    						{
+    							new AlertDialog.Builder(getActivity())
+    							.setTitle("Presets Already Exist")
+    							.setMessage("Do you want to overwrite the existing file?")
+    							.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									saveConfig(fileName);
+    								}
+    							})
+    							.setNegativeButton("No", new DialogInterface.OnClickListener() {
+    								public void onClick(DialogInterface dialog, int which) {
+    									dialog.dismiss();
+    								}
+    							})
+    							.show();
+    						}
+    						else
+    						{
+    							saveConfig(fileName);
+    						}
+    					}
+    				}
+    			});
+    			AlertDialog alertToShow = builder.create();
+    			alertToShow.getWindow().setSoftInputMode(WindowManager.LayoutParams.SOFT_INPUT_STATE_VISIBLE);
+    			alertToShow.show();
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonRemove = findPreference(getString(R.string.pref_key_remove_button));
+        buttonRemove.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {   
+        		File prefsdir = new File(getApplicationInfo().dataDir,"shared_prefs");
+        		if(prefsdir.exists() && prefsdir.isDirectory()){
+        			ArrayList<String> filesArray = new ArrayList<String>(Arrays.asList(prefsdir.list()));
+        			ArrayList<String> newList = new ArrayList<String>();
+        			filesArray.remove("com.introlab.rtabmap_preferences.xml");
+				filesArray.remove("WebViewChromiumPrefs.xml");
+        			for (String s : filesArray) {
+        				newList.add(s.substring(0, s.length()-4)); // rip off the ".xml"
+        			}
+        			final String[] files = newList.toArray(new String[filesArray.size()]);
+        			if(files.length > 0)
+        			{
+        				AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+        				builder.setTitle("Remove Presets:");
+        				builder.setItems(files, new DialogInterface.OnClickListener() {
+        					public void onClick(DialogInterface dialog, final int which) {
+        						File file = new File(getApplicationInfo().dataDir + "/shared_prefs/" + files[which] + ".xml");
+        						if(file.exists())
+        						{
+        							file.delete();
+        						}
+        						return;
+        					}
+        				});
+        				builder.show();
+        			}   
+        		}
+
+        		return true;
+        	}
+        });
+        
+        Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+        buttonGPS.setOnPreferenceClickListener(new Preference.OnPreferenceClickListener() {
+        	@Override
+        	public boolean onPreferenceClick(Preference preference) {  
+        		if(((CustomSwitchPreference)preference).isChecked())
+        		{
+	        		if (!PermissionHelper.hasPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION)) {
+	        			((CustomSwitchPreference)preference).setChecked(false);
+	        			PermissionHelper.requestPermission(getActivity(), Manifest.permission.ACCESS_FINE_LOCATION);
+	        			return false;
+	        		}
+        		}
+        		return true;
+        	}
+        });
+        if(((CustomSwitchPreference)buttonGPS).isChecked())
+		{
+    		if (!PermissionHelper.hasPermission(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+    			((CustomSwitchPreference)buttonGPS).setChecked(false);
+    			PermissionHelper.requestPermission(this, Manifest.permission.ACCESS_FINE_LOCATION);
+    		}
+		}
+        
+        
+        ((Preference)findPreference(getString(R.string.pref_key_camera_driver))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_camera_driver))).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
         ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density));
         ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth));
         ((Preference)findPreference(getString(R.string.pref_key_min_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_depth))).getEntry() + ") "+getString(R.string.pref_summary_min_depth));
@@ -38,6 +223,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -48,16 +234,20 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features));
         ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         ((Preference)findPreference(getString(R.string.pref_key_optimizer))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_optimizer))).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        ((Preference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_marker_detection_depth_error))).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
         
         ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        ((Preference)findPreference(getString(R.string.pref_key_texture_count))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_count))).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
         ((Preference)findPreference(getString(R.string.pref_key_normal_k))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_normal_k))).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
         ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
         ((Preference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_min_texture_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
 
         ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         ((Preference)findPreference(getString(R.string.pref_key_opt_color_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_color_radius))).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
-    
+        ((Preference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_min_cluster_size))).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
+
         ((Preference)findPreference(getString(R.string.pref_key_cluster_ratio))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cluster_ratio))).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         ((Preference)findPreference(getString(R.string.pref_key_gain_max_radius))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_gain_max_radius))).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
     }
@@ -66,6 +256,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         Preference pref = findPreference(key);
 
         if (pref instanceof ListPreference) {
+        	if(key.compareTo(getString(R.string.pref_key_camera_driver))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_camera_driver));
         	if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density));
         	if(key.compareTo(getString(R.string.pref_key_depth))==0) 
     		{
@@ -94,6 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
+        	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
         	if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr));
         	if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr));
@@ -104,15 +296,19 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features));
         	if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type));
         	if(key.compareTo(getString(R.string.pref_key_optimizer))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_optimizer));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection));
+        	if(key.compareTo(getString(R.string.pref_key_marker_detection_depth_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_marker_detection_depth_error));
         	
         	if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel));
         	if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size));
+        	if(key.compareTo(getString(R.string.pref_key_texture_count))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_count));
         	if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k));
         	if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance));
         	if(key.compareTo(getString(R.string.pref_key_min_texture_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_min_texture_cluster_size));
 
         	if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth));
         	if(key.compareTo(getString(R.string.pref_key_opt_color_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_color_radius));
+        	if(key.compareTo(getString(R.string.pref_key_opt_min_cluster_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_min_cluster_size));
         	
         	if(key.compareTo(getString(R.string.pref_key_cluster_ratio))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cluster_ratio));
         	if(key.compareTo(getString(R.string.pref_key_gain_max_radius))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_gain_max_radius));
@@ -135,4 +331,57 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         getPreferenceScreen().getSharedPreferences()
                 .unregisterOnSharedPreferenceChangeListener(this);
     }
-}
+    
+    private void saveConfig(String fileName)
+    {
+    	//sp1 is the shared pref to copy to
+		SharedPreferences.Editor ed = getActivity().getSharedPreferences(fileName, MODE_PRIVATE).edit(); 
+		SharedPreferences sp = getPreferenceScreen().getSharedPreferences(); //The shared preferences to copy from
+		ed.clear(); // This clears the one we are copying to, but you don't necessarily need to do that.
+		//Cycle through all the entries in the sp
+		for(Entry<String,?> entry : sp.getAll().entrySet()){ 
+		 Object v = entry.getValue(); 
+		 String key = entry.getKey();
+		 //Now we just figure out what type it is, so we can copy it.
+		 // Note that i am using Boolean and Integer instead of boolean and int.
+		 // That's because the Entry class can only hold objects and int and boolean are primatives.
+		 if(v instanceof Boolean) 
+		 // Also note that i have to cast the object to a Boolean 
+		 // and then use .booleanValue to get the boolean
+		    ed.putBoolean(key, ((Boolean)v).booleanValue());
+		 else if(v instanceof Float)
+		    ed.putFloat(key, ((Float)v).floatValue());
+		 else if(v instanceof Integer)
+		    ed.putInt(key, ((Integer)v).intValue());
+		 else if(v instanceof Long)
+		    ed.putLong(key, ((Long)v).longValue());
+		 else if(v instanceof String)
+		    ed.putString(key, ((String)v));         
+		}
+		ed.commit(); //save it.
+    }
+    
+    
+	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
+
+		switch (requestCode) {
+			case PermissionHelper.ACCESS_FINE_LOCATION_CODE: {
+				// If request is cancelled, the result arrays are empty.
+				if (results.length > 0 && results[0] == PackageManager.PERMISSION_GRANTED) {
+					// permission was granted, yay! Do the
+					// contacts-related task you need to do.
+					Preference buttonGPS = findPreference(getString(R.string.pref_key_gps_saved));
+					((CustomSwitchPreference)buttonGPS).setChecked(true);
+				} else {
+					// permission denied, boo! Disable the
+					// functionality that depends on this permission.
+					Toast.makeText(this, "Location permission is needed to use GPS functionality", Toast.LENGTH_LONG).show();
+					if (!PermissionHelper.shouldShowRequestPermissionRationale(this, Manifest.permission.ACCESS_FINE_LOCATION)) {
+						PermissionHelper.launchPermissionSettings(this); // Permission denied with checking "Do not ask again".
+					}
+				}
+				return;
+			}
+		}
+	}
+}

app/android/src/com/introlab/rtabmap/SketchfabActivity.java

@@ -44,7 +44,6 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	private Dialog mAuthDialog;
 	
 	private String mAuthToken;
-	private boolean mExportedOBJ;
 	private String mWorkingDirectory;
 	
 	EditText mFilename;
@@ -67,10 +66,10 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 		mButtonOk = (Button)findViewById(R.id.button_ok);
 		
 		mProgressDialog = new ProgressDialog(this);
+		mProgressDialog.setCancelable(false);
 		mProgressDialog.setCanceledOnTouchOutside(false);
 		
 		mAuthToken = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_AUTH_TOKEN_KEY);
-		mExportedOBJ = getIntent().getExtras().getBoolean(RTABMapActivity.RTABMAP_EXPORTED_OBJ_KEY);
 		mFilename.setText(getIntent().getExtras().getString(RTABMapActivity.RTABMAP_FILENAME_KEY));
 		mWorkingDirectory = getIntent().getExtras().getString(RTABMapActivity.RTABMAP_WORKING_DIR_KEY);
 		
@@ -129,51 +128,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 			editor.commit();
 		}
 		
-		final String extension  = mExportedOBJ?".obj":".ply";
+		authorizeAndPublish(mFilename.getText().toString());	
-		
-		String[] files = new String[0];
-		// verify if we have all files
-		if(extension.compareTo(".obj") == 0)
-		{
-			File objFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".obj");
-			File mltFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".mtl");
-			File jpgFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + ".jpg");
-			if(objFile.exists() && mltFile.exists() && jpgFile.exists())
-			{
-				files = new String[3];
-				files[0] = objFile.getAbsolutePath();
-				files[1] = mltFile.getAbsolutePath();
-				files[2] = jpgFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing OBJ files!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else if(extension.compareTo(".ply") == 0)
-		{
-			File plyFile = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + RTABMapActivity.RTABMAP_TMP_FILENAME + extension);
-			if(plyFile.exists())
-			{
-				files = new String[1];
-				files[0] = plyFile.getAbsolutePath();
-			}
-			else
-			{
-				Toast.makeText(getActivity(), String.format("Missing PLY file!"), Toast.LENGTH_LONG).show();
-			}
-		}
-		else
-		{
-			Toast.makeText(getActivity(), String.format("Unknown file extension \"%s\"!", extension), Toast.LENGTH_LONG).show();
-		}
-
-		if(files.length > 0)
-		{
-			final String[] filesToZip = files;
-			authorizeAndPublish(filesToZip, mFilename.getText().toString());
-		}
-	
 	}
 	
 	private boolean isNetworkAvailable() {
@@ -183,24 +138,27 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 	    return activeNetworkInfo != null && activeNetworkInfo.isConnected();
 	}
 	
-	private void authorizeAndPublish(final String[] filesToZip, final String fileName)
+	private void authorizeAndPublish(final String fileName)
 	{
 		if(!isNetworkAvailable())
 		{
 			// Visualize the result?
-			new AlertDialog.Builder(this)
+			AlertDialog ad = new AlertDialog.Builder(this)
+			.setCancelable(false)
 			.setTitle("Sharing to Sketchfab...")
 			.setMessage("Network is not available. Make sure you have internet before continuing.")
 			.setPositiveButton("Try Again", new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, int which) {
-					authorizeAndPublish(filesToZip, fileName);
+					authorizeAndPublish(fileName);
 				}
 			})
 			.setNeutralButton("Abort", new DialogInterface.OnClickListener() {
 				public void onClick(DialogInterface dialog, int which) {
 				}
 			})
-			.show();
+			.create();
+			ad.setCanceledOnTouchOutside(false);
+			ad.show();
 			return;
 		}	
 
@@ -211,6 +169,8 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 
 			WebView web;
 			mAuthDialog = new Dialog(this);
+			mAuthDialog.setCancelable(true);
+			mAuthDialog.setCanceledOnTouchOutside(true);
 			mAuthDialog.setContentView(R.layout.auth_dialog);
 			web = (WebView)mAuthDialog.findViewById(R.id.webv);
 			web.setWebContentsDebuggingEnabled(!RTABMapActivity.DISABLE_LOG);
@@ -240,82 +200,138 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						
 						mAuthDialog.dismiss();
 
-						zipAndPublish(filesToZip, fileName);
+						zipAndPublish(fileName);
 					}
 				}
 			});
 			mAuthDialog.show();
 			mAuthDialog.setTitle("Authorize RTAB-Map");
-			mAuthDialog.setCancelable(true);
 			web.loadUrl(auth_url);
 		}
 		else
 		{
-			zipAndPublish(filesToZip, fileName);
+			zipAndPublish(fileName);
 		}
 	}
 
-	private void zipAndPublish(final String[] filesToZip, final String fileName)
+	private void zipAndPublish(final String fileName)
 	{		
-		final String zipOutput = mWorkingDirectory+fileName+".zip";
-
 		mProgressDialog.setTitle("Upload to Sketchfab");
 		mProgressDialog.setMessage(String.format("Compressing the files..."));
+		mProgressDialog.setCancelable(false);
+		mProgressDialog.setCanceledOnTouchOutside(false);
 		mProgressDialog.show();
 
 		Thread workingThread = new Thread(new Runnable() {
 			public void run() {
 				try{
-					Util.zip(filesToZip, zipOutput);
+					
-					runOnUiThread(new Runnable() {
+					File tmpDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR);
-						public void run() {
+					tmpDir.mkdirs();
-							mProgressDialog.dismiss();
+					String[] fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
-
+					if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR));
-							File f = new File(zipOutput);
+					for(int i=0; i<fileNames.length; ++i)
-
+					{
-							// Continue?
+						File f = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i]);
-							AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+						if(f.delete())
-							builder.setTitle("File(s) compressed and ready to upload!");
+						{
-							
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Deleted \"%s\"", f.getPath()));
-							final int fileSizeMB = (int)f.length()/(1024 * 1024);
+						}
-							final int fileSizeKB = (int)f.length()/(1024);
+						else
-							if(fileSizeMB == 0)
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, String.format("Failed deleting \"%s\"", f.getPath()));
+						}
+					}
+					File exportDir = new File(mWorkingDirectory + RTABMapActivity.RTABMAP_EXPORT_DIR);
+					exportDir.mkdirs();
+					
+					if(RTABMapLib.writeExportedMesh(RTABMapActivity.nativeApplication, mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, RTABMapActivity.RTABMAP_TMP_FILENAME))
+					{
+						String[] files = new String[0];
+						// verify if we have all files
+				
+						fileNames = Util.loadFileList(mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR, false);
+						if(fileNames.length > 0)
+						{
+							files = new String[fileNames.length];
+							for(int i=0; i<fileNames.length; ++i)
 							{
-								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
+								files[i] = mWorkingDirectory + RTABMapActivity.RTABMAP_TMP_DIR + "/" + fileNames[i];
-								builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
-							}
-							else
-							{
-								Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
-								builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
-										+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options.", fileSizeMB,
-										fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
 							}
+						}
+						else
+						{
+							if(!RTABMapActivity.DISABLE_LOG) Log.i(RTABMapActivity.TAG, "Missing files!");
+						}
+				
+						if(files.length > 0)
+						{
+							final String[] filesToZip = files;
+							final String zipOutput = mWorkingDirectory+fileName+".zip";
+							Util.zip(filesToZip, zipOutput);
+							runOnUiThread(new Runnable() {
+								public void run() {
+									mProgressDialog.dismiss();
 
-							
+									File f = new File(zipOutput);
-							builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+
-								public void onClick(DialogInterface dialog, int which) {
+									// Continue?
-									mProgressDialog.setTitle("Upload to Sketchfab");
+									AlertDialog.Builder builder = new AlertDialog.Builder(getActivity());
+									builder.setTitle("File(s) compressed and ready to upload!");
+									
+									final int fileSizeMB = (int)f.length()/(1024 * 1024);
+									final int fileSizeKB = (int)f.length()/(1024);
 									if(fileSizeMB == 0)
 									{
-										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d KB", fileSizeKB));
+										builder.setMessage(String.format("Total size to upload = %d KB. Do you want to continue?\n\n", fileSizeKB));
 									}
 									else
 									{
-										mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+										Log.i(RTABMapActivity.TAG, String.format("Zipped files = %d MB", fileSizeMB));
+										builder.setMessage(String.format("Total size to upload = %d MB. %sDo you want to continue?\n\n"
+												+ "Tip: To reduce the model size, you can also look at the Settings->Exporting options.", fileSizeMB,
+												fileSizeMB>=50?"Note that for size over 50 MB, a Sketchfab PRO account is required, otherwise the upload may fail. ":""));
 									}
-									mProgressDialog.show();
+
-									new uploadToSketchfabTask().execute(zipOutput, fileName);
+									
+									builder.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											mProgressDialog.setTitle("Upload to Sketchfab");
+											if(fileSizeMB == 0)
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d KB) to Sketchfab...", fileName, fileSizeKB));
+											}
+											else
+											{
+												mProgressDialog.setMessage(String.format("Uploading model \"%s\" (%d MB) to Sketchfab...", fileName, fileSizeMB));
+											}
+											mProgressDialog.show();
+											new uploadToSketchfabTask().execute(zipOutput, fileName);
+										}
+									});
+									builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
+										public void onClick(DialogInterface dialog, int which) {
+											// do nothing...
+										}
+									});
+									AlertDialog ad = builder.create();
+									ad.setCancelable(false);
+									ad.setCanceledOnTouchOutside(false);
+									ad.show();
 								}
 							});
-							builder.setNegativeButton("No", new DialogInterface.OnClickListener() {
-								public void onClick(DialogInterface dialog, int which) {
-									// do nothing...
-								}
-							});
-							builder.show();
 						}
-					});
+					}
+					else
+					{
+						runOnUiThread(new Runnable() {
+							public void run() {
+								mProgressDialog.dismiss();
+								Toast.makeText(getActivity(), String.format("Failed writing files!"), Toast.LENGTH_LONG).show();
+							}
+						});
+					}
 				}
 				catch(IOException ex) {
 					Log.e(RTABMapActivity.TAG, "Failed to zip", ex);
@@ -365,6 +381,7 @@ public class SketchfabActivity extends Activity implements OnClickListener {
 						finish();
 					}
 				}).create();
+				d.setCanceledOnTouchOutside(false);
 				d.show();
 				((TextView)d.findViewById(android.R.id.message)).setMovementMethod(LinkMovementMethod.getInstance());
 			}

app/android/src/com/introlab/rtabmap/TextManager.java

@@ -17,6 +17,7 @@ import android.graphics.Typeface;
 import android.opengl.GLES20;
 import android.opengl.GLUtils;
 import android.text.TextPaint;
+import android.util.Log;
 
 public class TextManager {
 	
@@ -50,7 +51,7 @@ public class TextManager {
 	public static final int RI_TEXT_TEXTURE_SIZE = 512; // 512
 	public static final float RI_TEXT_HEIGHT_BASE = 32.0f;
 	public static final char RI_TEXT_START = ' ';
-	public static final char RI_TEXT_STOP = '~'+1;
+	public static final char RI_TEXT_STOP = '\u00B0'+1;
 	
 	public float getMaxTextHeight() {return mTextHeight;}
 	
@@ -336,6 +337,8 @@ public class TextManager {
 		float y = val.y;
 		String text = val.text;
 		
+		Log.i("RTABMapActivity", String.format("convertTextToTriangleInfo() set status=%s", text));
+		
 		// Create 
 		for(int j=0; j<text.length(); j++)
 		{

app/android/src/com/introlab/rtabmap/Util.java

@@ -2,9 +2,12 @@ package com.introlab.rtabmap;
 
 import java.io.BufferedInputStream;
 import java.io.BufferedOutputStream;
+import java.io.File;
 import java.io.FileInputStream;
 import java.io.FileOutputStream;
+import java.io.FilenameFilter;
 import java.io.IOException;
+import java.util.Arrays;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;
 
@@ -52,5 +55,72 @@ public class Util {
 		}
 	}
 
+	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+		File path = new File(directory); 
+		String fileList[];
+		try {
+			path.mkdirs();
+		}
+		catch(SecurityException e) {
+			Log.e(RTABMapActivity.TAG, "unable to write on the sd card " + e.toString());
+		}
+		if(path.exists()) {
+			FilenameFilter filter = new FilenameFilter() {
+
+				@Override
+				public boolean accept(File dir, String filename) {
+					File sel = new File(dir, filename);
+					if(databasesOnly)
+					{
+						return filename.compareTo(RTABMapActivity.RTABMAP_TMP_DB) != 0 && filename.endsWith(".db");
+					}
+					else
+					{
+						return sel.isFile();
+					}
+				}
+
+			};
+			fileList = path.list(filter);
+			Arrays.sort(fileList);
+		}
+		else {
+			fileList = new String[0];
+		}
+		return fileList;
+	}
+	
+	/**
+	 * https://stackoverflow.com/questions/6701948/efficient-way-to-compare-version-strings-in-java
+	 * Compares two version strings. 
+	 * 
+	 * Use this instead of String.compareTo() for a non-lexicographical 
+	 * comparison that works for version strings. e.g. "1.10".compareTo("1.6").
+	 * 
+	 * @note It does not work if "1.10" is supposed to be equal to "1.10.0".
+	 * 
+	 * @param str1 a string of ordinal numbers separated by decimal points. 
+	 * @param str2 a string of ordinal numbers separated by decimal points.
+	 * @return The result is a negative integer if str1 is _numerically_ less than str2. 
+	 *         The result is a positive integer if str1 is _numerically_ greater than str2. 
+	 *         The result is zero if the strings are _numerically_ equal.
+	 */
+	public static int versionCompare(String str1, String str2) {
+	    String[] vals1 = str1.split("\\.");
+	    String[] vals2 = str2.split("\\.");
+	    int i = 0;
+	    // set index to first non-equal ordinal or length of shortest version string
+	    while (i < vals1.length && i < vals2.length && vals1[i].equals(vals2[i])) {
+	      i++;
+	    }
+	    // compare first non-equal ordinal number
+	    if (i < vals1.length && i < vals2.length) {
+	        int diff = Integer.valueOf(vals1[i]).compareTo(Integer.valueOf(vals2[i]));
+	        return Integer.signum(diff);
+	    }
+	    // the strings are equal or one string is a substring of the other
+	    // e.g. "1.2.3" = "1.2.3" or "1.2.3" < "1.2.3.4"
+	    return Integer.signum(vals1.length - vals2.length);
+	}
 	
 }

app/src/CMakeLists.txt

@@ -74,7 +74,11 @@ ELSE()
 ENDIF()
 TARGET_LINK_LIBRARIES(rtabmap rtabmap_core rtabmap_gui rtabmap_utilite ${LIBRARIES})
 IF(Qt5_FOUND)
-    QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    IF(Qt5Svg_FOUND)
+        QT5_USE_MODULES(rtabmap Widgets Core Gui Svg PrintSupport)
+    ELSE()
+        QT5_USE_MODULES(rtabmap Widgets Core Gui PrintSupport)
+    ENDIF()
 ENDIF(Qt5_FOUND)
 
 IF(APPLE AND BUILD_AS_BUNDLE)
@@ -100,7 +104,7 @@ INSTALL(CODE "execute_process(COMMAND ln -s \"../MacOS/${CMAKE_BUNDLE_NAME}\" ${
         WORKING_DIRECTORY \$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin)")
 ENDIF(APPLE AND BUILD_AS_BUNDLE)
        
-IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+IF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   SET(APPS "\$ENV{DESTDIR}\${CMAKE_INSTALL_PREFIX}/bin/${PROJECT_NAME}${CMAKE_EXECUTABLE_SUFFIX}")
   SET(plugin_dest_dir bin)
   SET(qtconf_dest_dir bin)
@@ -133,6 +137,16 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
           DESTINATION ${openni2_dest_dir} 
           COMPONENT runtime)
   ENDIF(OpenNI2_FOUND)
+  
+  IF(k4a_FOUND)
+	# Install needed depthengine_2_0.dll
+    IF(WIN32)
+		file(TO_CMAKE_PATH "$ENV{K4A_ROOT_DIR}" ENV_K4A_ROOT_DIR)
+		INSTALL(FILES "${ENV_K4A_ROOT_DIR}/tools/depthengine_2_0.dll" 
+          DESTINATION ${plugin_dest_dir} 
+          COMPONENT runtime)
+	ENDIF(WIN32)
+  ENDIF(k4a_FOUND)
 
   # Install needed Qt plugins by copying directories from the qt installation
   # One can cull what gets copied by using 'REGEX "..." EXCLUDE'
@@ -150,11 +164,30 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
 		get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
 		string(REPLACE "plugins" ";" loc_list ${plugin_dir})
 		list(GET loc_list 1 plugin_type)
-	    #MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		IF(NOT plugin_root)
+			get_filename_component(plugin_root ${plugin_dir} DIRECTORY)
+		ENDIF(NOT plugin_root)
+		#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
 		INSTALL(FILES ${plugin_loc}
           DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
           COMPONENT runtime)
-	endforeach()
+	endforeach()
+	IF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
+		IF(WIN32)
+			SET(plugin_loc "${plugin_root}/styles/qwindowsvistastyle.dll")
+		ELSEIF(APPLE)
+			SET(plugin_loc "${plugin_root}/styles/libqmacstyle.dylib")
+		ENDIF()
+		IF(EXISTS ${plugin_loc})
+			get_filename_component(plugin_dir ${plugin_loc} DIRECTORY)
+			string(REPLACE "plugins" ";" loc_list ${plugin_dir})
+			list(GET loc_list 1 plugin_type)
+			INSTALL(FILES ${plugin_loc}
+				DESTINATION ${plugin_dest_dir}/plugins${plugin_type}
+				COMPONENT runtime)
+			#MESSAGE(STATUS "Qt5 plugin \"${plugin_loc}\" installed in \"${plugin_dest_dir}/plugins${plugin_type}\"")
+		ENDIF(EXISTS ${plugin_loc})
+	ENDIF(NOT Qt5Widgets_VERSION VERSION_LESS 5.10.0)
   ENDIF()
   
   # install a qt.conf file
@@ -185,5 +218,5 @@ IF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
     include(\"BundleUtilities\")
     fixup_bundle(\"${APPS}\" \"\${QTPLUGINS}\" \"${DIRS}\")
   " COMPONENT runtime)
-ENDIF((APPLE AND BUILD_AS_BUNDLE) OR WIN32)
+ENDIF(BUILD_AS_BUNDLE AND (APPLE OR WIN32))
   

app/src/ObjDeletionHandler.h

@@ -46,7 +46,7 @@ public:
 	}
 	virtual ~ObjDeletionHandler() {}
 
-signals:
+Q_SIGNALS:
 	void objDeletionEventReceived(int);
 
 protected:
@@ -55,7 +55,7 @@ protected:
 		if(event->getClassName().compare("UObjDeletedEvent") == 0 &&
 		   event->getCode() == _watchedId)
 		{
-			emit objDeletionEventReceived(_watchedId);
+			Q_EMIT objDeletionEventReceived(_watchedId);
 		}
 		return false;
 	}

app/src/main.cpp

@@ -43,13 +43,13 @@ int main(int argc, char* argv[])
 {
 	/* Set logger type */
 	ULogger::setType(ULogger::kTypeConsole);
-	ULogger::setLevel(ULogger::kInfo);
+	ULogger::setLevel(ULogger::kWarning);
 
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box
 
-	ParametersMap parameters = Parameters::parseArguments(argc, argv, true);
+	ParametersMap parameters = Parameters::parseArguments(argc, argv, false);
 	MainWindow * mainWindow = new MainWindow();
     app->installEventFilter(mainWindow); // to catch FileOpen events.
     
@@ -61,11 +61,10 @@ int main(int argc, char* argv[])
            UFile::getExtension(value).compare("db") == 0)
         {
             database = value;
-            break;
         }
     }
 
-	UINFO("Program started...");
+	printf("Program started...\n");
 
 	UEventsManager::addHandler(mainWindow);
 
@@ -85,9 +84,9 @@ int main(int argc, char* argv[])
     
     if(!database.empty())
     {
-    	mainWindow->openDatabase(database.c_str());
+    	mainWindow->openDatabase(database.c_str(), parameters);
     }
-    if(parameters.size())
+    else if(parameters.size())
     {
     	mainWindow->updateParameters(parameters);
     }
@@ -101,14 +100,13 @@ int main(int argc, char* argv[])
 	UEventsManager::removeHandler(mainWindow);
 	UEventsManager::removeHandler(rtabmap);
 
-	UINFO("Killing threads...");
 	rtabmap->join(true);
 
-	UINFO("Closing RTAB-Map...");
+	printf("Closing RTAB-Map...\n");
 	delete rtabmap;
 	delete mainWindow;
 	delete app;
-	UINFO("All done!");
+	printf("All done!\n");
 
     return 0;
 }

cmake_modules/FindARCore.cmake

@@ -0,0 +1,31 @@
+# - Find ARCore
+# This module finds an installed ARCore client C-API package.
+#
+# It sets the following variables:
+#  ARCore_FOUND       - Set to false, or undefined, if ARCore isn't found.
+#  ARCore_INCLUDE_DIRS - The ARCore include directory.
+#  ARCore_LIBRARIES     - The ARCore library to link against.
+
+FIND_PATH(ARCore_INCLUDE_DIR arcore_c_api.h)
+
+FIND_LIBRARY(ARCore_c_LIBRARY NAMES arcore_sdk_c PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(ARCore_jni_LIBRARY NAMES arcore_sdk_jni PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+   SET(ARCore_FOUND TRUE)
+   SET(ARCore_INCLUDE_DIRS ${ARCore_INCLUDE_DIR})
+   SET(ARCore_LIBRARIES ${ARCore_c_LIBRARY} ${ARCore_jni_LIBRARY})
+ENDIF (ARCore_INCLUDE_DIR AND ARCore_c_LIBRARY AND ARCore_jni_LIBRARY)
+
+IF (ARCore_FOUND)
+   # show which ARCore was found only if not quiet
+   IF (NOT ARCore_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ARCore: ${ARCore_INCLUDE_DIRS}")
+   ENDIF (NOT ARCore_FIND_QUIETLY)
+ELSE (ARCore_FOUND)
+   # fatal error if ARCore is required but not found
+   IF (ARCore_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ARCore (client and/or support libraries)")
+   ENDIF (ARCore_FIND_REQUIRED)
+ENDIF (ARCore_FOUND)
+

cmake_modules/FindAREngine.cmake

@@ -0,0 +1,32 @@
+# - Find AREngine
+# This module finds an installed AREngine client C-API package.
+#
+# It sets the following variables:
+#  AREngine_FOUND       - Set to false, or undefined, if AREngine isn't found.
+#  AREngine_INCLUDE_DIRS - The AREngine include directory.
+#  AREngine_LIBRARIES     - The AREngine library to link against.
+
+FIND_PATH(AREngine_INCLUDE_DIR huawei_arengine_interface.h)
+
+FIND_LIBRARY(AREngine_impl_LIBRARY NAMES huawei_arengine_impl PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_jni_LIBRARY NAMES huawei_arengine_jni PATH_SUFFIXES ${ANDROID_ABI})
+FIND_LIBRARY(AREngine_ndk_LIBRARY NAMES huawei_arengine_ndk PATH_SUFFIXES ${ANDROID_ABI})
+
+IF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+   SET(AREngine_FOUND TRUE)
+   SET(AREngine_INCLUDE_DIRS ${AREngine_INCLUDE_DIR})
+   SET(AREngine_LIBRARIES ${AREngine_impl_LIBRARY} ${AREngine_jni_LIBRARY} ${AREngine_ndk_LIBRARY})
+ENDIF (AREngine_INCLUDE_DIR AND AREngine_impl_LIBRARY AND AREngine_jni_LIBRARY AND AREngine_ndk_LIBRARY)
+
+IF (AREngine_FOUND)
+   # show which AREngine was found only if not quiet
+   IF (NOT AREngine_FIND_QUIETLY)
+      MESSAGE(STATUS "Found AREngine: ${AREngine_INCLUDE_DIRS}")
+   ENDIF (NOT AREngine_FIND_QUIETLY)
+ELSE (AREngine_FOUND)
+   # fatal error if AREngine is required but not found
+   IF (AREngine_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find AREngine's impl, jni and ndk libraries)")
+   ENDIF (AREngine_FIND_REQUIRED)
+ENDIF (AREngine_FOUND)
+

cmake_modules/FindFastCV.cmake

@@ -0,0 +1,37 @@
+# - Find FastCV (https://developer.qualcomm.com/software/fastcv-sdk)
+#
+# It sets the following variables:
+#  FastCV_FOUND         - Set to false, or undefined, if FastCV isn't found.
+#  FastCV_INCLUDE_DIRS  - The FastCV include directory.
+#  FastCV_LIBRARIES     - The FastCV library to link against.
+
+IF(NOT WIN32 AND NOT APPLE)
+
+   EXECUTE_PROCESS( COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCHITECTURE )
+   MESSAGE( STATUS "Architecture: ${ARCHITECTURE}" )
+
+   # Currently only tested on aarch64!
+   IF(${ARCHITECTURE} STREQUAL "aarch64")
+      find_path(FastCV_INCLUDE_DIRS NAMES fastcv.h PATH_SUFFIXES fastcv)
+      find_library(FastCV_LIBRARY NAMES fastcvopt fastcv)
+   ENDIF(${ARCHITECTURE} STREQUAL "aarch64")
+ENDIF(NOT WIN32 AND NOT APPLE)
+
+IF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+   SET(FastCV_FOUND TRUE)
+ENDIF (FastCV_INCLUDE_DIRS AND FastCV_LIBRARY)
+
+IF (FastCV_FOUND)
+   SET(FastCV_LIBRARIES ${FastCV_LIBRARY})
+     
+   # show which RealSense was found only if not quiet
+   IF (NOT FastCV_FIND_QUIETLY)
+      MESSAGE(STATUS "Found FastCV: ${FastCV_LIBRARIES}")
+   ENDIF (NOT FastCV_FIND_QUIETLY)
+ELSE (FastCV_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (FastCV_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find FastCV")
+   ENDIF (FastCV_FIND_REQUIRED)
+ENDIF (FastCV_FOUND)
+

cmake_modules/FindFlyCapture2.cmake

@@ -7,32 +7,39 @@
 #  FlyCapture2_LIBRARIES     - The FlyCapture2 library to link against.
 
 if(CMAKE_CL_64)
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib64 $ENV{FC2LIB}/lib64)
 else()
-set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib)
+set(FlyCapture2_LIBDIR $ENV{FlyCapture2_ROOT_DIR}/lib $ENV{FC2LIB}/lib)
 endif()
 
 if(CMAKE_CL_64)
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib64 $ENV{TRICLOPSLIB}/lib64)
 else()
-set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib)
+set(Triclops_LIBDIR $ENV{Triclops_ROOT_DIR}/lib $ENV{TRICLOPSLIB}/lib)
 endif()
 
 #FlyCapture2 SDK
-find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include)
+find_path(FlyCapture2_INCLUDE_DIR NAMES FlyCapture2.h PATHS $ENV{FlyCapture2_ROOT_DIR}/include $ENV{FlyCapture2_ROOT_DIR}/include/flycapture $ENV{FC2LIB}/include $ENV{FC2LIB}/include/flycapture)
-find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v100 FlyCapture2 flycapture2 NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR})
+find_library(FlyCapture2_LIBRARY NAMES FlyCapture2_v140 FlyCapture2_v100 FlyCapture2 flycapture2 flycapture NO_DEFAULT_PATH PATHS ${FlyCapture2_LIBDIR}/vs2015 ${FlyCapture2_LIBDIR})
+
+MESSAGE(STATUS "FlyCapture2_INCLUDE_DIR=${FlyCapture2_INCLUDE_DIR}")
+MESSAGE(STATUS "FlyCapture2_LIBRARY=${FlyCapture2_LIBRARY}")
 
 # Triclops SDK
-find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include)
+find_path(Triclops_INCLUDE_DIR NAMES triclops.h PATHS $ENV{Triclops_ROOT_DIR}/include $ENV{Triclops_ROOT_DIR}/include/triclops $ENV{TRICLOPSLIB}/include $ENV{TRICLOPSLIB}/include/triclops)
-find_library(Triclops_LIBRARY NAMES triclops triclops_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(Triclops_LIBRARY NAMES triclops triclops_v140 triclops_v100 libtriclops.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(FlyCaptureBridge_LIBRARY NAMES flycapture2bridge flycapture2bridge_v140 flycapture2bridge_v100 libflycapture2bridge.so.3 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
-find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
+find_library(pnmutils_LIBRARY NAMES pnmutils pnmutils_v140 pnmutils_v100 NO_DEFAULT_PATH PATHS ${Triclops_LIBDIR})
 
-IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+MESSAGE(STATUS "Triclops_INCLUDE_DIR=${Triclops_INCLUDE_DIR}")
+MESSAGE(STATUS "Triclops_LIBRARY=${Triclops_LIBRARY}")
+MESSAGE(STATUS "FlyCaptureBridge_LIBRARY=${FlyCaptureBridge_LIBRARY}")
+
+IF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
    SET(FlyCapture2_FOUND TRUE)
    SET(FlyCapture2_INCLUDE_DIRS ${FlyCapture2_INCLUDE_DIR} ${Triclops_INCLUDE_DIR})
-   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY} ${pnmutils_LIBRARY})
+   SET(FlyCapture2_LIBRARIES ${FlyCapture2_LIBRARY} ${Triclops_LIBRARY} ${FlyCaptureBridge_LIBRARY})
-ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY AND pnmutils_LIBRARY)
+ENDIF (FlyCapture2_INCLUDE_DIR AND Triclops_INCLUDE_DIR AND FlyCapture2_LIBRARY AND Triclops_LIBRARY AND FlyCaptureBridge_LIBRARY)
 
 IF (FlyCapture2_FOUND)
    # show which FlyCapture2 was found only if not quiet

cmake_modules/FindG2O.cmake

@@ -17,6 +17,14 @@ FIND_LIBRARY(CHOLMOD_LIB cholmod)
 FIND_PATH(G2O_INCLUDE_DIR g2o/core/base_vertex.h 
   PATHS "C:\\Program Files\\g2o\\include")
 
+FIND_FILE(G2O_CONFIG_FILE g2o/config.h 
+  PATHS ${G2O_INCLUDE_DIR}
+  NO_DEFAULT_PATH)
+
+#ifdef G2O_NUMBER_FORMAT_STR
+#define G2O_CPP11 // we assume that if G2O_NUMBER_FORMAT_STR is defined, this is the new g2o code with c++11 interface
+#endif
+
 # Macro to unify finding both the debug and release versions of the
 # libraries; this is adapted from the rtabmap config
 
@@ -75,7 +83,7 @@ ENDIF(G2O_SOLVER_CHOLMOD OR G2O_SOLVER_CSPARSE OR G2O_SOLVER_DENSE OR G2O_SOLVER
 
 # G2O itself declared found if we found the core libraries and at least one solver
 SET(G2O_FOUND "NO")
-IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)
   SET(G2O_INCLUDE_DIRS ${G2O_INCLUDE_DIR})
   SET(G2O_LIBRARIES 
 	${G2O_CORE_LIBRARY}
@@ -105,5 +113,15 @@ IF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FO
 	  ${CHOLMOD_LIB})
   ENDIF(G2O_SOLVER_CHOLMOD)
 
+  FILE(READ ${G2O_CONFIG_FILE} TMPTXT)
+  STRING(FIND "${TMPTXT}" "G2O_NUMBER_FORMAT_STR" matchres)
+  IF(${matchres} EQUAL -1)
+    MESSAGE(STATUS "Old g2o version detected with c++03 interface (config file: ${G2O_CONFIG_FILE}).")
+    SET(G2O_CPP11 0)
+  ELSE()
+    MESSAGE(WARNING "Latest g2o version detected with c++11 interface (config file: ${G2O_CONFIG_FILE}). Make sure g2o is built with \"-DBUILD_WITH_MARCH_NATIVE=OFF\" to avoid segmentation faults caused by Eigen.")
+    SET(G2O_CPP11 1)
+  ENDIF()
+
   SET(G2O_FOUND "YES")
-ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_SOLVERS_FOUND)
+ENDIF(G2O_STUFF_LIBRARY AND G2O_CORE_LIBRARY AND G2O_INCLUDE_DIR AND G2O_CONFIG_FILE AND G2O_SOLVERS_FOUND)

cmake_modules/FindK4A.cmake

@@ -0,0 +1,34 @@
+# - Find K4A 
+# This module finds an kinect 4 azure SDK
+#
+# It sets the following variables:
+#  K4A_FOUND       - Set to false, or undefined, if K4A isn't found.
+#  K4A_INCLUDE_DIRS - The K4A include directory.
+#  K4A_LIBRARIES     - The K4A library to link against.
+
+find_library(K4A_LIBRARY NAMES k4a NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_library(K4ARECORD_LIBRARY NAMES k4arecord NO_DEFAULT_PATH PATHS $ENV{K4A_ROOT_DIR}/sdk/windows-desktop/amd64/release/lib)
+find_path(K4A_INCLUDE_DIR NAMES k4a/k4a.h PATHS $ENV{K4A_ROOT_DIR}/sdk/include)
+
+IF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+   SET(K4A_FOUND TRUE)
+   SET(K4A_INCLUDE_DIRS ${K4A_INCLUDE_DIR})
+   SET(K4A_LIBRARIES ${K4A_LIBRARY} ${K4ARECORD_LIBRARY})
+
+   # Compatibility with linux names
+   SET(k4a_LIBRARIES ${K4A_LIBRARIES})
+   SET(k4a_INCLUDE_DIRS ${K4A_INCLUDE_DIRS})
+   SET(k4a_FOUND ${K4A_FOUND})
+ENDIF (K4A_INCLUDE_DIR AND K4A_LIBRARY AND K4ARECORD_LIBRARY)
+
+IF (K4A_FOUND)
+   # show which K4A was found only if not quiet
+   IF (NOT K4A_FIND_QUIETLY)
+      MESSAGE(STATUS "Found K4A: ${K4A_LIBRARIES}")
+   ENDIF (NOT K4A_FIND_QUIETLY)
+ELSE (K4A_FOUND)
+   # fatal error if K4A is required but not found
+   IF (K4A_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find K4A (Kinect for Azure SDK)")
+   ENDIF (K4A_FIND_REQUIRED)
+ENDIF (K4A_FOUND)

cmake_modules/FindKinectSDK2.cmake

@@ -0,0 +1,183 @@
+#.rst:
+# FindKinectSDK2
+# --------------
+#
+# Find Kinect for Windows SDK v2 (Kinect SDK v2) include dirs, library dirs, libraries
+#
+# Use this module by invoking find_package with the form::
+#
+#    find_package( KinectSDK2 [REQUIRED] )
+#
+# Results for users are reported in following variables::
+#
+#    KinectSDK2_FOUND                - Return "TRUE" when Kinect SDK v2 found. Otherwise, Return "FALSE".
+#    KinectSDK2_INCLUDE_DIRS         - Kinect SDK v2 include directories. (${KinectSDK2_DIR}/inc)
+#    KinectSDK2_LIBRARY_DIRS         - Kinect SDK v2 library directories. (${KinectSDK2_DIR}/Lib/x86 or ${KinectSDK2_DIR}/Lib/x64)
+#    KinectSDK2_LIBRARIES            - Kinect SDK v2 library files. (${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib (If check the box of any application festures, corresponding library will be added.))
+#    KinectSDK2_COMMANDS             - Copy commands of redist files for application functions of Kinect SDK v2. (If uncheck the box of all application features, this variable has defined empty command.)
+#
+# This module reads hints about search locations from following environment variables::
+#
+#    KINECTSDK20_DIR                 - Kinect SDK v2 root directory. (This environment variable has been set by installer of Kinect SDK v2.)
+#
+# CMake entries::
+#
+#    KinectSDK2_DIR                  - Kinect SDK v2 root directory. (Default $ENV{KINECTSDK20_DIR})
+#    KinectSDK2_FACE                 - Check the box when using Face or HDFace features. (Default uncheck)
+#    KinectSDK2_FUSION               - Check the box when using Fusion features. (Default uncheck)
+#    KinectSDK2_VGB                  - Check the box when using Visual Gesture Builder features. (Default uncheck)
+#
+# Example to find Kinect SDK v2::
+#
+#    cmake_minimum_required( VERSION 2.8 )
+#
+#    project( project )
+#    add_executable( project main.cpp )
+#    set_property( DIRECTORY PROPERTY VS_STARTUP_PROJECT "project" )
+#
+#    # Find package using this module.
+#    find_package( KinectSDK2 REQUIRED )
+#
+#    if(KinectSDK2_FOUND)
+#      # [C/C++]>[General]>[Additional Include Directories]
+#      include_directories( ${KinectSDK2_INCLUDE_DIRS} )
+#
+#      # [Linker]>[General]>[Additional Library Directories]
+#      link_directories( ${KinectSDK2_LIBRARY_DIRS} )
+#
+#      # [Linker]>[Input]>[Additional Dependencies]
+#      target_link_libraries( project ${KinectSDK2_LIBRARIES} )
+#
+#      # [Build Events]>[Post-Build Event]>[Command Line]
+#      add_custom_command( TARGET project POST_BUILD ${KinectSDK2_COMMANDS} )
+#    endif()
+#
+# =============================================================================
+#
+# Copyright (c) 2016 Tsukasa SUGIURA
+# Distributed under the MIT License.
+#
+# 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.
+#
+# =============================================================================
+
+##### Utility #####
+
+# Check Directory Macro
+macro(CHECK_DIR _DIR)
+  if(NOT EXISTS "${${_DIR}}")
+    message(WARNING "Directory \"${${_DIR}}\" not found.")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_DIR)
+  endif()
+endmacro()
+
+# Check Files Macro
+macro(CHECK_FILES _FILES _DIR)
+  set(_MISSING_FILES)
+  foreach(_FILE ${${_FILES}})
+    if(NOT EXISTS "${_FILE}")
+      get_filename_component(_FILE ${_FILE} NAME)
+      set(_MISSING_FILES "${_MISSING_FILES}${_FILE}, ")
+    endif()
+  endforeach()
+  if(_MISSING_FILES)
+    message(WARNING "In directory \"${${_DIR}}\" not found files: ${_MISSING_FILES}")
+    set(KinectSDK2_FOUND FALSE)
+    unset(_FILES)
+  endif()
+endmacro()
+
+# Target Platform
+set(TARGET_PLATFORM)
+if(NOT CMAKE_CL_64)
+  set(TARGET_PLATFORM x86)
+else()
+  set(TARGET_PLATFORM x64)
+endif()
+
+##### Find Kinect SDK v2 #####
+
+# Found
+set(KinectSDK2_FOUND TRUE)
+if(MSVC_VERSION LESS 1700)
+  message(WARNING "Kinect for Windows SDK v2 supported Visual Studio 2012 or later.")
+  set(KinectSDK2_FOUND FALSE)
+endif()
+
+# Options
+option(KinectSDK2_FACE "Face and HDFace features" FALSE)
+option(KinectSDK2_FUSION "Fusion features" FALSE)
+option(KinectSDK2_VGB "Visual Gesture Builder features" FALSE)
+
+# Root Directoty
+set(KinectSDK2_DIR)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_DIR $ENV{KINECTSDK20_DIR} CACHE PATH "Kinect for Windows SDK v2 Install Path." FORCE)
+  check_dir(KinectSDK2_DIR)
+endif()
+
+# Include Directories
+set(KinectSDK2_INCLUDE_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_INCLUDE_DIRS ${KinectSDK2_DIR}/inc)
+  check_dir(KinectSDK2_INCLUDE_DIRS)
+endif()
+
+# Library Directories
+set(KinectSDK2_LIBRARY_DIRS)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARY_DIRS ${KinectSDK2_DIR}/Lib/${TARGET_PLATFORM})
+  check_dir(KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Dependencies
+set(KinectSDK2_LIBRARIES)
+if(KinectSDK2_FOUND)
+  set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARY_DIRS}/Kinect20.lib)
+
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Face.lib)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.Fusion.lib)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_LIBRARIES ${KinectSDK2_LIBRARIES};${KinectSDK2_LIBRARY_DIRS}/Kinect20.VisualGestureBuilder.lib)
+  endif()
+
+  check_files(KinectSDK2_LIBRARIES KinectSDK2_LIBRARY_DIRS)
+endif()
+
+# Custom Commands
+set(KinectSDK2_COMMANDS)
+if(KinectSDK2_FOUND)
+  if(KinectSDK2_FACE)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Face/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_FUSION)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/Fusion/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  if(KinectSDK2_VGB)
+    set(KinectSDK2_REDIST_DIR ${KinectSDK2_DIR}/Redist/VGB/${TARGET_PLATFORM})
+    check_dir(KinectSDK2_REDIST_DIR)
+    list(APPEND KinectSDK2_COMMANDS COMMAND xcopy "${KinectSDK2_REDIST_DIR}" "$(OutDir)" /e /y /i /r > NUL)
+  endif()
+
+  # Empty Commands
+  if(NOT KinectSDK2_COMMANDS)
+    set(KinectSDK2_COMMANDS COMMAND)
+  endif()
+endif()
+
+message(STATUS "KinectSDK2_FOUND : ${KinectSDK2_FOUND}")

cmake_modules/FindORB_SLAM2.cmake

@@ -0,0 +1,33 @@
+# - Find ORB_SLAM2 
+#
+# It sets the following variables:
+#  ORB_SLAM2_FOUND         - Set to false, or undefined, if ORB_SLAM2 isn't found.
+#  ORB_SLAM2_INCLUDE_DIRS  - The ORB_SLAM2 include directory.
+#  ORB_SLAM2_LIBRARIES     - The ORB_SLAM2 library to link against.
+#
+#  Set ORB_SLAM2_ROOT_DIR environment variable as the path to ORB_SLAM2 root folder.
+
+find_path(ORB_SLAM2_INCLUDE_DIR NAMES System.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/include)
+find_library(ORB_SLAM2_LIBRARY NAMES ORB_SLAM2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/lib)
+find_path(g2o_INCLUDE_DIR NAMES g2o/core/sparse_optimizer.h PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o NO_DEFAULT_PATH)
+find_library(g2o_LIBRARY NAMES g2o PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/g2o/lib NO_DEFAULT_PATH)
+find_library(DBoW2_LIBRARY NAMES DBoW2 PATHS $ENV{ORB_SLAM2_ROOT_DIR}/Thirdparty/DBoW2/lib NO_DEFAULT_PATH)
+
+IF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+   SET(ORB_SLAM2_FOUND TRUE)
+   SET(ORB_SLAM2_INCLUDE_DIRS ${ORB_SLAM2_INCLUDE_DIR} ${g2o_INCLUDE_DIR} $ENV{ORB_SLAM2_ROOT_DIR})
+   SET(ORB_SLAM2_LIBRARIES ${g2o_LIBRARY} ${ORB_SLAM2_LIBRARY} ${DBoW2_LIBRARY})
+ENDIF (ORB_SLAM2_INCLUDE_DIR AND ORB_SLAM2_LIBRARY AND DBoW2_LIBRARY AND g2o_INCLUDE_DIR AND g2o_LIBRARY)
+
+IF (ORB_SLAM2_FOUND)
+   # show which ORB_SLAM2 was found only if not quiet
+   IF (NOT ORB_SLAM2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found ORB_SLAM2: ${ORB_SLAM2_LIBRARIES}")
+   ENDIF (NOT ORB_SLAM2_FIND_QUIETLY)
+ELSE (ORB_SLAM2_FOUND)
+   # fatal error if ORB_SLAM2 is required but not found
+   IF (ORB_SLAM2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find ORB_SLAM2")
+   ENDIF (ORB_SLAM2_FIND_REQUIRED)
+ENDIF (ORB_SLAM2_FOUND)
+

cmake_modules/FindRealSense2.cmake

@@ -0,0 +1,41 @@
+# - Find librealsense (https://github.com/IntelRealSense/librealsense)
+#
+#  RealSense2_ROOT_DIR environment variable can be set to find the library.
+#
+# It sets the following variables:
+#  RealSense2_FOUND         - Set to false, or undefined, if RealSense2 isn't found.
+#  RealSense2_INCLUDE_DIRS  - The RealSense2 include directory.
+#  RealSense2_LIBRARIES     - The RealSense2 library to link against.
+
+#RealSense library
+
+find_path(RealSense2_INCLUDE_DIRS NAMES librealsense2/rs.hpp PATHS $ENV{RealSense2_ROOT_DIR}/include)
+if(CMAKE_CL_64)
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x64  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x64)
+else()
+find_library(RealSense2_LIBRARY NAMES realsense2 PATHS $ENV{RealSense2_ROOT_DIR}/lib $ENV{RealSense2_ROOT_DIR}/lib/x86  $ENV{RealSense2_ROOT_DIR}/bin $ENV{RealSense2_ROOT_DIR}/bin/x86)
+endif()
+
+IF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+   SET(RealSense2_FOUND TRUE)
+ENDIF (RealSense2_INCLUDE_DIRS AND RealSense2_LIBRARY)
+
+IF (RealSense2_FOUND)
+   SET(RealSense2_LIBRARIES ${RealSense2_LIBRARY})
+   
+   # Compatibility with linux names
+   SET(realsense2_LIBRARIES ${RealSense2_LIBRARIES})
+   SET(realsense2_INCLUDE_DIRS ${RealSense2_INCLUDE_DIRS})
+   SET(realsense2_FOUND ${RealSense2_FOUND})
+  
+   # show which RealSense was found only if not quiet
+   IF (NOT RealSense2_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RealSense: ${RealSense2_LIBRARIES}")
+   ENDIF (NOT RealSense2_FIND_QUIETLY)
+ELSE (RealSense2_FOUND)
+   # fatal error if RealSense is required but not found
+   IF (RealSense2_FIND_REQUIRED)
+      MESSAGE(FATAL_ERROR "Could not find RealSense2 (librealsense2)")
+   ENDIF (RealSense2_FIND_REQUIRED)
+ENDIF (RealSense2_FOUND)
+

cmake_modules/FindSqlite3.cmake

@@ -2,46 +2,29 @@
 # This module finds an installed Sqlite3 package.
 #
 # It sets the following variables:
-#  SQLITE3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
+#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  SQLITE3_INCLUDE_DIR - The Sqlite3 include directory.
+#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
-#  SQLITE3_LIBRARY     - The Sqlite3 library to link against.
+#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
 
-SET(SQLITE3_VERSION_REQUIRED "3.6.0")
+FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
 
-IF(UNIX)
+FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
-	FIND_PROGRAM(SQLITE3_EXEC NAME sqlite3 PATHS)  
-	IF(SQLITE3_EXEC)  
-        MESSAGE(STATUS "Found Sqlite3 executable : ${SQLITE3_EXEC}")
-		EXECUTE_PROCESS(COMMAND ${SQLITE3_EXEC} --version 
-				   	    OUTPUT_VARIABLE SQLITE3_VERSION
-						OUTPUT_STRIP_TRAILING_WHITESPACE
-						WORKING_DIRECTORY "./"
-   		)
-		IF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-		    MESSAGE(FATAL_ERROR "Sqlite ${SQLITE3_VERSION} found, but version ${SQLITE3_VERSION_REQUIRED} minimum is required")
-		ENDIF(SQLITE3_VERSION VERSION_LESS SQLITE3_VERSION_REQUIRED)
-	ELSE(SQLITE3_EXEC)
-		MESSAGE(FATAL_ERROR "Could not find Sqlite3 executable")
-	ENDIF(SQLITE3_EXEC)
-ENDIF(UNIX)
 
-FIND_PATH(SQLITE3_INCLUDE_DIR sqlite3.h)
+IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
+   SET(Sqlite3_FOUND TRUE)
+   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
+   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
+ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
 
-FIND_LIBRARY(SQLITE3_LIBRARY NAMES sqlite3.dll sqlite3)
+IF (Sqlite3_FOUND)
-
-IF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-   SET(SQLITE3_FOUND TRUE)
-ENDIF (SQLITE3_INCLUDE_DIR AND SQLITE3_LIBRARY)
-
-IF (SQLITE3_FOUND)
    # show which Sqlite3 was found only if not quiet
    IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3")
+      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
    ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (SQLITE3_FOUND)
+ELSE (Sqlite3_FOUND)
    # fatal error if Sqlite3 is required but not found
    IF (Sqlite3_FIND_REQUIRED)
       MESSAGE(FATAL_ERROR "Could not find Sqlite3")
    ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (SQLITE3_FOUND)
+ENDIF (Sqlite3_FOUND)
 

corelib/include/rtabmap/core/BayesFilter.h

@@ -59,18 +59,14 @@ public:
 	const std::vector<double> & getPredictionLC() const; // {Vp, Lc, l1, l2, l3, l4...}
 	std::string getPredictionLCStr() const; // for convenience {Vp, Lc, l1, l2, l3, l4...}
 
-	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids) const;
+	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,
 			const std::vector<int> & oldIds,
-			const std::vector<int> & newIds) const;
+			const std::vector<int> & newIds);
 	void updatePosterior(const Memory * memory, const std::vector<int> & likelihoodIds);
-	float addNeighborProb(cv::Mat & prediction,
-			unsigned int col,
-			const std::map<int, int> & neighbors,
-			const std::map<int, int> & idToIndexMap) const;
 	void normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const;
 
 private:
@@ -80,6 +76,8 @@ private:
 	std::vector<double> _predictionLC; // {Vp, Lc, l1, l2, l3, l4...}
 	bool _fullPredictionUpdate;
 	float _totalPredictionLCValues;
+	float _predictionEpsilon;
+	std::map<int, std::map<int, int> > _neighborsIndex;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Camera.h

@@ -53,6 +53,7 @@ public:
 	virtual ~Camera();
 	SensorData takeImage(CameraInfo * info = 0);
 
+	bool initFromFile(const std::string & calibrationPath);
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
@@ -66,13 +67,14 @@ public:
 	void setImageRate(float imageRate) {_imageRate = imageRate;}
 	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
 
+	void resetTimer();
 protected:
 	/**
 	 * Constructor
 	 *
 	 * @param imageRate : image/second , 0 for fast as the camera can
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
+	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraInfo.h

@@ -43,6 +43,7 @@ public:
 		timeCapture(0.0f),
 		timeDisparity(0.0f),
 		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
 		timeImageDecimation(0.0f),
 		timeScanFromDepth(0.0f),
 		timeUndistortDepth(0.0f),
@@ -59,6 +60,7 @@ public:
 	float timeCapture;
 	float timeDisparity;
 	float timeMirroring;
+	float timeStereoExposureCompensation;
 	float timeImageDecimation;
 	float timeScanFromDepth;
 	float timeUndistortDepth;
@@ -66,6 +68,7 @@ public:
 	float timeTotal;
 	Transform odomPose;
 	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/CameraModel.h

@@ -37,6 +37,13 @@ namespace rtabmap {
 
 class RTABMAP_EXP CameraModel
 {
+public:
+	/**
+	 * Optical rotation used to transform image coordinate frame (x->right, y->down, z->forward)
+	 * to robot coordinate frame (x->forward, y->left, z->up).
+	 */
+	static Transform opticalRotation() {return Transform(0,0,1,0, -1,0,0,0, 0,-1,0,0);}
+
 public:
 	CameraModel();
 	// K is the camera intrinsic 3x3 CV_64FC1
@@ -50,7 +57,7 @@ public:
 			const cv::Mat & D,
 			const cv::Mat & R,
 			const cv::Mat & P,
-			const Transform & localTransform = Transform::getIdentity());
+			const Transform & localTransform = opticalRotation());
 
 	// minimal
 	CameraModel(
@@ -58,7 +65,7 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 	// minimal to be saved
@@ -68,13 +75,14 @@ public:
 			double fy,
 			double cx,
 			double cy,
-			const Transform & localTransform = Transform::getIdentity(),
+			const Transform & localTransform = opticalRotation(),
 			double Tx = 0.0f,
 			const cv::Size & imageSize = cv::Size(0,0));
 
 	virtual ~CameraModel() {}
 
 	void initRectificationMap();
+	bool isRectificationMapInitialized() const {return !mapX_.empty() && !mapY_.empty();}
 
 	bool isValidForProjection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0;}
 	bool isValidForReprojection() const {return fx()>0.0 && fy()>0.0 && cx()>0.0 && cy()>0.0 && imageWidth()>0 && imageHeight()>0;}
@@ -112,15 +120,21 @@ public:
 	int imageWidth() const {return imageSize_.width;}
 	int imageHeight() const {return imageSize_.height;}
 
+	double fovX() const; // in radians
+	double fovY() const; // in radians
+	double horizontalFOV() const; // in degrees
+	double verticalFOV() const;   // in degrees
+
+	bool load(const std::string & filePath);
 	bool load(const std::string & directory, const std::string & cameraName);
 	bool save(const std::string & directory) const;
+	std::vector<unsigned char> serialize() const;
+	unsigned int deserialize(const std::vector<unsigned char>& data);
+	unsigned int deserialize(const unsigned char * data, unsigned int dataSize);
 
 	CameraModel scaled(double scale) const;
 	CameraModel roi(const cv::Rect & roi) const;
 
-	double horizontalFOV() const; // in degrees
-	double verticalFOV() const;   // in degrees
-
 	// For depth images, your should use cv::INTER_NEAREST
 	cv::Mat rectifyImage(const cv::Mat & raw, int interpolation = cv::INTER_LINEAR) const;
 	cv::Mat rectifyDepth(const cv::Mat & raw) const;

corelib/include/rtabmap/core/CameraRGB.h

@@ -27,207 +27,5 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <rtabmap/core/camera/CameraImages.h>
-
+#include <rtabmap/core/camera/CameraVideo.h>
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/utilite/UTimer.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraImages
-/////////////////////////
-class RTABMAP_EXP CameraImages :
-	public Camera
-{
-public:
-	CameraImages();
-	CameraImages(
-			const std::string & path,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const { return odometry_.size() > 0; }
-	std::string getPath() const {return _path;}
-	unsigned int imagesCount() const;
-	std::vector<std::string> filenames() const;
-	bool isImagesRectified() const {return _rectifyImages;}
-	int getBayerMode() const {return _bayerMode;}
-	const CameraModel & cameraModel() const {return _model;}
-
-	void setPath(const std::string & dir) {_path=dir;}
-	void setStartIndex(int index) {_startAt = index;} // negative means last
-	void setDirRefreshed(bool enabled) {_refreshDir = enabled;}
-	void setImagesRectified(bool enabled) {_rectifyImages = enabled;}
-	void setBayerMode(int mode) {_bayerMode = mode;} // -1=disabled (default) 0=BayerBG, 1=BayerGB, 2=BayerRG, 3=BayerGR
-
-	void setTimestamps(bool fileNamesAreStamps, const std::string & filePath = "", bool syncImageRateWithStamps=true)
-	{
-		_filenamesAreTimestamps = fileNamesAreStamps;
-		_timestampsPath=filePath;
-		_syncImageRateWithStamps = syncImageRateWithStamps;
-	}
-
-	void setScanPath(
-			const std::string & dir,
-			int maxScanPts = 0,
-			int downsampleStep = 1,
-			float voxelSize = 0.0f,
-			int normalsK = 0, // compute normals if > 0
-			const Transform & localTransform=Transform::getIdentity())
-	{
-		_scanPath = dir;
-		_scanLocalTransform = localTransform;
-		_scanMaxPts = maxScanPts;
-		_scanDownsampleStep = downsampleStep;
-		_scanNormalsK = normalsK;
-		_scanVoxelSize = voxelSize;
-		if(_scanDownsampleStep>1)
-		{
-			_scanMaxPts /= _scanDownsampleStep;
-		}
-	}
-
-	void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
-	{
-		_depthFromScan = enabled;
-		_depthFromScanFillHoles = fillHoles;
-		_depthFromScanFillHolesFromBorder = fillHolesFromBorder;
-	}
-
-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
-	void setOdometryPath(const std::string & filePath, int format = 0)
-	{
-		_odometryPath = filePath;
-		_odometryFormat = format;
-	}
-
-	// Format: 0=Raw, 1=RGBD-SLAM, 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe
-	void setGroundTruthPath(const std::string & filePath, int format = 0)
-	{
-		_groundTruthPath = filePath;
-		_groundTruthFormat = format;
-	}
-
-	void setDepth(bool isDepth, float depthScaleFactor = 1.0f)
-	{
-		_isDepth = isDepth;
-		_depthScaleFactor=depthScaleFactor;
-	}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	bool readPoses(
-			std::list<Transform> & outputPoses,
-			std::list<double> & stamps,
-			const std::string & filePath,
-			int format) const;
-
-private:
-	std::string _path;
-	int _startAt;
-	// If the list of files in the directory is refreshed
-	// on each call of takeImage()
-	bool _refreshDir;
-	bool _rectifyImages;
-	int _bayerMode;
-	bool _isDepth;
-	float _depthScaleFactor;
-	int _count;
-	UDirectory * _dir;
-	std::string _lastFileName;
-
-	int _countScan;
-	UDirectory * _scanDir;
-	std::string _lastScanFileName;
-	std::string _scanPath;
-	Transform _scanLocalTransform;
-	int _scanMaxPts;
-	int _scanDownsampleStep;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-
-	bool _depthFromScan;
-	int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical
-	bool _depthFromScanFillHolesFromBorder;
-
-	bool _filenamesAreTimestamps;
-	std::string _timestampsPath;
-	bool _syncImageRateWithStamps;
-
-	std::string _odometryPath;
-	int _odometryFormat;
-
-	std::string _groundTruthPath;
-	int _groundTruthFormat;
-
-	std::list<double> _stamps;
-	std::list<Transform> odometry_;
-	std::list<Transform> groundTruth_;
-	CameraModel _model;
-
-	UTimer _captureTimer;
-	double _captureDelay;
-};
-
-
-
-
-/////////////////////////
-// CameraVideo
-/////////////////////////
-class RTABMAP_EXP CameraVideo :
-	public Camera
-{
-public:
-	enum Source{kVideoFile, kUsbDevice};
-
-public:
-	CameraVideo(int usbDevice = 0,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraVideo(const std::string & filePath,
-			bool rectifyImages = false,
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	int getUsbDevice() const {return _usbDevice;}
-	const std::string & getFilePath() const {return _filePath;}
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	// File type
-	std::string _filePath;
-	bool _rectifyImages;
-
-	cv::VideoCapture _capture;
-	Source _src;
-
-	// Usb camera
-	int _usbDevice;
-	std::string _guid;
-
-	CameraModel _model;
-};
-
-
-} // namespace rtabmap

corelib/include/rtabmap/core/CameraRGBD.h

@@ -27,347 +27,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <rtabmap/core/camera/CameraFreenect.h>
+#include <rtabmap/core/camera/CameraFreenect2.h>
+#include <rtabmap/core/camera/CameraK4W2.h>
+#include <rtabmap/core/camera/CameraOpenni.h>
+#include <rtabmap/core/camera/CameraOpenNI2.h>
+#include <rtabmap/core/camera/CameraOpenNICV.h>
+#include <rtabmap/core/camera/CameraRealSense.h>
+#include <rtabmap/core/camera/CameraRealSense2.h>
+#include <rtabmap/core/camera/CameraRGBDImages.h>
+#include <rtabmap/core/camera/CameraK4A.h>
 
-#include "rtabmap/utilite/UMutex.h"
-#include "rtabmap/utilite/USemaphore.h"
-#include "rtabmap/core/CameraModel.h"
-#include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraRGB.h"
-#include "rtabmap/core/Version.h"
-
-#include <pcl/pcl_config.h>
-
-#ifdef HAVE_OPENNI
-#include <pcl/io/openni_camera/openni_depth_image.h>
-#include <pcl/io/openni_camera/openni_image.h>
-#endif
-
-#include <boost/signals2/connection.hpp>
-
-namespace openni
-{
-class Device;
-class VideoStream;
-}
-
-namespace pcl
-{
-class Grabber;
-}
-
-namespace libfreenect2
-{
-class Freenect2;
-class Freenect2Device;
-class SyncMultiFrameListener;
-class Registration;
-class PacketPipeline;
-}
-
-namespace rs
-{
-	class context;
-	class device;
-	namespace slam {
-	class slam;
-	}
-}
-
-typedef struct _freenect_context freenect_context;
-typedef struct _freenect_device freenect_device;
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraOpenNIPCL
-/////////////////////////
-class RTABMAP_EXP CameraOpenni :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraOpenni(const std::string & deviceId="",
-			float imageRate = 0,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenni();
-#ifdef HAVE_OPENNI
-    void image_cb (
-    		const boost::shared_ptr<openni_wrapper::Image>& rgb,
-			const boost::shared_ptr<openni_wrapper::DepthImage>& depth,
-			float constant);
-#endif
-
-    virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-    virtual bool isCalibrated() const;
-    virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-    pcl::Grabber* interface_;
-    std::string deviceId_;
-    boost::signals2::connection connection_;
-    cv::Mat depth_;
-    cv::Mat rgb_;
-    float depthConstant_;
-    UMutex dataMutex_;
-    USemaphore dataReady_;
-};
-
-/////////////////////////
-// CameraOpenNICV
-/////////////////////////
-class RTABMAP_EXP CameraOpenNICV :
-	public Camera
-{
-
-public:
-	static bool available();
-
-public:
-	CameraOpenNICV(bool asus = false,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNICV();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const {return "";} // unknown with OpenCV
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	bool _asus;
-	cv::VideoCapture _capture;
-	float _depthFocal;
-};
-
-/////////////////////////
-// CameraOpenNI2
-/////////////////////////
-class RTABMAP_EXP CameraOpenNI2 :
-	public Camera
-{
-
-public:
-	static bool available();
-	static bool exposureGainAvailable();
-	enum Type {kTypeColorDepth, kTypeIRDepth, kTypeIR};
-
-public:
-	CameraOpenNI2(const std::string & deviceId = "",
-					Type type = kTypeColorDepth,
-					float imageRate = 0,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraOpenNI2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-	bool setAutoWhiteBalance(bool enabled);
-	bool setAutoExposure(bool enabled);
-	bool setExposure(int value);
-	bool setGain(int value);
-	bool setMirroring(bool enabled);
-	void setOpenNI2StampsAndIDsUsed(bool used);
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_OPENNI2
-	Type _type;
-	openni::Device * _device;
-	openni::VideoStream * _color;
-	openni::VideoStream * _depth;
-	float _depthFx;
-	float _depthFy;
-	std::string _deviceId;
-	bool _openNI2StampsAndIDsUsed;
-	StereoCameraModel _stereoModel;
-#endif
-};
-
-
-/////////////////////////
-// CameraFreenect
-/////////////////////////
-class FreenectDevice;
-
-class RTABMAP_EXP CameraFreenect :
-	public Camera
-{
-public:
-	static bool available();
-	enum Type {kTypeColorDepth, kTypeIRDepth};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect(int deviceId= 0,
-					Type type = kTypeColorDepth,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraFreenect();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT
-	int deviceId_;
-	Type type_;
-	freenect_context * ctx_;
-	FreenectDevice * freenectDevice_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraFreenect2
-/////////////////////////
-
-class RTABMAP_EXP CameraFreenect2 :
-	public Camera
-{
-public:
-	static bool available();
-
-	enum Type{
-		kTypeColor2DepthSD,
-		kTypeDepth2ColorSD,
-		kTypeDepth2ColorHD,
-		kTypeDepth2ColorHD2,
-		kTypeIRDepth,
-		kTypeColorIR
-	};
-
-public:
-	// default local transform z in, x right, y down));
-	CameraFreenect2(int deviceId= 0,
-					Type type = kTypeColor2DepthSD,
-					float imageRate=0.0f,
-					const Transform & localTransform = Transform::getIdentity(),
-					float minDepth = 0.3f,
-					float maxDepth = 12.0f,
-					bool bilateralFiltering = true,
-					bool edgeAwareFiltering = true,
-					bool noiseFiltering = true);
-	virtual ~CameraFreenect2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FREENECT2
-	int deviceId_;
-	Type type_;
-	StereoCameraModel stereoModel_;
-	libfreenect2::Freenect2 * freenect2_;
-	libfreenect2::Freenect2Device *dev_;
-	libfreenect2::SyncMultiFrameListener * listener_;
-	libfreenect2::Registration * reg_;
-	float minKinect2Depth_;
-	float maxKinect2Depth_;
-	bool bilateralFiltering_;
-	bool edgeAwareFiltering_;
-	bool noiseFiltering_;
-#endif
-};
-
-/////////////////////////
-// CameraRealSense
-/////////////////////////
-class slam_event_handler;
-class RTABMAP_EXP CameraRealSense :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	// default local transform z in, x right, y down));
-	CameraRealSense(
-		int deviceId = 0,
-		int presetRGB = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		int presetDepth = 0, // 0=best quality, 1=largest image, 2=highest framerate
-		bool computeOdometry = false,
-		float imageRate = 0,
-		const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRealSense();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_REALSENSE
-	rs::context * ctx_;
-	rs::device * dev_;
-	int deviceId_;
-	int presetRGB_;
-	int presetDepth_;
-	bool computeOdometry_;
-
-	int motionSeq_[2];
-	rs::slam::slam * slam_;
-	UMutex slamLock_;
-
-	std::map<double, std::pair<cv::Mat, cv::Mat> > bufferedFrames_;
-	std::pair<cv::Mat, cv::Mat> lastSyncFrames_;
-	UMutex dataMutex_;
-	USemaphore dataReady_;
-#endif
-};
-
-
-/////////////////////////
-// CameraRGBDImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraRGBDImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraRGBDImages(
-			const std::string & pathRGBImages,
-			const std::string & pathDepthImages,
-			float depthScaleFactor = 1.0f,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraRGBDImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages cameraDepth_;
-};
-
-} // namespace rtabmap

corelib/include/rtabmap/core/CameraStereo.h

@@ -27,222 +27,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #pragma once
 
-#include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
+#include <rtabmap/core/camera/CameraStereoDC1394.h>
-
+#include <rtabmap/core/camera/CameraStereoFlyCapture2.h>
-#include "rtabmap/core/CameraModel.h"
+#include <rtabmap/core/camera/CameraStereoImages.h>
-#include "rtabmap/core/Camera.h"
+#include <rtabmap/core/camera/CameraStereoVideo.h>
-#include "rtabmap/core/CameraRGB.h"
+#include <rtabmap/core/camera/CameraStereoZed.h>
-#include "rtabmap/core/Version.h"
+#include <rtabmap/core/camera/CameraStereoTara.h>
-#include <list>
+#include <rtabmap/core/camera/CameraMyntEye.h>
-
-namespace FlyCapture2
-{
-class Camera;
-}
-
-namespace sl
-{
-class Camera;
-}
-
-namespace rtabmap
-{
-
-/////////////////////////
-// CameraStereoDC1394
-/////////////////////////
-class DC1394Device;
-
-class RTABMAP_EXP CameraStereoDC1394 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoDC1394( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoDC1394();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_DC1394
-	DC1394Device *device_;
-	StereoCameraModel stereoModel_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoFlyCapture2
-/////////////////////////
-class RTABMAP_EXP CameraStereoFlyCapture2 :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoFlyCapture2( float imageRate=0.0f, const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoFlyCapture2();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_FLYCAPTURE2
-	FlyCapture2::Camera * camera_;
-	void * triclopsCtx_; // TriclopsContext
-#endif
-};
-
-/////////////////////////
-// CameraStereoZED
-/////////////////////////
-class RTABMAP_EXP CameraStereoZed :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoZed(
-			int deviceId,
-			int resolution = 2, // 0=HD2K, 1=HD1080, 2=HD720, 3=VGA
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 0,// 0=STANDARD, 1=FILL
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	CameraStereoZed(
-			const std::string & svoFilePath,
-			int quality = 1,    // 0=NONE, 1=PERFORMANCE, 2=QUALITY
-			int sensingMode = 0,// 0=STANDARD, 1=FILL
-			int confidenceThr = 100,
-			bool computeOdometry = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity(),
-			bool selfCalibration = false);
-	virtual ~CameraStereoZed();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-	virtual bool odomProvided() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-#ifdef RTABMAP_ZED
-	sl::Camera * zed_;
-	StereoCameraModel stereoModel_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-	std::string svoFilePath_;
-	int resolution_;
-	int quality_;
-	bool selfCalibration_;
-	int sensingMode_;
-	int confidenceThr_;
-	bool computeOdometry_;
-	bool lost_;
-#endif
-};
-
-/////////////////////////
-// CameraStereoImages
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoImages :
-	public CameraImages
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoImages(
-			const std::string & pathLeftImages,
-			const std::string & pathRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoImages(
-			const std::string & pathLeftRightImages,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoImages();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	CameraImages * camera2_;
-	StereoCameraModel stereoModel_;
-};
-
-
-/////////////////////////
-// CameraStereoVideo
-/////////////////////////
-class CameraImages;
-class RTABMAP_EXP CameraStereoVideo :
-	public Camera
-{
-public:
-	static bool available();
-
-public:
-	CameraStereoVideo(
-			const std::string & pathSideBySide,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			const std::string & pathLeft,
-			const std::string & pathRight,
-			bool rectifyImages = false,
-			float imageRate=0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	CameraStereoVideo(
-			int device,
-			bool rectifyImages = false,
-			float imageRate = 0.0f,
-			const Transform & localTransform = Transform::getIdentity());
-	virtual ~CameraStereoVideo();
-
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual bool isCalibrated() const;
-	virtual std::string getSerial() const;
-
-protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-
-private:
-	cv::VideoCapture capture_;
-	cv::VideoCapture capture2_;
-	std::string path_;
-	std::string path2_;
-	bool rectifyImages_;
-	StereoCameraModel stereoModel_;
-	std::string cameraName_;
-	CameraVideo::Source src_;
-	int usbDevice_;
-};
-
-} // namespace rtabmap

corelib/include/rtabmap/core/CameraThread.h

@@ -45,6 +45,7 @@ class Camera;
 class CameraInfo;
 class SensorData;
 class StereoDense;
+class IMUFilter;
 
 /**
  * Class CameraThread
@@ -60,6 +61,7 @@ public:
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
 	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
 	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
 	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
@@ -67,19 +69,27 @@ public:
 	void setDistortionModel(const std::string & path);
 	void enableBilateralFiltering(float sigmaS, float sigmaR);
 	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap());
+	void disableIMUFiltering();
 
-	void setScanFromDepth(
+	void setScanParameters(
-			bool enabled,
+			bool fromDepth,
-			int decimation=4,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
-			float maxDepth=4.0f,
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
 			float voxelSize = 0.0f,
-			int normalsK = 0)
+			int normalsK = 0,
+			int normalsRadius = 0.0f,
+			bool forceGroundNormalsUp = false)
 	{
-		_scanFromDepth = enabled;
+		_scanFromDepth = fromDepth;
-		_scanDecimation=decimation;
+		_scanDownsampleStep=downsampleStep;
-		_scanMaxDepth = maxDepth;
+		_scanRangeMin = rangeMin;
+		_scanRangeMax = rangeMax;
 		_scanVoxelSize = voxelSize;
 		_scanNormalsK = normalsK;
+		_scanNormalsRadius = normalsRadius;
+		_scanForceGroundNormalsUp = forceGroundNormalsUp;
 	}
 
 	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -98,20 +108,24 @@ private:
 private:
 	Camera * _camera;
 	bool _mirroring;
+	bool _stereoExposureCompensation;
 	bool _colorOnly;
 	int _imageDecimation;
 	bool _stereoToDepth;
 	bool _scanFromDepth;
-	int _scanDecimation;
+	int _scanDownsampleStep;
-	float _scanMaxDepth;
+	float _scanRangeMin;
-	float _scanMinDepth;
+	float _scanRangeMax;
 	float _scanVoxelSize;
 	int _scanNormalsK;
+	float _scanNormalsRadius;
+	bool _scanForceGroundNormalsUp;
 	StereoDense * _stereoDense;
 	clams::DiscreteDepthDistortionModel * _distortionModel;
 	bool _bilateralFiltering;
 	float _bilateralSigmaS;
 	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/Compression.h

@@ -83,5 +83,8 @@ cv::Mat RTABMAP_EXP uncompressData(const cv::Mat & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const std::vector<unsigned char> & bytes);
 cv::Mat RTABMAP_EXP uncompressData(const unsigned char * bytes, unsigned long size);
 
+cv::Mat RTABMAP_EXP compressString(const std::string & str);
+std::string RTABMAP_EXP uncompressString(const cv::Mat & bytes);
+
 } /* namespace rtabmap */
 #endif /* COMPRESSION_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -68,6 +68,7 @@ public:
 	virtual ~DBDriver();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return _url.empty();}
 	const std::string & getUrl() const {return _url;}
 
 	void beginTransaction() const;
@@ -91,13 +92,38 @@ public:
 				int nodeId,
 				const cv::Mat & ground,
 				const cv::Mat & obstacles,
+				const cv::Mat & empty,
 				float cellSize,
 				const cv::Point3f & viewpoint);
 	void updateDepthImage(int nodeId, const cv::Mat & image);
+	void updateLaserScan(int nodeId, const LaserScan & scan);
 
 public:
 	void addInfoAfterRun(int stMemSize, int lastSignAdded, int processMemUsed, int databaseMemUsed, int dictionarySize, const ParametersMap & parameters) const;
-	void addStatistics(const Statistics & statistics) const;
+	void addStatistics(const Statistics & statistics, bool saveWmState) const;
+	void savePreviewImage(const cv::Mat & image) const;
+	cv::Mat loadPreviewImage() const;
+	void saveOptimizedPoses(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	std::map<int, Transform> loadOptimizedPoses(Transform * lastlocalizationPose = 0) const;
+	void save2DMap(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	cv::Mat load2DMap(float & xMin, float & yMin, float & cellSize) const;
+	void saveOptimizedMesh(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons = std::vector<std::vector<std::vector<unsigned int> > >(),      // Textures -> polygons -> vertices
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords = std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > >(), // Textures -> uv coords for each vertex of the polygons
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords = std::vector<std::vector<Eigen::Vector2f> >(), // Textures -> uv coords for each vertex of the polygons
+#endif
+			const cv::Mat & textures = cv::Mat()) const; // concatenated textures (assuming square textures with all same size);
+	cv::Mat loadOptimizedMesh(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons = 0,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords = 0,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords = 0,
+#endif
+			cv::Mat * textures = 0) const;
 
 public:
 	// Mutex-protected methods of abstract versions below
@@ -107,67 +133,88 @@ public:
 	bool isConnected() const;
 	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
+	long getNodesMemoryUsed() const;
+	long getLinksMemoryUsed() const;
 	long getImagesMemoryUsed() const;
 	long getDepthImagesMemoryUsed() const;
+	long getCalibrationsMemoryUsed() const;
+	long getGridsMemoryUsed() const;
 	long getLaserScansMemoryUsed() const;
 	long getUserDataMemoryUsed() const;
 	long getWordsMemoryUsed() const;
+	long getFeaturesMemoryUsed() const;
+	long getStatisticsMemoryUsed() const;
 	int getLastNodesSize() const; // working memory
 	int getLastDictionarySize() const; // working memory
 	int getTotalNodesSize() const;
 	int getTotalDictionarySize() const;
 	ParametersMap getLastParameters() const;
-	std::map<std::string, float> getStatistics(int nodeId, double & stamp) const;
+	std::map<std::string, float> getStatistics(int nodeId, double & stamp, std::vector<int> * wmState=0) const;
+	std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatistics() const;
+	std::map<int, std::vector<int> > getAllStatisticsWmStates() const;
 
 	void executeNoResult(const std::string & sql) const;
 
 	// Load objects
-	void load(VWDictionary * dictionary) const;
+	void load(VWDictionary * dictionary, bool lastStateOnly = true) const;
-	void loadLastNodes(std::list<Signature *> & signatures) const;
+	void loadLastNodes(std::list<Signature *> & signatures) const; // returned signatures must be freed after usage
-	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0);
+	Signature * loadSignature(int id, bool * loadedFromTrash = 0); // returned signature must be freed after usage, call loadSignatures() instead if more than one signature should be loaded
-	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws);
+	void loadSignatures(const std::list<int> & ids, std::list<Signature *> & signatures, std::set<int> * loadedFromTrash = 0); // returned signatures must be freed after usage
+	void loadWords(const std::set<int> & wordIds, std::list<VisualWord *> & vws); // returned words must be freed after usage
 
 	// Specific queries...
+	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
-	void loadLinks(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 	void getWeight(int signatureId, int & weight) const;
+	void getLastNodeIds(std::set<int> & ids) const;
 	void getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false) const;
-	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true) const;
+	void getAllLinks(std::multimap<int, Link> & links, bool ignoreNullLinks = true, bool withLandmarks = false) const;
 	void getLastNodeId(int & id) const;
+	void getLastMapId(int & mapId) const;
 	void getLastWordId(int & id) const;
 	void getInvertedIndexNi(int signatureId, int & ni) const;
+	void getNodesObservingLandmark(int landmarkId, std::map<int, Link> & nodes) const;
 	void getNodeIdByLabel(const std::string & label, int & id) const;
 	void getAllLabels(std::map<int, std::string> & labels) const;
 
 protected:
 	DBDriver(const ParametersMap & parameters = ParametersMap());
 
-private:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
 	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
+	virtual long getNodesMemoryUsedQuery() const = 0;
+	virtual long getLinksMemoryUsedQuery() const = 0;
 	virtual long getImagesMemoryUsedQuery() const = 0;
 	virtual long getDepthImagesMemoryUsedQuery() const = 0;
+	virtual long getCalibrationsMemoryUsedQuery() const = 0;
+	virtual long getGridsMemoryUsedQuery() const = 0;
 	virtual long getLaserScansMemoryUsedQuery() const = 0;
 	virtual long getUserDataMemoryUsedQuery() const = 0;
 	virtual long getWordsMemoryUsedQuery() const = 0;
+	virtual long getFeaturesMemoryUsedQuery() const = 0;
+	virtual long getStatisticsMemoryUsedQuery() const = 0;
 	virtual int getLastNodesSizeQuery() const = 0;
 	virtual int getLastDictionarySizeQuery() const = 0;
 	virtual int getTotalNodesSizeQuery() const = 0;
 	virtual int getTotalDictionarySizeQuery() const = 0;
 	virtual ParametersMap getLastParametersQuery() const = 0;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const = 0;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const = 0;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const = 0;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const = 0;
 
 	virtual void executeNoResultQuery(const std::string & sql) const = 0;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const = 0;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const = 0;
+	virtual void saveQuery(const std::list<Signature *> & signatures) = 0;
 	virtual void saveQuery(const std::list<VisualWord *> & words) const = 0;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const = 0;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const = 0;
@@ -176,38 +223,69 @@ private:
 	virtual void updateLinkQuery(const Link & link) const = 0;
 
 	virtual void updateOccupancyGridQuery(
-				int nodeId,
+			int nodeId,
-				const cv::Mat & ground,
+			const cv::Mat & ground,
-				const cv::Mat & obstacles,
+			const cv::Mat & obstacles,
-				float cellSize,
+			const cv::Mat & empty,
-				const cv::Point3f & viewpoint) const = 0;
+			float cellSize,
+			const cv::Point3f & viewpoint) const = 0;
 
 	virtual void updateDepthImageQuery(
-					int nodeId,
+			int nodeId,
-					const cv::Mat & image) const = 0;
+			const cv::Mat & image) const = 0;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const = 0;
+	virtual void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const = 0;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const = 0;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const = 0;
+	virtual cv::Mat loadPreviewImageQuery() const = 0;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const = 0;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const = 0;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const = 0;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const = 0;
+	virtual void saveOptimizedMeshQuery(
+				const cv::Mat & cloud,
+				const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+				const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+				const cv::Mat & textures) const = 0;
+	virtual cv::Mat loadOptimizedMeshQuery(
+				std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+				std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+				std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+				cv::Mat * textures) const = 0;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const = 0;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const = 0;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const = 0;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const = 0;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const = 0;
-	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const = 0;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const = 0;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const = 0;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const = 0;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const = 0;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const = 0;
 
 private:
 	//non-abstract methods
-	void saveOrUpdate(const std::vector<Signature *> & signatures) const;
+	void saveOrUpdate(const std::vector<Signature *> & signatures);
 	void saveOrUpdate(const std::vector<VisualWord *> & words) const;
 
 	//thread stuff

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -31,7 +31,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/DBDriver.h"
 #include <opencv2/features2d/features2d.hpp>
-#include "sqlite3/sqlite3.h"
+
+typedef struct sqlite3_stmt sqlite3_stmt;
+typedef struct sqlite3 sqlite3;
 
 namespace rtabmap {
 
@@ -41,35 +43,44 @@ public:
 	virtual ~DBDriverSqlite3();
 
 	virtual void parseParameters(const ParametersMap & parameters);
+	virtual bool isInMemory() const {return getUrl().empty() || _dbInMemory;}
 	void setDbInMemory(bool dbInMemory);
 	void setJournalMode(int journalMode);
 	void setCacheSize(unsigned int cacheSize);
 	void setSynchronous(int synchronous);
 	void setTempStore(int tempStore);
 
-private:
+protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
 	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
+	virtual long getNodesMemoryUsedQuery() const;
+	virtual long getLinksMemoryUsedQuery() const;
 	virtual long getImagesMemoryUsedQuery() const;
 	virtual long getDepthImagesMemoryUsedQuery() const;
+	virtual long getCalibrationsMemoryUsedQuery() const;
+	virtual long getGridsMemoryUsedQuery() const;
 	virtual long getLaserScansMemoryUsedQuery() const;
 	virtual long getUserDataMemoryUsedQuery() const;
 	virtual long getWordsMemoryUsedQuery() const;
+	virtual long getFeaturesMemoryUsedQuery() const;
+	virtual long getStatisticsMemoryUsedQuery() const;
 	virtual int getLastNodesSizeQuery() const;
 	virtual int getLastDictionarySizeQuery() const;
 	virtual int getTotalNodesSizeQuery() const;
 	virtual int getTotalDictionarySizeQuery() const;
 	virtual ParametersMap getLastParametersQuery() const;
-	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp) const;
+	virtual std::map<std::string, float> getStatisticsQuery(int nodeId, double & stamp, std::vector<int> * wmState) const;
+	virtual std::map<int, std::pair<std::map<std::string, float>, double> > getAllStatisticsQuery() const;
+	virtual std::map<int, std::vector<int> > getAllStatisticsWmStatesQuery() const;
 
 	virtual void executeNoResultQuery(const std::string & sql) const;
 
 	virtual void getWeightQuery(int signatureId, int & weight) const;
 
-	virtual void saveQuery(const std::list<Signature *> & signatures) const;
+	virtual void saveQuery(const std::list<Signature *> & signatures);
 	virtual void saveQuery(const std::list<VisualWord *> & words) const;
 	virtual void updateQuery(const std::list<Signature *> & signatures, bool updateTimestamp) const;
 	virtual void updateQuery(const std::list<VisualWord *> & words, bool updateTimestamp) const;
@@ -81,29 +92,60 @@ private:
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;
 
 	virtual void updateDepthImageQuery(
-				int nodeId,
+			int nodeId,
-				const cv::Mat & image) const;
+			const cv::Mat & image) const;
 
-	virtual void addStatisticsQuery(const Statistics & statistics) const;
+	void updateLaserScanQuery(
+			int nodeId,
+			const LaserScan & scan) const;
+
+	virtual void addStatisticsQuery(const Statistics & statistics, bool saveWmState) const;
+	virtual void savePreviewImageQuery(const cv::Mat & image) const;
+	virtual cv::Mat loadPreviewImageQuery() const;
+	virtual void saveOptimizedPosesQuery(const std::map<int, Transform> & optimizedPoses, const Transform & lastlocalizationPose) const;
+	virtual std::map<int, Transform> loadOptimizedPosesQuery(Transform * lastlocalizationPose = 0) const;
+	virtual void save2DMapQuery(const cv::Mat & map, float xMin, float yMin, float cellSize) const;
+	virtual cv::Mat load2DMapQuery(float & xMin, float & yMin, float & cellSize) const;
+	virtual void saveOptimizedMeshQuery(
+			const cv::Mat & cloud,
+			const std::vector<std::vector<std::vector<unsigned int> > > & polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			const std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > & texCoords,
+#else
+			const std::vector<std::vector<Eigen::Vector2f> > & texCoords,
+#endif
+			const cv::Mat & textures) const;
+	virtual cv::Mat loadOptimizedMeshQuery(
+			std::vector<std::vector<std::vector<unsigned int> > > * polygons,
+#if PCL_VERSION_COMPARE(>=, 1, 8, 0)
+			std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > * texCoords,
+#else
+			std::vector<std::vector<Eigen::Vector2f> > * texCoords,
+#endif
+			cv::Mat * textures) const;
 
 	// Load objects
-	virtual void loadQuery(VWDictionary * dictionary) const;
+	virtual void loadQuery(VWDictionary * dictionary, bool lastStateOnly = true) const;
 	virtual void loadLastNodesQuery(std::list<Signature *> & signatures) const;
 	virtual void loadSignaturesQuery(const std::list<int> & ids, std::list<Signature *> & signatures) const;
 	virtual void loadWordsQuery(const std::set<int> & wordIds, std::list<VisualWord *> & vws) const;
-	virtual void loadLinksQuery(int signatureId, std::map<int, Link> & links, Link::Type type = Link::kUndef) const;
+	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
 	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
-	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const;
+	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
+	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
+	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;
 	virtual void getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures) const;
-	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks) const;
+	virtual void getAllLinksQuery(std::multimap<int, Link> & links, bool ignoreNullLinks, bool withLandmarks) const;
-	virtual void getLastIdQuery(const std::string & tableName, int & id) const;
+	virtual void getLastIdQuery(const std::string & tableName, int & id, const std::string & fieldName="id") const;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const;
+	virtual void getNodesObservingLandmarkQuery(int landmarkId, std::map<int, Link> & nodes) const;
 	virtual void getNodeIdByLabelQuery(const std::string & label, int & id) const;
 	virtual void getAllLabelsQuery(std::map<int, std::string> & labels) const;
 
@@ -112,27 +154,29 @@ private:
 	std::string queryStepImage() const;
 	std::string queryStepDepth() const;
 	std::string queryStepDepthUpdate() const;
+	std::string queryStepScanUpdate() const;
 	std::string queryStepSensorData() const;
 	std::string queryStepLinkUpdate() const;
 	std::string queryStepLink() const;
 	std::string queryStepWordsChanged() const;
 	std::string queryStepKeypoint() const;
+	std::string queryStepGlobalDescriptor() const;
 	std::string queryStepOccupancyGridUpdate() const;
 	void stepNode(sqlite3_stmt * ppStmt, const Signature * s) const;
-	void stepImage(
+	void stepImage(sqlite3_stmt * ppStmt, int id, const cv::Mat & imageBytes) const;
-			sqlite3_stmt * ppStmt,
-			int id,
-			const cv::Mat & imageBytes) const;
 	void stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepDepthUpdate(sqlite3_stmt * ppStmt, int nodeId, const cv::Mat & imageCompressed) const;
+	void stepScanUpdate(sqlite3_stmt * ppStmt, int nodeId, const LaserScan & image) const;
 	void stepSensorData(sqlite3_stmt * ppStmt, const SensorData & sensorData) const;
 	void stepLink(sqlite3_stmt * ppStmt, const Link & link) const;
 	void stepWordsChanged(sqlite3_stmt * ppStmt, int signatureId, int oldWordId, int newWordId) const;
-	void stepKeypoint(sqlite3_stmt * ppStmt, int signatureId, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepKeypoint(sqlite3_stmt * ppStmt, int nodeID, int wordId, const cv::KeyPoint & kp, const cv::Point3f & pt, const cv::Mat & descriptor) const;
+	void stepGlobalDescriptor(sqlite3_stmt * ppStmt, int nodeId, const GlobalDescriptor & descriptor) const;
 	void stepOccupancyGridUpdate(sqlite3_stmt * ppStmt,
 			int nodeId,
 			const cv::Mat & ground,
 			const cv::Mat & obstacles,
+			const cv::Mat & empty,
 			float cellSize,
 			const cv::Point3f & viewpoint) const;
 
@@ -140,9 +184,12 @@ private:
 	void loadLinksQuery(std::list<Signature *> & signatures) const;
 	int loadOrSaveDb(sqlite3 *pInMemory, const std::string & fileName, int isSave) const;
 
-private:
+protected:
 	sqlite3 * _ppDb;
 	std::string _version;
+
+private:
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/DBReader.h

@@ -50,15 +50,17 @@ public:
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
-			 int startIndex = 0,
+			 int startId = 0,
-			 int cameraIndex = -1);
+			 int cameraIndex = -1,
+			 int stopId = 0);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
 			 bool ignoreGoalDelay = false,
 			 bool goalsIgnored = false,
-			 int startIndex = 0,
+			 int startId = 0,
-			 int cameraIndex = -1);
+			 int cameraIndex = -1,
+			 int stopId = 0);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -80,7 +82,8 @@ private:
 	bool _odometryIgnored;
 	bool _ignoreGoalDelay;
 	bool _goalsIgnored;
-	int _startIndex;
+	int _startId;
+	int _stopId;
 	int _cameraIndex;
 
 	DBDriver * _dbDriver;

corelib/include/rtabmap/core/EnvSensor.h

@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_
+
+namespace rtabmap {
+
+class EnvSensor
+{
+public:
+	enum Type {
+		// built-in types
+		kUndefined = 0,
+		kWifiSignalStrength,      // dBm
+		kAmbientTemperature,      // Celcius
+		kAmbientAirPressure,      // hPa
+		kAmbientLight,            // lx
+		kAmbientRelativeHumidity, // %
+
+		// user types
+		kCustomSensor1 = 100,
+		kCustomSensor2,
+		kCustomSensor3,
+		kCustomSensor4,
+		kCustomSensor5,
+		kCustomSensor6,
+		kCustomSensor7,
+		kCustomSensor8,
+		kCustomSensor9
+	};
+
+public:
+	EnvSensor() :
+		type_(kUndefined),
+		value_(0.0),
+		stamp_(0.0)
+	{}
+	EnvSensor(const Type & type, const double & value,const double & stamp = 0) :
+		type_(type),
+		value_(value),
+		stamp_(stamp)
+	{}
+
+	virtual ~EnvSensor() {}
+
+	const Type & type() const {return type_;}
+	const double & value() const {return value_;}
+	const double & stamp() const {return stamp_;}
+
+private:
+	Type type_;
+	double value_;
+	double stamp_;
+};
+
+typedef std::map<EnvSensor::Type, EnvSensor> EnvSensors;
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_ENVSENSOR_H_ */

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -75,8 +75,8 @@ public:
 	static cv::Mat findFFromWords(
 			const std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs, // id, kpt1, kpt2
 			std::vector<uchar> & status,
-			double ransacParam1 = 3.0,
+			double ransacReprojThreshold = 3.0,
-			double ransacParam2 = 0.99);
+			double ransacConfidence = 0.99);
 
 	// assume a canonical camera (without K)
 	static void findRTFromP(

corelib/include/rtabmap/core/Features2d.h

@@ -62,7 +62,9 @@ namespace cv{
 namespace xfeatures2d {
 class FREAK;
 class BriefDescriptorExtractor;
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 class SIFT;
+#endif
 class SURF;
 }
 namespace cuda {
@@ -71,7 +73,11 @@ class ORB;
 class SURF_CUDA;
 }
 }
+#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 typedef cv::xfeatures2d::SIFT CV_SIFT;
+#else
+typedef cv::SIFT CV_SIFT; // SIFT is back in features2d since 4.4.0 / 3.4.11
+#endif
 typedef cv::xfeatures2d::SURF CV_SURF;
 typedef cv::FastFeatureDetector CV_FAST;
 typedef cv::xfeatures2d::FREAK CV_FREAK;
@@ -84,9 +90,11 @@ typedef cv::cuda::ORB CV_ORB_GPU;
 typedef cv::cuda::FastFeatureDetector CV_FAST_GPU;
 #endif
 
-
 namespace rtabmap {
 
+class ORBextractor;
+class SPDetector;
+
 class Stereo;
 #if CV_MAJOR_VERSION < 3
 class CV_ORB;
@@ -105,7 +113,40 @@ public:
 		kFeatureGfttBrief=6,
 		kFeatureBrisk=7,
 		kFeatureGfttOrb=8,  //new 0.10.11
-		kFeatureFreak=9};   //new 0.11.14
+		kFeatureKaze=9,     //new 0.13.2
+		kFeatureOrbOctree=10, //new 0.19.2
+		kFeatureSuperPointTorch=11}; //new 0.19.7
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kFeatureSurf:
+			return "SURF";
+		case kFeatureSift:
+			return "SIFT";
+		case kFeatureOrb:
+			return "ORB";
+		case kFeatureFastFreak:
+			return "FAST+FREAK";
+		case kFeatureFastBrief:
+			return "FAST+BRIEF";
+		case kFeatureGfttFreak:
+			return "GFTT+Freak";
+		case kFeatureGfttBrief:
+			return "GFTT+Brief";
+		case kFeatureBrisk:
+			return "BRISK";
+		case kFeatureGfttOrb:
+			return "GFTT+ORB";
+		case kFeatureKaze:
+			return "KAZE";
+		case kFeatureOrbOctree:
+			return "ORB-OCTREE";
+		case kFeatureSuperPointTorch:
+			return "SUPERPOINT";
+		default:
+			return "Unknown";
+		}
+	}
 
 	static Feature2D * create(const ParametersMap & parameters = ParametersMap());
 	static Feature2D * create(Feature2D::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
@@ -121,6 +162,12 @@ public:
 			const cv::Mat & depth,
 			float minDepth,
 			float maxDepth);
+	static void filterKeypointsByDepth(
+			std::vector<cv::KeyPoint> & keypoints,
+			cv::Mat & descriptors,
+			std::vector<cv::Point3f> & keypoints3D,
+			float minDepth,
+			float maxDepth);
 
 	static void filterKeypointsByDisparity(
 				std::vector<cv::KeyPoint> & keypoints,
@@ -136,6 +183,7 @@ public:
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
 	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
 
 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);
@@ -143,13 +191,15 @@ public:
 	int getMaxFeatures() const {return maxFeatures_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
+	int getGridRows() const {return gridRows_;}
+	int getGridCols() const {return gridCols_;}
 
 public:
 	virtual ~Feature2D();
 
 	std::vector<cv::KeyPoint> generateKeypoints(
 			const cv::Mat & image,
-			const cv::Mat & mask = cv::Mat()) const;
+			const cv::Mat & mask = cv::Mat());
 	cv::Mat generateDescriptors(
 			const cv::Mat & image,
 			std::vector<cv::KeyPoint> & keypoints) const;
@@ -165,7 +215,7 @@ protected:
 	Feature2D(const ParametersMap & parameters = ParametersMap());
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const = 0;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) = 0;
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const = 0;
 
 private:
@@ -177,6 +227,8 @@ private:
 	int _subPixWinSize;
 	int _subPixIterations;
 	double _subPixEps;
+	int gridRows_;
+	int gridCols_;
 	// Stereo stuff
 	Stereo * _stereo;
 };
@@ -192,7 +244,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureSurf;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -219,7 +271,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureSift;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -227,6 +279,7 @@ private:
 	double contrastThreshold_;
 	double edgeThreshold_;
 	double sigma_;
+	bool rootSIFT_;
 
 	cv::Ptr<CV_SIFT> _sift;
 };
@@ -242,7 +295,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureOrb;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -273,8 +326,8 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureUndef;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const {return cv::Mat();}
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat &, std::vector<cv::KeyPoint> &) const {return cv::Mat();}
 
 private:
 	int threshold_;
@@ -285,6 +338,14 @@ private:
 	int maxThreshold_;
 	int gridRows_;
 	int gridCols_;
+	int fastCV_;
+
+	bool fastCVinit_;
+	int fastCVMaxFeatures_;
+	int fastCVLastImageHeight_;
+	uint32_t* fastCVCorners_= NULL;
+	uint32_t* fastCVCornerScores_ = NULL;
+	void* fastCVTempBuf_ = NULL;
 
 	cv::Ptr<cv::FeatureDetector> _fast;
 	cv::Ptr<CV_FAST_GPU> _gpuFast;
@@ -341,7 +402,7 @@ public:
 	virtual void parseParameters(const ParametersMap & parameters);
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 
 private:
 	double _qualityLevel;
@@ -422,7 +483,7 @@ public:
 	virtual Feature2D::Type getType() const {return kFeatureBrisk;}
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
@@ -433,29 +494,79 @@ private:
 	cv::Ptr<CV_BRISK> brisk_;
 };
 
-//FREAK
+//KAZE
-class RTABMAP_EXP FREAK : public Feature2D
+class RTABMAP_EXP KAZE : public Feature2D
 {
 public:
-	FREAK(const ParametersMap & parameters = ParametersMap());
+	KAZE(const ParametersMap & parameters = ParametersMap());
-	virtual ~FREAK();
+	virtual ~KAZE();
 
 	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const { return kFeatureFreak; }
+	virtual Feature2D::Type getType() const { return kFeatureKaze; }
 
 private:
-	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat()) const;
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
 	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
 
 private:
-	bool orientationNormalized_;
+	bool extended_;
-	bool scaleNormalized_;
+	bool upright_;
-	float patternScale_;
+	float threshold_;
 	int nOctaves_;
+	int nOctaveLayers_;
+	int diffusivity_;
 
-	cv::Ptr<CV_FREAK> _freak;
+#if CV_MAJOR_VERSION > 2
+	cv::Ptr<cv::KAZE> kaze_;
+#endif
 };
 
+//ORB OCTREE
+class RTABMAP_EXP ORBOctree : public Feature2D
+{
+public:
+	ORBOctree(const ParametersMap & parameters = ParametersMap());
+	virtual ~ORBOctree();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const {return kFeatureOrbOctree;}
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+private:
+	float scaleFactor_;
+	int nLevels_;
+	int fastThreshold_;
+	int fastMinThreshold_;
+
+	cv::Ptr<ORBextractor> _orb;
+	cv::Mat descriptors_;
+};
+
+//SuperPointTorch
+class RTABMAP_EXP SuperPointTorch : public Feature2D
+{
+public:
+	SuperPointTorch(const ParametersMap & parameters = ParametersMap());
+	virtual ~SuperPointTorch();
+
+	virtual void parseParameters(const ParametersMap & parameters);
+	virtual Feature2D::Type getType() const { return kFeatureSuperPointTorch; }
+
+private:
+	virtual std::vector<cv::KeyPoint> generateKeypointsImpl(const cv::Mat & image, const cv::Rect & roi, const cv::Mat & mask = cv::Mat());
+	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
+
+	cv::Ptr<SPDetector> superPoint_;
+
+	std::string path_;
+	float threshold_;
+	bool nms_;
+	int minDistance_;
+	bool cuda_;
+};
 
 }
 

corelib/include/rtabmap/core/FlannIndex.h

@@ -49,21 +49,25 @@ public:
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
 			const cv::Mat & features,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeIndex(
 			const cv::Mat & features,
 			int trees = 4,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildKDTreeSingleIndex(
 			const cv::Mat & features,
 			int leafMaxSize = 10,
 			bool reorder = true,
-			bool useDistanceL1 = false);
+			bool useDistanceL1 = false,
+			float rebalancingFactor = 2.0f);
 	void buildLSHIndex(
 			const cv::Mat & features,
 			unsigned int table_number = 12,
 			unsigned int key_size = 20,
-			unsigned int multi_probe_level = 2);
+			unsigned int multi_probe_level = 2,
+			float rebalancingFactor = 2.0f);
 
 	bool isBuilt();
 
@@ -74,7 +78,7 @@ public:
 
 	void removePoint(unsigned int index);
 
-	// return squared distances
+	// return squared distances (indices should be casted in size_t)
 	void knnSearch(
 			const cv::Mat & query,
 			cv::Mat & indices,
@@ -102,6 +106,7 @@ private:
 	int featuresDim_;
 	bool isLSH_;
 	bool useDistanceL1_; // true=EUCLEDIAN_L2 false=MANHATTAN_L1
+	float rebalancingFactor_;
 
 	// keep feature in memory until the tree is rebuilt
 	// (in case the word is deleted when removed from the VWDictionary)

corelib/include/rtabmap/core/GPS.h

@@ -0,0 +1,78 @@
+/*
+Copyright (c) 2010-2018, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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 CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_
+
+#include <rtabmap/core/GeodeticCoords.h>
+
+namespace rtabmap {
+
+class GPS
+{
+public:
+	GPS():
+		stamp_(0.0),
+		longitude_(0.0),
+		latitude_(0.0),
+		altitude_(0.0),
+		error_(0.0),
+		bearing_(0.0)
+	{}
+	GPS(const double & stamp,
+		const double & longitude,
+		const double & latitude,
+		const double & altitude,
+		const double & error,
+		const double & bearing):
+			stamp_(stamp),
+			longitude_(longitude),
+			latitude_(latitude),
+			altitude_(altitude),
+			error_(error),
+			bearing_(bearing)
+	{}
+	const double & stamp() const {return stamp_;}
+	const double & longitude() const {return longitude_;}
+	const double & latitude() const {return latitude_;}
+	const double & altitude() const {return altitude_;}
+	const double & error() const {return error_;}
+	const double & bearing() const {return bearing_;}
+
+	GeodeticCoords toGeodeticCoords() const {return GeodeticCoords(latitude_, longitude_, altitude_);}
+private:
+	double stamp_;     // in sec
+	double longitude_; // DD
+	double latitude_;  // DD
+	double altitude_;  // m
+	double error_;     // m
+	double bearing_;   // deg (North 0->360 clockwise)
+};
+
+}
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_GPS_H_ */

corelib/include/rtabmap/core/GainCompensator.h

@@ -73,18 +73,22 @@ public:
 
 	void apply(
 			int id,
-			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) const;
+			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
 			pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
-			const pcl::IndicesPtr & indices) const;
+			const pcl::IndicesPtr & indices,
+			bool rgb = true) const;
 	void apply(
 			int id,
-			cv::Mat & image) const;
+			cv::Mat & image,
+			bool rgb = true) const;
 
 	double getGain(int id, double * r=0, double * g=0, double * b=0) const;
 	int getIndex(int id) const;

corelib/include/rtabmap/core/GeodeticCoords.h

@@ -25,6 +25,20 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */
 
+/*
+ * The methods in this file were modified from the originals of the MRPT toolkit (see notice below):
+ * https://github.com/MRPT/mrpt/blob/master/libs/topography/src/conversions.cpp
+ */
+
+/* +---------------------------------------------------------------------------+
+   |                     Mobile Robot Programming Toolkit (MRPT)               |
+   |                          http://www.mrpt.org/                             |
+   |                                                                           |
+   | Copyright (c) 2005-2016, Individual contributors, see AUTHORS file        |
+   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
+   | Released under BSD License. See details in http://www.mrpt.org/License    |
+   +---------------------------------------------------------------------------+ */
+
 
 #ifndef GEODETICCOORDS_H_
 #define GEODETICCOORDS_H_
@@ -52,6 +66,15 @@ public:
 	cv::Point3d toGeocentric_WGS84() const;
 	cv::Point3d toENU_WGS84(const GeodeticCoords & origin) const; // East=X, North=Y
 
+	void fromGeocentric_WGS84(const cv::Point3d& geocentric);
+	void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+
+	static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
+	static cv::Point3d Geocentric_WGS84ToENU_WGS84(
+			const cv::Point3d & geocentric_WGS84,
+			const cv::Point3d & origin_geocentric_WGS84,
+			const GeodeticCoords & origin);
+
 private:
 	double latitude_;  // deg
 	double longitude_; // deg

corelib/include/rtabmap/core/GlobalDescriptor.h

@@ -0,0 +1,59 @@
+/*
+Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+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 Universite de Sherbrooke 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 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.
+*/
+
+#pragma once
+
+#include <opencv2/core/core.hpp>
+
+namespace rtabmap
+{
+
+class GlobalDescriptor
+{
+
+public:
+	GlobalDescriptor(int type, const cv::Mat & data, const cv::Mat & info = cv::Mat()) :
+		type_(type),
+		info_(info),
+		data_(data)
+	{}
+	GlobalDescriptor() :
+		type_(-1) // Not set
+	{}
+	virtual ~GlobalDescriptor() {}
+
+	int type() const {return type_;}
+	const cv::Mat info() const {return info_;}
+	const cv::Mat data() const {return data_;}
+
+private:
+	int type_;
+	cv::Mat info_;
+	cv::Mat data_;
+};
+
+} // namespace rtabmap