CameraRealSense2: Odom extrinsics against another sensor can be calibrated with having to calibrate stereo first. DBViewer: fixed local grid wrongly using global grid parameters.

Setting WITH_FLOAM to OFF by default because floam binaries (this [version](https://github.com/flynneva/floam)) in ros is not compatible.

.github/workflows/cmake.yml

@@ -0,0 +1,65 @@
+name: CMake
+
+on:
+  push:
+    branches: [ master ]
+  pull_request:
+    branches: [ master ]
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build:
+    # The CMake configure and build commands are platform agnostic and should work equally
+    # well on Windows or Mac.  You can convert this to a matrix build if you need
+    # cross-platform coverage.
+    # See: https://docs.github.com/en/free-pro-team@latest/actions/learn-github-actions/managing-complex-workflows#using-a-build-matrix
+    name: Build on ros ${{ matrix.ros_distro }} and ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      matrix:
+        os: [ubuntu-20.04, ubuntu-18.04]
+        include:
+        - os: ubuntu-20.04
+          ros_distro: 'noetic'
+        - os: ubuntu-18.04
+          ros_distro: 'melodic'
+
+    steps:
+    - uses: ros-tooling/setup-ros@v0.2
+      with:
+        required-ros-distributions: ${{ matrix.ros_distro }}
+    
+    - name: Install dependencies
+      run: |
+        sudo apt-get update
+        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+        sudo apt-get -y remove ros-${{ matrix.ros_distro }}-rtabmap
+
+    - uses: actions/checkout@v2
+
+    - name: Configure CMake
+      # Configure CMake in a 'build' subdirectory. `CMAKE_BUILD_TYPE` is only required if you are using a single-configuration generator such as make.
+      # See https://cmake.org/cmake/help/latest/variable/CMAKE_BUILD_TYPE.html?highlight=cmake_build_type
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        cmake -B ${{github.workspace}}/build -DCMAKE_BUILD_TYPE=${{env.BUILD_TYPE}}
+
+    - name: Build
+      # Build your program with the given configuration
+      run: cmake --build ${{github.workspace}}/build --config ${{env.BUILD_TYPE}}
+
+    - name: Info
+      working-directory: ${{github.workspace}}/bin
+      run: |
+        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
+        ./rtabmap-console --version
+
+#    - name: Test
+#      working-directory: ${{github.workspace}}/build
+#      # Execute tests defined by the CMake configuration.  
+#      # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
+#      run: ctest -C ${{env.BUILD_TYPE}}
+      

.github/workflows/docker.yml

@@ -0,0 +1,90 @@
+name: docker
+
+on:
+  push:
+    branches:
+      - 'master'
+
+jobs:
+  docker:
+    runs-on: ubuntu-latest
+    
+    strategy:
+      matrix:
+        docker_tag: [xenial, bionic, focal, android23, android24, android26]
+        include:
+        - docker_tag: xenial
+          docker_tags: |
+            introlab3it/rtabmap:xenial
+            introlab3it/rtabmap:16.04
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'xenial'
+        - docker_tag: bionic
+          docker_tags: |
+            introlab3it/rtabmap:bionic
+            introlab3it/rtabmap:18.04
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'bionic'
+        - docker_tag: focal
+          docker_tags: |
+            introlab3it/rtabmap:focal
+            introlab3it/rtabmap:20.04
+            introlab3it/rtabmap:latest
+          docker_platforms: |
+            linux/amd64
+            linux/arm64
+          docker_path: 'focal'
+        - docker_tag: android23
+          docker_tags: |
+            introlab3it/rtabmap:android23
+            introlab3it/rtabmap:tango
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api23'
+        - docker_tag: android24
+          docker_tags: |
+            introlab3it/rtabmap:android24
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api24'
+        - docker_tag: android26
+          docker_tags: |
+            introlab3it/rtabmap:android26
+          docker_platforms: |
+            linux/amd64
+          docker_path: 'bionic/android/rtabmap_api26'
+    
+    steps:
+      -
+        name: Checkout
+        uses: actions/checkout@v2
+      - 
+        name: Set up QEMU
+        uses: docker/setup-qemu-action@v1
+        with:
+          platforms: all
+      -
+        name: Set up Docker Buildx
+        uses: docker/setup-buildx-action@v1
+      -
+        name: Login to DockerHub
+        uses: docker/login-action@v1
+        with:
+          username: ${{ secrets.DOCKERHUB_USERNAME }}
+          password: ${{ secrets.DOCKERHUB_TOKEN }}
+      -
+        name: Build and push
+        uses: docker/build-push-action@v2
+        with:
+          context: ./docker/${{ matrix.docker_path }}
+          push: true
+          platforms: ${{ matrix.docker_platforms }}
+          build-args: |
+            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+          tags: ${{ matrix.docker_tags }}
+          cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
+          cache-to: type=inline
+

.travis.yml

@@ -1,80 +0,0 @@
-language: cpp
-   
-jobs:
-  include:
-#    - name: osx
-#      compiler: clang
-#      os: osx
-#      install:
-#        - brew install sqlite
-#        - brew install pcl
-#        - brew install opencv@3
-    
-#    - name: linux-trusty
-#      compiler: gcc
-#      os: linux
-#      dist: trusty
-#      install:
-#        - sudo sh -c 'echo "deb http://packages.ros.org/ros/ubuntu trusty 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-indigo-rtabmap-ros
-#        - sudo apt-get -y remove ros-indigo-rtabmap
-#
-#      before_script: 
-#        - source /opt/ros/indigo/setup.bash
-
-    - name: linux-xenial
-      compiler: gcc
-      os: linux
-      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
-
-      before_script: 
-        - source /opt/ros/kinetic/setup.bash
-
-    - name: linux-bionic
-      compiler: gcc
-      os: linux
-      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
-
-      before_script: 
-        - source /opt/ros/melodic/setup.bash
-
-    - name: linux-focal
-      compiler: gcc
-      os: linux
-      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
-
-      before_script: 
-        - source /opt/ros/noetic/setup.bash
-
-script:
-  - mkdir -p build && cd build
-  - cmake .. 
-  - make
-
-notifications:
-  email:
-    - matlabbe@gmail.com

CMakeLists.txt

@@ -1,6 +1,5 @@
 # Top-Level CmakeLists.txt
-# For GNUInstallDirs
+cmake_minimum_required(VERSION 3.5)
-cmake_minimum_required(VERSION 2.8.5)
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)
 
@@ -21,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 10)
+SET(RTABMAP_PATCH_VERSION 15)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -41,7 +40,13 @@ IF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
     ENDIF("${CMAKE_BUILD_TYPE}" STREQUAL "")
 ENDIF(${CMAKE_GENERATOR} MATCHES ".*Makefiles")
 
-IF(NOT ANDROID)
+SET(MOBILE_BUILD OFF)
+IF(ANDROID OR CMAKE_SYSTEM_NAME STREQUAL "iOS")
+    SET(MOBILE_BUILD ON)
+ENDIF(ANDROID OR CMAKE_SYSTEM_NAME STREQUAL "iOS")
+MESSAGE(STATUS "MOBILE_BUILD=${MOBILE_BUILD}")
+
+IF(NOT MOBILE_BUILD)
     SET(CMAKE_DEBUG_POSTFIX "d")
     option(FLANN_KDTREE_MEM_OPT  "Disable multi-threaded FLANN kd-tree to minimize memory allocations"   OFF)
 ELSE()
@@ -157,7 +162,7 @@ OPTION(BUILD_TOOLS "Build tools" ON)
 OPTION(BUILD_EXAMPLES "Build examples" ON)
 
 ####### DEPENDENCIES #######
-IF(ANDROID)
+IF(MOBILE_BUILD)
 option(WITH_QT            "Include Qt support"                   OFF)
 ELSE()
 option(WITH_QT            "Include Qt support"                   ON)
@@ -182,6 +187,7 @@ option(WITH_CVSBA         "Include cvsba support"                ON)
 option(WITH_POINTMATCHER  "Include libpointmatcher support"      ON)
 option(WITH_CCCORELIB     "Include CCCoreLib support"            ON)
 option(WITH_LOAM          "Include LOAM support"                 ON)
+option(WITH_FLOAM         "Include FLOAM support"                OFF)
 option(WITH_FLYCAPTURE2   "Include FlyCapture2/Triclops support" ON)
 option(WITH_ZED           "Include ZED sdk support"              ON)
 option(WITH_ZEDOC         "Include ZED Open Capture support"     ON)
@@ -204,7 +210,8 @@ option(WITH_VINS          "Include VINS-Fusion support"          ON)
 option(WITH_OPENVINS      "Include OpenVINS support"             ON)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
-IF(ANDROID)
+option(WITH_OPENMP        "Include OpenMP support"               ON)
+IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
 option(PCL_OMP            "With PCL OMP implementations"         ON)
@@ -248,18 +255,19 @@ endif()
 
 # OpenMP ("-fopenmp" should be added for flann included in PCL)
 # the gcc-4.2.1 coming with MacOS X is not compatible with the OpenMP pragmas we use, so disabling OpenMP for it
-if((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang"))
+if(((NOT APPLE) OR (NOT CMAKE_COMPILER_IS_GNUCXX) OR (GCC_VERSION VERSION_GREATER 4.2.1) OR (CMAKE_CXX_COMPILER_ID STREQUAL "Clang")) AND WITH_OPENMP)
-  find_package(OpenMP)
+  find_package(OpenMP COMPONENTS C CXX)
 endif()
 if(OPENMP_FOUND)
   set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
   set(CMAKE_INSTALL_OPENMP_LIBRARIES TRUE)
-  message (STATUS "Found OpenMP")
+  message (STATUS "Found OpenMP: ${OpenMP_CXX_LIBRARIES}")
   if(PCL_OMP)
+    message (STATUS "Add PCL_OMP to definitions")
     add_definitions(-DPCL_OMP)
   endif(PCL_OMP)
-else(OPENMP_FOUND)
+elseif(WITH_OPENMP)
   message (STATUS "Not found OpenMP")
 endif()
 
@@ -488,6 +496,14 @@ IF(WITH_LOAM)
     ENDIF(loam_velodyne_FOUND)
 ENDIF(WITH_LOAM)
 
+IF(WITH_FLOAM)
+    find_package(floam QUIET)
+    IF(floam_FOUND)
+       MESSAGE(STATUS "Found floam: ${floam_INCLUDE_DIRS}")
+       FIND_PACKAGE(Ceres QUIET REQUIRED)
+    ENDIF(floam_FOUND)
+ENDIF(WITH_FLOAM)
+
 SET(ZED_FOUND FALSE)
 IF(WITH_ZED)
     find_package(ZED 2 QUIET)
@@ -589,6 +605,7 @@ IF(WITH_ALICE_VISION)
         ENDIF(${AliceVision_VERSION} VERSION_LESS_EQUAL "2.2")
         SET(CMAKE_MODULE_PATH "${CMAKE_MODULE_PATH};/usr/local/lib/cmake/modules")
         find_package(Geogram REQUIRED QUIET)
+        find_package(assimp QUIET)
         add_definitions("-DRTABMAP_ALICE_VISION_MAJOR=${AliceVision_VERSION_MAJOR}")
         add_definitions("-DRTABMAP_ALICE_VISION_MINOR=${AliceVision_VERSION_MINOR}")
         add_definitions("-DRTABMAP_ALICE_VISION_PATCH=${AliceVision_VERSION_PATCH}")
@@ -630,9 +647,14 @@ IF(WITH_OKVIS)
 ENDIF(WITH_OKVIS)
 
 # If built with okvis, we found already ceres above
-IF(NOT okvis_FOUND AND WITH_CERES)
+IF(WITH_CERES)
+  IF(NOT okvis_FOUND AND NOT floam_FOUND)
     FIND_PACKAGE(Ceres QUIET)
-ENDIF(NOT okvis_FOUND AND WITH_CERES)
+    MESSAGE(STATUS "Found ceres ${Ceres_VERSION}: ${CERES_INCLUDE_DIRS}")
+  ENDIF(NOT okvis_FOUND AND NOT floam_FOUND)
+ELSEIF(Ceres_FOUND)
+  MESSAGE(WARNING "WITH_CERES is OFF, but it still included by dependencies Okvis or FLOAM")
+ENDIF()
 
 IF(WITH_MSCKF_VIO)
     FIND_PACKAGE(msckf_vio QUIET)
@@ -673,7 +695,7 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(NOT MSVC)
-	IF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
 	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
 	 include(CheckCXXCompilerFlag)
 	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
@@ -683,7 +705,7 @@ IF(NOT MSVC)
 	  ELSE()
 	    message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler if you want to use LOAM, latest PCL or g2o.")
 	  ENDIF()
-	ENDIF(loam_velodyne_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
+	ENDIF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND)
 
 	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -808,6 +830,9 @@ ENDIF(NOT PDAL_FOUND)
 IF(NOT loam_velodyne_FOUND)
    SET(LOAM "//")
 ENDIF(NOT loam_velodyne_FOUND)
+IF(NOT floam_FOUND)
+   SET(FLOAM "//")
+ENDIF(NOT floam_FOUND)
 IF(NOT Freenect_FOUND)
    SET(FREENECT "//")
 ELSE()
@@ -870,8 +895,12 @@ IF(NOT mynteye_FOUND)
    SET(MYNTEYE "//")
 ENDIF(NOT mynteye_FOUND)
 IF(NOT depthai_FOUND)
+   SET(CONF_DEPTH_AI OFF)
    SET(DEPTHAI "//")
-ENDIF(NOT depthai_FOUND)
+ELSE()
+   SET(CONF_DEPTH_AI ON) 
+   SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} depthai::core depthai::opencv) 
+ENDIF()
 IF(NOT octomap_FOUND)
    SET(OCTOMAP "//")
 ELSE()
@@ -1262,11 +1291,11 @@ MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
 IF(CERES_FOUND)
-MESSAGE(STATUS " *With Ceres                = YES (License: BSD)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = YES (License: BSD)")
 ELSEIF(NOT WITH_CERES)
-MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (WITH_CERES=OFF)")
 ELSE()
-MESSAGE(STATUS " *With Ceres                = NO (Ceres not found)")
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}         = NO (Ceres not found)")
 ENDIF()
 
 IF(G2O_FOUND OR GTSAM_FOUND)
@@ -1297,7 +1326,7 @@ ENDIF()
 
 IF(CCCoreLib_FOUND)
 MESSAGE(STATUS "  With CCCoreLib            = YES (License: GPLv2)")
-ELSEIF(NOT WITH_POINTMATCHER)
+ELSEIF(NOT WITH_CCCORELIB)
 MESSAGE(STATUS "  With CCCoreLib            = NO (WITH_CCCORELIB=OFF)")
 ELSE()
 MESSAGE(STATUS "  With CCCoreLib            = NO (CCCoreLib not found)")
@@ -1460,6 +1489,14 @@ ELSE()
 MESSAGE(STATUS "  With loam_velodyne        = NO (loam_velodyne not found)")
 ENDIF()
 
+IF(floam_FOUND)
+MESSAGE(STATUS "  With floam                = YES (License: BSD)")
+ELSEIF(NOT WITH_FLOAM)
+MESSAGE(STATUS "  With floam                = NO (WITH_FLOAM=OFF)")
+ELSE()
+MESSAGE(STATUS "  With floam                = NO (floam not found)")
+ENDIF()
+
 IF(libfovis_FOUND)
 MESSAGE(STATUS "  With libfovis             = YES (License: GPLv2)")
 ELSEIF(NOT WITH_FOVIS)

README.md

@@ -1,13 +1,13 @@
-rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/github-main?pixel) 
+rtabmap ![.](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)
+Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![docker](https://github.com/introlab/rtabmap/actions/workflows/docker.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/docker.yml) 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.7-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.8-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

RTABMapConfig.cmake.in

@@ -81,6 +81,9 @@ endif()
 if(@CONF_VTK_QT@ AND ${WITH_GUI})
    find_package(VTK COMPONENTS vtkGUISupportQt NO_MODULE) # to define vtkGUISupportQt target
 endif(@CONF_VTK_QT@ AND ${WITH_GUI})
+if(@CONF_DEPTH_AI@)
+   FIND_PACKAGE(depthai 2 QUIET REQUIRED)
+endif(@CONF_DEPTH_AI@)
 SET(RTABMap_LIBRARIES ${RTABMap_LIBRARIES} "@CONF_DEPENDENCIES@")
 
 #backward compatibilities

Version.h.in

@@ -55,6 +55,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FASTCV@#define RTABMAP_FASTCV
 @PDAL@#define RTABMAP_PDAL
 @LOAM@#define RTABMAP_LOAM
+@FLOAM@#define RTABMAP_FLOAM
 @DC1394@#define RTABMAP_DC1394
 @FLYCAPTURE2@#define RTABMAP_FLYCAPTURE2
 @ZED@#define RTABMAP_ZED

app/android/jni/CameraARCore.cpp

@@ -43,9 +43,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
 	context_(context),
 	activity_(activity),
 	arInstallRequested_(false),
-	textureId_(9999),
-	uvs_initialized_(false),
-	updateOcclusionImage_(false),
 	depthFromMotion_(depthFromMotion)
 {
 }
@@ -53,12 +50,6 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthF
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
-
-	if(textureId_ != 9999)
-	{
-		glDeleteTextures(1, &textureId_);
-		textureId_ = 9999;
-	}
 }
 
 
@@ -176,7 +167,7 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
 	}
 
-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -279,7 +270,6 @@ void CameraARCore::close()
 	arPose_ = nullptr;
 
 	CameraMobile::close();
-	occlusionImage_ = cv::Mat();
 }
 
 LaserScan CameraARCore::scanFromPointCloudData(
@@ -355,7 +345,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}
 
-	if(textureId_ == 9999)
+	if(textureId_ == 0)
 	{
 		glGenTextures(1, &textureId_);
 		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
@@ -364,7 +354,8 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		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_);
+	if(textureId_!=0)
+		ArSession_setCameraTextureName(arSession_, textureId_);
 
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -393,6 +384,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							   /*near=*/0.1f, /*far=*/100.f,
 							   glm::value_ptr(projectionMatrix_));
 
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -407,6 +404,22 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		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;
 
+		Transform poseArCore = pose;
+		if(pose.isNull())
+		{
+			LOGE("CameraARCore: Pose is null");
+		}
+		else
+		{
+			this->poseReceived(pose);
+			// adjust origin
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
+			info->odomPose = pose;
+		}
+
 		// Get calibration parameters
 		float fx,fy, cx, cy;
 		int32_t width, height;
@@ -432,7 +445,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
-				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
+				if(is_depth_supported)
 				{
 					LOGD("Acquire depth image!");
 					ArImage * depthImage = nullptr;
@@ -459,11 +472,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 						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();
+						cv::Mat 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));
+						CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
+						this->setOcclusionImage(occlusionImage, occlusionModel);
 					}
 					ArImage_release(depthImage);
 				}
@@ -527,7 +541,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 #endif
 							if(pointCloudData && points>0)
 							{
-								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
+								scan = scanFromPointCloudData(pointCloudData, points, poseArCore, model, rgb, &kpts, &kpts3);
 							}
 						}
 						else
@@ -535,7 +549,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -556,15 +570,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 
 	ArCamera_release(ar_camera);
 
-	if(pose.isNull())
-	{
-		LOGE("CameraARCore: Pose is null");
-	}
-	else
-	{
-		this->poseReceived(pose);
-		info->odomPose = pose;
-	}
 	return data;
 
 }
@@ -579,16 +584,15 @@ void CameraARCore::capturePoseOnly()
 		return;
 	}
 
-	if(textureId_ == 9999)
+	if(textureId_ != 0)
 	{
-		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_);
 	}
-	ArSession_setCameraTextureName(arSession_, textureId_);
 
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
@@ -617,6 +621,12 @@ void CameraARCore::capturePoseOnly()
 							   /*near=*/0.1f, /*far=*/100.f,
 							   glm::value_ptr(projectionMatrix_));
 
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -633,12 +643,17 @@ void CameraARCore::capturePoseOnly()
 		{
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
+
+			if(!getOriginOffset().isNull())
+			{
+				pose = getOriginOffset() * pose;
+			}
 		}
 
 		int32_t is_depth_supported = 0;
 		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
 
-		if(is_depth_supported && updateOcclusionImage_)
+		if(is_depth_supported)
 		{
 			LOGD("Acquire depth image!");
 			ArImage * depthImage = nullptr;
@@ -665,7 +680,7 @@ void CameraARCore::capturePoseOnly()
 
 				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
 
-				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+				cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
 
 				float fx,fy, cx, cy;
 				int32_t rgb_width, rgb_height;
@@ -676,7 +691,8 @@ void CameraARCore::capturePoseOnly()
 
 				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));
+				CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+				this->setOcclusionImage(occlusionImage, occlusionModel);
 			}
 			ArImage_release(depthImage);
 		}

app/android/jni/CameraARCore.h

@@ -67,9 +67,6 @@ public:
 	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 = "");
@@ -97,17 +94,8 @@ private:
 	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_;
 };
 

app/android/jni/CameraAREngine.cpp

@@ -300,6 +300,11 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 	{
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 		this->poseReceived(pose);
+        // adjust origin
+        if(!getOriginOffset().isNull())
+        {
+            pose = getOriginOffset() * pose;
+        }
 		info->odomPose = pose;
 	}
 	return data;

app/android/jni/CameraMobile.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util3d_transforms.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util2d.h"
+#include <glm/gtx/transform.hpp>
 
 namespace rtabmap {
 
@@ -55,6 +56,8 @@ CameraMobile::CameraMobile(bool smoothing) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		spinOncePreviousStamp_(0.0),
+		textureId_(0),
+		uvs_initialized_(false),
 		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		smoothing_(smoothing),
@@ -84,10 +87,22 @@ void CameraMobile::close()
 	originUpdate_ = false;
 	pose_ = Transform();
 	data_ = SensorData();
+
+    if(textureId_ != 0)
+    {
+        glDeleteTextures(1, &textureId_);
+        textureId_ = 0;
+    }
 }
 
 void CameraMobile::resetOrigin()
 {
+	previousPose_.setNull();
+	previousStamp_ = 0.0;
+	lastKnownGPS_ = GPS();
+	lastEnvSensors_.clear();
+	pose_ = Transform();
+	data_ = SensorData();
 	originUpdate_ = true;
 }
 
@@ -124,11 +139,60 @@ void CameraMobile::setGPS(const GPS & gps)
 	lastKnownGPS_ = gps;
 }
 
-void CameraMobile::setData(const SensorData & data, const Transform & pose)
+void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
 {
 	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
 	data_ = data;
-	pose_ = pose;
+    pose_ = pose;
+
+    viewMatrix_ = viewMatrix;
+    projectionMatrix_ = projectionMatrix;
+    
+    // adjust origin
+    if(!originOffset_.isNull())
+    {
+        pose_ = originOffset_ * pose_;
+        viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+    }
+    
+    if(textureId_ == 0)
+    {
+    	glGenTextures(1, &textureId_);
+    }
+
+    if(texCoord)
+    {
+        memcpy(transformed_uvs_, texCoord, 8*sizeof(float));
+        uvs_initialized_ = true;
+    }
+    
+    LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
+
+    if(textureId_ != 0 && texCoord != 0)
+    {
+        cv::Mat rgbImage;
+        cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);
+        
+        glBindTexture(GL_TEXTURE_2D, textureId_);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+        glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+        
+        glPixelStorei(GL_UNPACK_ALIGNMENT, 4);
+        //glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_PIXELS, 0);
+        //glPixelStorei(GL_UNPACK_SKIP_ROWS, 0);
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, rgbImage.cols, rgbImage.rows, 0, GL_RGBA, GL_UNSIGNED_BYTE, rgbImage.data);
+
+        GLint error = glGetError();
+        if(error != GL_NO_ERROR)
+        {
+            LOGE("OpenGL: Could not allocate texture (0x%x)\n", error);
+            textureId_ = 0;
+            return;
+        }
+    }
 }
 
 void CameraMobile::addEnvSensor(int type, float value)
@@ -141,16 +205,16 @@ void CameraMobile::spinOnce()
 	if(!this->isRunning())
 	{
 		bool ignoreFrame = false;
-		float rate = 10.0f; // maximum 10 FPS for image data
+		//float rate = 10.0f; // maximum 10 FPS for image data
 		double now = UTimer::now();
-		if(rate>0.0f)
+		/*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)
 		{
@@ -322,7 +386,7 @@ void CameraMobile::mainLoop()
 		previousPose_ = pose;
 		previousStamp_ = data.stamp();
 	}
-	else if(!this->isKilled())
+	else if(!this->isKilled() && info.odomPose.isNull())
 	{
 		LOGW("Odometry lost");
 		this->post(new OdometryEvent());
@@ -338,4 +402,60 @@ SensorData CameraMobile::captureImage(CameraInfo * info)
 	return data_;
 }
 
+LaserScan CameraMobile::scanFromPointCloudData(
+        const cv::Mat & pointCloudData,
+        int points,
+        const Transform & pose,
+        const CameraModel & model,
+        const cv::Mat & rgb,
+        std::vector<cv::KeyPoint> * kpts,
+        std::vector<cv::Point3f> * kpts3D,
+        int kptsSize)
+{
+    if(!pointCloudData.empty())
+    {
+        cv::Mat scanData(1, pointCloudData.cols, CV_32FC4);
+        float * ptr = scanData.ptr<float>();
+        const float * inPtr = pointCloudData.ptr<float>();
+        int ic = pointCloudData.channels();
+        UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
+        
+        int oi = 0;
+        for(unsigned int i=0;i<points; ++i)
+        {
+            cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
+            pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+            ptr[oi*4] = pt.x;
+            ptr[oi*4 + 1] = pt.y;
+            ptr[oi*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,kptsSize));
+                if(kpts3D)
+                    kpts3D->push_back(org);
+                
+                *(int*)&ptr[oi*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+                ++oi;
+            }
+
+            //confidence
+            //*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+        }
+        return LaserScan::backwardCompatibility(scanData.colRange(0, oi), 0, 10, rtabmap::Transform::getIdentity());
+    }
+    return LaserScan();
+}
+
 } /* namespace rtabmap */

app/android/jni/CameraMobile.h

@@ -76,6 +76,17 @@ public:
 	static const rtabmap::Transform opticalRotation;
 	static const rtabmap::Transform opticalRotationInv;
     
+public:
+    static LaserScan scanFromPointCloudData(
+            const cv::Mat & 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,
+            int kptsSize = 3);
+
 public:
 	CameraMobile(bool smoothing = false);
 	virtual ~CameraMobile();
@@ -97,10 +108,19 @@ public:
 	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 setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
 
 	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
     
+    GLuint getTextureId() {return textureId_;}
+    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_;}
+    ScreenRotation getScreenRotation() const {return colorCameraToDisplayRotation_;}
+    
+    void setOcclusionImage(const cv::Mat & image, const CameraModel & model) {occlusionModel_ = model; occlusionImage_ = image;}
+    const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
 protected:
 	virtual SensorData captureImage(CameraInfo * info = 0);
 	virtual void capturePoseOnly() {}
@@ -114,6 +134,12 @@ protected:
 	UTimer spinOnceFrameRateTimer_;
 	double spinOncePreviousStamp_;
     
+    GLuint textureId_;
+    glm::mat4 viewMatrix_;
+    glm::mat4 projectionMatrix_;
+    float transformed_uvs_[8];
+    bool uvs_initialized_ = false;
+
 private:
 	Transform previousPose_;
 	double previousStamp_;
@@ -128,6 +154,9 @@ private:
 
 	SensorData data_;
 	Transform pose_;
+    
+    cv::Mat occlusionImage_;
+    CameraModel occlusionModel_;
 };
 
 } /* namespace rtabmap */

app/android/jni/CameraTango.cpp

@@ -33,6 +33,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/util2d.h"
 #include <tango_client_api.h>
 #include <tango_support_api.h>
+#include "tango-gl/camera.h"
 
 namespace rtabmap {
 
@@ -42,11 +43,22 @@ const int holeSize = 5;
 const float maxDepthError = 0.10;
 const int scanDownsampling = 1;
 
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
+const float kTextureCoords0[] = {1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0, 0.0};
+const float kTextureCoords90[] = {0.0, 1.0, 1.0, 1.0, 0.0, 0.0, 1.0, 0.0};
+const float kTextureCoords180[] = {0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0, 1.0};
+const float kTextureCoords270[] = {1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 1.0};
+
 // Callbacks
 void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_cloud)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(app->isRunning() && point_cloud->num_points>0)
+	if(point_cloud->num_points>0)
 	{
 		app->cloudReceived(cv::Mat(1, point_cloud->num_points, CV_32FC4, point_cloud->points[0]), point_cloud->timestamp);
 	}
@@ -55,34 +67,32 @@ void onPointCloudAvailableRouter(void* context, const TangoPointCloud* point_clo
 void onFrameAvailableRouter(void* context, TangoCameraId id, const TangoImageBuffer* color)
 {
 	CameraTango* app = static_cast<CameraTango*>(context);
-	if(app->isRunning())
-	{
-		cv::Mat tangoImage;
-		if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-		{
-			tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
-		}
-		else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			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)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else if(color->format == 35)
-		{
-			tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", color->format);
-		}
 
-		if(!tangoImage.empty())
+	cv::Mat tangoImage;
-		{
+	if(color->format == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-			app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
+	{
-		}
+		tangoImage = cv::Mat(color->height, color->width, CV_8UC4, color->data);
+	}
+	else if(color->format == TANGO_HAL_PIXEL_FORMAT_YV12)
+	{
+		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)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else if(color->format == 35)
+	{
+		tangoImage = cv::Mat(color->height+color->height/2, color->width, CV_8UC1, color->data);
+	}
+	else
+	{
+		LOGE("Not supported color format : %d.", color->format);
+	}
+
+	if(!tangoImage.empty())
+	{
+		app->rgbReceived(tangoImage, (unsigned int)color->format, color->timestamp);
 	}
 }
 
@@ -110,7 +120,6 @@ CameraTango::CameraTango(bool colorCamera, int decimation, bool publishRawScan,
 		colorCamera_(colorCamera),
 		decimation_(decimation),
 		rawScanPublished_(publishRawScan),
-		cloudStamp_(0),
 		tangoColorType_(0),
 		tangoColorStamp_(0)
 {
@@ -430,7 +439,7 @@ void CameraTango::close()
 
 void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 {
-	if(this->isRunning() && !cloud.empty())
+	if(!cloud.empty())
 	{
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);
 
@@ -447,20 +456,248 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//  So, synchronize with the last RGB frame before the Depth is acquired
 		if(!tangoColor_.empty())
 		{
+			UTimer timer;
 			double dt = fabs(timestamp - tangoColorStamp_);
 
 			//LOGD("Depth: %f vs %f = %f", tangoColorStamp_, timestamp, dt);
 
 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = cloud_.empty();
+				bool notify = !data_.isValid();
-				cloud_ = cloud.clone();
+
-				cloudStamp_ = timestamp;
+				cv::Mat tangoImage = tangoColor_;
+				cv::Mat rgb;
+				double cloudStamp = timestamp;
+				double rgbStamp = tangoColorStamp_;
+				int tangoColorType = tangoColorType_;
+
+				tangoColor_ = cv::Mat();
+				tangoColorStamp_ = 0.0;
+				tangoColorType_ = 0;
+
+				LOGD("tangoColorType=%d", tangoColorType);
+				if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
+				}
+				else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
+				{
+					cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
+				}
+				else if(tangoColorType == 35)
+				{
+					cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
+				}
+				else
+				{
+					LOGE("Not supported color format : %d.", tangoColorType);
+					data_ = SensorData();
+					return;
+				}
+
+				//for(int i=0; i<rgb.cols; ++i)
+				//{
+				//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
+				//}
+
+				CameraModel model = model_;
+
+				if(colorCamera_)
+				{
+					if(decimation_ > 1)
+					{
+						rgb = util2d::decimate(rgb, decimation_);
+						model = model.scaled(1.0/double(decimation_));
+					}
+				}
+				else
+				{
+					//UTimer t;
+					cv::Mat rgbRect;
+					cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
+					rgb = rgbRect;
+					//LOGD("Rectification time=%fs", t.ticks());
+				}
+
+				// Querying the depth image's frame transformation based on the depth image's
+				// timestamp.
+				cv::Mat depth;
+
+				// Calculate the relative pose from color camera frame at timestamp
+				// color_timestamp t1 and depth
+				// camera frame at depth_timestamp t0.
+				Transform colorToDepth;
+				TangoPoseData pose_color_image_t1_T_depth_image_t0;
+				if (TangoSupport_calculateRelativePose(
+						rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+				{
+					colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
+				}
+				else
+				{
+					LOGE(
+						"SynchronizationApplication: Could not find a valid relative pose at "
+						"time for color and "
+						" depth cameras.");
+				}
+
+				if(colorToDepth.getNormSquared() > 100000)
+				{
+					LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
+					colorToDepth.setNull();
+				}
+				cv::Mat scan;
+				if(!colorToDepth.isNull())
+				{
+					// The Color Camera frame at timestamp t0 with respect to Depth
+					// Camera frame at timestamp t1.
+					//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
+					LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
+
+					int pixelsSet = 0;
+					int depthSizeDec = colorCamera_?8:1;
+					depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
+					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)
+					{
+						const float * p = cloud.ptr<float>(0,i);
+						cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+
+						if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+						{
+							scanData.at(oi++) = pt;
+						}
+
+						int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+						// get the coordinate on image plane.
+						pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
+						pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
+						pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
+						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
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+						   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
+						   depth_value)
+						{
+							unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+							if(depthPixel == 0 || depthPixel > depth_value)
+							{
+								depthPixel = depth_value;
+								pixelSet = true;
+							}
+						}
+						if(pixelSet)
+						{
+							pixelsSet += 1;
+						}
+					}
+
+					if(closePoints > 100)
+					{
+						this->post(new CameraInfoEvent(0, "TooClose", ""));
+					}
+
+					if(oi)
+					{
+						scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
+					}
+					//LOGD("pixels depth set= %d", pixelsSet);
+				}
+				else
+				{
+					LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
+				}
+
+				if(!rgb.empty() && !depth.empty())
+				{
+					depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
+
+					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());
+
+					// adjust origin
+					if(!getOriginOffset().isNull())
+					{
+						odom = getOriginOffset() * odom;
+					}
+
+					// occlusion depth
+					if(!depth.empty())
+					{
+						rtabmap::CameraModel depthModel = model.scaled(float(depth.cols) / float(model.imageWidth()));
+						depthModel.setLocalTransform(odom*model.localTransform());
+						this->setOcclusionImage(depth, depthModel);
+					}
+
+					//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();
+
+					if(rawScanPublished_)
+					{
+						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);
+				}
+				else
+				{
+					LOGE("Could not get depth and rgb images!?!");
+					data_ = SensorData();
+					return;
+				}
+
 				if(notify)
 				{
-					//LOGD("Cloud: Release semaphore");
 					dataReady_.release();
 				}
+				LOGD("process cloud received %fs", timer.ticks());
 			}
 		}
 	}
@@ -468,7 +705,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 
 void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double timestamp)
 {
-	if(this->isRunning() && !tangoImage.empty())
+	if(!tangoImage.empty())
 	{
 		//LOGD("RGB received! %fs", timestamp);
 
@@ -532,7 +769,6 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	}
 	else
 	{
-
 		pose = rtabmap_world_T_tango_world * tangoPoseToTransform(&pose_start_service_T_device) * tango_device_T_rtabmap_world;
 	}
 
@@ -543,254 +779,112 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 {
 	//LOGI("Capturing image...");
 
-	SensorData data;
+	if(textureId_ == 0)
-	if(!dataReady_.acquire(1, 2000))
 	{
-		if(this->isRunning())
+		glGenTextures(1, &textureId_);
-		{
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
-			LOGE("Not received any frames since 2 seconds, try to restart the camera again.");
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-			this->post(new CameraInfoEvent(0, "CameraTango", "No frames received since 2 seconds."));
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-			boost::mutex::scoped_lock  lock(dataMutex_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-			if(!cloud_.empty() && !tangoColor_.empty())
-			{
-				UERROR("cloud and image were set!?");
-			}
-		}
-		cloud_ = cv::Mat();
-		cloudStamp_ = 0.0;
-		tangoColor_ = cv::Mat();
-		tangoColorStamp_ = 0.0;
-		tangoColorType_ = 0;
 	}
-	else
+
+	// Update Texture (optional, just for first-view rendering)
+	if(colorCamera_ && textureId_)
 	{
-		cv::Mat cloud;
+		double video_overlay_timestamp;
-		cv::Mat tangoImage;
+		TangoErrorType status = TangoService_updateTextureExternalOes(TANGO_CAMERA_COLOR, textureId_, &video_overlay_timestamp);
-		cv::Mat rgb;
-		double cloudStamp = 0.0;
-		double rgbStamp = 0.0;
-		int tangoColorType = 0;
 
+		if (status == TANGO_SUCCESS)
 		{
-			boost::mutex::scoped_lock  lock(dataMutex_);
+			if(info)
-			cloud = cloud_;
-			cloudStamp = cloudStamp_;
-			cloud_ = cv::Mat();
-			cloudStamp_ = 0.0;
-			tangoImage = tangoColor_;
-			rgbStamp = tangoColorStamp_;
-			tangoColorType = tangoColorType_;
-			tangoColor_ = cv::Mat();
-			tangoColorStamp_ = 0.0;
-			tangoColorType_ = 0;
-		}
-
-		LOGD("tangoColorType=%d", tangoColorType);
-		if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_RGBA_8888)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_RGBA2BGR);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YV12)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_YV12);
-		}
-		else if(tangoColorType == TANGO_HAL_PIXEL_FORMAT_YCrCb_420_SP)
-		{
-			cv::cvtColor(tangoImage, rgb, CV_YUV2BGR_NV21);
-		}
-		else if(tangoColorType == 35)
-		{
-			cv::cvtColor(tangoImage, rgb, cv::COLOR_YUV420sp2GRAY);
-		}
-		else
-		{
-			LOGE("Not supported color format : %d.", tangoColorType);
-			return data;
-		}
-
-		//for(int i=0; i<rgb.cols; ++i)
-		//{
-		//	UERROR("%d,%d,%d", (int)rgb.at<cv::Vec3b>(i)[0], (int)rgb.at<cv::Vec3b>(i)[1], (int)rgb.at<cv::Vec3b>(i)[2]);
-		//}
-
-		CameraModel model = model_;
-
-		if(colorCamera_)
-		{
-			if(decimation_ > 1)
 			{
-				rgb = util2d::decimate(rgb, decimation_);
+				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-				model = model.scaled(1.0/double(decimation_));
 			}
-		}
-		else
-		{
-			//UTimer t;
-			cv::Mat rgbRect;
-			cv::remap(rgb, rgbRect, fisheyeRectifyMapX_, fisheyeRectifyMapY_, cv::INTER_LINEAR, cv::BORDER_CONSTANT, 0);
-			rgb = rgbRect;
-			//LOGD("Rectification time=%fs", t.ticks());
-		}
 
-		// Querying the depth image's frame transformation based on the depth image's
+			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
-		// timestamp.
+			  if (rotation > 3) {
-		cv::Mat depth;
+				  rotation -= 4;
+			  }
 
-		// Calculate the relative pose from color camera frame at timestamp
+			TangoDoubleMatrixTransformData matrix_transform;
-		// color_timestamp t1 and depth
+			status = TangoSupport_getDoubleMatrixTransformAtTime(
-		// camera frame at depth_timestamp t0.
+					video_overlay_timestamp,
-		Transform colorToDepth;
+					TANGO_COORDINATE_FRAME_CAMERA_COLOR,
-		TangoPoseData pose_color_image_t1_T_depth_image_t0;
+					TANGO_COORDINATE_FRAME_START_OF_SERVICE,
-		if (TangoSupport_calculateRelativePose(
+					TANGO_SUPPORT_ENGINE_OPENGL,
-				rgbStamp, colorCamera_?TANGO_COORDINATE_FRAME_CAMERA_COLOR:TANGO_COORDINATE_FRAME_CAMERA_FISHEYE, cloudStamp,
+					TANGO_SUPPORT_ENGINE_OPENGL,
-			  TANGO_COORDINATE_FRAME_CAMERA_DEPTH,
+					static_cast<TangoSupportRotation>(rotation),
-			  &pose_color_image_t1_T_depth_image_t0) == TANGO_SUCCESS)
+					&matrix_transform);
-		{
+			if (matrix_transform.status_code == TANGO_POSE_VALID)
-			colorToDepth = tangoPoseToTransform(&pose_color_image_t1_T_depth_image_t0);
-		}
-		else
-		{
-			LOGE(
-				"SynchronizationApplication: Could not find a valid relative pose at "
-				"time for color and "
-				" depth cameras.");
-		}
-
-		if(colorToDepth.getNormSquared() > 100000)
-		{
-			LOGE("Very large color to depth error detected (%s)! Ignoring this frame!", colorToDepth.prettyPrint().c_str());
-			colorToDepth.setNull();
-		}
-		cv::Mat scan;
-		if(!colorToDepth.isNull())
-		{
-			// The Color Camera frame at timestamp t0 with respect to Depth
-			// Camera frame at timestamp t1.
-			//LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str());
-			LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total());
-
-			int pixelsSet = 0;
-			int depthSizeDec = colorCamera_?8:1;
-			depth = cv::Mat::zeros(model_.imageHeight()/depthSizeDec, model_.imageWidth()/depthSizeDec, CV_16UC1); // mm
-			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);
+				// Get projection matrix
-				cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth);
+				TangoCameraIntrinsics color_camera_intrinsics;
+				int ret = TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation(
+						TANGO_CAMERA_COLOR,
+						static_cast<TangoSupportRotation>(rotation),
+						&color_camera_intrinsics);
 
-				if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_)
+				if (ret == TANGO_SUCCESS) {
-				{
+					float image_width = static_cast<float>(color_camera_intrinsics.width);
-					scanData.at(oi++) = pt;
+					float image_height = static_cast<float>(color_camera_intrinsics.height);
-				}
+					float fx = static_cast<float>(color_camera_intrinsics.fx);
+					float fy = static_cast<float>(color_camera_intrinsics.fy);
+					float cx = static_cast<float>(color_camera_intrinsics.cx);
+					float cy = static_cast<float>(color_camera_intrinsics.cy);
 
-				int pixel_x_l, pixel_y_l, pixel_x_h, pixel_y_h;
+					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
-				// get the coordinate on image plane.
+					if(!getOriginOffset().isNull())
-				pixel_x_l = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx());
-				pixel_y_l = static_cast<int>((depthModel.fy()) * (pt.y / pt.z) + depthModel.cy());
-				pixel_x_h = static_cast<int>((depthModel.fx()) * (pt.x / pt.z) + depthModel.cx() + 0.5f);
-				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;
+						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
 					}
-				}
 
-				bool pixelSet = false;
+					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
-				if(pixel_x_l>=0 && pixel_x_l<depth.cols &&
+							image_width, image_height, fx, fy, cx, cy, 0.3, 50);
-				   pixel_y_l>0 && pixel_y_l<depth.rows && // ignore first line
+
-				   depth_value)
+					switch(rotation)
-				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_l, pixel_x_l);
-					if(depthPixel == 0 || depthPixel > depth_value)
 					{
-						depthPixel = depth_value;
+					case ROTATION_90:
-						pixelSet = true;
+						memcpy(transformed_uvs_, kTextureCoords90, 8*sizeof(float));
+						break;
+					case ROTATION_180:
+						memcpy(transformed_uvs_, kTextureCoords180, 8*sizeof(float));
+						break;
+					case ROTATION_270:
+						memcpy(transformed_uvs_, kTextureCoords270, 8*sizeof(float));
+						break;
+					case ROTATION_0:
+					default:
+						memcpy(transformed_uvs_, kTextureCoords0, 8*sizeof(float));
 					}
+					uvs_initialized_ = true;
 				}
-				if(pixel_x_h>=0 && pixel_x_h<depth.cols &&
+				else
-				   pixel_y_h>0 && pixel_y_h<depth.rows && // ignore first line
-				   depth_value)
 				{
-					unsigned short & depthPixel = depth.at<unsigned short>(pixel_y_h, pixel_x_h);
+					UERROR("TangoSupport_getCameraIntrinsicsBasedOnDisplayRotation failed!");
-					if(depthPixel == 0 || depthPixel > depth_value)
-					{
-						depthPixel = depth_value;
-						pixelSet = true;
-					}
 				}
-				if(pixelSet)
-				{
-					pixelsSet += 1;
-				}
-			}
-
-			if(closePoints > 100)
-			{
-				this->post(new CameraInfoEvent(0, "TooClose", ""));
-			}
-
-			if(oi)
-			{
-				scan = cv::Mat(1, oi, CV_32FC3, scanData.data()).clone();
-			}
-			//LOGD("pixels depth set= %d", pixelsSet);
-		}
-		else
-		{
-			LOGE("color to depth pose is null?!? (rgb stamp=%f) (depth stamp=%f)", rgbStamp, cloudStamp);
-		}
-
-		if(!rgb.empty() && !depth.empty())
-		{
-			depth = rtabmap::util2d::fillDepthHoles(depth, holeSize, maxDepthError);
-
-			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());
-
-			// adjust origin
-			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();
-
-			if(rawScanPublished_)
-			{
-				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);
+				UERROR("TangoSupport_getDoubleMatrixTransformAtTime failed!");
 			}
-			info->odomPose = odom;
 		}
 		else
 		{
-			LOGE("Could not get depth and rgb images!?!");
+			UERROR("TangoService_updateTextureExternalOes failed!");
+		}
+	}
+
+	SensorData data;
+	if(dataReady_.acquireTry(1))
+	{
+		boost::mutex::scoped_lock  lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
+		if(info)
+		{
+			info->odomPose = data.groundTruth();
+			data.setGroundTruth(Transform());
 		}
 	}
 	return data;

app/android/jni/CameraTango.h

@@ -71,8 +71,7 @@ private:
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	cv::Mat cloud_;
+	SensorData data_;
-	double cloudStamp_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;

app/android/jni/ProgressionStatus.h

@@ -11,7 +11,9 @@
 #include <rtabmap/core/ProgressState.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UEventsManager.h>
+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 
 namespace rtabmap {
 
@@ -27,16 +29,29 @@ public:
 class ProgressionStatus: public ProgressState, public UEventsHandler
 {
 public:
-	ProgressionStatus() : count_(0), max_(100), jvm_(0), rtabmap_(0)
+	ProgressionStatus() : count_(0), max_(100)
+#ifdef __ANDROID__
+    , jvm_(0), rtabmap_(0)
+#else
+    , swiftClassPtr_(0)
+#endif
 	{
 		registerToEventsManager();
 	}
 
+#ifdef __ANDROID__
 	void setJavaObjects(JavaVM * jvm, jobject rtabmap)
 	{
 		jvm_ = jvm;
 		rtabmap_ = rtabmap;
 	}
+#else
+    void setSwiftCallback(void * classPtr, void(*callback)(void *, int, int))
+    {
+        swiftClassPtr_ = classPtr;
+        swiftCallback = callback;
+    }
+#endif
 
 	void reset(int max)
 	{
@@ -82,6 +97,7 @@ protected:
 			count_ += ((ProgressEvent*)event)->count_;
 			// Call JAVA callback
 			bool success = false;
+#ifdef __ANDROID__
 			if(jvm_ && rtabmap_)
 			{
 				JNIEnv *env = 0;
@@ -103,6 +119,16 @@ protected:
 				}
 				jvm_->DetachCurrentThread();
 			}
+#else // APPLE
+            if(swiftClassPtr_)
+            {
+                std::function<void()> actualCallback = [&](){
+                    swiftCallback(swiftClassPtr_, count_, max_);
+                };
+                actualCallback();
+                success = true;
+            }
+#endif
 			if(!success)
 			{
 				UERROR("Failed to call rtabmap::updateProgressionCallback");
@@ -114,8 +140,13 @@ protected:
 private:
 	int count_;
 	int max_;
+#ifdef __ANDROID__
 	JavaVM *jvm_;
 	jobject rtabmap_;
+#else
+    void * swiftClassPtr_;
+    void(*swiftCallback)(void *, int, int);
+#endif
 };
 
 }

app/android/jni/RTABMapApp.h

@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef RTABMAP_APP_H_
 #define RTABMAP_APP_H_
 
+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 #include <memory>
 
 #include <tango-gl/util.h>
@@ -49,7 +51,26 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 class RTABMapApp : public UEventsHandler {
  public:
   // Constructor and deconstructor.
+#ifdef __ANDROID__
   RTABMapApp(JNIEnv* env, jobject caller_activity);
+#else // __APPLE__
+  RTABMapApp();
+  void setupSwiftCallbacks(void * classPtr,
+                           void(*progressCallback)(void *, int, int),
+                           void(*initCallback)(void *, int, const char*),
+                           void(*statsUpdatedCallback)(void *,
+                                                    int, int, int, int,
+                                                    float,
+                                                    int, int, int, int, int ,int,
+                                                    float,
+                                                    int,
+                                                    float,
+                                                    int,
+                                                    float, float, float, float,
+                                                    int, int,
+                                                    float, float, float, float, float, float));
+    
+#endif
   ~RTABMapApp();
 
   void setScreenRotation(int displayRotation, int cameraRotation);
@@ -57,8 +78,11 @@ class RTABMapApp : public UEventsHandler {
   int openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource=std::string());
 
   bool isBuiltWith(int cameraDriver) const;
+#ifdef __ANDROID__
   bool startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver);
-
+#else // __APPLE__
+    bool startCamera();
+#endif
   // Allocate OpenGL resources for rendering, mainly for initializing the Scene.
   void InitializeGLContent();
 
@@ -117,11 +141,13 @@ class RTABMapApp : public UEventsHandler {
   void setMinCloudDepth(float value);
   void setCloudDensityLevel(int value);
   void setMeshAngleTolerance(float value);
+  void setMeshDecimationFactor(float value);
   void setMeshTriangleSize(int value);
   void setClusterRatio(float value);
   void setMaxGainRadius(float value);
   void setRenderingTextureDecimation(int value);
   void setBackgroundColor(float gray);
+  void setDepthConfidence(int value);
   int setMappingParameter(const std::string & key, const std::string & value);
   void setGPS(const rtabmap::GPS & gps);
   void addEnvSensor(int type, float value);
@@ -150,20 +176,29 @@ class RTABMapApp : public UEventsHandler {
   int postProcessing(int approach);
 
   void postCameraPoseEvent(
-  		float x, float y, float z, float qx, float qy, float qz, float qw);
+  		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
 
   void postOdometryEvent(
-  		float x, float y, float z, float qx, float qy, float qz, float qw,
+		rtabmap::Transform pose,
-  		float fx, float fy, float cx, float cy,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
-  		double stamp,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
-		void * yPlane, void * uPlane, void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+		const rtabmap::Transform & rgbFrame,
-  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		const rtabmap::Transform & depthFrame,
-		float * points, int pointsLen);
+		double stamp,
+		double depthStamp,
+        const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+        const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+        const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+        const float * points, int pointsLen, int pointsChannels,
+		const rtabmap::Transform & viewMatrix, //view matrix
+        float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+        float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
 
  protected:
   virtual bool handleEvent(UEvent * event);
 
  private:
+  int updateMeshDecimation(int width, int height);
   rtabmap::ParametersMap getRtabmapParameters();
   bool smoothMesh(int id, rtabmap::Mesh & mesh);
   void gainCompensation(bool full = false);
@@ -193,10 +228,12 @@ class RTABMapApp : public UEventsHandler {
   int cloudDensityLevel_;
   int meshTrianglePix_;
   float meshAngleToleranceDeg_;
+  float meshDecimationFactor_;
   float clusterRatio_;
   float maxGainRadius_;
   int renderingTextureDecimation_;
   float backgroundColor_;
+  int depthConfidence_;
 
   rtabmap::ParametersMap mappingParameters_;
 
@@ -210,7 +247,6 @@ class RTABMapApp : public UEventsHandler {
   bool bilateralFilteringOnNextRender_;
   bool takeScreenshotOnNextRender_;
   bool cameraJustInitialized_;
-  int meshDecimation_;
   int totalPoints_;
   int totalPolygons_;
   int lastDrawnCloudsCount_;
@@ -233,6 +269,7 @@ class RTABMapApp : public UEventsHandler {
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
+	std::map<double, rtabmap::Transform> poseBuffer_;
 
 	rtabmap::Transform mapToOdom_;
 
@@ -251,6 +288,23 @@ class RTABMapApp : public UEventsHandler {
 	std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
 
 	rtabmap::ProgressionStatus progressionStatus_;
+    
+#ifndef __ANDROID__
+    void * swiftClassPtr_;
+    void(*swiftInitCallback)(void *, int, const char *);
+    void(*swiftStatsUpdatedCallback)(void *,
+                             int, int, int, int,
+                             float,
+                             int, int, int, int, int ,int,
+                             float,
+                             int,
+                             float,
+                             int,
+                             float, float, float, float,
+                             int, int,
+                             float, float, float, float, float, float);
+    
+#endif
 };
 
 #endif  // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

app/android/jni/background_renderer.cc

@@ -34,55 +34,173 @@ const std::string kVertexShader =
 		"   v_TexCoord = a_TexCoord;\n"
 		"}\n";
 
-const std::string kFragmentShader =
+const std::string kFragmentShaderOES =
 	"#extension GL_OES_EGL_image_external : require\n"
-
 	"precision mediump float;\n"
 	"varying vec2 v_TexCoord;\n"
 	"uniform samplerExternalOES sTexture;\n"
+    "uniform bool uRedUnknown;\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, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+	"}\n";
 
+const std::string kFragmentShaderBlendingOES =
+    "#extension GL_OES_EGL_image_external : require\n"
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform samplerExternalOES sTexture;\n"
+	"uniform sampler2D uDepthTexture;\n"
+	"uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+const std::string kFragmentShader =
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform bool uRedUnknown;\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"
+	"    gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
 	"}\n";
 
+const std::string kFragmentShaderBlending =
+    "precision mediump float;\n"
+    "varying vec2 v_TexCoord;\n"
+    "uniform sampler2D sTexture;\n"
+    "uniform sampler2D uDepthTexture;\n"
+    "uniform vec2 uScreenScale;\n"
+    "uniform bool uRedUnknown;\n"
+    "void main() {\n"
+    "    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+    "    vec2 coord = uScreenScale * gl_FragCoord.xy;\n;"
+    "    vec4 depthPacked = texture2D(uDepthTexture, coord);\n"
+    "    float depth = dot(depthPacked, 1./vec4(1.,255.,65025.,16581375.));\n"
+    "    if(depth > 0.0)\n"
+    "        gl_FragColor = vec4(sample.r, sample.g, sample.b, 0.5);\n"
+    "    else {\n"
+    "        float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+    "        gl_FragColor = vec4(grey, uRedUnknown?0.0:grey, uRedUnknown?0.0:grey, 0.5);\n"
+    "    }\n"
+    "}\n";
+
+/* To debug depth texture
+ const std::string kFragmentShader =
+     "precision mediump float;\n"
+     "varying vec2 v_TexCoord;\n"
+     "uniform sampler2D sTexture;\n"
+
+     "void main() {\n"
+     "  float uNearZ = 0.2;\n"
+     "  float uFarZ = 1000.0;\n"
+     "  float depth = texture2D(sTexture, v_TexCoord).r;\n"
+     "  float num =  (2.0 * uNearZ * uFarZ);\n"
+     "  float diff = (uFarZ - uNearZ);\n"
+     "  float add = (uFarZ + uNearZ);\n"
+     "  float ndcDepth = depth * 2.0 - 1.0;\n" // Back to NDC
+     "  float linearDepth = num / (add - ndcDepth * diff);\n" // inverse projection matrix
+     "  float grey = linearDepth/3.0;\n"
+     "  gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+     "}\n";
+ */
+
 }  // namespace
 
-void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+std::vector<GLuint> BackgroundRenderer::shaderPrograms_;
-{
-  texture_id_ = textureId;
 
-  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+BackgroundRenderer::~BackgroundRenderer()
-  if (!shader_program_) {
+{
-    LOGE("Could not create program.");
+	for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
-  }
+	{
-  glUseProgram(shader_program_);
+		glDeleteShader(shaderPrograms_[i]);
-  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+	}
-  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+	shaderPrograms_.clear();
-  glUseProgram(0);
 }
 
-void BackgroundRenderer::Draw(const float * transformed_uvs) {
+void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
+{
+	LOGI("textureId=%d", textureId);
+
+  texture_id_ = textureId;
+#ifdef __ANDROID__
+  oes_ = oes;
+#endif
+
+    if(shaderPrograms_.empty())
+    {
+        shaderPrograms_.resize(2,0);
+        shaderPrograms_[0] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderOES.c_str():kFragmentShader.c_str());
+        UASSERT(shaderPrograms_[0]!=0);
+        shaderPrograms_[1] = tango_gl::util::CreateProgram(
+              kVertexShader.c_str(),
+              oes_?kFragmentShaderBlendingOES.c_str():kFragmentShaderBlending.c_str());
+        UASSERT(shaderPrograms_[1]!=0);
+    }
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
   static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
 
-  glUseProgram(shader_program_);
+  GLuint program = shaderPrograms_[depthTexture>0?1:0];
+    
+  glUseProgram(program);
   glDepthMask(GL_FALSE);
   glEnable (GL_BLEND);
 
   glActiveTexture(GL_TEXTURE0);
-  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+#ifdef __ANDROID__
+  if(oes_)
+    glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+  else
+#endif
+    glBindTexture(GL_TEXTURE_2D, texture_id_);
     
-  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  if(depthTexture>0)
-  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+  {
+    // Texture activate unit 1
+    glActiveTexture(GL_TEXTURE1);
+    // Bind the texture to this unit.
+    glBindTexture(GL_TEXTURE_2D, depthTexture);
+    // Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 1.
+    GLuint depth_texture_handle = glGetUniformLocation(program, "uDepthTexture");
+    glUniform1i(depth_texture_handle, 1);
       
-  glEnableVertexAttribArray(attribute_vertices_);
+    GLuint screenScale_handle = glGetUniformLocation(program, "uScreenScale");
-  glEnableVertexAttribArray(attribute_uvs_);
+    glUniform2f(screenScale_handle, 1.0f/(float)screenWidth, 1.0f/(float)screenHeight);
+  }
+    
+  GLuint screenScale_handle = glGetUniformLocation(program, "uRedUnknown");
+  glUniform1i(screenScale_handle, redUnknown);
+
+  GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
+  GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
+
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
+
+  glEnableVertexAttribArray(attributeVertices);
+  glEnableVertexAttribArray(attributeUvs);
 
   glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
 
-  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attributeVertices);
-  glDisableVertexAttribArray(attribute_uvs_);
+  glDisableVertexAttribArray(attributeUvs);
 
   glUseProgram(0);
   glDepthMask(GL_TRUE);

app/android/jni/background_renderer.h

@@ -17,8 +17,13 @@
 #ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
 #define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
 
+#ifdef __ANDROID__
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+#endif
 #include <cstdlib>
 
 #include "util.h"
@@ -26,6 +31,16 @@
 static const GLfloat BackgroundRenderer_kVertices[] = {
 	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
 };
+static const GLfloat BackgroundRenderer_kTexCoord[] = {
+   1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
+};
+
+//android phone
+//11 10 01 00 // portrait
+//01 11 00 10 // left
+//10 00 11 01 // right
+//00 01 10 11 // down
+
 
 // This class renders the passthrough camera image into the OpenGL frame.
 class BackgroundRenderer {
@@ -36,23 +51,20 @@ public:
 
  public:
   BackgroundRenderer() = default;
-  ~BackgroundRenderer() = default;
+  ~BackgroundRenderer();
 
   // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
   // other methods below.
-  void InitializeGlContent(GLuint textureId);
+  void InitializeGlContent(GLuint textureId, bool oes);
 
   // 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);
+  void Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown);
 
- private:
+private:
-
+  static std::vector<GLuint> shaderPrograms_;
-  GLuint shader_program_;
   GLuint texture_id_;
-
+  bool oes_ = false;
-  GLuint attribute_vertices_;
-  GLuint attribute_uvs_;
 };
 
 #endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

app/android/jni/graph_drawable.cpp

@@ -31,7 +31,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/ULogger.h"
 #include "util.h"
 
+#ifdef __ANDROID__
 #include <GLES2/gl2.h>
+#else //__APPLE__
+#include <OpenGLES/ES2/gl.h>
+#endif
 
 GraphDrawable::GraphDrawable(
 		GLuint shaderProgram,

app/android/jni/graph_drawable.h

@@ -28,8 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef GRAPH_DRAWABLE_H_
 #define GRAPH_DRAWABLE_H_
 
+#ifdef __ANDROID__
 #include <jni.h>
-
+#endif
 #include <tango-gl/util.h>
 #include <vector>
 #include <pcl/point_cloud.h>

app/android/jni/jni_interface.cpp

@@ -869,11 +869,11 @@ Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postCameraPoseEvent(
 		JNIEnv* env, jclass, jlong native_application,
-		float x, float y, float z, float qx, float qy, float qz, float qw)
+		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
 	if(native_application)
 	{
-		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw);
+		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw, stamp);
 	}
 	else
 	{
@@ -886,26 +886,36 @@ JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 		JNIEnv* env, jclass, jlong native_application,
 		float x, float y, float z, float qx, float qy, float qz, float qw,
-		float fx, float fy, float cx, float cy,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
 		double stamp,
 		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
-		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		jobject points, int pointsLen,
-		jobject points, int pointsLen)
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord
 {
 	if(native_application)
 	{
 		void *yPtr = env->GetDirectBufferAddress(yPlane);
 		void *uPtr = env->GetDirectBufferAddress(uPlane);
 		void *vPtr = env->GetDirectBufferAddress(vPlane);
-		void *depthPtr = env->GetDirectBufferAddress(depth);
 		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
 		native(native_application)->postOdometryEvent(
-				x,y,z,qx,qy,qz,qw,
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
-				fx,fy,cx,cy,
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				0,0,0,0,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(),
 				stamp,
+				0,
 				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
-				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				0,0,0,0,0, //depth
-				pointsPtr, pointsLen);
+				0,0,0,0,0, //conf
+				pointsPtr, pointsLen, 4,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+				p00, p11, p02, p12, p22, p32, p23,
+				t0, t1, t2, t3, t4, t5, t6, t7);
 	}
 	else
 	{
@@ -914,6 +924,44 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 	}
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postOdometryEventDepth(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+		double rgbStamp,
+		double depthStamp,
+		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+ 		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+ 		jobject points, int pointsLen,
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord)
+ {
+		void *yPtr = env->GetDirectBufferAddress(yPlane);
+		void *uPtr = env->GetDirectBufferAddress(uPlane);
+		void *vPtr = env->GetDirectBufferAddress(vPlane);
+ 		void *depthPtr = env->GetDirectBufferAddress(depth);
+ 		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
+ 		native(native_application)->postOdometryEvent(
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				depth_fx,depth_fy,depth_cx,depth_cy,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(depthFrameX, depthFrameY, depthFrameZ, depthFrameQX, depthFrameQY, depthFrameQZ, depthFrameQW),
+				rgbStamp,
+				depthStamp,
+ 				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+ 				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				0,0,0,0,0, // conf
+ 				pointsPtr, pointsLen, 4,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+				p00, p11, p02, p12, p22, p32, p23,
+				t0, t1, t2, t3, t4, t5, t6, t7);
+ }
 
 
 #ifdef __cplusplus

app/android/jni/point_cloud_drawable.cpp

@@ -35,7 +35,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "pcl/common/transforms.h"
 
+#ifdef __ANDROID__
 #include <GLES2/gl2.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#endif
 
 #define LOW_DEC 2
 #define LOWLOW_DEC 4
@@ -257,7 +261,7 @@ const std::string kTextureMeshBlendingFragmentShader =
 	"  float linearFragz = num / (add - ndcFragz * diff);\n" // inverse projection matrix
 	"  if(linearFragz > linearDepth + 0.05)\n"
 	"    alpha=0.0;\n"
-	"  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
+    "  gl_FragColor = vec4(textureColor.r * uGainR * vLightWeighting, textureColor.g * uGainG * vLightWeighting, textureColor.b * uGainB * vLightWeighting, alpha);\n"
     "}\n";
 
 std::vector<GLuint> PointCloudDrawable::shaderPrograms_;
@@ -356,7 +360,7 @@ PointCloudDrawable::~PointCloudDrawable()
 
 void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes, bool createWireframe)
 {
-	LOGD("Update polygons");
+	//LOGD("Update polygons");
 	polygons_.clear();
 	polygonLines_.clear();
 	polygonsLowRes_.clear();
@@ -580,13 +584,13 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	int totalPoints = 0;
 	std::vector<pcl::Vertices> polygons = mesh.polygons;
 	std::vector<pcl::Vertices> polygonsLowRes;
-	hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
+    hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
 	UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
 	if(mesh.cloud->isOrganized()) // assume organized mesh
 	{
 		polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
 		organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
-		totalPoints = mesh.indices->size();
+		totalPoints = (int)mesh.indices->size();
 		verticesLowRes_.resize(totalPoints);
 		verticesLowLowRes_.resize(totalPoints);
 		int oi_low = 0;
@@ -672,7 +676,7 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 	}
 	else // assume dense mesh with texCoords set to polygons
 	{
-		if(textures_ && polygons.size() && mesh.normals->size())
+		if(textures_ && polygons.size())
 		{
 			//LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
 			//		(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
@@ -681,14 +685,14 @@ void PointCloudDrawable::updateMesh(const rtabmap::Mesh & mesh, bool createWiref
 			//  tex_coordinates should be linked to points, not
 			//  polygon vertices. Points linked to multiple different texCoords (different textures) should
 			//  be duplicated.
-			totalPoints = mesh.texCoords.size();
+			totalPoints = (int)mesh.texCoords.size();
-			vertices = std::vector<float>(mesh.texCoords.size()*9);
+            int items = hasNormals_?9:6;
+			vertices = std::vector<float>(mesh.texCoords.size()*items);
 			organizedToDenseIndices_ = std::vector<unsigned int>(totalPoints, -1);
 
 			UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
 					uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
 
-			int items = hasNormals_?9:6;
 			unsigned int oi=0;
 			for(unsigned int i=0; i<polygons.size(); ++i)
 			{

app/android/jni/point_cloud_drawable.h

@@ -28,7 +28,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 #define TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_
 
+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 
 #include <tango-gl/util.h>
 #include <vector>

app/android/jni/scene.cpp

@@ -88,7 +88,7 @@ Scene::Scene() :
 		mapRendering_(true),
 		meshRendering_(true),
 		meshRenderingTexture_(true),
-		pointSize_(5.0f),
+		pointSize_(10.0f),
 		boundingBoxRendering_(false),
 		lighting_(false),
 		backfaceCulling_(true),
@@ -98,11 +98,11 @@ Scene::Scene() :
 		b_(0.0f),
 		fboId_(0),
 		rboId_(0),
-		depthTexture_(0),
 		screenWidth_(0),
 		screenHeight_(0),
 		doubleTapOn_(false)
 {
+    depthTextures_[0] = depthTextures_[1] = 0;
 	gesture_camera_ = new tango_gl::GestureCamera();
 	gesture_camera_->SetCameraType(
 	      tango_gl::GestureCamera::kThirdPersonFollow);
@@ -179,8 +179,8 @@ void Scene::DeleteResources() {
 		fboId_ = 0;
 		glDeleteRenderbuffers(1, &rboId_);
 		rboId_ = 0;
-		glDeleteTextures(1, &depthTexture_);
+        glDeleteTextures(2, depthTextures_);
-		depthTexture_ = 0;
+        depthTextures_[0] = depthTextures_[1] = 0;
 	}
 
 	clear();
@@ -220,29 +220,35 @@ void Scene::SetupViewPort(int w, int h) {
 	if (h == 0) {
 		LOGE("Setup graphic height not valid");
 	}
+    
 	UASSERT(gesture_camera_ != 0);
 	gesture_camera_->SetWindowSize(static_cast<float>(w), static_cast<float>(h));
 	glViewport(0, 0, w, h);
 	if(screenWidth_ != w || screenHeight_ != h || fboId_ == 0)
 	{
+        UINFO("Setup viewport OpenGL: %dx%d", w, h);
+        
 		if(fboId_>0)
 		{
 			glDeleteFramebuffers(1, &fboId_);
 			fboId_ = 0;
 			glDeleteRenderbuffers(1, &rboId_);
 			rboId_ = 0;
-			glDeleteTextures(1, &depthTexture_);
+			glDeleteTextures(2, depthTextures_);
-			depthTexture_ = 0;
+            depthTextures_[0] = depthTextures_[1] = 0;
 		}
 
+        GLint originid = 0;
+        glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
+        
 		// regenerate fbo texture
 		// create a framebuffer object, you need to delete them when program exits.
 		glGenFramebuffers(1, &fboId_);
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
 		// Create depth texture
-		glGenTextures(1, &depthTexture_);
+		glGenTextures(2, depthTextures_);
-		glBindTexture(GL_TEXTURE_2D, depthTexture_);
+		glBindTexture(GL_TEXTURE_2D, depthTextures_[0]);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
 		glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
@@ -250,22 +256,27 @@ void Scene::SetupViewPort(int w, int h) {
 		glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
 		glBindTexture(GL_TEXTURE_2D, 0);
         
+        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
+        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+        glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+        glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
+        glBindTexture(GL_TEXTURE_2D, 0);
+
 		glGenRenderbuffers(1, &rboId_);
 		glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
 		glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT16, w, h);
 		glBindRenderbuffer(GL_RENDERBUFFER, 0);
 
 		// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
-		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
+		glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTextures_[0], 0);
 		glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
 
 		GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
-		if ( status != GL_FRAMEBUFFER_COMPLETE)
+        UASSERT ( status == GL_FRAMEBUFFER_COMPLETE);
-		{
+		glBindFramebuffer(GL_FRAMEBUFFER, originid);
-			LOGE("Frame buffer cannot be generated! Status: %in", status);
+    }
-		}
-		glBindFramebuffer(GL_FRAMEBUFFER,0);
-	}
 	screenWidth_ = w;
 	screenHeight_ = h;
 }
@@ -378,7 +389,7 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh, bool mapping) {
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -411,13 +422,21 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 	bool renderBackgroundCamera =
 			background_renderer_ &&
-			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+            gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
-			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+            !rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
-			uvsTransformed;
+            uvsTransformed;
+
 	if(renderBackgroundCamera)
 	{
-		projectionMatrix = arProjectionMatrix;
+        if(projectionMatrix[0][0] > arProjectionMatrix[0][0]-0.2)
-		viewMatrix = arViewMatrix;
+        {
+            projectionMatrix = arProjectionMatrix;
+            viewMatrix = arViewMatrix;
+        }
+        else
+        {
+            renderBackgroundCamera = false;
+        }
 	}
 
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
@@ -469,32 +488,41 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 
 	UTimer timer;
 
-	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
+	bool onlineBlending = (!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_);
 	if(onlineBlending && fboId_)
 	{
+        GLint originid = 0;
+        glGetIntegerv(GL_FRAMEBUFFER_BINDING, &originid);
+        
 		// set the rendering destination to FBO
 		glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
 
 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
-		if(renderBackgroundCamera)
+        // Draw scene
-		{
+        for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
-			PointCloudDrawable drawable(occlusionMesh);
+        {
-			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+            Eigen::Vector3f cloudToCamera(
-		}
+                      (*iter)->getPose().x() - openglCamera.x(),
-		else
+                      (*iter)->getPose().y() - openglCamera.y(),
-		{
+                      (*iter)->getPose().z() - openglCamera.z());
-			// Draw scene
+            float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
-			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+            (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
-			{
+        }
-				// set large distance to cam to use low res polygons for fast processing
+        
-				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+        glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
-			}
+        glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, screenWidth_, screenHeight_);
-		}
+        glBindTexture(GL_TEXTURE_2D, 0);
+        
+        if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
+        {
+            PointCloudDrawable drawable(occlusionMesh);
+            drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
+        }
 
 		// back to normal window-system-provided framebuffer
-		glBindFramebuffer(GL_FRAMEBUFFER, 0); // unbind
+		glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
 	}
 
 	if(doubleTapOn_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -502,10 +530,16 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glClearColor(0, 0, 0, 0);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+        // FIXME: we could use the depthTexture if already computed!
 		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
+            Eigen::Vector3f cloudToCamera(
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, 999.0f, 0, 0, 0, 0, 0, true);
+                      (*iter)->getPose().x() - openglCamera.x(),
+                      (*iter)->getPose().y() - openglCamera.y(),
+                      (*iter)->getPose().z() - openglCamera.z());
+            float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
+            
+            (*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_*10.0f, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
 		}
 
 		GLubyte zValue[4];
@@ -525,14 +559,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
     
-	if(renderBackgroundCamera)
+    if(renderBackgroundCamera && !onlineBlending)
-	{
+    {
-		background_renderer_->Draw(uvsTransformed);
+        background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
 
-		//To debug occlusion image:
+        //To debug occlusion image:
-		//PointCloudDrawable drawable(occlusionMesh);
+        //PointCloudDrawable drawable(occlusionMesh);
-		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+        //drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
-	}
+    }
 
 	if(!currentPose_->isNull())
 	{
@@ -595,11 +629,16 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 				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(), false, wireFrame_);
+		cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
 	}
 
 	if(onlineBlending)
 	{
+        if(renderBackgroundCamera)
+        {
+            background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
+        }
+        
 		glDisable (GL_BLEND);
 		glDepthMask(GL_TRUE);
 	}
@@ -680,18 +719,10 @@ void Scene::updateGraph(
 		const std::multimap<int, rtabmap::Link> & links)
 {
 	LOGI("updateGraph");
-	if(graph_)
-	{
-		delete graph_;
-		graph_ = 0;
-	}
-
 	//create
-	if(graphVisible_)
+    UASSERT(graph_shader_program_ != 0);
-	{
+    delete graph_;
-		UASSERT(graph_shader_program_ != 0);
+    graph_ = new GraphDrawable(graph_shader_program_, poses, links);
-		graph_ = new GraphDrawable(graph_shader_program_, poses, links);
-	}
 }
 
 void Scene::setGraphVisible(bool visible)

app/android/jni/scene.h

@@ -17,7 +17,9 @@
 #ifndef TANGO_POINT_CLOUD_SCENE_H_
 #define TANGO_POINT_CLOUD_SCENE_H_
 
+#ifdef __ANDROID__
 #include <jni.h>
+#endif
 #include <memory>
 #include <set>
 
@@ -35,10 +37,10 @@
 #include <rtabmap/core/Transform.h>
 #include <rtabmap/core/Link.h>
 
-#include <point_cloud_drawable.h>
+#include "point_cloud_drawable.h"
-#include <graph_drawable.h>
+#include "graph_drawable.h"
-#include <bounding_box_drawable.h>
+#include "bounding_box_drawable.h"
-#include <background_renderer.h>
+#include "background_renderer.h"
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -72,7 +74,7 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh(), bool mapping=false);
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
@@ -153,6 +155,7 @@ class Scene {
   float getPointSize() const {return pointSize_;}
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
+  bool isWireframe() const {return wireFrame_;}
 
   BackgroundRenderer * background_renderer_;
 
@@ -205,7 +208,7 @@ class Scene {
   float b_;
   GLuint fboId_;
   GLuint rboId_;
-  GLuint depthTexture_;
+  GLuint depthTextures_[2]; // 0=objects+occlusion 1=objects only
   GLsizei screenWidth_;
   GLsizei screenHeight_;
   bool doubleTapOn_;

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

@@ -24,8 +24,8 @@ Camera::Camera() {
   aspect_ratio_ = 4.0f / 3.0f;
   width_ = 800.0f;
   height_ = 600.0f;
-  near_clip_plane_ = 0.2f;
+  near_clip_plane_ = 0.5f;
-  far_clip_plane_ = 1000.0f;
+  far_clip_plane_ = 50.0f;
   ortho_ = false;
   orthoScale_ = 2.0f;
   orthoCropFactor_ = -1.0f;

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

@@ -43,9 +43,11 @@ const float kCamViewMaxDist = 100.f;
 
 // FOV set up values.
 // Third and top down camera's FOV is 65 degrees.
-// First person camera's FOV is 45 degrees.
+// First person camera's FOV is 85 degrees.
-const float kHighFov = 65.0f;
+const float kHighestFov = 120.0f;
+const float kHighFov = 85.0f;
 const float kLowFov = 65.0f;
+const float kLowestFov = 40.0f;
 }
 
 namespace tango_gl {
@@ -111,7 +113,7 @@ void GestureCamera::OnTouchEvent(int touch_count, TouchEvent event, float x0,
 
 			if(camera_type_ == kFirstPerson)
 			{
-				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, 45, 90));
+				this->SetFieldOfView(tango_gl::util::Clamp(cam_start_fov_ + dist * kZoomSpeed*10.0f, kLowestFov, kHighestFov));
 			}
 			else
 			{
@@ -179,8 +181,9 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
   camera_type_ = camera_index;
   switch (camera_index) {
     case kFirstPerson:
-    	SetOrthoMode(false);
+      SetOrthoMode(false);
-      SetFieldOfView(kLowFov);
+      SetFieldOfView(kLowestFov);
+      SetNearFarClipPlanes(0.1, 50);
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       cam_cur_dist_ = 0.0f;
@@ -193,11 +196,12 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       break;
     case kThirdPerson:
     case kThirdPersonFollow:
-    	SetOrthoMode(false);
+      SetOrthoMode(false);
-    	SetFieldOfView(kHighFov);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
       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;
+      cam_cur_dist_ = camera_index==kThirdPersonFollow?kThirdPersonFollowCameraDist:kThirdPersonCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = -M_PI / 12.0f;
       cam_cur_angle_.y = kThirdPersonFollow?0:M_PI / 2.0f;
@@ -208,7 +212,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
       SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
       SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
       SetOrthoMode(false);
-      SetFieldOfView(kHighFov);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
       cam_cur_dist_ = kTopDownCameraDist;
       anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
       cam_cur_angle_.x = -M_PI / 2.0f;
@@ -222,6 +227,8 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
 	  SetOrthoMode(true);
 	  SetOrthoScale(kTopDownCameraDist);
 	  SetOrthoCropFactor(-1.0f);
+      SetFieldOfView(kLowFov);
+      SetNearFarClipPlanes(1, 50);
 	  cam_cur_dist_ = kTopDownCameraDist;
 	  anchor_offset_ = glm::vec3(0.0f,0.0f,0.0f);
 	  cam_cur_angle_.x = -M_PI / 2.0f;

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

@@ -18,7 +18,10 @@
 #define TANGO_GL_TEXTURE_H_
 
 #include <errno.h>
+
+#ifdef __ANDROID__
 #include <png.h>
+#endif
 
 #include "tango-gl/util.h"
 
@@ -30,11 +33,15 @@ class Texture {
   Texture& operator=(const Texture&) = delete;
   ~Texture();
 
+#ifdef __ANDROID__
   bool LoadFromPNG(const char* file_path);
+#endif
   GLuint GetTextureID() const;
 
  private:
+#ifdef __ANDROID__
   png_uint_32 width_, height_;
+#endif
   int bit_depth_, color_type_;
   char* byte_data_;
   GLuint texture_id_;

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

@@ -21,10 +21,16 @@
 #define GL_VERTEX_PROGRAM_POINT_SIZE 0x8642
 
 #include <stdlib.h>
+#ifdef __ANDROID__
 #include <jni.h>
 #include <android/log.h>
 #include <GLES2/gl2.h>
 #include <GLES2/gl2ext.h>
+#else // __APPLE__
+#include <OpenGLES/ES2/gl.h>
+#include <OpenGLES/ES2/glext.h>
+#include <syslog.h>
+#endif
 
 #include "glm/glm.hpp"
 #include "glm/gtc/matrix_transform.hpp"
@@ -33,17 +39,28 @@
 #include "glm/gtx/matrix_decompose.hpp"
 
 #define LOG_TAG "rtabmap"
-#ifdef DISABLE_LOG
+#if defined(DISABLE_LOG)
 #define LOGD(...) ;
 #define LOGI(...) ;
 #define LOGW(...) ;
 #else
+#ifdef __APPLE__
+#define LOGD(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGI(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGW(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#else
 #define LOGD(...) __android_log_print(ANDROID_LOG_DEBUG,LOG_TAG,__VA_ARGS__)
 #define LOGI(...) __android_log_print(ANDROID_LOG_INFO,LOG_TAG,__VA_ARGS__)
 #define LOGW(...) __android_log_print(ANDROID_LOG_WARN,LOG_TAG,__VA_ARGS__)
 #endif
+#endif
+#ifdef __APPLE__
+#define LOGE(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#define LOGF(...) fprintf(stderr, __VA_ARGS__); fprintf(stderr, "\n")
+#else
 #define LOGE(...) __android_log_print(ANDROID_LOG_ERROR,LOG_TAG,__VA_ARGS__)
 #define LOGF(...) __android_log_print(ANDROID_LOG_FATAL,LOG_TAG,__VA_ARGS__)
+#endif
 
 #ifndef M_PI
 #define M_PI 3.1415926f

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

@@ -57,7 +57,7 @@ Quad::Quad() {
 
 Quad::~Quad() { glDeleteShader(shader_program_); }
 
-void Quat::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
+void Quad::SetTextureId(GLuint texture_id) { texture_id_ = texture_id; }
 
 void Quad::Render(const glm::mat4& projection_mat,
                   const glm::mat4& view_mat) const {

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

@@ -34,11 +34,13 @@ static int RoundUpPowerOfTwo(int w) {
 }
 
 Texture::Texture(const char* file_path) {
+#ifdef __ANDROID__
   if (!LoadFromPNG(file_path)) {
     LOGE("Texture initialing error");
   }
+#endif
 }
-
+#ifdef __ANDROID__
 bool Texture::LoadFromPNG(const char* file_path) {
   FILE* file = fopen(file_path, "rb");
 
@@ -94,7 +96,7 @@ bool Texture::LoadFromPNG(const char* file_path) {
 
   return true;
 }
-
+#endif
 GLuint Texture::GetTextureID() const { return texture_id_; }
 
 Texture::~Texture() {

app/android/jni/util.h

@@ -28,8 +28,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef UTIL_H_
 #define UTIL_H_
 
-
-#include <android/log.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/core/CameraModel.h>
@@ -46,8 +44,13 @@ class LogHandler : public UEventsHandler
 public:
 	LogHandler()
 	{
-		ULogger::setLevel(ULogger::kDebug);
+#ifdef DISABLE_LOG
-		ULogger::setEventLevel(ULogger::kDebug);
+		ULogger::setLevel(ULogger::kWarning);
+		ULogger::setEventLevel(ULogger::kWarning);
+#else
+        ULogger::setLevel(ULogger::kDebug);
+        ULogger::setEventLevel(ULogger::kDebug);
+#endif
 		ULogger::setPrintThreadId(true);
 
 		registerToEventsManager();
@@ -55,7 +58,7 @@ public:
 protected:
 	virtual bool handleEvent(UEvent * event)
 	{
-		if(event->getClassName().compare("ULogEvent") == 0)
+        if(event->getClassName().compare("ULogEvent") == 0)
 		{
 			ULogEvent * logEvent = (ULogEvent*)event;
 			if(logEvent->getCode() == ULogger::kDebug)
@@ -78,7 +81,6 @@ protected:
 			{
 				LOGF("%s", logEvent->getMsg().c_str());
 			}
-
 		}
 		return false;
 	}

app/android/res/layout/activity_settings.xml

@@ -97,11 +97,18 @@
 	            android:title="@string/pref_title_resolution"
 	            android:summary="@string/pref_summary_resolution"
 	            android:defaultValue="@string/pref_default_resolution"/>
-		     <com.introlab.rtabmap.CustomSwitchPreference
+		    <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"/>
+             <ListPreference
+		        android:key="@string/pref_key_arcore_localization_filtering_speed"
+		        android:title="@string/pref_title_arcore_localization_filtering_speed"
+		        android:summary="@string/pref_summary_arcore_localization_filtering_speed"
+		        android:entries="@array/pref_arcore_localization_filtering_speed_keys"
+		   	    android:entryValues="@array/pref_arcore_localization_filtering_speed_values"
+		   		android:defaultValue="@string/pref_default_arcore_localization_filtering_speed"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"

app/android/res/menu/optionmenu.xml

@@ -12,23 +12,6 @@
        <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:title="Export">
-          <menu>
-              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
-                  <menu>
-                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
-                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
-                  </menu>
-              </item>
-	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
-	              <menu>
-		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
-			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
-		          </menu>
-	          </item>
-          </menu>
-       </item>
-       
         <item android:id="@+id/post_processing" android:title="Optimize">
           <menu>
              <item android:id="@+id/post_processing_standard" android:title="Standard Optimization" />
@@ -46,6 +29,23 @@
           </menu>
        </item>
        
+       <item android:id="@+id/export" android:title="Assemble">
+          <menu>
+              <item android:id="@+id/export_point_cloud_menu" android:title="Point Cloud">
+                  <menu>
+                  	<item android:id="@+id/export_point_cloud" android:title="Current Density" />
+                  	<item android:id="@+id/export_point_cloud_highrez" android:title="Max Density" />
+                  </menu>
+              </item>
+	          <item android:id="@+id/export_optimized_mesh_menu" android:title="Optimized Mesh..." >
+	              <menu>
+		              <item android:id="@+id/export_optimized_mesh" android:title="Colored Mesh" />
+			          <item android:id="@+id/export_optimized_mesh_texture" android:title="Textured Mesh" />
+		          </menu>
+	          </item>
+          </menu>
+       </item>
+       
        <item android:id="@+id/menu_rendering_settings" android:title="Visibility...">
         <menu >
         <group android:id="@+id/group_rendering_visibility" android:checkableBehavior="all">

app/android/res/values/strings.xml

@@ -51,7 +51,7 @@
     <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>
+    <string name="pref_default_density">2</string>
     <string name="pref_key_min_depth">pref_key_min_depth</string>
     <string name="pref_default_min_depth">0</string>
     <string name="pref_key_depth">pref_key_depth</string>
@@ -65,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">false</string>
+    <string name="pref_default_blending">true</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>
@@ -75,7 +75,7 @@
     <string name="pref_key_resolution">pref_key_resolution</string>
     <string name="pref_default_resolution">false</string>
     <string name="pref_key_smoothing">pref_key_smoothing</string>
-    <string name="pref_default_smoothing">true</string>
+    <string name="pref_default_smoothing">false</string>
     <string name="pref_key_fisheye">pref_key_fisheye</string>
     <string name="pref_default_fisheye">false</string>
     
@@ -83,12 +83,14 @@
     <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_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
+    <string name="pref_default_arcore_localization_filtering_speed">1</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_default_time_thr">0</string>
     <string name="pref_key_mem_thr">pref_key_mem_thr</string>
     <string name="pref_default_mem_thr">0</string>
     <string name="pref_key_loop_thr">pref_key_loop_thr</string>
@@ -98,7 +100,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">3</string>
+    <string name="pref_default_opt_error">1</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>
@@ -106,7 +108,7 @@
     <string name="pref_key_features_type">pref_key_features_type</string>
     <string name="pref_default_features_type">6</string>
     <string name="pref_key_optimizer">pref_key_optimizer</string>
-    <string name="pref_default_optimizer">2</string>
+    <string name="pref_default_optimizer">1</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>
@@ -162,7 +164,7 @@
     <string name="pref_title_triangle">Mesh Triangle Size</string>
     <string name="pref_summary_triangle">Size in pixels of the polygons created from the depth image.</string>
     <string name="pref_title_rendering_texture_decimation">Texture Resolution</string>
-    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Export results.</string>
+    <string name="pref_summary_rendering_texture_decimation">Resolution of the texture for online rendering. This doesn\'t affect Assembling results.</string>
     <string name="pref_title_min_depth">Min Depth</string>
     <string name="pref_summary_min_depth">Points under the minimum depth are not rendered.</string>
     <string name="pref_title_depth">Max Depth</string>
@@ -324,9 +326,11 @@
     <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_summary_camera_driver">AR sdk used 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_arcore_localization_filtering_speed">ARCore Localization Filtering Speed</string>
+    <string name="pref_summary_arcore_localization_filtering_speed">Filter ARCore\'s localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set a speed threshold to detect those events.</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>
@@ -373,7 +377,7 @@
     <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_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">
@@ -406,6 +410,24 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_keys">
+        <item>"Disabled"</item>
+        <item>"5 m/s"</item>
+        <item>"4 m/s"</item>
+        <item>"3 m/s"</item>
+        <item>"2 m/s"</item>
+        <item>"1 m/s"</item>
+        <item>"0.5 m/s"</item>
+    </string-array>
+    <string-array name="pref_arcore_localization_filtering_speed_values">
+        <item>"0"</item>
+        <item>"5"</item>
+        <item>"4"</item>
+        <item>"3"</item>
+        <item>"2"</item>
+        <item>"1"</item>
+        <item>"0.5"</item>
+    </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>
         <item>"High"</item>
@@ -433,7 +455,7 @@
         <item>"700 ms"</item>
         <item>"600 ms"</item>
         <item>"500 ms"</item>
-        <item>"400"</item>
+        <item>"400 ms"</item>
     </string-array>
     <string-array name="pref_time_thr_values">
         <item>"0"</item>
@@ -529,29 +551,25 @@
         <item>"10"</item>
     </string-array>
     <string-array name="pref_opt_error_keys">
-        <item>"10x"</item>
-        <item>"9x"</item>
-        <item>"8x"</item>
-        <item>"7x"</item>
-        <item>"6x"</item>
-        <item>"5x"</item>
         <item>"4x"</item>
+        <item>"3.5x"</item>
         <item>"3x"</item>
+        <item>"2.5x"</item>
         <item>"2x"</item>
+        <item>"1.5x"</item>
         <item>"1x"</item>
+        <item>"0.5x"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_error_values">
-        <item>"10"</item>
-        <item>"9"</item>
-        <item>"8"</item>
-        <item>"7"</item>
-        <item>"6"</item>
-        <item>"5"</item>
         <item>"4"</item>
+        <item>"3.5"</item>
         <item>"3"</item>
+        <item>"2.5"</item>
         <item>"2"</item>
+        <item>"1.5"</item>
         <item>"1"</item>
+        <item>"0.5"</item>
         <item>"0"</item>
     </string-array>
     <string-array name="pref_features_voc_keys">
@@ -712,9 +730,9 @@
         <item>"1"</item>
     </string-array>
 
-    <string name="pref_title_export_sub">Exporting&#8230;</string>
+    <string name="pref_title_export_sub">Assembling&#8230;</string>
-    <string name="pref_title_export">Exporting</string>
+    <string name="pref_title_export">Assembling</string>
-    <string name="pref_summary_export">Advanced parameters used when exporting the map.</string>
+    <string name="pref_summary_export">Advanced parameters used when assembling the map.</string>
     <string name="pref_title_cloud_voxel">Voxel Size</string>
     <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>
@@ -727,8 +745,8 @@
     <string name="pref_summary_max_texture_distance">Maximum distance from a camera for polygons to be textured by this camera.</string>
     <string name="pref_title_min_texture_cluster_size">Min Texture Cluster Size</string>
     <string name="pref_summary_min_texture_cluster_size">Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</string>
-    <string name="pref_title_block_render">Block Rendering Thread While Exporting</string>
+    <string name="pref_title_block_render">Block Rendering Thread While Assembling</string>
-    <string name="pref_summary_block_render">This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors.</string>
+    <string name="pref_summary_block_render">This decreases assembling time, but freezes rendering while assembling. This also clears temporary the rendered clouds/meshes from memory during assembling, this can be useful to avoid out of memory errors.</string>
 
     <string-array name="pref_cloud_voxel_keys">
         <item>"0.2 m"</item>
@@ -834,7 +852,7 @@
     <string name="pref_title_opt_depth">Reconstruction Depth</string>
     <string name="pref_summary_opt_depth">Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). \"Auto\" means that depth is chosen so that polygon size is just under 3 cm.</string>
     <string name="pref_title_opt_color_radius">Color Radius</string>
-    <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_summary_opt_color_radius">Radius used to transfer nearest color from the point cloud to reconstructed mesh. When assembling 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>
@@ -868,10 +886,10 @@
         <item>"0.2 m"</item>
         <item>"0.1 m"</item>
         <item>"0.05 m"</item>
-        <item>"0.025"</item>
+        <item>"0.025 m"</item>
-        <item>"0.02"</item>
+        <item>"0.02 m"</item>
-        <item>"0.015"</item>
+        <item>"0.015 m"</item>
-        <item>"0.01"</item>
+        <item>"0.01 m"</item>
         <item>"Disabled"</item>
     </string-array>
     <string-array name="pref_opt_color_radius_values">

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

@@ -8,11 +8,14 @@ import java.util.EnumSet;
 import java.util.List;
 import java.util.concurrent.atomic.AtomicBoolean;
 
+import com.google.ar.core.Anchor;
 import com.google.ar.core.Camera;
+import com.google.ar.core.CameraConfig;
+import com.google.ar.core.CameraConfigFilter;
 import com.google.ar.core.CameraIntrinsics;
 import com.google.ar.core.Config;
+import com.google.ar.core.Coordinates2d;
 import com.google.ar.core.Frame;
-import com.google.ar.core.ImageMetadata;
 import com.google.ar.core.PointCloud;
 import com.google.ar.core.Pose;
 import com.google.ar.core.Session;
@@ -29,6 +32,7 @@ import android.hardware.camera2.CameraCaptureSession;
 import android.hardware.camera2.CameraCharacteristics;
 import android.hardware.camera2.CameraDevice;
 import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CameraMetadata;
 import android.hardware.camera2.CaptureFailure;
 import android.hardware.camera2.CaptureRequest;
 import android.hardware.camera2.TotalCaptureResult;
@@ -40,21 +44,27 @@ import android.os.HandlerThread;
 import android.support.annotation.NonNull;
 import android.util.Log;
 import android.view.Surface;
+import android.widget.Toast;
 
 public class ARCoreSharedCamera {
 
 	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
 
+	private static final float[] QUAD_COORDS =
+      new float[] {
+        -1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+      };
 	
 	private static RTABMapActivity mActivity;
-	public ARCoreSharedCamera(RTABMapActivity c) {
+	public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
 		mActivity = c;
+		mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
 	}
 
 	// Depth TOF Image.
 	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
-	private static final int DEPTH_WIDTH = 240;
+	private int depthWidth = 640;
-	private static final int DEPTH_HEIGHT = 180;
+	private int depthHeight = 480;
 
 	// GL Surface used to draw camera preview image.
 	public GLSurfaceView surfaceView;
@@ -62,6 +72,12 @@ public class ARCoreSharedCamera {
 	// ARCore session that supports camera sharing.
 	private Session sharedSession;
 	
+	private Pose previousAnchorPose = null;
+	private long previousAnchorTimeStamp;
+	private Pose arCoreCorrection = Pose.IDENTITY;
+	private Pose odomPose = Pose.IDENTITY;
+	private float mARCoreLocalizationFilteringSpeed = 1.0f;
+
 	// Camera capture session. Used by both non-AR and AR modes.
 	private CameraCaptureSession captureSession;
 
@@ -79,8 +95,15 @@ public class ARCoreSharedCamera {
 	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
 	private SharedCamera sharedCamera;
 	
+	private Toast mToast = null;
+
 	// Camera ID for the camera used by ARCore.
 	private String cameraId;
+	private String depthCameraId;
+	
+	private Pose rgbExtrinsics;
+	private Pose depthExtrinsics;
+	private float[] depthIntrinsics = null;
 	
 	private AtomicBoolean mReady = new AtomicBoolean(false);
 
@@ -95,6 +118,11 @@ public class ARCoreSharedCamera {
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
+	public void setToast(Toast toast)
+	{
+		mToast = toast;
+	}
+	
 	// Camera device state callback.
 	private final CameraDevice.StateCallback cameraDeviceCallback =
 			new CameraDevice.StateCallback() {
@@ -176,7 +204,7 @@ public class ARCoreSharedCamera {
 				@NonNull CameraCaptureSession session,
 				@NonNull CaptureRequest request,
 				@NonNull TotalCaptureResult result) {
-			Log.i(TAG, "onCaptureCompleted");
+			//Log.i(TAG, "onCaptureCompleted");
 		}
 
 		//@Override // android 23 
@@ -251,7 +279,7 @@ public class ARCoreSharedCamera {
 
 			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
 			// may arrive significantly later, or not arrive at all.
-			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
+			if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) surfaceList.add(mTOFImageReader.imageReader.getSurface());
 			// Surface list should now contain three surfacemReadymReadys:
 			// 0. sharedCamera.getSurfaceTexture()
 			// 1. …
@@ -272,8 +300,6 @@ public class ARCoreSharedCamera {
 		} catch (CameraAccessException e) {
 			Log.e(TAG, "CameraAccessException", e);
 		}
-
-
 	}
 
 	// Start background handler thread, used to run callbacks without blocking UI thread.
@@ -299,7 +325,24 @@ public class ARCoreSharedCamera {
 		mTOFImageReader.stopBackgroundThread();
 	}
 
-	private long mPreviousTime = 0;
+    /**
+     * Return true if the given array contains the given integer.
+     *
+     * @param modes array to check.
+     * @param mode  integer to get for.
+     * @return true if the array contains the given integer, otherwise false.
+     */
+    private static boolean contains(int[] modes, int mode) {
+        if (modes == null) {
+            return false;
+        }
+        for (int i : modes) {
+            if (i == mode) {
+                return true;
+            }
+        }
+        return false;
+    }
     
 	// Perform various checks, then open camera device and create CPU image reader.
 	public boolean openCamera() {
@@ -308,8 +351,6 @@ public class ARCoreSharedCamera {
 		
 		startBackgroundThread();
 
-		mPreviousTime = System.currentTimeMillis();
-
 		if(cameraTextureId == -1)
 		{
 			int[] textures = new int[1];
@@ -317,7 +358,7 @@ public class ARCoreSharedCamera {
 			cameraTextureId = textures[0];
 		}
 
-		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
+		Log.v(TAG, "Perform various checks, then open camera device and create CPU image reader.");
 		// Don't open camera if already opened.
 		if (cameraDevice != null) {
 			return false;
@@ -332,13 +373,39 @@ public class ARCoreSharedCamera {
 				return false;
 			}
 			
+			// First obtain the session handle before getting the list of various camera configs.
+			// Create filter here with desired fps filters.
+			CameraConfigFilter cameraConfigFilter = new CameraConfigFilter(sharedSession);
+			CameraConfig[] cameraConfigs = sharedSession.getSupportedCameraConfigs(cameraConfigFilter).toArray(new CameraConfig[0]);
+			Log.i(TAG, "Size of supported CameraConfigs list is " + cameraConfigs.length);
+
+			// Determine the highest and lowest CPU resolutions.
+			int highestResolutionIndex=-1;
+			int highestResolution = 0;
+			for(int i=0; i<cameraConfigs.length; ++i)
+			{
+				Log.i(TAG, "Camera ID: " + cameraConfigs[i].getCameraId());
+				Log.i(TAG, "Resolution: " + cameraConfigs[i].getImageSize().getWidth() + "x" + cameraConfigs[i].getImageSize().getHeight());
+				if(highestResolution == 0 || highestResolution < cameraConfigs[i].getImageSize().getWidth())
+				{
+					highestResolutionIndex = i;
+					highestResolution = cameraConfigs[i].getImageSize().getWidth();
+				}
+			}
+			if(highestResolutionIndex>=0)
+			{
+				//Log.i(TAG, "Setting camera resolution to " + cameraConfigs[highestResolutionIndex].getImageSize().getWidth() + "x" + cameraConfigs[highestResolutionIndex].getImageSize().getHeight());
+				//FIXME: Can we make it work to use HD rgb images? To avoid this error "CaptureRequest contains unconfigured Input/Output Surface!"
+				//sharedSession.setCameraConfig(cameraConfigs[highestResolutionIndex]);
+			}
+
 			// Enable auto focus mode while ARCore is running.
 			Config config = sharedSession.getConfig();
 			config.setFocusMode(Config.FocusMode.FIXED);
-			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
+			config.setUpdateMode(Config.UpdateMode.BLOCKING);
 			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
 			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
-			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
+			config.setCloudAnchorMode(Config.CloudAnchorMode.DISABLED);
 			sharedSession.configure(config);
 
 		}
@@ -347,27 +414,128 @@ public class ARCoreSharedCamera {
 		sharedCamera = sharedSession.getSharedCamera();
 		// Store the ID of the camera used by ARCore.
 		cameraId = sharedSession.getCameraConfig().getCameraId();
-		initCamera(mActivity, cameraId, 1);
+
-		ArrayList<String> resolutions;
+		Log.d(TAG, "Shared camera ID: " + cameraId);
 
 		mTOFAvailable = false;
 
-		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
+		// Store a reference to the camera system service.
-		if (resolutions != null) {
+		cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
-			for( String temp : resolutions) {
+		
-				Log.e(TAG + "DEPTH16 resolution: ", temp);
+		depthCameraId = null;
-			};
+		// show all cameras
-			if (resolutions.size()>0) mTOFAvailable = true;
+		try {
+            // Find a CameraDevice that supports DEPTH16 captures, and configure state.
+            for (String tmpCameraId : cameraManager.getCameraIdList()) {
+                CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(tmpCameraId);
+
+                Log.i(TAG, "Camera " + tmpCameraId + " extrinsics:");
+                
+            	float[] translation = characteristics.get(CameraCharacteristics.LENS_POSE_TRANSLATION);
+                if(translation != null)
+                {
+                	 Log.i(TAG, String.format("Translation (x,y,z): %f,%f,%f", translation[0], translation[1], translation[2]));
+                }
+                float[] rotation = characteristics.get(CameraCharacteristics.LENS_POSE_ROTATION);
+                if(rotation != null)
+                {
+                	 Log.i(TAG, String.format("Rotation (qx,qy,qz,qw): %f,%f,%f,%f",  rotation[0], rotation[1], rotation[2], rotation[3]));
+                }
+                
+                if(tmpCameraId.compareTo(cameraId)==0 && translation!=null && rotation!=null)
+                {
+                	rgbExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set rgb extrinsics!");
+                }
+                
+                if (!contains(characteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES_DEPTH_OUTPUT) ||
+                		characteristics.get(CameraCharacteristics.LENS_FACING) == CameraMetadata.LENS_FACING_FRONT) {
+                    continue;
+                }
+                Log.i(TAG, "Camera " + tmpCameraId + " has depth output available");
+                
+                depthCameraId = tmpCameraId;
+                if(translation!=null && rotation != null)
+                {
+                	depthExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set depth extrinsics!");
+                }
+                depthIntrinsics = characteristics.get(CameraCharacteristics.LENS_INTRINSIC_CALIBRATION);
+                Log.i(TAG, String.format("Intrinsics (fx,fy,cx,cy,s): %f,%f,%f,%f,%f", 
+                		depthIntrinsics[0],
+                		depthIntrinsics[1],
+                		depthIntrinsics[2],
+                		depthIntrinsics[3],
+                		depthIntrinsics[4]));
+            }
+        } catch (CameraAccessException e) {
+            e.printStackTrace();
+        }
+		if(rgbExtrinsics == null)
+		{
+			float[] translation = {0,0,0};
+			float[] rotation = {0,0,0,1};
+			rgbExtrinsics = new Pose(translation, rotation);
 		}
+		if(depthExtrinsics == null)
+		{
+			depthExtrinsics = rgbExtrinsics;
+		}
+
+		if(depthCameraId != null)
+		{
+			ArrayList<String> resolutions = getResolutions(mActivity, depthCameraId, ImageFormat.DEPTH16);
+			if (resolutions != null) {
+				float[] newDepthIntrinsics = null;
+				int largestWidth = 0;
+				for( String temp : resolutions) {
+					Log.i(TAG, "DEPTH16 resolution: " + temp);
+					depthWidth = Integer.parseInt(temp.split("x")[0]);
+					depthHeight = Integer.parseInt(temp.split("x")[1]);
+					if(depthIntrinsics != null)
+					{
+						if(depthIntrinsics[0] != 0)
+						{
+							if(largestWidth == 0 && depthWidth>largestWidth)
+							{
+								largestWidth = depthWidth; // intrinsics should match this resolution
+							}
+							// Samsung Galaxy Note10+: take smallest resolution and match the intrinsics
+							if(depthWidth < largestWidth)
+							{
+								float scale = (float)depthWidth/(float)largestWidth;
+								newDepthIntrinsics = depthIntrinsics.clone();
+								newDepthIntrinsics[0] *= scale;
+								newDepthIntrinsics[1] *= scale;
+								newDepthIntrinsics[2] *= scale;
+								newDepthIntrinsics[3] *= scale;
+							}
+						}
+						else if(depthWidth ==240 && depthHeight==180)
+						{
+							// Huawei P30 Pro: only 240x180 is working
+							break;
+						}
+					}
+				}
+				if (resolutions.size()>0) {
+					mTOFAvailable = true;
+					if(newDepthIntrinsics!=null) {
+						depthIntrinsics = newDepthIntrinsics;
+					}
+				}
+			}
+		}
+		Log.i(TAG, "TOF_available: " + mTOFAvailable);
 
 		// Color CPU Image.
 		// Use the currently configured CPU image size.
 		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
 
-		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
+		if (mTOFAvailable) mTOFImageReader.createImageReader(depthWidth, depthHeight);
 
 		// When ARCore is running, make sure it also updates our CPU image surface.
-		if (mTOFAvailable) {
+		if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) {
 			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
 		}
 
@@ -376,15 +544,14 @@ public class ARCoreSharedCamera {
 			// Wrap our callback in a shared camera callback.
 			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
 
-			// Store a reference to the camera system service.
-			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
-
-			// Get the characteristics for the ARCore camera.
-			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
-
 			// Open the camera device using the ARCore wrapped callback.
 			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
 			
+			if(mTOFAvailable && cameraId.compareTo(depthCameraId) != 0)
+			{
+				cameraManager.openCamera(depthCameraId, mTOFImageReader.cameraDeviceCallback, mTOFImageReader.backgroundHandler);
+			}
+			
 		} catch (CameraAccessException e) {
 			Log.e(TAG, "Failed to open camera", e);
 			return false;
@@ -396,16 +563,61 @@ public class ARCoreSharedCamera {
 			return false;
 		} 
 		
-		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
 		return true;
 	}
 	
+	public static void rotationMatrixToQuaternion(float[] R, float[] q) {
+        final float m00 = R[0];
+        final float m10 = R[1];
+        final float m20 = R[2];
+        final float m01 = R[4];
+        final float m11 = R[5];
+        final float m21 = R[6];
+        final float m02 = R[8];
+        final float m12 = R[9];
+        final float m22 = R[10];
+
+        float tr = m00 + m11 + m22;
+
+        if (tr > 0) {
+            float S = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw 
+            q[0] = 0.25f * S;/* w  w  w.j ava  2s.co m*/
+            q[1] = (m21 - m12) / S;
+            q[2] = (m02 - m20) / S;
+            q[3] = (m10 - m01) / S;
+        } else if ((m00 > m11) & (m00 > m22)) {
+            float S = (float) Math.sqrt(1.0 + m00 - m11 - m22) * 2; //
+            // S=4*q[1]
+            q[0] = (m21 - m12) / S;
+            q[1] = 0.25f * S;
+            q[2] = (m01 + m10) / S;
+            q[3] = (m02 + m20) / S;
+        } else if (m11 > m22) {
+            float S = (float) Math.sqrt(1.0 + m11 - m00 - m22) * 2; //
+            // S=4*q[2]
+            q[0] = (m02 - m20) / S;
+            q[1] = (m01 + m10) / S;
+            q[2] = 0.25f * S;
+            q[3] = (m12 + m21) / S;
+        } else {
+            float S = (float) Math.sqrt(1.0 + m22 - m00 - m11) * 2; //
+            // S=4*q[3]
+            q[0] = (m10 - m01) / S;
+            q[1] = (m02 + m20) / S;
+            q[2] = (m12 + m21) / S;
+            q[3] = 0.25f * S;
+        }
+
+    }
+
 
 	// Close the camera device.
 	public void close() {
+		Log.w(TAG, "close()");
 		
 		if (sharedSession != null)  {
-			sharedSession.pause();
+			sharedSession.close();
+			sharedSession = null;
 		}
 
 		if (captureSession != null) {
@@ -414,12 +626,10 @@ public class ARCoreSharedCamera {
 		}
 		if (cameraDevice != null) {
 			cameraDevice.close();
+			cameraDevice = null;
 		}
 		
-		if (mTOFImageReader.imageReader != null) {
+		mTOFImageReader.close();
-			mTOFImageReader.imageReader.close();
-			mTOFImageReader.imageReader = null;
-		}
 
 		if(cameraTextureId>=0)
 		{
@@ -429,12 +639,17 @@ public class ARCoreSharedCamera {
 		stopBackgroundThread();
 	}
 
-	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
+	public void setDisplayGeometry(int rotation, int width, int height)
+	{
+		if(sharedSession!=null)
+			sharedSession.setDisplayGeometry(rotation, width, height);
+	}
 	
+	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */	
 	// Draw frame when in AR mode. Called on the GL thread.
 	public void updateGL() throws CameraNotAvailableException {
 
-		if(!mReady.get())
+		if(!mReady.get() || sharedSession == null)
 		{
 			return;
 		}
@@ -455,29 +670,98 @@ public class ARCoreSharedCamera {
 			camera = frame.getCamera();
 		}else
 		{
+			Log.e(TAG, String.format("frame.getCamera() null!"));
 			return;
 		}
 
-		if (camera == null) return;
+		if (camera == null) { 
-		// If not tracking, don't draw 3D objects.
+			Log.e(TAG, String.format("camera is null!"));
-		if (camera.getTrackingState() == TrackingState.PAUSED) return;
+			return;
+		}
+		if (camera.getTrackingState() != TrackingState.TRACKING) {
+			final String trackingState = camera.getTrackingState().toString();
+			Log.e(TAG, String.format("Tracking lost! state=%s", trackingState));
+			// This will force a new session on the next frame received
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0);
+			mActivity.runOnUiThread(new Runnable() {
+				public void run() {
+					if(mToast!=null && previousAnchorPose != null)
+					{
+						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
+						if(!mToast.getView().isShown())
+						{
+							mToast.makeText(mActivity.getApplicationContext(), 
+									msg, Toast.LENGTH_LONG).show();
+						}
+						else
+						{
+							mToast.setText(msg);
+						}
+						previousAnchorPose = null;
+					}
+				}
+			});
+			return;
+		}
 
 		if (frame.getTimestamp() != 0) {
-			
 			Pose pose = camera.getPose();
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
 			
-			int rateMs = 100; // send images at most 10 Hz
+			// Remove ARCore SLAM corrections by integrating pose from previous frame anchor
-			if(System. currentTimeMillis() - mPreviousTime < rateMs)
+			if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0)
 			{
-				return;
+				odomPose = pose;
 			}
-			mPreviousTime = System. currentTimeMillis();
+			else
+			{
+				float[] t = previousAnchorPose.inverse().compose(pose).getTranslation();
+				final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
+				if(speed>=mARCoreLocalizationFilteringSpeed)
+				{
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
+					t = arCoreCorrection.getTranslation();
+					Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
+					
+					t = odomPose.getTranslation();
+					float[] t2 = (arCoreCorrection.compose(pose)).getTranslation();
+					float[] t3 = previousAnchorPose.getTranslation();
+					float[] t4 = pose.getTranslation();
+					Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2]));
+				
+					mActivity.runOnUiThread(new Runnable() {
+						public void run() {
+							if(mToast!=null)
+							{
+								String msg = String.format("ARCore localization has been suppressed "
+										+ "because of high speed detected (%f m/s) causing a jump! You can change "
+										+ "ARCore localization filtering speed in Settings->Mapping if you are "
+										+ "indeed moving as fast.", speed);
+								if(!mToast.getView().isShown())
+								{
+									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
+								}
+								else
+								{
+									mToast.setText(msg);
+								}
+								previousAnchorPose = null;
+							}
+						}
+					});
+				}
+				odomPose = arCoreCorrection.compose(pose);
+			}
+			previousAnchorPose = pose;
+			previousAnchorTimeStamp = frame.getTimestamp();
+			
+			double stamp = (double)frame.getTimestamp()/10e8;
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
 			
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("frame=%d vs image=%d", frame.getTimestamp(), image.getTimestamp()));
 				PointCloud cloud = frame.acquirePointCloud();
 				FloatBuffer points = cloud.getPoints();
 
@@ -502,33 +786,66 @@ public class ARCoreSharedCamera {
 				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
 				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
 
-				double stamp = (double)image.getTimestamp()/10e8;
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d stamp=%f", 
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
 						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
 				
+				float[] texCoord = new float[8];
+				frame.transformCoordinates2d(
+				        Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				        QUAD_COORDS,
+				        Coordinates2d.IMAGE_NORMALIZED,
+				        texCoord);
+				
+				float[] p = new float[16];
+		        camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
+   
+		        float[] viewMatrix = new float[16];
+		        arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
+		        float[] quat = new float[4];
+		        rotationMatrixToQuaternion(viewMatrix, quat);
+
+		        
 				if(mTOFAvailable)
 				{
-					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+					ByteBuffer depth;
-							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
+					double depthStamp;
+					synchronized (mTOFImageReader) {
+						depth = mTOFImageReader.depth16_raw;
+						depthStamp = (double)mTOFImageReader.timestamp/10e8;
+					}
 					
-					RTABMapLib.postOdometryEvent(
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
+
+					RTABMapLib.postOdometryEventDepth(
 							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
+							fl[0], fl[1], pp[0], pp[1], 
+							depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
+							stamp,
+							depthStamp,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
-							points, points.limit()/4);
+							points, points.limit()/4,
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
 				}
 				else
 				{
-					ByteBuffer bb = ByteBuffer.allocate(0);
 					RTABMapLib.postOdometryEvent(
 							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
+							fl[0], fl[1], pp[0], pp[1], 
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							stamp, 
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							bb, 0, 0, 0, ImageFormat.DEPTH16,
+							points, points.limit()/4,
-							points, points.limit()/4);
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
 				}
 				
 				image.close();
@@ -545,8 +862,8 @@ public class ARCoreSharedCamera {
 	/********************************************************************************************************************* */
 
 
-	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
+	public ArrayList<String> getResolutions (Context context, String cameraId, int imageFormat){
-		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
+		Log.v(TAG, "getResolutions: cameraId:" + cameraId + " imageFormat: " + imageFormat);
 
 		ArrayList<String> output = new ArrayList<String>();
 		try {
@@ -562,27 +879,4 @@ public class ARCoreSharedCamera {
 		return output;
 	}
 
-
-	public void initCamera (Context context, String cameraId,int index){
-		boolean ok = false;
-		try {
-			int current = 0;
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
-				ok = true;
-				if (current == index)
-					break;
-				else ;
-				current++;
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		if (!ok) {
-			Log.e(TAG + " initCamera", "Depth sensor not found!");
-		}
-	}
-
-
 }

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

@@ -0,0 +1,166 @@
+package com.introlab.rtabmap;
+
+/*
+ * Copyright 2017 Google LLC
+ *
+ * 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.
+ */
+
+import android.app.Activity;
+import android.content.Context;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraManager;
+import android.hardware.display.DisplayManager;
+import android.hardware.display.DisplayManager.DisplayListener;
+import android.view.Display;
+import android.view.Surface;
+import android.view.WindowManager;
+import com.google.ar.core.Session;
+
+/**
+ * Helper to track the display rotations. In particular, the 180 degree rotations are not notified
+ * by the onSurfaceChanged() callback, and thus they require listening to the android display
+ * events.
+ */
+public final class DisplayRotationHelper implements DisplayListener {
+  private boolean viewportChanged;
+  private int viewportWidth;
+  private int viewportHeight;
+  private final Display display;
+  private final DisplayManager displayManager;
+  private final CameraManager cameraManager;
+
+  /**
+   * Constructs the DisplayRotationHelper but does not register the listener yet.
+   *
+   * @param context the Android {@link Context}.
+   */
+  public DisplayRotationHelper(Context context) {
+    displayManager = (DisplayManager) context.getSystemService(Context.DISPLAY_SERVICE);
+    cameraManager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+    WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE);
+    display = windowManager.getDefaultDisplay();
+  }
+
+  /** Registers the display listener. Should be called from {@link Activity#onResume()}. */
+  public void onResume() {
+    displayManager.registerDisplayListener(this, null);
+  }
+
+  /** Unregisters the display listener. Should be called from {@link Activity#onPause()}. */
+  public void onPause() {
+    displayManager.unregisterDisplayListener(this);
+  }
+
+  /**
+   * Records a change in surface dimensions. This will be later used by {@link
+   * #updateSessionIfNeeded(Session)}. Should be called from {@link
+   * android.opengl.GLSurfaceView.Renderer
+   * #onSurfaceChanged(javax.microedition.khronos.opengles.GL10, int, int)}.
+   *
+   * @param width the updated width of the surface.
+   * @param height the updated height of the surface.
+   */
+  public void onSurfaceChanged(int width, int height) {
+    viewportWidth = width;
+    viewportHeight = height;
+    viewportChanged = true;
+  }
+
+  /**
+   * Updates the session display geometry if a change was posted either by {@link
+   * #onSurfaceChanged(int, int)} call or by {@link #onDisplayChanged(int)} system callback. This
+   * function should be called explicitly before each call to {@link Session#update()}. This
+   * function will also clear the 'pending update' (viewportChanged) flag.
+   *
+   * @param session the {@link Session} object to update if display geometry changed.
+   */
+  public void updateSessionIfNeeded(ARCoreSharedCamera session) {
+    if (viewportChanged) {
+      int displayRotation = display.getRotation();
+      session.setDisplayGeometry(displayRotation, viewportWidth, viewportHeight);
+      viewportChanged = false;
+    }
+  }
+
+  /**
+   *  Returns the aspect ratio of the GL surface viewport while accounting for the display rotation
+   *  relative to the device camera sensor orientation.
+   */
+  public float getCameraSensorRelativeViewportAspectRatio(String cameraId) {
+    float aspectRatio;
+    int cameraSensorToDisplayRotation = getCameraSensorToDisplayRotation(cameraId);
+    switch (cameraSensorToDisplayRotation) {
+      case 90:
+      case 270:
+        aspectRatio = (float) viewportHeight / (float) viewportWidth;
+        break;
+      case 0:
+      case 180:
+        aspectRatio = (float) viewportWidth / (float) viewportHeight;
+        break;
+      default:
+        throw new RuntimeException("Unhandled rotation: " + cameraSensorToDisplayRotation);
+    }
+    return aspectRatio;
+  }
+
+  /**
+   * Returns the rotation of the back-facing camera with respect to the display. The value is one of
+   * 0, 90, 180, 270.
+   */
+  public int getCameraSensorToDisplayRotation(String cameraId) {
+    CameraCharacteristics characteristics;
+    try {
+      characteristics = cameraManager.getCameraCharacteristics(cameraId);
+    } catch (CameraAccessException e) {
+      throw new RuntimeException("Unable to determine display orientation", e);
+    }
+
+    // Camera sensor orientation.
+    int sensorOrientation = characteristics.get(CameraCharacteristics.SENSOR_ORIENTATION);
+
+    // Current display orientation.
+    int displayOrientation = toDegrees(display.getRotation());
+
+    // Make sure we return 0, 90, 180, or 270 degrees.
+    return (sensorOrientation - displayOrientation + 360) % 360;
+  }
+
+  private int toDegrees(int rotation) {
+    switch (rotation) {
+      case Surface.ROTATION_0:
+        return 0;
+      case Surface.ROTATION_90:
+        return 90;
+      case Surface.ROTATION_180:
+        return 180;
+      case Surface.ROTATION_270:
+        return 270;
+      default:
+        throw new RuntimeException("Unknown rotation " + rotation);
+    }
+  }
+
+  @Override
+  public void onDisplayAdded(int displayId) {}
+
+  @Override
+  public void onDisplayRemoved(int displayId) {}
+
+  @Override
+  public void onDisplayChanged(int displayId) {
+    viewportChanged = true;
+  }
+}

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

@@ -104,7 +104,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	// Tag for debug logging.
 	public static final String TAG = RTABMapActivity.class.getSimpleName();
-	public static boolean DISABLE_LOG = true;
+	public static boolean DISABLE_LOG = false;
 
 	// The minimum Tango Core version required from this application.
 	private static final int  MIN_TANGO_CORE_VERSION = 9377;
@@ -1075,6 +1075,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		setAndroidOrientation();
 
+		updateCameraDriverSettings();
 		updatePreferences();
 
 		if(mState == State.STATE_MAPPING || mState == State.STATE_CAMERA)
@@ -1235,7 +1236,10 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							if(cameraStartSucess && mCameraDriver == 3)
 							{
 								synchronized (this) {
-									mArCoreCamera = new ARCoreSharedCamera(getActivity());
+									SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
+									String arCoreLocalizationFiltering = sharedPref.getString(getString(R.string.pref_key_arcore_localization_filtering_speed), getString(R.string.pref_default_arcore_localization_filtering_speed));
+									mArCoreCamera = new ARCoreSharedCamera(getActivity(), Float.parseFloat(arCoreLocalizationFiltering));
+									mArCoreCamera.setToast(mToast);
 									mProgressDialog.setTitle("");
 									mProgressDialog.setMessage(message);
 									mProgressDialog.show();

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

@@ -134,13 +134,30 @@ public class RTABMapLib
     public static native float getUpdateTime(long nativeApplication);
     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 postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
     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,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
     		double stamp, 
     		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		FloatBuffer points, int pointsLen,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
+    public static native void postOdometryEventDepth(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+    		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+    		double rgbStamp,
+    		double depthStamp,
+    		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);
+    		FloatBuffer points, int pointsLen,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
     
 }

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

@@ -44,12 +44,14 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
 	private ARCoreSharedCamera mCamera = null;
+	private DisplayRotationHelper mDisplayRotationHelper = null;
 	
 	private Vector<TextObject> mTexts;
 
 	private static RTABMapActivity mActivity;
 	public Renderer(RTABMapActivity c) {
 		mActivity = c;
+		mDisplayRotationHelper = new DisplayRotationHelper(/*context=*/ c);
 	}
 
 	private ProgressDialog mProgressDialog = null;
@@ -76,6 +78,11 @@ public class Renderer implements GLSurfaceView.Renderer {
 	public void setCamera(ARCoreSharedCamera camera)
 	{
 		mCamera = camera;
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.onDisplayChanged(0);
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
 	}
 
 	// Render loop of the Gl context.
@@ -84,17 +91,23 @@ public class Renderer implements GLSurfaceView.Renderer {
 		synchronized (this) {
 			if(mActivity.nativeApplication != 0)
 			{
+				int step = 0;
 				try
 				{
 					if(mCamera!=null)
 					{
+						step=1;
 						mCamera.updateGL();
+						
 					}
 					
+					step=2;
+					
 					final int value = RTABMapLib.render(mActivity.nativeApplication);
 		
 					if(mTextManager!=null)
 					{
+						step=3;
 						if(mTextChanged)
 						{
 							mTextChanged = false;
@@ -118,7 +131,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 						Matrix.translateM(mvp, 0, mtrxProjectionAndView, 0, 0, mOffset, 0);
 						mTextManager.Draw(mvp);
 					}
-		
+					step=4;
 					if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing())
 					{
 						mActivity.runOnUiThread(new Runnable() {
@@ -135,7 +148,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 							public void run() {
 								if(mToast!=null)
 								{
-									mToast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
+									mToast.makeText(mActivity.getApplicationContext(), String.format("Out of Memory!"), Toast.LENGTH_SHORT).show();
 								}
 							}
 						});
@@ -146,7 +159,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 							public void run() {
 								if(mToast!=null)
 								{
-									mToast.makeText(mActivity, String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
+									mToast.makeText(mActivity.getApplicationContext(), String.format("Rendering Error!"), Toast.LENGTH_SHORT).show();
 								}
 							}
 						});
@@ -154,11 +167,14 @@ public class Renderer implements GLSurfaceView.Renderer {
 				}
 				catch(final Exception e)
 				{
+					final int stepF = step;
 					mActivity.runOnUiThread(new Runnable() {
 						public void run() {
 							if(mToast!=null)
 							{
-								mToast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_SHORT).show();
+								String msg = String.format("Rendering error! (exception=%s) step=%d", e.getMessage(), stepF);
+								Log.e("RTABMapActivity", msg);
+								mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
 							}
 						}
 		
@@ -176,6 +192,12 @@ public class Renderer implements GLSurfaceView.Renderer {
 			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
 		}
 		
+		mDisplayRotationHelper.onSurfaceChanged(width, height);
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
+		
 		mSurfaceHeight = (float)height;
 
 		// Clear our matrices

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

@@ -222,6 +222,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((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_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
         ((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));
@@ -284,6 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	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_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
         	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));

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

@@ -2,18 +2,33 @@ package com.introlab.rtabmap;
 
 
 import android.graphics.ImageFormat;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+import android.hardware.camera2.CameraDevice;
+import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CaptureFailure;
+import android.hardware.camera2.CaptureRequest;
+import android.hardware.camera2.TotalCaptureResult;
 import android.media.Image;
 import android.media.ImageReader;
 import android.os.Handler;
 import android.os.HandlerThread;
+import android.support.annotation.NonNull;
 import android.util.Log;
+import android.view.Surface;
 
 import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
 
 public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
 
-    public int WIDTH;
+	public static final String TAG = TOF_ImageReader.class.getSimpleName();
-    public int HEIGHT;
+	
+	public int WIDTH;
+	public int HEIGHT;
+	
     public ImageReader imageReader;
     public int frameCount = 0;
     public long timestamp;
@@ -21,16 +36,177 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
     // Looper handler thread.
     private HandlerThread backgroundThread;
     // Looper handler.
-    private Handler backgroundHandler;
+    public Handler backgroundHandler;
 
     public ByteBuffer depth16_raw;
     
+    // Camera capture session. 
+ 	private CameraCaptureSession captureSession = null;
+ 	// Camera device. 
+ 	private CameraDevice cameraDevice = null;
+ 	
+ // Camera preview capture request builder
+ 	private CaptureRequest.Builder previewCaptureRequestBuilder;
+
+
     TOF_ImageReader(){
     }
     
+    public void close()
+    {
+    	Log.i(TAG, "close()");
+    	if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+			cameraDevice = null;
+		}
+		
+		if (imageReader != null) {
+			imageReader.close();
+			imageReader = null;
+		}
+    }
+    
+ // Camera device state callback.
+ 	public final CameraDevice.StateCallback cameraDeviceCallback =
+ 			new CameraDevice.StateCallback() {
+ 		@Override
+ 		public void onOpened(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera depth ID " + cameraDevice.getId() + " opened.");
+ 			TOF_ImageReader.this.cameraDevice = cameraDevice;
+ 			createCameraPreviewSession();
+ 		}
+
+ 		@Override
+ 		public void onClosed(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
+ 			Log.w(TAG, "Camera depth ID " + cameraDevice.getId() + " disconnected.");
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onError(@NonNull CameraDevice cameraDevice, int error) {
+ 			Log.e(TAG, "Camera depth ID " + cameraDevice.getId() + " error " + error);
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+ 	};
+ 	
+ 	private CameraCaptureSession.StateCallback captureStateCallback = new CameraCaptureSession.StateCallback() {
+ 		// Called when the camera capture session is first configured after the app
+		// is initialized, and again each time the activity is resumed.
+		@Override
+		public void onConfigured(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session configured.");
+			captureSession = session;
+			setRepeatingCaptureRequest();
+		}
+
+		@Override
+		public void onSurfacePrepared(
+				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
+			Log.d(TAG, "Camera capture surface prepared.");
+		}
+
+		@Override
+		public void onReady(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session ready.");
+		}
+
+		@Override
+		public void onActive(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session active.");
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session closed.");
+		}
+
+		@Override
+		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
+			Log.e(TAG, "Failed to configure camera capture session.");
+		}
+ 	};
+ 	
+ // Repeating camera capture session capture callback.
+ 	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
+ 			new CameraCaptureSession.CaptureCallback() {
+
+ 		@Override
+ 		public void onCaptureCompleted(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull TotalCaptureResult result) {
+ 			//Log.i(TAG, "onCaptureCompleted");
+ 		}
+
+ 		//@Override // android 23 
+ 		public void onCaptureBufferLost(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull Surface target,
+ 				long frameNumber) {
+ 			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
+ 		}
+
+ 		@Override
+ 		public void onCaptureFailed(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull CaptureFailure failure) {
+ 			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
+ 		}
+
+ 		@Override
+ 		public void onCaptureSequenceAborted(
+ 				@NonNull CameraCaptureSession session, int sequenceId) {
+ 			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
+ 		}
+ 	};
+ 	
+ 	// Called when starting non-AR mode or switching to non-AR mode.
+ 	// Also called when app starts in AR mode, or resumes in AR mode.
+ 	private void setRepeatingCaptureRequest() {
+ 		try {
+ 			captureSession.setRepeatingRequest(
+ 					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
+ 		} catch (CameraAccessException e) {
+ 			Log.e(TAG, "Failed to set repeating request", e);
+ 		}
+ 	}
+ 	
+ 	private void createCameraPreviewSession() {
+		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
+		frameCount = 0;
+		try {
+			
+			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
+			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
+
+			previewCaptureRequestBuilder.addTarget(imageReader.getSurface());
+
+			// Create camera capture session for camera preview using callback.
+			cameraDevice.createCaptureSession(Arrays.asList(imageReader.getSurface()), captureStateCallback, backgroundHandler);
+
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "CameraAccessException", e);
+		}
+	}
+
     public void createImageReader(int width, int height){
-        this.WIDTH = width;
+    	Log.w(TAG, String.valueOf(width) + "x" + String.valueOf(height));
-        this.HEIGHT = height;
+    	WIDTH = width;
+    	HEIGHT = height;
         this.imageReader =
                 ImageReader.newInstance(
                         width,
@@ -45,18 +221,20 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
     public void onImageAvailable(ImageReader imageReader) {
         Image image  = imageReader.acquireLatestImage();
         if (image == null) {
-            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
+            Log.w(TAG, "onImageAvailable: Skipping null image.");
             return;
         }
         else{
             if(image.getFormat() == ImageFormat.DEPTH16){
-                this.timestamp = image.getTimestamp();
+                synchronized (this) {
-                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                	 depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                	 timestamp = image.getTimestamp();
+				}
                 // copy raw undecoded DEPTH16 format depth data to NativeBuffer
                 frameCount++;
             }
             else{
-                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
+                Log.w(TAG, "onImageAvailable: depth image not in DEPTH16 format, skipping image");
             }
         }
         image.close();
@@ -78,7 +256,7 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
                 this.backgroundThread = null;
                 this.backgroundHandler = null;
             } catch (InterruptedException e) {
-                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
+                Log.e(TAG, "Interrupted while trying to join depth background handler thread", e);
             }
         }
     }

app/ios/.gitignore

@@ -0,0 +1,68 @@
+# Xcode
+#
+# gitignore contributors: remember to update Global/Xcode.gitignore, Objective-C.gitignore & Swift.gitignore
+
+## User settings
+xcuserdata/
+
+## compatibility with Xcode 8 and earlier (ignoring not required starting Xcode 9)
+*.xcscmblueprint
+*.xccheckout
+
+## compatibility with Xcode 3 and earlier (ignoring not required starting Xcode 4)
+build/
+DerivedData/
+*.moved-aside
+*.pbxuser
+!default.pbxuser
+*.mode1v3
+!default.mode1v3
+*.mode2v3
+!default.mode2v3
+*.perspectivev3
+!default.perspectivev3
+
+## Obj-C/Swift specific
+*.hmap
+
+## App packaging
+*.ipa
+*.dSYM.zip
+*.dSYM
+
+# CocoaPods
+#
+# We recommend against adding the Pods directory to your .gitignore. However
+# you should judge for yourself, the pros and cons are mentioned at:
+# https://guides.cocoapods.org/using/using-cocoapods.html#should-i-check-the-pods-directory-into-source-control
+#
+# Pods/
+#
+# Add this line if you want to avoid checking in source code from the Xcode workspace
+# *.xcworkspace
+
+# Carthage
+#
+# Add this line if you want to avoid checking in source code from Carthage dependencies.
+# Carthage/Checkouts
+
+Carthage/Build/
+
+# fastlane
+#
+# It is recommended to not store the screenshots in the git repo.
+# Instead, use fastlane to re-generate the screenshots whenever they are needed.
+# For more information about the recommended setup visit:
+# https://docs.fastlane.tools/best-practices/source-control/#source-control
+
+fastlane/report.xml
+fastlane/Preview.html
+fastlane/screenshots/**/*.png
+fastlane/test_output
+
+# Code Injection
+#
+# After new code Injection tools there's a generated folder /iOSInjectionProject
+# https://github.com/johnno1962/injectionforxcode
+
+iOSInjectionProject/

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/contents.xcworkspacedata

@@ -0,0 +1,7 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Workspace
+   version = "1.0">
+   <FileRef
+      location = "self:">
+   </FileRef>
+</Workspace>

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/xcshareddata/IDEWorkspaceChecks.plist

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>IDEDidComputeMac32BitWarning</key>
+	<true/>
+</dict>
+</plist>

app/ios/RTABMapApp.xcodeproj/project.xcworkspace/xcshareddata/swiftpm/Package.resolved

@@ -0,0 +1,16 @@
+{
+  "object": {
+    "pins": [
+      {
+        "package": "Zip",
+        "repositoryURL": "https://github.com/marmelroy/Zip.git",
+        "state": {
+          "branch": null,
+          "revision": "bd19d974e8a38cc8d3a88c90c8a107386c3b8ccf",
+          "version": "2.1.1"
+        }
+      }
+    ]
+  },
+  "version": 1
+}

app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme

@@ -0,0 +1,78 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<Scheme
+   LastUpgradeVersion = "1230"
+   version = "1.3">
+   <BuildAction
+      parallelizeBuildables = "YES"
+      buildImplicitDependencies = "YES">
+      <BuildActionEntries>
+         <BuildActionEntry
+            buildForTesting = "YES"
+            buildForRunning = "YES"
+            buildForProfiling = "YES"
+            buildForArchiving = "YES"
+            buildForAnalyzing = "YES">
+            <BuildableReference
+               BuildableIdentifier = "primary"
+               BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+               BuildableName = "RTABMapApp.app"
+               BlueprintName = "RTABMapApp"
+               ReferencedContainer = "container:RTABMapApp.xcodeproj">
+            </BuildableReference>
+         </BuildActionEntry>
+      </BuildActionEntries>
+   </BuildAction>
+   <TestAction
+      buildConfiguration = "Debug"
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      shouldUseLaunchSchemeArgsEnv = "YES">
+      <Testables>
+      </Testables>
+   </TestAction>
+   <LaunchAction
+      buildConfiguration = "Release"
+      selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
+      selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
+      launchStyle = "0"
+      useCustomWorkingDirectory = "NO"
+      ignoresPersistentStateOnLaunch = "NO"
+      debugDocumentVersioning = "YES"
+      debugServiceExtension = "internal"
+      allowLocationSimulation = "YES">
+      <BuildableProductRunnable
+         runnableDebuggingMode = "0">
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+            BuildableName = "RTABMapApp.app"
+            BlueprintName = "RTABMapApp"
+            ReferencedContainer = "container:RTABMapApp.xcodeproj">
+         </BuildableReference>
+      </BuildableProductRunnable>
+   </LaunchAction>
+   <ProfileAction
+      buildConfiguration = "Release"
+      shouldUseLaunchSchemeArgsEnv = "YES"
+      savedToolIdentifier = ""
+      useCustomWorkingDirectory = "NO"
+      debugDocumentVersioning = "YES">
+      <BuildableProductRunnable
+         runnableDebuggingMode = "0">
+         <BuildableReference
+            BuildableIdentifier = "primary"
+            BlueprintIdentifier = "4EE015C1259A2AF0008CCE65"
+            BuildableName = "RTABMapApp.app"
+            BlueprintName = "RTABMapApp"
+            ReferencedContainer = "container:RTABMapApp.xcodeproj">
+         </BuildableReference>
+      </BuildableProductRunnable>
+   </ProfileAction>
+   <AnalyzeAction
+      buildConfiguration = "Debug">
+   </AnalyzeAction>
+   <ArchiveAction
+      buildConfiguration = "Release"
+      revealArchiveInOrganizer = "YES">
+   </ArchiveAction>
+</Scheme>

app/ios/RTABMapApp/AppDelegate.swift

@@ -0,0 +1,76 @@
+//
+//  AppDelegate.swift
+//  GLKittutorial
+//
+//  Created by Mathieu Labbe on 2020-12-28.
+//
+
+import UIKit
+import ARKit
+
+func setDefaultsFromSettingsBundle() {
+    
+    let plistFiles = ["Root", "Mapping", "Assembling"]
+    
+    for plistName in plistFiles {
+        //Read PreferenceSpecifiers from Root.plist in Settings.Bundle
+        if let settingsURL = Bundle.main.url(forResource: plistName, withExtension: "plist", subdirectory: "Settings.bundle"),
+            let settingsPlist = NSDictionary(contentsOf: settingsURL),
+            let preferences = settingsPlist["PreferenceSpecifiers"] as? [NSDictionary] {
+
+            for prefSpecification in preferences {
+
+                if let key = prefSpecification["Key"] as? String, let value = prefSpecification["DefaultValue"] {
+
+                    //If key doesn't exists in userDefaults then register it, else keep original value
+                    if UserDefaults.standard.value(forKey: key) == nil {
+
+                        UserDefaults.standard.set(value, forKey: key)
+                        NSLog("registerDefaultsFromSettingsBundle: Set following to UserDefaults - (key: \(key), value: \(value), type: \(type(of: value)))")
+                    }
+                }
+            }
+        } else {
+            NSLog("registerDefaultsFromSettingsBundle: Could not find Settings.bundle")
+        }
+    }
+}
+
+@main
+class AppDelegate: UIResponder, UIApplicationDelegate {
+
+    var window: UIWindow?
+    
+    func application(_ application: UIApplication, didFinishLaunchingWithOptions launchOptions: [UIApplication.LaunchOptionsKey: Any]?) -> Bool {
+        
+        // Always set Version to default
+        let defaults = UserDefaults.standard
+        defaults.removeObject(forKey: "Version")
+        
+        setDefaultsFromSettingsBundle()
+        
+        // Override point for customization after application launch.
+        if !ARWorldTrackingConfiguration.supportsFrameSemantics(.sceneDepth) {
+            // Ensure that the device supports scene depth and present
+            //  an error-message view controller, if not.
+            let storyboard = UIStoryboard(name: "Main", bundle: nil)
+            window?.rootViewController = storyboard.instantiateViewController(withIdentifier: "unsupportedDeviceMessage")
+        }
+        return true
+    }
+
+    // MARK: UISceneSession Lifecycle
+
+    func application(_ application: UIApplication, configurationForConnecting connectingSceneSession: UISceneSession, options: UIScene.ConnectionOptions) -> UISceneConfiguration {
+        // Called when a new scene session is being created.
+        // Use this method to select a configuration to create the new scene with.
+        return UISceneConfiguration(name: "Default Configuration", sessionRole: connectingSceneSession.role)
+    }
+
+    func application(_ application: UIApplication, didDiscardSceneSessions sceneSessions: Set<UISceneSession>) {
+        // Called when the user discards a scene session.
+        // If any sessions were discarded while the application was not running, this will be called shortly after application:didFinishLaunchingWithOptions.
+        // Use this method to release any resources that were specific to the discarded scenes, as they will not return.
+    }
+}
+

app/ios/RTABMapApp/Assets.xcassets/AccentColor.colorset/Contents.json

@@ -0,0 +1,11 @@
+{
+  "colors" : [
+    {
+      "idiom" : "universal"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Assets.xcassets/AppIcon.appiconset/Contents.json

@@ -0,0 +1,116 @@
+{
+  "images" : [
+    {
+      "filename" : "RTAB-Map40-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map60.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map58-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map87.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map80-1.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map120-1.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map120.png",
+      "idiom" : "iphone",
+      "scale" : "2x",
+      "size" : "60x60"
+    },
+    {
+      "filename" : "RTAB-Map180.png",
+      "idiom" : "iphone",
+      "scale" : "3x",
+      "size" : "60x60"
+    },
+    {
+      "filename" : "RTAB-Map20.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map40.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "20x20"
+    },
+    {
+      "filename" : "RTAB-Map29.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map58.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "29x29"
+    },
+    {
+      "filename" : "RTAB-Map40-2.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map80.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "40x40"
+    },
+    {
+      "filename" : "RTAB-Map76.png",
+      "idiom" : "ipad",
+      "scale" : "1x",
+      "size" : "76x76"
+    },
+    {
+      "filename" : "RTAB-Map152.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "76x76"
+    },
+    {
+      "filename" : "RTAB-Map167.png",
+      "idiom" : "ipad",
+      "scale" : "2x",
+      "size" : "83.5x83.5"
+    },
+    {
+      "filename" : "RTAB-Map1024.png",
+      "idiom" : "ios-marketing",
+      "scale" : "1x",
+      "size" : "1024x1024"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Assets.xcassets/Contents.json

@@ -0,0 +1,6 @@
+{
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Base.lproj/LaunchScreen.storyboard

@@ -0,0 +1,85 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" launchScreen="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="01J-lp-oVM">
+    <device id="retina6_1" orientation="portrait" appearance="light"/>
+    <dependencies>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <capability name="Safe area layout guides" minToolsVersion="9.0"/>
+        <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+    </dependencies>
+    <scenes>
+        <!--View Controller-->
+        <scene sceneID="EHf-IW-A2E">
+            <objects>
+                <viewController id="01J-lp-oVM" sceneMemberID="viewController">
+                    <view key="view" contentMode="scaleToFill" id="Ze5-6b-2t3">
+                        <rect key="frame" x="0.0" y="0.0" width="414" height="896"/>
+                        <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+                        <subviews>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="DqX-Vd-Tpo" userLabel="ViewButton">
+                                <rect key="frame" x="342" y="799.5" width="52" height="40.5"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="52" id="m5V-wG-7GI"/>
+                                    <constraint firstAttribute="height" constant="44" id="pbH-yg-ZHL"/>
+                                </constraints>
+                                <color key="tintColor" white="0.33333333333333331" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" backgroundImage="eye" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                            </button>
+                            <stackView opaque="NO" contentMode="scaleToFill" spacing="20" translatesAutoresizingMaskIntoConstraints="NO" id="Ybb-6v-Prf">
+                                <rect key="frame" x="254" y="64" width="140" height="60"/>
+                                <subviews>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="qpg-QD-geo" userLabel="LibraryButton">
+                                        <rect key="frame" x="0.0" y="1" width="60" height="57.5"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="MMK-i4-ipl"/>
+                                            <constraint firstAttribute="width" constant="60" id="MSB-kf-WKH"/>
+                                        </constraints>
+                                        <color key="tintColor" white="0.33333333333333331" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <state key="normal" backgroundImage="folder" catalog="system">
+                                            <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        </state>
+                                    </button>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="Xjn-gQ-W57" userLabel="MenuButton">
+                                        <rect key="frame" x="80" y="0.5" width="60" height="59"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="h7m-oO-bNI"/>
+                                            <constraint firstAttribute="width" constant="60" id="q9I-XS-C8P"/>
+                                        </constraints>
+                                        <color key="tintColor" white="0.33333333333333331" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <state key="normal" backgroundImage="ellipsis.circle" catalog="system"/>
+                                    </button>
+                                </subviews>
+                            </stackView>
+                            <imageView clipsSubviews="YES" userInteractionEnabled="NO" contentMode="scaleAspectFit" horizontalHuggingPriority="251" verticalHuggingPriority="251" image="RTAB-Map1024" translatesAutoresizingMaskIntoConstraints="NO" id="TNo-z5-pM0">
+                                <rect key="frame" x="177" y="418" width="60" height="60"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="60" id="Lio-4G-f3t"/>
+                                    <constraint firstAttribute="height" constant="60" id="Q5D-zG-PAv"/>
+                                </constraints>
+                            </imageView>
+                        </subviews>
+                        <viewLayoutGuide key="safeArea" id="6Tk-OE-BBY"/>
+                        <color key="backgroundColor" white="0.0" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                        <constraints>
+                            <constraint firstItem="TNo-z5-pM0" firstAttribute="centerY" secondItem="Ze5-6b-2t3" secondAttribute="centerY" id="4fp-du-TTg"/>
+                            <constraint firstItem="Ybb-6v-Prf" firstAttribute="top" secondItem="6Tk-OE-BBY" secondAttribute="top" constant="20" id="Dcs-0a-Uhe"/>
+                            <constraint firstItem="6Tk-OE-BBY" firstAttribute="trailing" secondItem="DqX-Vd-Tpo" secondAttribute="trailing" constant="20" id="TTR-AB-iCr"/>
+                            <constraint firstItem="6Tk-OE-BBY" firstAttribute="trailing" secondItem="Ybb-6v-Prf" secondAttribute="trailing" constant="20" id="YPK-bW-6AC"/>
+                            <constraint firstItem="6Tk-OE-BBY" firstAttribute="bottom" secondItem="DqX-Vd-Tpo" secondAttribute="bottom" constant="20" id="cnW-a0-bhz"/>
+                            <constraint firstItem="TNo-z5-pM0" firstAttribute="centerX" secondItem="Ze5-6b-2t3" secondAttribute="centerX" id="epw-Qw-hdY"/>
+                        </constraints>
+                    </view>
+                </viewController>
+                <placeholder placeholderIdentifier="IBFirstResponder" id="iYj-Kq-Ea1" userLabel="First Responder" sceneMemberID="firstResponder"/>
+            </objects>
+            <point key="canvasLocation" x="52.173913043478265" y="375"/>
+        </scene>
+    </scenes>
+    <resources>
+        <image name="RTAB-Map1024" width="591" height="649"/>
+        <image name="ellipsis.circle" catalog="system" width="128" height="121"/>
+        <image name="eye" catalog="system" width="128" height="81"/>
+        <image name="folder" catalog="system" width="128" height="97"/>
+    </resources>
+</document>

app/ios/RTABMapApp/Base.lproj/Main.storyboard

@@ -0,0 +1,270 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="18122" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
+    <device id="retina3_5" orientation="portrait" appearance="light"/>
+    <dependencies>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="18093"/>
+        <capability name="Image references" minToolsVersion="12.0"/>
+        <capability name="Safe area layout guides" minToolsVersion="9.0"/>
+        <capability name="System colors in document resources" minToolsVersion="11.0"/>
+        <capability name="documents saved in the Xcode 8 format" minToolsVersion="8.0"/>
+    </dependencies>
+    <scenes>
+        <!--GLKit View Controller-->
+        <scene sceneID="1cO-Jt-3GG">
+            <objects>
+                <glkViewController preferredFramesPerSecond="30" id="zah-iI-EPt" customClass="ViewController" customModule="RTABMapApp" customModuleProvider="target" sceneMemberID="viewController">
+                    <glkView key="view" opaque="NO" clipsSubviews="YES" multipleTouchEnabled="YES" contentMode="center" drawableDepthFormat="24" enableSetNeedsDisplay="NO" id="sqF-4e-2HU">
+                        <rect key="frame" x="0.0" y="0.0" width="320" height="480"/>
+                        <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+                        <subviews>
+                            <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
+                                <rect key="frame" x="250" y="180" width="60" height="120"/>
+                                <subviews>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="mJN-Ru-yPg" userLabel="StopButton">
+                                        <rect key="frame" x="0.0" y="0.5" width="60" height="59"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="g9J-0P-CmQ"/>
+                                            <constraint firstAttribute="width" constant="60" id="m06-eJ-dIn"/>
+                                        </constraints>
+                                        <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <state key="normal" backgroundImage="stop.circle" catalog="system"/>
+                                        <connections>
+                                            <action selector="stopAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="3iJ-xb-gzb"/>
+                                        </connections>
+                                    </button>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="7Lv-u6-mGh" userLabel="RecordButton">
+                                        <rect key="frame" x="0.0" y="60.5" width="60" height="59"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="DQI-eQ-v3X"/>
+                                            <constraint firstAttribute="width" constant="60" id="Oeu-YT-wMz"/>
+                                        </constraints>
+                                        <fontDescription key="fontDescription" type="system" pointSize="18"/>
+                                        <color key="tintColor" systemColor="systemRedColor"/>
+                                        <inset key="imageEdgeInsets" minX="0.0" minY="0.0" maxX="0.0" maxY="20"/>
+                                        <state key="normal" backgroundImage="record.circle" catalog="system"/>
+                                        <connections>
+                                            <action selector="recordAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="dzY-yL-YBg"/>
+                                        </connections>
+                                    </button>
+                                </subviews>
+                            </stackView>
+                            <stackView opaque="NO" contentMode="scaleToFill" spacing="20" translatesAutoresizingMaskIntoConstraints="NO" id="ozY-dg-YOd">
+                                <rect key="frame" x="160" y="20" width="140" height="60"/>
+                                <subviews>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="9kh-4C-KyP" userLabel="LibraryButton">
+                                        <rect key="frame" x="0.0" y="1" width="60" height="57.5"/>
+                                        <constraints>
+                                            <constraint firstAttribute="width" constant="60" id="Ekq-3J-kUL"/>
+                                            <constraint firstAttribute="height" constant="60" id="fqe-e7-0uk"/>
+                                        </constraints>
+                                        <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <state key="normal" backgroundImage="folder" catalog="system">
+                                            <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        </state>
+                                        <connections>
+                                            <action selector="libraryAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="WOh-uA-T5h"/>
+                                        </connections>
+                                    </button>
+                                    <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="y53-OI-O76" userLabel="MenuButton">
+                                        <rect key="frame" x="80" y="0.5" width="60" height="59"/>
+                                        <constraints>
+                                            <constraint firstAttribute="height" constant="60" id="L5T-8h-ale"/>
+                                            <constraint firstAttribute="width" constant="60" id="pLI-ns-KHQ"/>
+                                        </constraints>
+                                        <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                        <state key="normal" backgroundImage="ellipsis.circle" catalog="system"/>
+                                    </button>
+                                </subviews>
+                            </stackView>
+                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Label" textAlignment="natural" lineBreakMode="wordWrap" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="KlF-4H-k6I">
+                                <rect key="frame" x="16" y="20" width="31" height="14.5"/>
+                                <color key="backgroundColor" red="0.0" green="0.0" blue="0.0" alpha="0.29820365646258501" colorSpace="custom" customColorSpace="sRGB"/>
+                                <fontDescription key="fontDescription" type="system" pointSize="12"/>
+                                <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <nil key="highlightedColor"/>
+                            </label>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="773-n4-lob" userLabel="ViewButton">
+                                <rect key="frame" x="248" y="417.5" width="52" height="40.5"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="52" id="epT-x2-WRc"/>
+                                    <constraint firstAttribute="height" constant="44" id="wAe-I1-qhs"/>
+                                </constraints>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" backgroundImage="eye" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="012-W3-dOS" userLabel="CloseVisualization">
+                                <rect key="frame" x="75" y="434" width="170.5" height="22"/>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" title="Close Visualization" image="xmark.square" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                                <connections>
+                                    <action selector="closeVisualizationAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="DqP-oy-XPe"/>
+                                </connections>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="9oq-op-NGV" userLabel="StopCamera">
+                                <rect key="frame" x="98" y="433" width="124.5" height="22"/>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" title="Stop Camera" image="xmark.square" catalog="system">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                </state>
+                                <connections>
+                                    <action selector="stopCameraAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="YZq-pZ-8Te"/>
+                                </connections>
+                            </button>
+                            <button opaque="NO" contentMode="scaleAspectFit" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="Jj4-ow-MwD" userLabel="NewScanButtonLarge">
+                                <rect key="frame" x="73.5" y="224" width="173" height="32"/>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" title="Start the Camera">
+                                    <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                    <imageReference key="image" image="plus.app" catalog="system" symbolScale="large"/>
+                                    <preferredSymbolConfiguration key="preferredSymbolConfiguration" configurationType="font">
+                                        <fontDescription key="fontDescription" style="UICTFontTextStyleTitle1"/>
+                                    </preferredSymbolConfiguration>
+                                </state>
+                                <connections>
+                                    <action selector="newScanAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="jL9-xF-V9K"/>
+                                </connections>
+                            </button>
+                            <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="3Sc-lx-YKH">
+                                <rect key="frame" x="161" y="96" width="149" height="22"/>
+                                <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <state key="normal" title="Export OBJ-PLY" image="square.and.arrow.up" catalog="system"/>
+                                <connections>
+                                    <action selector="exportOBJPLYAction:" destination="zah-iI-EPt" eventType="touchUpInside" id="md0-pv-zlf"/>
+                                </connections>
+                            </button>
+                            <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="80" minValue="0.0" maxValue="120" translatesAutoresizingMaskIntoConstraints="NO" id="30J-Tx-NRW" userLabel="OrthoClipDistanceSlider">
+                                <rect key="frame" x="-2" y="344" width="122" height="31"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="118" id="dsv-im-K52"/>
+                                </constraints>
+                                <connections>
+                                    <action selector="clipDistanceAction:" destination="zah-iI-EPt" eventType="valueChanged" id="iKj-nz-0KW"/>
+                                </connections>
+                            </slider>
+                            <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="90" minValue="0.0" maxValue="180" translatesAutoresizingMaskIntoConstraints="NO" id="Z1x-Af-hdf" userLabel="GridRotationSlider">
+                                <rect key="frame" x="99" y="389" width="122" height="31"/>
+                                <constraints>
+                                    <constraint firstAttribute="width" constant="118" id="KDl-lP-Jj7"/>
+                                </constraints>
+                                <connections>
+                                    <action selector="rotateGridAction:" destination="zah-iI-EPt" eventType="valueChanged" id="eiC-QR-TYH"/>
+                                </connections>
+                            </slider>
+                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Toast Label" textAlignment="natural" lineBreakMode="tailTruncation" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="JMv-UE-cvK" userLabel="ToastLabel">
+                                <rect key="frame" x="116.5" y="330" width="87.5" height="20.5"/>
+                                <color key="backgroundColor" white="0.0" alpha="0.3002232142857143" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <accessibility key="accessibilityConfiguration">
+                                    <accessibilityTraits key="traits" staticText="YES" notEnabled="YES"/>
+                                </accessibility>
+                                <fontDescription key="fontDescription" type="system" pointSize="17"/>
+                                <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
+                                <nil key="highlightedColor"/>
+                            </label>
+                        </subviews>
+                        <viewLayoutGuide key="safeArea" id="qAR-pI-uT2"/>
+                        <constraints>
+                            <constraint firstItem="KlF-4H-k6I" firstAttribute="leading" secondItem="qAR-pI-uT2" secondAttribute="leading" constant="16" id="14l-HO-Yqo"/>
+                            <constraint firstItem="kPv-DO-hpd" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="4S8-5c-tBm"/>
+                            <constraint firstItem="KlF-4H-k6I" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="CS3-Rk-Wxs"/>
+                            <constraint firstItem="012-W3-dOS" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="IsC-yB-kNs"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="773-n4-lob" secondAttribute="trailing" constant="20" id="N8s-Yg-VsL"/>
+                            <constraint firstItem="JMv-UE-cvK" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="NIf-UH-gZ7"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="773-n4-lob" secondAttribute="bottom" constant="20" id="O9t-O0-69d"/>
+                            <constraint firstItem="Jj4-ow-MwD" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="OLx-LR-8xj"/>
+                            <constraint firstItem="012-W3-dOS" firstAttribute="top" secondItem="Z1x-Af-hdf" secondAttribute="bottom" constant="15" id="RpS-k0-btM"/>
+                            <constraint firstItem="ozY-dg-YOd" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="TFT-we-noh"/>
+                            <constraint firstItem="30J-Tx-NRW" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leading" id="ZBG-GX-P1X"/>
+                            <constraint firstItem="3Sc-lx-YKH" firstAttribute="top" secondItem="ozY-dg-YOd" secondAttribute="bottom" constant="16" id="abR-Tj-Nua"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="012-W3-dOS" secondAttribute="bottom" constant="24" id="dcl-mV-bH1"/>
+                            <constraint firstItem="9oq-op-NGV" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="g5b-gW-Amg"/>
+                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="i2j-zQ-m1V"/>
+                            <constraint firstItem="Z1x-Af-hdf" firstAttribute="top" secondItem="JMv-UE-cvK" secondAttribute="bottom" constant="38.5" id="icg-DR-ZpX"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="9oq-op-NGV" secondAttribute="bottom" constant="25" id="lFI-66-D3N"/>
+                            <constraint firstAttribute="trailing" secondItem="kPv-DO-hpd" secondAttribute="trailing" constant="10" id="lFv-p3-LIS"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="ozY-dg-YOd" secondAttribute="trailing" constant="20" id="ov4-d2-OMy"/>
+                            <constraint firstItem="012-W3-dOS" firstAttribute="top" secondItem="30J-Tx-NRW" secondAttribute="bottom" constant="60" id="tbj-nG-nfI"/>
+                            <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="3Sc-lx-YKH" secondAttribute="trailing" constant="10" id="v2K-Dp-Kyn"/>
+                            <constraint firstItem="Jj4-ow-MwD" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="vmc-4u-BtF"/>
+                        </constraints>
+                        <connections>
+                            <outlet property="delegate" destination="zah-iI-EPt" id="zXM-3t-Cw4"/>
+                        </connections>
+                    </glkView>
+                    <connections>
+                        <outlet property="closeVisualizationButton" destination="012-W3-dOS" id="qSV-q9-eOP"/>
+                        <outlet property="exportOBJPLYButton" destination="3Sc-lx-YKH" id="ZvS-VQ-nsg"/>
+                        <outlet property="libraryButton" destination="9kh-4C-KyP" id="Mvv-uQ-kmi"/>
+                        <outlet property="menuButton" destination="y53-OI-O76" id="v3g-JB-SsY"/>
+                        <outlet property="newScanButtonLarge" destination="Jj4-ow-MwD" id="0Hv-3K-Xjh"/>
+                        <outlet property="orthoDistanceSlider" destination="30J-Tx-NRW" id="4J1-8u-tDY"/>
+                        <outlet property="orthoGridSlider" destination="Z1x-Af-hdf" id="Zsn-xz-jn0"/>
+                        <outlet property="recordButton" destination="7Lv-u6-mGh" id="xTu-EK-FH2"/>
+                        <outlet property="statusLabel" destination="KlF-4H-k6I" id="NmA-K5-UOw"/>
+                        <outlet property="stopButton" destination="mJN-Ru-yPg" id="DND-gz-jLg"/>
+                        <outlet property="stopCameraButton" destination="9oq-op-NGV" id="sxO-jc-luC"/>
+                        <outlet property="toastLabel" destination="JMv-UE-cvK" id="9a7-QF-h02"/>
+                        <outlet property="viewButton" destination="773-n4-lob" id="deU-Na-mBk"/>
+                    </connections>
+                </glkViewController>
+                <placeholder placeholderIdentifier="IBFirstResponder" id="plN-WG-seA" userLabel="First Responder" customClass="UIResponder" sceneMemberID="firstResponder"/>
+            </objects>
+            <point key="canvasLocation" x="-234.375" y="-586.25"/>
+        </scene>
+        <!--Unsupported View Controller-->
+        <scene sceneID="sJB-xy-u63">
+            <objects>
+                <viewController storyboardIdentifier="unsupportedDeviceMessage" id="v3G-nW-gVc" userLabel="Unsupported View Controller" sceneMemberID="viewController">
+                    <view key="view" contentMode="scaleToFill" id="hzM-1k-POo">
+                        <rect key="frame" x="0.0" y="0.0" width="320" height="480"/>
+                        <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
+                        <subviews>
+                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Unsupported Device" textAlignment="center" lineBreakMode="tailTruncation" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="kZt-ct-4KD">
+                                <rect key="frame" x="21.5" y="165" width="277.5" height="30"/>
+                                <fontDescription key="fontDescription" style="UICTFontTextStyleTitle1"/>
+                                <nil key="textColor"/>
+                                <nil key="highlightedColor"/>
+                            </label>
+                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" textAlignment="center" lineBreakMode="tailTruncation" numberOfLines="0" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="4Js-Hw-3QU">
+                                <rect key="frame" x="21.5" y="203" width="277.5" height="74"/>
+                                <string key="text">This sample app requires a LiDAR-capable device, such as the second-generation iPad Pro 11-inch and fourth-generation iPad Pro 12.9-inch.</string>
+                                <fontDescription key="fontDescription" style="UICTFontTextStyleBody"/>
+                                <nil key="textColor"/>
+                                <nil key="highlightedColor"/>
+                            </label>
+                        </subviews>
+                        <viewLayoutGuide key="safeArea" id="Wfl-Ts-arS"/>
+                        <color key="backgroundColor" white="1" alpha="1" colorSpace="calibratedWhite"/>
+                        <constraints>
+                            <constraint firstItem="kZt-ct-4KD" firstAttribute="centerX" secondItem="hzM-1k-POo" secondAttribute="centerX" id="5BV-Ql-eBq"/>
+                            <constraint firstAttribute="trailing" relation="greaterThanOrEqual" secondItem="4Js-Hw-3QU" secondAttribute="trailing" constant="20" symbolic="YES" id="5Sg-eL-Yhf"/>
+                            <constraint firstItem="4Js-Hw-3QU" firstAttribute="centerY" secondItem="hzM-1k-POo" secondAttribute="centerY" id="M1k-d1-BNV"/>
+                            <constraint firstItem="4Js-Hw-3QU" firstAttribute="top" secondItem="kZt-ct-4KD" secondAttribute="bottom" constant="8" symbolic="YES" id="PjQ-ik-Z1B"/>
+                            <constraint firstItem="kZt-ct-4KD" firstAttribute="width" secondItem="4Js-Hw-3QU" secondAttribute="width" id="aQe-nl-Pyq"/>
+                            <constraint firstItem="4Js-Hw-3QU" firstAttribute="centerX" secondItem="hzM-1k-POo" secondAttribute="centerX" id="cww-hm-oLb"/>
+                            <constraint firstItem="4Js-Hw-3QU" firstAttribute="leading" relation="greaterThanOrEqual" secondItem="hzM-1k-POo" secondAttribute="leading" constant="20" symbolic="YES" id="jfM-fP-rth"/>
+                        </constraints>
+                    </view>
+                </viewController>
+                <placeholder placeholderIdentifier="IBFirstResponder" id="jam-V0-csO" userLabel="First Responder" sceneMemberID="firstResponder"/>
+            </objects>
+            <point key="canvasLocation" x="476" y="-586"/>
+        </scene>
+    </scenes>
+    <resources>
+        <image name="ellipsis.circle" catalog="system" width="128" height="121"/>
+        <image name="eye" catalog="system" width="128" height="81"/>
+        <image name="folder" catalog="system" width="128" height="97"/>
+        <image name="plus.app" catalog="system" width="128" height="114"/>
+        <image name="record.circle" catalog="system" width="128" height="121"/>
+        <image name="square.and.arrow.up" catalog="system" width="115" height="128"/>
+        <image name="stop.circle" catalog="system" width="128" height="121"/>
+        <image name="xmark.square" catalog="system" width="128" height="114"/>
+        <systemColor name="systemRedColor">
+            <color red="1" green="0.23137254901960785" blue="0.18823529411764706" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+        </systemColor>
+    </resources>
+</document>

app/ios/RTABMapApp/DatabaseView.swift

@@ -0,0 +1,186 @@
+/**
+ * Copyright (c) 2017 Razeware LLC
+ *
+ * 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.
+ *
+ * Notwithstanding the foregoing, you may not use, copy, modify, merge, publish,
+ * distribute, sublicense, create a derivative work, and/or sell copies of the
+ * Software in any work that is designed, intended, or marketed for pedagogical or
+ * instructional purposes related to programming, coding, application development,
+ * or information technology.  Permission for such use, copying, modification,
+ * merger, publication, distribution, sublicensing, creation of derivative works,
+ * or sale is expressly withheld.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+
+import UIKit
+
+protocol DatabaseViewDelegate: class {
+    func databaseShared(databaseURL: URL)
+    func databaseRenamed(databaseURL: URL)
+    func databaseDeleted(databaseURL: URL)
+}
+
+class DatabaseView: UIView {
+  
+  private var coverImageView: UIImageView!
+  private var indicatorView: UIActivityIndicatorView!
+  private var valueObservation: NSKeyValueObservation!
+  private var textLabel: UILabel!
+  private var databaseURL: URL!
+  public var delegate: DatabaseViewDelegate!
+  
+  required init?(coder aDecoder: NSCoder) {
+    super.init(coder: aDecoder)
+    commonInit(databasePath: "NA")
+  }
+  
+    init(frame: CGRect, databaseURL: URL) {
+        super.init(frame: frame)
+        self.databaseURL = databaseURL
+        commonInit(databasePath: databaseURL.path)
+        DispatchQueue.global().async {
+            let downloadedImage = getPreviewImage(databasePath: databaseURL.path)
+          DispatchQueue.main.async {
+            self.coverImageView.image = downloadedImage
+          }
+        }
+    }
+
+  private func commonInit(databasePath: String) {
+    
+    // Setup the background
+    backgroundColor = .black
+    // Create the cover image view
+    coverImageView = UIImageView()
+    coverImageView.translatesAutoresizingMaskIntoConstraints = false
+    
+    valueObservation = coverImageView.observe(\.image, options: [.new]) { [unowned self] observed, change in
+      if change.newValue is UIImage {
+        self.indicatorView.stopAnimating()
+      }
+    }
+    
+    //Text Label
+    textLabel = UILabel()
+    textLabel.numberOfLines = 0 
+    textLabel.text  =
+        URL(fileURLWithPath: databasePath).lastPathComponent + "\n" +
+        URL(fileURLWithPath: databasePath).fileSizeString + "\n" +
+        (try! URL(fileURLWithPath: databasePath).resourceValues(forKeys: [.contentModificationDateKey]).contentModificationDate!.getFormattedDate(format: "yyyy-MM-dd HH:mm:ss"))
+    textLabel.textColor = .white
+    textLabel.textAlignment = .left
+
+    //Stack View
+    let stackView   = UIStackView()
+    stackView.axis  = NSLayoutConstraint.Axis.horizontal
+    stackView.distribution  = UIStackView.Distribution.fill
+    stackView.alignment = UIStackView.Alignment.center
+    stackView.spacing   = 8.0
+
+    stackView.addArrangedSubview(coverImageView)
+    stackView.addArrangedSubview(textLabel)
+    stackView.translatesAutoresizingMaskIntoConstraints = false
+
+    addSubview(stackView)
+
+    // Create the indicator view
+    indicatorView = UIActivityIndicatorView()
+    indicatorView.translatesAutoresizingMaskIntoConstraints = false
+    indicatorView.style = .whiteLarge
+    indicatorView.startAnimating()
+    addSubview(indicatorView)
+    
+    NSLayoutConstraint.activate([
+      stackView.leftAnchor.constraint(equalTo: self.leftAnchor),
+      stackView.rightAnchor.constraint(equalTo: self.rightAnchor),
+      stackView.topAnchor.constraint(equalTo: self.topAnchor),
+      stackView.bottomAnchor.constraint(equalTo: self.bottomAnchor),
+      stackView.leadingAnchor.constraint(equalTo: self.leadingAnchor),
+      coverImageView.widthAnchor.constraint(equalToConstant: 180 - 5),
+      indicatorView.centerXAnchor.constraint(equalTo: coverImageView.centerXAnchor),
+      indicatorView.centerYAnchor.constraint(equalTo: coverImageView.centerYAnchor)
+      ])
+    
+    let interaction = UIContextMenuInteraction(delegate: self)
+    self.addInteraction(interaction)
+  }
+    
+    func highlightDatabase(_ didHighlightView: Bool) {
+      if didHighlightView == true {
+        backgroundColor = .white
+        textLabel.textColor = .black
+      } else {
+        backgroundColor = .black
+        textLabel.textColor = .white
+      }
+    }
+}
+
+extension URL {
+    var attributes: [FileAttributeKey : Any]? {
+        do {
+            return try FileManager.default.attributesOfItem(atPath: path)
+        } catch let error as NSError {
+            print("FileAttribute error: \(error)")
+        }
+        return nil
+    }
+
+    var fileSize: UInt64 {
+        return attributes?[.size] as? UInt64 ?? UInt64(0)
+    }
+
+    var fileSizeString: String {
+        return ByteCountFormatter.string(fromByteCount: Int64(fileSize), countStyle: .file)
+    }
+
+    var creationDate: Date? {
+        return attributes?[.creationDate] as? Date
+    }
+}
+
+extension DatabaseView: UIContextMenuInteractionDelegate {
+               
+      func contextMenuInteraction(_ interaction: UIContextMenuInteraction, configurationForMenuAtLocation location: CGPoint) -> UIContextMenuConfiguration? {
+        return UIContextMenuConfiguration(identifier: nil, previewProvider: nil) { suggestedActions in
+           
+            // Create an action for sharing
+            let share = UIAction(title: "Share", image: UIImage(systemName: "square.and.arrow.up")) { action in
+                // Show system share sheet
+                self.delegate?.databaseShared(databaseURL: self.databaseURL)
+            }
+    
+            // Create an action for renaming
+            let rename = UIAction(title: "Rename", image: UIImage(systemName: "square.and.pencil")) { action in
+                // Perform renaming
+                self.delegate?.databaseRenamed(databaseURL: self.databaseURL)
+            }
+    
+            // Here we specify the "destructive" attribute to show that it’s destructive in nature
+            let delete = UIAction(title: "Delete", image: UIImage(systemName: "trash"), attributes: .destructive) { action in
+                // Perform delete
+                self.delegate?.databaseDeleted(databaseURL: self.databaseURL)
+            }
+    
+            // Create and return a UIMenu with all of the actions as children
+            return UIMenu(title: "", children: [share, rename, delete])
+        }
+    }
+}

app/ios/RTABMapApp/Images.xcassets/Contents.json

@@ -0,0 +1,6 @@
+{
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Images.xcassets/RTAB-Map1024.imageset/Contents.json

@@ -0,0 +1,21 @@
+{
+  "images" : [
+    {
+      "filename" : "RTAB-Map1024.png",
+      "idiom" : "universal",
+      "scale" : "1x"
+    },
+    {
+      "idiom" : "universal",
+      "scale" : "2x"
+    },
+    {
+      "idiom" : "universal",
+      "scale" : "3x"
+    }
+  ],
+  "info" : {
+    "author" : "xcode",
+    "version" : 1
+  }
+}

app/ios/RTABMapApp/Info.plist

@@ -0,0 +1,80 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>CFBundleDevelopmentRegion</key>
+	<string>$(DEVELOPMENT_LANGUAGE)</string>
+	<key>CFBundleExecutable</key>
+	<string>$(EXECUTABLE_NAME)</string>
+	<key>CFBundleIcons</key>
+	<dict/>
+	<key>CFBundleIcons~ipad</key>
+	<dict/>
+	<key>CFBundleIdentifier</key>
+	<string>$(PRODUCT_BUNDLE_IDENTIFIER)</string>
+	<key>CFBundleInfoDictionaryVersion</key>
+	<string>6.0</string>
+	<key>CFBundleName</key>
+	<string>RTAB-Map</string>
+	<key>CFBundlePackageType</key>
+	<string>$(PRODUCT_BUNDLE_PACKAGE_TYPE)</string>
+	<key>CFBundleShortVersionString</key>
+	<string>$(MARKETING_VERSION)</string>
+	<key>CFBundleVersion</key>
+	<string>$(CURRENT_PROJECT_VERSION)</string>
+	<key>LSRequiresIPhoneOS</key>
+	<true/>
+	<key>LSSupportsOpeningDocumentsInPlace</key>
+	<true/>
+	<key>NSCameraUsageDescription</key>
+	<string>The camera is used for scanning (ideally with LiDAR).</string>
+	<key>NSLocationWhenInUseUsageDescription</key>
+	<string>The GPS is used when GPS option is enabled in Settings-&gt;Mapping...</string>
+	<key>UIApplicationSceneManifest</key>
+	<dict>
+		<key>UIApplicationSupportsMultipleScenes</key>
+		<false/>
+		<key>UISceneConfigurations</key>
+		<dict>
+			<key>UIWindowSceneSessionRoleApplication</key>
+			<array>
+				<dict>
+					<key>UISceneConfigurationName</key>
+					<string>Default Configuration</string>
+					<key>UISceneDelegateClassName</key>
+					<string>$(PRODUCT_MODULE_NAME).SceneDelegate</string>
+					<key>UISceneStoryboardFile</key>
+					<string>Main</string>
+				</dict>
+			</array>
+		</dict>
+	</dict>
+	<key>UIApplicationSupportsIndirectInputEvents</key>
+	<true/>
+	<key>UIFileSharingEnabled</key>
+	<true/>
+	<key>UILaunchStoryboardName</key>
+	<string>LaunchScreen</string>
+	<key>UIMainStoryboardFile</key>
+	<string>Main</string>
+	<key>UIRequiredDeviceCapabilities</key>
+	<array>
+		<string>armv7</string>
+		<string>opengles-2</string>
+		<string>arkit</string>
+	</array>
+	<key>UISupportedInterfaceOrientations</key>
+	<array>
+		<string>UIInterfaceOrientationPortrait</string>
+		<string>UIInterfaceOrientationLandscapeLeft</string>
+		<string>UIInterfaceOrientationLandscapeRight</string>
+	</array>
+	<key>UISupportedInterfaceOrientations~ipad</key>
+	<array>
+		<string>UIInterfaceOrientationPortrait</string>
+		<string>UIInterfaceOrientationPortraitUpsideDown</string>
+		<string>UIInterfaceOrientationLandscapeLeft</string>
+		<string>UIInterfaceOrientationLandscapeRight</string>
+	</array>
+</dict>
+</plist>

app/ios/RTABMapApp/Libraries/.gitignore

@@ -0,0 +1,6 @@
+# Ignore everything
+*
+
+# But not these files...
+!.gitignore
+!install_deps.sh

app/ios/RTABMapApp/Libraries/install_deps.sh

@@ -0,0 +1,150 @@
+#!/bin/bash
+
+# Tested on Apple Silicon Mac, with cmake 3.19.2.
+
+pwd=$(pwd)
+prefix=$pwd
+sysroot=iphoneos
+#sysroot=iphonesimulator
+
+
+# Install directory for all dependencies
+mkdir -p $prefix
+
+# openmp
+# based on https://github.com/Homebrew/homebrew-core/blob/HEAD/Formula/libomp.rb
+#curl -L https://github.com/llvm/llvm-project/releases/download/llvmorg-11.1.0/openmp-11.1.0.src.tar.xz -o openmp-11.1.0.src.tar.xz
+#tar -xzf openmp-11.1.0.src.tar.xz
+#cd openmp-11.1.0.src
+#curl -L https://raw.githubusercontent.com/Homebrew/formula-patches/7e2ee1d7/libomp/arm.patch -o arm.patch
+#patch -p1 < arm.patch
+#mkdir build
+#cd build
+#cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DLIBOMP_INSTALL_ALIASES=OFF -DLIBOMP_ENABLE_SHARED=OFF ..
+#cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+#cp runtime/src/Release-iphoneos/libomp.a runtime/src/.
+#cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+
+# Boost
+echo "wget boost..."
+curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz
+tar -xzf boost_1_59_0.tar.gz
+cd boost_1_59_0
+curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/489ff2869eccd6f8d03ffb9090ef839108762741/BoostConfig.cmake.in -o BoostConfig.cmake.in
+curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/b07fe7d4e5dfe5f1d110c733e5cf660d79a26378/CMakeLists.txt -o CMakeLists.txt
+mkdir build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix ..
+cmake --build . --config Release
+cmake --build . --config Release --target install
+cd $pwd
+#rm -r boost_1_59_0.tar.gz boost_1_59_0
+
+# eigen
+echo "wget eigen..."
+curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
+tar -xzf 3.3.9.tar.gz
+cd eigen-3.3.9
+mkdir build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix ..
+cmake --build . --config Release
+cmake --build . --config Release --target install
+cd $pwd
+#rm -r 3.3.9.tar.gz eigen-3.3.9
+
+# FLANN
+echo "wget flann..."
+curl -L http://www.cs.ubc.ca/research/flann/uploads/FLANN/flann-1.8.4-src.zip -o flann-1.8.4-src.zip
+unzip -qq flann-1.8.4-src.zip
+cd flann-1.8.4-src
+mkdir build
+cd build
+# comment "add_subdirectory( test )" in top CMakeLists.txt
+# comment "add_subdirectory( doc )" in top CMakeLists.txt
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_PYTHON_BINDINGS=OFF -DBUILD_MATLAB_BINDINGS=OFF -DBUILD_C_BINDINGS=OFF  ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+#rm -r flann-1.8.4-src.zip flann-1.8.4-src
+
+# GTSAM
+git clone https://bitbucket.org/gtborg/gtsam.git
+cd gtsam
+git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
+# patch
+curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
+git apply gtsam_GKlib_ios_fix.patch
+mkdir build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WRAP_SERIALIZATION=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+#rm -rf gtsam
+
+# g2o
+git clone https://github.com/RainerKuemmerle/g2o.git
+cd g2o
+git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
+# patch
+curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
+git apply g2o_ios_fix.patch
+mkdir build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+#rm -rf g2o
+
+# VTK
+git clone https://github.com/Kitware/VTK.git
+cd VTK
+git checkout tags/v8.2.0
+git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
+mkdir build
+cd build
+cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DIOS_SIMULATOR_ARCHITECTURES=arm64 -DModule_vtkFiltersModeling=ON ..
+# For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
+cmake --build . --config Release
+cd $pwd
+#rm -rf VTK
+
+# PCL
+git clone https://github.com/PointCloudLibrary/pcl.git
+cd pcl
+git checkout tags/pcl-1.11.1
+# patch
+curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/4a66ebb9faa1dfe997a0860d733bc5473cff20ee/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
+git apply pcl_1_11_1_vtk_ios_support.patch
+mkdir build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
+cmake --build . --config Release
+cmake --build . --config Release --target install
+cd $pwd
+#rm -rf pcl
+
+# OpenCV
+git clone https://github.com/opencv/opencv_contrib.git
+cd opencv_contrib
+git checkout tags/3.4.2
+cd $pwd
+git clone https://github.com/opencv/opencv.git
+cd opencv
+git checkout tags/3.4.2
+mkdir build
+cd build
+# add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF ..
+cmake --build . --config Release
+cmake --build . --config Release --target install
+cd $pwd
+#rm -rf opencv opencv_contrib
+
+mkdir rtabmap
+cd rtabmap
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=11.0 -DCMAKE_C_FLAGS=-fembed-bitcode -DCMAKE_CXX_FLAGS=-fembed-bitcode -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../..
+cmake --build . --config Release
+cmake --build . --config Release --target install

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -0,0 +1,609 @@
+//
+//  PCLWrapper.cpp
+//  ThreeDScanner
+//
+//  Created by Steven Roach on 2/9/18.
+//  Copyright © 2018 Steven Roach. All rights reserved.
+//
+
+#include "NativeWrapper.hpp"
+#include "RTABMapApp.h"
+#include <rtabmap/core/DBDriverSqlite3.h>
+
+inline RTABMapApp *native(const void *object) {
+  return (RTABMapApp *)object;
+}
+
+const void * createNativeApplication()
+{
+    RTABMapApp *app = new RTABMapApp();
+    return (void *)app;
+}
+
+void setupCallbacksNative(const void *object, void * classPtr,
+                          void(*progressCallback)(void*, int, int),
+                          void(*initCallback)(void *, int, const char*),
+                          void(*statsUpdatedCallback)(void *,
+                                                   int, int, int, int,
+                                                   float,
+                                                   int, int, int, int, int ,int,
+                                                   float,
+                                                   int,
+                                                   float,
+                                                   int,
+                                                   float, float, float, float,
+                                                   int, int,
+                                                   float, float, float, float, float, float))
+{
+    if(object)
+    {
+        native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void destroyNativeApplication(const void *object)
+{
+    if(object)
+    {
+        delete native(object);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setScreenRotationNative(const void *object, int displayRotation)
+{
+    if(object)
+    {
+        return native(object)->setScreenRotation(displayRotation, 0);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize)
+{
+    if(object)
+    {
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return -1;
+    }
+}
+
+int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize)
+{
+    if(object)
+    {
+        return native(object)->openDatabase(databasePath, databaseInMemory, optimize, databaseSource);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return -1;
+    }
+}
+
+void saveNative(const void *object, const char * databasePath)
+{
+    if(object)
+    {
+        return native(object)->save(databasePath);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int postProcessingNative(const void *object, int approach)
+{
+    if(object)
+    {
+        return native(object)->postProcessing(approach);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return -1;
+}
+
+bool exportMeshNative(
+            const void *object,
+            float cloudVoxelSize,
+            bool regenerateCloud,
+            bool meshing,
+            int textureSize,
+            int textureCount,
+            int normalK,
+            bool optimized,
+            float optimizedVoxelSize,
+            int optimizedDepth,
+            int optimizedMaxPolygons,
+            float optimizedColorRadius,
+            bool optimizedCleanWhitePolygons,
+            int optimizedMinClusterSize,
+            float optimizedMaxTextureDistance,
+            int optimizedMinTextureClusterSize,
+            bool blockRendering)
+{
+    if(object)
+    {
+        return native(object)->exportMesh(cloudVoxelSize, regenerateCloud, meshing, textureSize, textureCount, normalK, optimized, optimizedVoxelSize, optimizedDepth, optimizedMaxPolygons, optimizedColorRadius, optimizedCleanWhitePolygons, optimizedMinClusterSize, optimizedMaxTextureDistance, optimizedMinTextureClusterSize, blockRendering);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+bool postExportationNative(const void *object, bool visualize)
+{
+    if(object)
+    {
+        return native(object)->postExportation(visualize);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+bool writeExportedMeshNative(const void *object, const char * directory, const char * name)
+{
+    if(object)
+    {
+        return native(object)->writeExportedMesh(directory, name);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+    return false;
+}
+
+void initGlContentNative(const void *object) {
+    if(object)
+    {
+        native(object)->InitializeGLContent();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setupGraphicNative(const void *object, int width, int height) {
+    if(object)
+    {
+        native(object)->SetViewPort(width, height);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void onTouchEventNative(const void *object, int touch_count, int event, float x0, float y0, float x1,
+    float y1) {
+    if(object)
+    {
+        using namespace tango_gl;
+        GestureCamera::TouchEvent touch_event =
+          static_cast<GestureCamera::TouchEvent>(event);
+        native(object)->OnTouchEvent(touch_count, touch_event, x0, y0, x1, y1);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setPausedMappingNative(const void *object, bool paused)
+{
+    if(object)
+    {
+        return native(object)->setPausedMapping(paused);
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+int renderNative(const void *object) {
+    if(object)
+    {
+        return native(object)->Render();
+    }
+    else
+    {
+        UERROR("object is null!");
+        return -1;
+    }
+}
+
+bool startCameraNative(const void *object) {
+    if(object)
+    {
+        return native(object)->startCamera();
+    }
+    else
+    {
+        UERROR("object is null!");
+        return false;
+    }
+}
+
+void stopCameraNative(const void *object) {
+    if(object)
+    {
+        native(object)->stopCamera();
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void setCameraNative(const void *object, int type) {
+    if(object)
+    {
+        native(object)->SetCameraType(tango_gl::GestureCamera::CameraType(type));
+    }
+    else
+    {
+        UERROR("object is null!");
+    }
+}
+
+void postCameraPoseEventNative(const void *object,
+        float x, float y, float z, float qx, float qy, float qz, float qw)
+{
+    if(object)
+    {
+        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return;
+    }
+}
+
+void postOdometryEventNative(const void *object,
+        float x, float y, float z, float qx, float qy, float qz, float qw,
+        float fx, float fy, float cx, float cy,
+        double stamp,
+        const void * yPlane,  const void * uPlane,  const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+        const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+        const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+        const void * points, int pointsLen, int pointsChannels,
+        float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw,
+        float p00, float p11, float p02, float p12, float p22, float p32, float p23,
+        float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
+{
+    if(object)
+    {
+        native(object)->postOdometryEvent(
+                rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+                fx,fy,cx,cy, 0,0,0,0,
+                rtabmap::Transform(), rtabmap::Transform(),
+                stamp, 0,
+                yPlane, uPlane, vPlane, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+                depth, depthLen, depthWidth, depthHeight, depthFormat,
+                conf, confLen, confWidth, confHeight, confFormat,
+                (const float *)points, pointsLen, pointsChannels,
+                rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+                p00, p11, p02, p12, p22, p32, p23,
+                t0, t1, t2, t3, t4, t5, t6, t7);
+    }
+    else
+    {
+        UERROR("object is null!");
+        return;
+    }
+}
+
+ImageNative getPreviewImageNative(const char * databasePath)
+{
+    ImageNative imageNative;
+    imageNative.data = 0;
+    imageNative.objectPtr = 0;
+    rtabmap::DBDriverSqlite3 driver;
+    if(driver.openConnection(databasePath))
+    {
+        cv::Mat image = driver.loadPreviewImage();
+        if(image.empty())
+        {
+            return imageNative;
+        }
+        cv::Mat * imagePtr = new cv::Mat();
+        // We should add alpha channel
+        cv::cvtColor(image, *imagePtr, cv::COLOR_BGR2BGRA);
+        std::vector<cv::Mat> channels;
+        cv::split(*imagePtr, channels);
+        channels.back() = cv::Scalar(255);
+        cv::merge(channels, *imagePtr);
+        imageNative.objectPtr = imagePtr;
+        imageNative.data = imagePtr->data;
+        imageNative.width = imagePtr->cols;
+        imageNative.height = imagePtr->rows;
+        imageNative.channels = imagePtr->channels();
+    }
+    return imageNative;
+}
+
+void releasePreviewImageNative(ImageNative image)
+{
+    if(image.objectPtr)
+    {
+        delete (cv::Mat*)image.objectPtr;
+    }
+}
+
+
+// Parameters
+void setOnlineBlendingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setOnlineBlending(enabled);
+    else
+        UERROR("object is null!");
+}
+void setMapCloudShownNative(const void *object, bool shown)
+{
+    if(object)
+        native(object)->setMapCloudShown(shown);
+    else
+        UERROR("object is null!");
+}
+void setOdomCloudShownNative(const void *object, bool shown)
+{
+    if(object)
+        native(object)->setOdomCloudShown(shown);
+    else
+        UERROR("object is null!");
+}
+void setMeshRenderingNative(const void *object, bool enabled, bool withTexture)
+{
+    if(object)
+        native(object)->setMeshRendering(enabled, withTexture);
+    else
+        UERROR("object is null!");
+}
+void setPointSizeNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setPointSize(value);
+    else
+        UERROR("object is null!");
+}
+void setFOVNative(const void *object, float angle)
+{
+    if(object)
+        native(object)->setFOV(angle);
+    else
+        UERROR("object is null!");
+}
+void setOrthoCropFactorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setOrthoCropFactor(value);
+    else
+        UERROR("object is null!");
+}
+void setGridRotationNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setGridRotation(value);
+    else
+        UERROR("object is null!");
+}
+void setLightingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setLighting(enabled);
+    else
+        UERROR("object is null!");
+}
+void setBackfaceCullingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setBackfaceCulling(enabled);
+    else
+        UERROR("object is null!");
+}
+void setWireframeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setWireframe(enabled);
+    else
+        UERROR("object is null!");
+}
+void setLocalizationModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setLocalizationMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setTrajectoryModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setTrajectoryMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setGraphOptimizationNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setGraphOptimization(enabled);
+    else
+        UERROR("object is null!");
+}
+void setNodesFilteringNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setNodesFiltering(enabled);
+    else
+        UERROR("object is null!");
+}
+void setGraphVisibleNative(const void *object, bool visible)
+{
+    if(object)
+        native(object)->setGraphVisible(visible);
+    else
+        UERROR("object is null!");
+}
+void setGridVisibleNative(const void *object, bool visible)
+{
+    if(object)
+        native(object)->setGridVisible(visible);
+    else
+        UERROR("object is null!");
+}
+void setRawScanSavedNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setRawScanSaved(enabled);
+    else
+        UERROR("object is null!");
+}
+void setFullResolutionNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setFullResolution(enabled);
+    else
+        UERROR("object is null!");
+}
+void setSmoothingNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setSmoothing(enabled);
+    else
+        UERROR("object is null!");
+}
+void setAppendModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setAppendMode(enabled);
+    else
+        UERROR("object is null!");
+}
+void setMaxCloudDepthNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMaxCloudDepth(value);
+    else
+        UERROR("object is null!");
+}
+void setMinCloudDepthNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMinCloudDepth(value);
+    else
+        UERROR("object is null!");
+}
+void setCloudDensityLevelNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setCloudDensityLevel(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshAngleToleranceNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMeshAngleTolerance(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshDecimationFactorNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMeshDecimationFactor(value);
+    else
+        UERROR("object is null!");
+}
+void setMeshTriangleSizeNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setMeshTriangleSize(value);
+    else
+        UERROR("object is null!");
+}
+void setClusterRatioNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setClusterRatio(value);
+    else
+        UERROR("object is null!");
+}
+void setMaxGainRadiusNative(const void *object, float value)
+{
+    if(object)
+        native(object)->setMaxGainRadius(value);
+    else
+        UERROR("object is null!");
+}
+void setRenderingTextureDecimationNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setRenderingTextureDecimation(value);
+    else
+        UERROR("object is null!");
+}
+void setBackgroundColorNative(const void *object, float gray)
+{
+    if(object)
+        native(object)->setBackgroundColor(gray);
+    else
+        UERROR("object is null!");
+}
+void setDepthConfidenceNative(const void *object, int value)
+{
+    if(object)
+        native(object)->setDepthConfidence(value);
+    else
+        UERROR("object is null!");
+}
+int setMappingParameterNative(const void *object, const char * key, const char * value)
+{
+    if(object)
+        return native(object)->setMappingParameter(key, value);
+    else
+        UERROR("object is null!");
+    return -1;
+}
+
+void setGPSNative(const void *object, double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing)
+{
+    rtabmap::GPS gps(stamp, longitude, latitude, altitude, accuracy, bearing);
+    if(object)
+        return native(object)->setGPS(gps);
+    else
+        UERROR("object is null!");
+}
+
+void addEnvSensorNative(const void *object, int type, float value)
+{
+    if(object)
+        return native(object)->addEnvSensor(type, value);
+    else
+        UERROR("object is null!");
+}

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -0,0 +1,136 @@
+//
+//  PCLWrapper.hpp
+//  ThreeDScanner
+//
+//  Created by Steven Roach on 2/9/18.
+//  Copyright © 2018 Steven Roach. All rights reserved.
+//
+
+#ifndef NativeWrapper_hpp
+#define NativeWrapper_hpp
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+    
+const void *createNativeApplication();
+void setupCallbacksNative(const void *object, void * classPtr,
+                          void(*progressCallback)(void*, int, int),
+                          void(*initCallback)(void *, int, const char*),
+                          void(*statsUpdatedCallback)(void *,
+                                                   int, int, int, int,
+                                                   float,
+                                                   int, int, int, int, int ,int,
+                                                   float,
+                                                   int,
+                                                   float,
+                                                   int,
+                                                   float, float, float, float,
+                                                   int, int,
+                                                   float, float, float, float, float, float));
+void destroyNativeApplication(const void *object);
+void setScreenRotationNative(const void *object, int displayRotation);
+int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize);
+int openDatabase2Native(const void *object, const char * databaseSource, const char * databasePath, bool databaseInMemory, bool optimize);
+void saveNative(const void *object, const char * databasePath);
+int postProcessingNative(const void *object, int approach);
+bool exportMeshNative(
+            const void *object,
+            float cloudVoxelSize,
+            bool regenerateCloud,
+            bool meshing,
+            int textureSize,
+            int textureCount,
+            int normalK,
+            bool optimized,
+            float optimizedVoxelSize,
+            int optimizedDepth,
+            int optimizedMaxPolygons,
+            float optimizedColorRadius,
+            bool optimizedCleanWhitePolygons,
+            int optimizedMinClusterSize,
+            float optimizedMaxTextureDistance,
+            int optimizedMinTextureClusterSize,
+            bool blockRendering);
+bool postExportationNative(const void *object, bool visualize);
+bool writeExportedMeshNative(const void *object, const char * directory, const char * name);
+void initGlContentNative(const void *object);
+void setupGraphicNative(const void *object, int width, int height);
+void onTouchEventNative(const void *object, int touch_count, int event, float x0, float y0, float x1,
+                        float y1);
+void setPausedMappingNative(const void *object, bool paused);
+int renderNative(const void *object);
+bool startCameraNative(const void *object);
+void stopCameraNative(const void *object);
+void setCameraNative(const void *object, int type);
+void postCameraPoseEventNative(const void *object,
+                         float x, float y, float z, float qx, float qy, float qz, float qw);
+void postOdometryEventNative(const void *object,
+                       float x, float y, float z, float qx, float qy, float qz, float qw,
+                       float fx, float fy, float cx, float cy,
+                       double stamp,
+                       const void * yPlane,  const void * uPlane,  const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+                       const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+                       const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
+                       const void * points, int pointsLen, int pointsChannels,
+                       float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw,
+                       float p00, float p11, float p02, float p12, float p22, float p32, float p23,
+                       float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7);
+
+void setOnlineBlendingNative(const void *object, bool enabled);
+void setMapCloudShownNative(const void *object, bool shown);
+void setOdomCloudShownNative(const void *object, bool shown);
+void setMeshRenderingNative(const void *object, bool enabled, bool withTexture);
+void setPointSizeNative(const void *object, float value);
+void setFOVNative(const void *object, float angle);
+void setOrthoCropFactorNative(const void *object, float value);
+void setGridRotationNative(const void *object, float value);
+void setLightingNative(const void *object, bool enabled);
+void setBackfaceCullingNative(const void *object, bool enabled);
+void setWireframeNative(const void *object, bool enabled);
+void setLocalizationModeNative(const void *object, bool enabled);
+void setTrajectoryModeNative(const void *object, bool enabled);
+void setGraphOptimizationNative(const void *object, bool enabled);
+void setNodesFilteringNative(const void *object, bool enabled);
+void setGraphVisibleNative(const void *object, bool visible);
+void setGridVisibleNative(const void *object, bool visible);
+void setRawScanSavedNative(const void *object, bool enabled);
+void setFullResolutionNative(const void *object, bool enabled);
+void setSmoothingNative(const void *object, bool enabled);
+void setAppendModeNative(const void *object, bool enabled);
+void setMaxCloudDepthNative(const void *object, float value);
+void setMinCloudDepthNative(const void *object, float value);
+void setCloudDensityLevelNative(const void *object, int value);
+void setMeshAngleToleranceNative(const void *object, float value);
+void setMeshDecimationFactorNative(const void *object, float value);
+void setMeshTriangleSizeNative(const void *object, int value);
+void setClusterRatioNative(const void *object, float value);
+void setMaxGainRadiusNative(const void *object, float value);
+void setRenderingTextureDecimationNative(const void *object, int value);
+void setBackgroundColorNative(const void *object, float gray);
+void setDepthConfidenceNative(const void *object, int value);
+int setMappingParameterNative(const void *object, const char * key, const char * value);
+
+typedef struct ImageNative
+{
+    const void * objectPtr;
+    void * data;
+    int width;
+    int height;
+    int channels;
+    int totalBytes;
+} ImageNative;
+
+ImageNative getPreviewImageNative(const char * databasePath);
+void releasePreviewImageNative(ImageNative image);
+
+void setGPSNative(const void *object, double stamp, double longitude, double latitude, double altitude, double accuracy, double bearing);
+void addEnvSensorNative(const void *object, int type, float value);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* NativeWrapper_hpp */

app/ios/RTABMapApp/PointCloud.swift

@@ -0,0 +1,16 @@
+//
+//  PointCloud.swift
+//  ThreeDScanner
+//
+//  Created by Steven Roach on 4/3/18.
+//  Copyright © 2018 Steven Roach. All rights reserved.
+//
+
+import Foundation
+import SceneKit
+
+internal struct PointCloud {
+    internal var points: [vector_float3] = []
+    internal var framePointsSizes: [Int32] = []
+    internal var frameViewpoints: [SCNVector3] = []
+}

app/ios/RTABMapApp/PointCloudData.swift

@@ -0,0 +1,16 @@
+//
+//  PointCloudData.swift
+//  ThreeDScanner
+//
+//  Created by Steven Roach on 4/3/18.
+//  Copyright © 2018 Steven Roach. All rights reserved.
+//
+
+import Foundation
+import SceneKit
+
+internal struct PointCloud {
+    internal var points: [vector_float3] = []
+    internal var pointCloudFrameSizes: [Int32] = []
+    internal var pointCloudFrameViewpoints: [SCNVector3] = []
+}

app/ios/RTABMapApp/RTABMap.swift

@@ -0,0 +1,545 @@
+//
+//  RTABMap.swift
+//  RTABMapApp
+//
+//  Created by Mathieu Labbe on 2020-12-31.
+//
+
+import Foundation
+import ARKit
+
+class RTABMap {
+    var native_rtabmap: UnsafeMutableRawPointer
+    
+    struct Observation {
+        weak var observer: RTABMapObserver?
+    }
+    private var observations = [ObjectIdentifier : Observation]()
+    
+    func addObserver(_ observer: RTABMapObserver) {
+        let id = ObjectIdentifier(observer)
+        observations[id] = Observation(observer: observer)
+    }
+
+    func removeObserver(_ observer: RTABMapObserver) {
+        let id = ObjectIdentifier(observer)
+        observations.removeValue(forKey: id)
+    }
+    
+    init() {
+        native_rtabmap = UnsafeMutableRawPointer(mutating: createNativeApplication())
+    }
+    
+    func setupCallbacksWithCPP()
+    {
+        setupCallbacksNative(native_rtabmap,
+             UnsafeMutableRawPointer(Unmanaged.passUnretained(self).toOpaque()),
+             //progressCallback
+             {(observer, count, max) -> Void in
+                         // Extract pointer to `self` from void pointer:
+                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
+                         // Call instance method:
+                         //mySelf.TestMethod();
+                for (id, observation) in mySelf.observations {
+                    // If the observer is no longer in memory, we
+                    // can clean up the observation for its ID
+                    guard let observer = observation.observer else {
+                        mySelf.observations.removeValue(forKey: id)
+                        continue
+                    }
+
+                    observer.progressUpdated(mySelf, count: Int(count), max: Int(max))
+                }
+             },
+             //initCallback
+             {(observer, status, msg) -> Void in
+                         // Extract pointer to `self` from void pointer:
+                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
+                         // Call instance method:
+                         //mySelf.TestMethod();
+                for (id, observation) in mySelf.observations {
+                    // If the observer is no longer in memory, we
+                    // can clean up the observation for its ID
+                    guard let observer = observation.observer else {
+                        mySelf.observations.removeValue(forKey: id)
+                        continue
+                    }
+
+                    let str = String(cString: msg!)
+                    observer.initEventReceived(mySelf, status: Int(status), msg: str)
+                }
+             },
+             //statsUpdatedCallback
+             {(observer, nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, matches, featuresExtracted, hypothesis, nodesDrawn, fps, rejected, rehearsalValue, optimizationMaxError, optimizationMaxErrorRatio, distanceTravelled, fastMovement, landmarkDetected, x, y, z, roll, pitch, yaw) -> Void in
+                         // Extract pointer to `self` from void pointer:
+                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
+                         // Call instance method:
+                         //mySelf.TestMethod();
+                for (id, observation) in mySelf.observations {
+                    // If the observer is no longer in memory, we
+                    // can clean up the observation for its ID
+                    guard let observer = observation.observer else {
+                        mySelf.observations.removeValue(forKey: id)
+                        continue
+                    }
+
+                    observer.statsUpdated(mySelf, nodes: Int(nodes), words: Int(words), points: Int(points), polygons: Int(polygons), updateTime: updateTime, loopClosureId: Int(loopClosureId), highestHypId: Int(highestHypId), databaseMemoryUsed: Int(databaseMemoryUsed), inliers: Int(inliers), matches: Int(matches), featuresExtracted: Int(featuresExtracted), hypothesis: hypothesis, nodesDrawn: Int(nodesDrawn), fps: fps, rejected: Int(rejected), rehearsalValue: rehearsalValue, optimizationMaxError: optimizationMaxError, optimizationMaxErrorRatio: optimizationMaxErrorRatio, distanceTravelled: distanceTravelled, fastMovement: Int(fastMovement), landmarkDetected: Int(landmarkDetected), x: x, y: y, z: z, roll: roll, pitch: pitch, yaw: yaw)
+                }
+             })
+    }
+    
+    deinit {
+        destroyNativeApplication(native_rtabmap)
+    }
+    
+    func initGlContent() {
+        initGlContentNative(native_rtabmap)
+    }
+    
+    func setupGraphic(size: CGSize, orientation: UIInterfaceOrientation) {
+        var rotation: Int32
+        switch orientation.rawValue {
+        case 4:
+            rotation=2
+        case 1:
+            rotation=3
+        case 2:
+            rotation=1
+        default:
+            rotation=0
+        }
+        //NSLog("Orientation: ios %d android %d, view=%dx%d", orientation.rawValue, rotation, Int32(size.width), Int32(size.height))
+        setScreenRotationNative(native_rtabmap, rotation)
+        setupGraphicNative(native_rtabmap, Int32(size.width), Int32(size.height));
+    }
+    
+    func openDatabase(databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
+        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
+            return Int(openDatabaseNative(native_rtabmap, buffer.baseAddress, databaseInMemory, optimize))
+        }
+    }
+    
+    func openDatabase(databaseSource:String, databasePath:String, databaseInMemory:Bool, optimize:Bool) -> Int {
+        databasePath.utf8CString.withUnsafeBufferPointer { buffer -> Int in
+            databaseSource.utf8CString.withUnsafeBufferPointer { bufferSource -> Int in
+                return Int(openDatabase2Native(native_rtabmap, bufferSource.baseAddress, buffer.baseAddress, databaseInMemory, optimize))
+            }
+        }
+    }
+    
+    func save(databasePath:String) {
+        databasePath.utf8CString.withUnsafeBufferPointer { buffer in
+            saveNative(native_rtabmap, databasePath)
+        }
+    }
+    
+    func postProcessing(approach: Int) -> Int {
+        return Int(postProcessingNative(native_rtabmap, Int32(approach)))
+    }
+    
+    func exportMesh(
+        cloudVoxelSize: Float,
+        regenerateCloud: Bool,
+        meshing: Bool,
+        textureSize: Int,
+        textureCount: Int,
+        normalK: Int,
+        optimized: Bool,
+        optimizedVoxelSize: Float,
+        optimizedDepth: Int,
+        optimizedMaxPolygons: Int,
+        optimizedColorRadius: Float,
+        optimizedCleanWhitePolygons: Bool,
+        optimizedMinClusterSize: Int,
+        optimizedMaxTextureDistance: Float,
+        optimizedMinTextureClusterSize: Int,
+        blockRendering: Bool) -> Bool
+    {
+       return exportMeshNative(native_rtabmap,
+                                    cloudVoxelSize,
+                                    regenerateCloud,
+                                    meshing,
+                                    Int32(textureSize),
+                                    Int32(textureCount),
+                                    Int32(normalK),
+                                    optimized,
+                                    optimizedVoxelSize,
+                                    Int32(optimizedDepth),
+                                    Int32(optimizedMaxPolygons),
+                                    optimizedColorRadius,
+                                    optimizedCleanWhitePolygons,
+                                    Int32(optimizedMinClusterSize),
+                                    optimizedMaxTextureDistance,
+                                    Int32(optimizedMinTextureClusterSize),
+                                    blockRendering)
+    }
+    
+    func postExportation(visualize: Bool) -> Bool
+    {
+        return postExportationNative(native_rtabmap, visualize)
+    }
+    
+    func writeExportedMesh(directory: String, name: String) -> Bool
+    {
+        directory.utf8CString.withUnsafeBufferPointer { bufferDir in
+            name.utf8CString.withUnsafeBufferPointer { bufferName in
+                return writeExportedMeshNative(native_rtabmap, bufferDir.baseAddress, bufferName.baseAddress)
+            }
+        }
+    }
+    
+    func onTouchEvent(touch_count: Int, event: Int, x0: Float, y0: Float, x1: Float, y1: Float) {
+        onTouchEventNative(native_rtabmap, Int32(touch_count), Int32(event), x0, y0, x1, y1)
+    }
+    
+    func setPausedMapping(paused: Bool) {
+        setPausedMappingNative(native_rtabmap, paused)
+    }
+    
+    func render() -> Int {
+        return Int(renderNative(native_rtabmap))
+    }
+    
+    func startCamera() -> Bool {
+        return startCameraNative(native_rtabmap)
+    }
+    
+    func stopCamera() {
+        stopCameraNative(native_rtabmap)
+    }
+    
+    func setCamera(type: Int) {
+        setCameraNative(native_rtabmap, Int32(type))
+    }
+    
+    func postCameraPoseEvent(pose: simd_float4x4) {
+        let rotation = GLKMatrix3(
+            m: (pose[0,0], pose[0,1], pose[0,2],
+            pose[1,0], pose[1,1], pose[1,2],
+            pose[2,0], pose[2,1], pose[2,2]))
+        let quat = GLKQuaternionMakeWithMatrix3(rotation)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+    }
+    
+    func notifyLost() {
+        // a null transform will make rtabmap creating a new session
+        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0)
+    }
+    
+    func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
+        let pose = frame.camera.transform   // ViewMatrix
+        let rotation = GLKMatrix3(
+            m: (pose[0,0], pose[0,1], pose[0,2],
+            pose[1,0], pose[1,1], pose[1,2],
+            pose[2,0], pose[2,1], pose[2,2]))
+        
+        let quat = GLKQuaternionMakeWithMatrix3(rotation)
+                
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+                
+        let confMap = frame.sceneDepth?.confidenceMap
+        let depthMap = frame.sceneDepth?.depthMap
+        let points = frame.rawFeaturePoints?.points
+                
+        if points != nil && (depthMap != nil || points!.count>0)
+        {
+            let v = frame.camera.viewMatrix(for: orientation)
+            let p = frame.camera.projectionMatrix(for: orientation, viewportSize: viewport, zNear: 0.5, zFar: 50.0)
+            
+            let rotation = GLKMatrix3(
+                m: (v[0,0], v[0,1], v[0,2],
+                    v[1,0], v[1,1], v[1,2],
+                    v[2,0], v[2,1], v[2,2]))
+            
+            let quatv = GLKQuaternionMakeWithMatrix3(rotation)
+            
+            let texX1 = (1-(2*frame.camera.intrinsics[0,0]/Float(frame.camera.imageResolution.width)) / p[0,0])/2
+            let texY1 = (1-(2*frame.camera.intrinsics[1,1]/Float(frame.camera.imageResolution.height)) / p[1,1])/2
+            
+            let texX2 = (1-(2*frame.camera.intrinsics[0,0]/Float(frame.camera.imageResolution.width)) / p[1,1])/2
+            let texY2 = (1-(2*frame.camera.intrinsics[1,1]/Float(frame.camera.imageResolution.height)) / p[0,0])/2
+            
+            //11 10 01 00 // portrait
+            //01 11 00 10 // right
+            //10 00 11 01 // left
+            //00 01 10 11 // down
+            var texCoord: [Float]
+            switch orientation {
+            case .portrait:
+                texCoord = [1-texX2, 1-texY2, 1-texX2,texY2, texX2, 1-texY2, texX2, texY2]
+            case .landscapeRight:
+                texCoord = [texX1, 1-texY1, 1-texX1, 1-texY1, texX1, texY1, 1-texX1, texY1]
+            case .landscapeLeft:
+                texCoord = [1-texX1, texY1, texX1, texY1, 1-texX1, 1-texY1, texX1, 1-texY1]
+            default: // down
+                texCoord = [texX2, texY2, texX2, 1-texY2, 1-texX2, texY2, 1-texX2, 1-texY2]
+            }
+            
+            frame.rawFeaturePoints?.points.withUnsafeBufferPointer { bufferPoints in
+                
+                CVPixelBufferLockBaseAddress(frame.capturedImage, CVPixelBufferLockFlags.readOnly)
+                var depthDataPtr: UnsafeMutableRawPointer?
+                var depthSize: Int32 = 0
+                var depthWidth: Int32 = 0
+                var depthHeight: Int32 = 0
+                var depthFormat: Int32 = 0
+                if depthMap != nil {
+                    CVPixelBufferLockBaseAddress(depthMap!, CVPixelBufferLockFlags.readOnly)
+                    depthDataPtr = CVPixelBufferGetBaseAddress(depthMap!)!
+                    depthSize = Int32(CVPixelBufferGetDataSize(depthMap!))
+                    depthWidth = Int32(CVPixelBufferGetWidth(depthMap!))
+                    depthHeight = Int32(CVPixelBufferGetHeight(depthMap!))
+                    depthFormat = Int32(CVPixelBufferGetPixelFormatType(depthMap!))
+                }
+                
+                var confDataPtr: UnsafeMutableRawPointer?
+                var confSize: Int32 = 0
+                var confWidth: Int32 = 0
+                var confHeight: Int32 = 0
+                var confFormat: Int32 = 0
+                if confMap != nil {
+                    CVPixelBufferLockBaseAddress(confMap!, CVPixelBufferLockFlags.readOnly)
+                    confDataPtr = CVPixelBufferGetBaseAddress(confMap!)!
+                    confSize = Int32(CVPixelBufferGetDataSize(confMap!))
+                    confWidth = Int32(CVPixelBufferGetWidth(confMap!))
+                    confHeight = Int32(CVPixelBufferGetHeight(confMap!))
+                    confFormat = Int32(CVPixelBufferGetPixelFormatType(confMap!))
+                }
+                
+                if(frame.lightEstimate != nil) {
+                    addEnvSensor(type: 4, value: Float(frame.lightEstimate!.ambientIntensity))
+                }
+                                
+                postOdometryEventNative(native_rtabmap,
+                                        pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w,
+                                        frame.camera.intrinsics[0,0], // fx
+                                        frame.camera.intrinsics[1,1], // fy
+                                        frame.camera.intrinsics[2,0], // cx
+                                        frame.camera.intrinsics[2,1], // cy
+                                        frame.timestamp,
+                                        CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 0),  // y plane pointer
+                                        nil,                                                         // u plane pointer
+                                        CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 1),  // v plane pointer
+                                        Int32(CVPixelBufferGetBytesPerRowOfPlane(frame.capturedImage, 0)) * Int32(CVPixelBufferGetHeightOfPlane(frame.capturedImage, 0)),  // yPlaneLen
+                                        Int32(CVPixelBufferGetWidth(frame.capturedImage)),           // rgb width
+                                        Int32(CVPixelBufferGetHeight(frame.capturedImage)),          // rgb height
+                                        Int32(CVPixelBufferGetPixelFormatType(frame.capturedImage)), // rgb format
+                                        depthDataPtr, // depth pointer
+                                        depthSize,    // depth size
+                                        depthWidth,   // depth width
+                                        depthHeight,  // depth height
+                                        depthFormat,  // depth format
+                                        confDataPtr, // conf pointer
+                                        confSize,    // conf size
+                                        confWidth,   // conf width
+                                        confHeight,  // conf height
+                                        confFormat,  // conf format
+                                        bufferPoints.baseAddress, Int32(frame.rawFeaturePoints!.points.count), 4,
+                                        v[3,0], v[3,1], v[3,2], quatv.x, quatv.y, quatv.z, quatv.w,
+                                        p[0,0], p[1,1], p[2,0], p[2,1], p[2,2], p[2,3], p[3,2],
+                                        texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7])
+                if depthMap != nil {
+                    CVPixelBufferUnlockBaseAddress(depthMap!, CVPixelBufferLockFlags.readOnly)
+                }
+                if confMap != nil {
+                    CVPixelBufferUnlockBaseAddress(confMap!, CVPixelBufferLockFlags.readOnly)
+                }
+                CVPixelBufferUnlockBaseAddress(frame.capturedImage, CVPixelBufferLockFlags.readOnly)
+            }
+        }
+    }
+        
+    // Parameters
+    func setOnlineBlending(enabled: Bool) {
+        setOnlineBlendingNative(native_rtabmap, enabled)
+    }
+    func setMapCloudShown(shown: Bool) {
+        setMapCloudShownNative(native_rtabmap, shown)
+    }
+    func setOdomCloudShown(shown: Bool) {
+        setOdomCloudShownNative(native_rtabmap, shown)
+    }
+    func setMeshRendering(enabled: Bool, withTexture: Bool) {
+        setMeshRenderingNative(native_rtabmap, enabled, withTexture)
+    }
+    func setPointSize(value: Float) {
+        setPointSizeNative(native_rtabmap, value)
+    }
+    func setFOV(angle: Float) {
+        setFOVNative(native_rtabmap, angle)
+    }
+    func setOrthoCropFactor(value: Float) {
+        setOrthoCropFactorNative(native_rtabmap, value)
+    }
+    func setGridRotation(value: Float) {
+        setGridRotationNative(native_rtabmap, value)
+    }
+    func setLighting(enabled: Bool) {
+        setLightingNative(native_rtabmap, enabled)
+    }
+    func setBackfaceCulling(enabled: Bool) {
+        setBackfaceCullingNative(native_rtabmap, enabled)
+    }
+    func setWireframe(enabled: Bool) {
+        setWireframeNative(native_rtabmap, enabled)
+    }
+    func setLocalizationMode(enabled: Bool) {
+        setLocalizationModeNative(native_rtabmap, enabled)
+    }
+    func setTrajectoryMode(enabled: Bool) {
+        setTrajectoryModeNative(native_rtabmap, enabled)
+    }
+    func setGraphOptimization(enabled: Bool) {
+        setGraphOptimizationNative(native_rtabmap, enabled)
+    }
+    func setNodesFiltering(enabled: Bool) {
+        setNodesFilteringNative(native_rtabmap, enabled)
+    }
+    func setGraphVisible(visible: Bool) {
+        setGraphVisibleNative(native_rtabmap, visible)
+    }
+    func setGridVisible(visible: Bool) {
+        setGridVisibleNative(native_rtabmap, visible)
+    }
+    func setRawScanSaved(enabled: Bool) {
+        setRawScanSavedNative(native_rtabmap, enabled)
+    }
+    func setFullResolution(enabled: Bool) {
+        setFullResolutionNative(native_rtabmap, enabled)
+    }
+    func setSmoothing(enabled: Bool) {
+        setSmoothingNative(native_rtabmap, enabled)
+    }
+    func setAppendMode(enabled: Bool) {
+        setAppendModeNative(native_rtabmap, enabled)
+    }
+    func setMaxCloudDepth(value: Float) {
+        setMaxCloudDepthNative(native_rtabmap, value)
+    }
+    func setMinCloudDepth(value: Float) {
+        setMinCloudDepthNative(native_rtabmap, value)
+    }
+    func setCloudDensityLevel(value: Int) {
+        setCloudDensityLevelNative(native_rtabmap, Int32(value))
+    }
+    func setMeshAngleTolerance(value: Float) {
+        setMeshAngleToleranceNative(native_rtabmap, value)
+    }
+    func setMeshDecimationFactor(value: Float) {
+        setMeshDecimationFactorNative(native_rtabmap, value)
+    }
+    func setMeshTriangleSize(value: Int) {
+        setMeshTriangleSizeNative(native_rtabmap, Int32(value))
+    }
+    func setClusterRatio(value: Float) {
+        setClusterRatioNative(native_rtabmap, value)
+    }
+    func setMaxGainRadius(value: Float) {
+        setMaxGainRadiusNative(native_rtabmap, value)
+    }
+    func setRenderingTextureDecimation(value: Int) {
+        setRenderingTextureDecimationNative(native_rtabmap, Int32(value))
+    }
+    func setBackgroundColor(gray: Float) {
+        setBackgroundColorNative(native_rtabmap, gray)
+    }
+    func setDepthConfidence(value: Int) {
+        setDepthConfidenceNative(native_rtabmap, Int32(value))
+    }
+    func setMappingParameter(key: String, value: String) {
+        key.utf8CString.withUnsafeBufferPointer { bufferKey in
+            value.utf8CString.withUnsafeBufferPointer { bufferValue in
+                setMappingParameterNative(native_rtabmap, bufferKey.baseAddress, bufferValue.baseAddress)
+            }
+        }
+    }
+    func setGPS(location: CLLocation)
+    {
+        setGPSNative(native_rtabmap, location.timestamp.timeIntervalSince1970, location.coordinate.longitude, location.coordinate.latitude, location.altitude, location.horizontalAccuracy, location.course < 0.0 ? 0.0 : location.course)
+    }
+    func addEnvSensor(type: Int, value: Float)
+    {
+        addEnvSensorNative(native_rtabmap, Int32(type), value)
+    }
+    func setOrthoCropFactor(_ value: Float)
+    {
+        setOrthoCropFactorNative(native_rtabmap, value)
+    }
+    func setGridRotation(_ value: Float)
+    {
+        setGridRotationNative(native_rtabmap, value)
+    }
+}
+
+func getPreviewImage(databasePath: String) -> UIImage?
+{
+    let imageOut = databasePath.utf8CString.withUnsafeBufferPointer { buffer -> UIImage? in
+        var image = ImageNative()
+        image = getPreviewImageNative(buffer.baseAddress)
+        if let dataPtr = UnsafeRawPointer(image.data)
+        {
+            // copy data in swift
+            let data = Data(bytes: dataPtr, count: Int(image.width*image.height*image.channels))
+            // release memory
+            releasePreviewImageNative(image)
+            // Create bitmap image
+            let bitmap = CIImage(bitmapData: data, bytesPerRow: Int(image.width*image.channels), size: CGSize(width: Int(image.width), height: Int(image.height)), format: CIFormat.BGRA8, colorSpace: nil)
+            return UIImage(ciImage: bitmap)
+        }
+        return nil
+    }
+    return imageOut
+}
+
+protocol RTABMapObserver: class {
+    func progressUpdated(_ rtabmap: RTABMap, count: Int, max: Int)
+    func initEventReceived(_ rtabmap: RTABMap, status: Int, msg: String)
+    func statsUpdated(_ rtabmap: RTABMap,
+                      nodes: Int,
+                      words: Int,
+                      points: Int,
+                      polygons: Int,
+                      updateTime: Float,
+                      loopClosureId: Int,
+                      highestHypId: Int,
+                      databaseMemoryUsed: Int,
+                      inliers: Int,
+                      matches: Int,
+                      featuresExtracted: Int,
+                      hypothesis: Float,
+                      nodesDrawn: Int,
+                      fps: Float,
+                      rejected: Int,
+                      rehearsalValue: Float,
+                      optimizationMaxError: Float,
+                      optimizationMaxErrorRatio: Float,
+                      distanceTravelled: Float,
+                      fastMovement: Int,
+                      landmarkDetected: Int,
+                      x: Float,
+                      y: Float,
+                      z: Float,
+                      roll: Float,
+                      pitch: Float,
+                      yaw: Float)
+}
+
+extension String {
+
+  func toPointer() -> UnsafePointer<UInt8>? {
+    guard let data = self.data(using: String.Encoding.utf8) else { return nil }
+
+    let buffer = UnsafeMutablePointer<UInt8>.allocate(capacity: data.count)
+    let stream = OutputStream(toBuffer: buffer, capacity: data.count)
+
+    stream.open()
+    data.withUnsafeBytes({ (p: UnsafePointer<UInt8>) -> Void in
+      stream.write(p, maxLength: data.count)
+    })
+
+    stream.close()
+
+    return UnsafePointer<UInt8>(buffer)
+  }
+}
+

app/ios/RTABMapApp/RTABMapApp-Bridging-Header.h

@@ -0,0 +1,6 @@
+//
+//  Use this file to import your target's public headers that you would like to expose to Swift.
+//
+
+#include "NativeWrapper.hpp"
+#import <sqlite3.h>

app/ios/RTABMapApp/SceneDelegate.swift

@@ -0,0 +1,52 @@
+//
+//  SceneDelegate.swift
+//  GLKittutorial
+//
+//  Created by Mathieu Labbe on 2020-12-28.
+//
+
+import UIKit
+
+class SceneDelegate: UIResponder, UIWindowSceneDelegate {
+
+    var window: UIWindow?
+
+
+    func scene(_ scene: UIScene, willConnectTo session: UISceneSession, options connectionOptions: UIScene.ConnectionOptions) {
+        // Use this method to optionally configure and attach the UIWindow `window` to the provided UIWindowScene `scene`.
+        // If using a storyboard, the `window` property will automatically be initialized and attached to the scene.
+        // This delegate does not imply the connecting scene or session are new (see `application:configurationForConnectingSceneSession` instead).
+        guard let _ = (scene as? UIWindowScene) else { return }
+    }
+
+    func sceneDidDisconnect(_ scene: UIScene) {
+        // Called as the scene is being released by the system.
+        // This occurs shortly after the scene enters the background, or when its session is discarded.
+        // Release any resources associated with this scene that can be re-created the next time the scene connects.
+        // The scene may re-connect later, as its session was not necessarily discarded (see `application:didDiscardSceneSessions` instead).
+    }
+
+    func sceneDidBecomeActive(_ scene: UIScene) {
+        // Called when the scene has moved from an inactive state to an active state.
+        // Use this method to restart any tasks that were paused (or not yet started) when the scene was inactive.
+    }
+
+    func sceneWillResignActive(_ scene: UIScene) {
+        // Called when the scene will move from an active state to an inactive state.
+        // This may occur due to temporary interruptions (ex. an incoming phone call).
+    }
+
+    func sceneWillEnterForeground(_ scene: UIScene) {
+        // Called as the scene transitions from the background to the foreground.
+        // Use this method to undo the changes made on entering the background.
+    }
+
+    func sceneDidEnterBackground(_ scene: UIScene) {
+        // Called as the scene transitions from the foreground to the background.
+        // Use this method to save data, release shared resources, and store enough scene-specific state information
+        // to restore the scene back to its current state.
+    }
+
+
+}
+

app/ios/RTABMapApp/VerticalScrollerView.swift

@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) 2017 Razeware LLC
+ *
+ * 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.
+ *
+ * Notwithstanding the foregoing, you may not use, copy, modify, merge, publish,
+ * distribute, sublicense, create a derivative work, and/or sell copies of the
+ * Software in any work that is designed, intended, or marketed for pedagogical or
+ * instructional purposes related to programming, coding, application development,
+ * or information technology.  Permission for such use, copying, modification,
+ * merger, publication, distribution, sublicensing, creation of derivative works,
+ * or sale is expressly withheld.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+import UIKit
+
+protocol VerticalViewDataSource: class {
+    // Ask the data source how many views it wants to present inside the horizontal scroller
+    func numberOfViews(in verticalScrollerView: VerticalScrollerView) -> Int
+    // Ask the data source to return the view that should appear at <index>
+    func getScrollerViewItem(_ verticalScrollerView: VerticalScrollerView, viewAt index: Int) -> UIView
+}
+
+protocol VerticalScrollerViewDelegate: class {
+    // inform the delegate that the view at <index> has been selected
+    func verticalScrollerView(_ verticalScrollerView: VerticalScrollerView, didSelectViewAt index: Int)
+}
+
+class VerticalScrollerView: UIView {
+    weak var dataSource: VerticalViewDataSource?
+    weak var delegate: VerticalScrollerViewDelegate?
+    
+    // 1
+    private enum ViewConstants {
+        static let Padding: CGFloat = 5
+        static let Dimensions: CGFloat = 185
+        static let Offset: CGFloat = 10
+    }
+    
+    // 2
+    private let scroller = UIScrollView()
+    
+    // 3
+    private var contentViews = [UIView]()
+    
+    override init(frame: CGRect) {
+        super.init(frame: frame)
+        initializeScrollView()
+    }
+    
+    required init?(coder aDecoder: NSCoder) {
+        super.init(coder: aDecoder)
+        initializeScrollView()
+    }
+    
+    func initializeScrollView() {
+        
+        addSubview(scroller)
+        
+        scroller.translatesAutoresizingMaskIntoConstraints = false
+        
+        NSLayoutConstraint.activate([
+            scroller.leadingAnchor.constraint(equalTo: self.leadingAnchor),
+            scroller.trailingAnchor.constraint(equalTo: self.trailingAnchor),
+            scroller.topAnchor.constraint(equalTo: self.topAnchor),
+            scroller.bottomAnchor.constraint(equalTo: self.bottomAnchor)
+        ])
+        
+        let tapRecognizer = UITapGestureRecognizer(target: self, action: #selector(scrollerTapped(gesture:)))
+        scroller.addGestureRecognizer(tapRecognizer)
+    }
+    
+    func scrollToView(at index: Int, animated: Bool = true) {
+      let centralView = contentViews[index]
+      let targetCenter = centralView.center
+      let targetOffsetY = targetCenter.y - (scroller.bounds.height / 2)
+      scroller.setContentOffset(CGPoint(x: 0, y: targetOffsetY), animated: animated)
+    }
+    
+    @objc func scrollerTapped(gesture: UITapGestureRecognizer) {
+        let location = gesture.location(in: scroller)
+        guard
+            let index = contentViews.index(where: { $0.frame.contains(location)})
+        else { return }
+        
+        delegate?.verticalScrollerView(self, didSelectViewAt: index)
+        scrollToView(at: index)
+    }
+    
+    func view(at index :Int) -> UIView {
+        return contentViews[index]
+    }
+    
+    func reload() {
+        // 1 - Check if there is a data source, if not there is nothing to load.
+        guard let dataSource = dataSource else {
+            return
+        }
+        
+        //2 - Remove the old content views
+        contentViews.forEach { $0.removeFromSuperview() }
+
+        // 3 - yValue is the starting point of each view inside the scroller
+        var yValue = ViewConstants.Offset
+        // 4 - Fetch and add the new views
+        contentViews = (0..<dataSource.numberOfViews(in: self)).map {
+            index in
+            // 5 - add a view at the right position
+            yValue += ViewConstants.Padding
+            let view = dataSource.getScrollerViewItem(self, viewAt: index)
+            view.frame = CGRect(x: ViewConstants.Padding, y: CGFloat(yValue), width: ViewConstants.Dimensions*2, height: ViewConstants.Dimensions)
+            scroller.addSubview(view)
+            yValue += ViewConstants.Dimensions + ViewConstants.Padding
+            // 6 - Store the view so we can reference it later
+            return view
+        }
+
+        // 7
+        scroller.contentSize = CGSize(width: frame.size.width, height: CGFloat(yValue + ViewConstants.Offset))
+    }
+}
+

app/ios/Settings.bundle/Assembling.plist

@@ -0,0 +1,351 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>StringsTable</key>
+	<string>Assembling</string>
+	<key>PreferenceSpecifiers</key>
+	<array>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>If you don&apos;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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Voxel Size</string>
+			<key>Key</key>
+			<string>VoxelSize</string>
+			<key>DefaultValue</key>
+			<real>0.01</real>
+			<key>Titles</key>
+			<array>
+				<string>20 cm</string>
+				<string>10 cm</string>
+				<string>5 cm</string>
+				<string>2 cm</string>
+				<string>1 cm</string>
+				<string>0.5 cm</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0.05</real>
+				<real>0.02</real>
+				<real>0.01</real>
+				<real>0.005</real>
+				<real>0</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>If the map is large, you may want to increase this to maximize the texture resolution.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Texture Size</string>
+			<key>Key</key>
+			<string>TextureSize</string>
+			<key>DefaultValue</key>
+			<integer>4096</integer>
+			<key>Titles</key>
+			<array>
+				<string>16384x16384</string>
+				<string>8192x8192</string>
+				<string>4096x4096</string>
+				<string>2048x2048</string>
+				<string>1024x1024</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>16384</integer>
+				<integer>8192</integer>
+				<integer>4096</integer>
+				<integer>2048</integer>
+				<integer>1024</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Note that all textures are still merged into one for visualization, but exported in multiple files.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Maximum Output Textures</string>
+			<key>Key</key>
+			<string>MaximumOutputTextures</string>
+			<key>DefaultValue</key>
+			<integer>1</integer>
+			<key>Titles</key>
+			<array>
+				<string>8</string>
+				<string>7</string>
+				<string>6</string>
+				<string>5</string>
+				<string>4</string>
+				<string>3</string>
+				<string>2</string>
+				<string>1</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>8</integer>
+				<integer>7</integer>
+				<integer>6</integer>
+				<integer>5</integer>
+				<integer>4</integer>
+				<integer>3</integer>
+				<integer>2</integer>
+				<integer>1</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>K-nearest neighbors used for normal computation when a mesh is created.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Normal K</string>
+			<key>Key</key>
+			<string>NormalK</string>
+			<key>DefaultValue</key>
+			<integer>18</integer>
+			<key>Titles</key>
+			<array>
+				<string>30</string>
+				<string>24</string>
+				<string>18</string>
+				<string>12</string>
+				<string>6</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>30</integer>
+				<integer>24</integer>
+				<integer>18</integer>
+				<integer>12</integer>
+				<integer>6</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Maximum distance from a camera for polygons to be textured by this camera.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Max Texture Distance</string>
+			<key>Key</key>
+			<string>MaxTextureDistance</string>
+			<key>DefaultValue</key>
+			<real>3</real>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>5 m</string>
+				<string>4.5 m</string>
+				<string>4 m</string>
+				<string>3.5 m</string>
+				<string>3 m</string>
+				<string>2.5 m</string>
+				<string>2 m</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>0</real>
+				<real>5</real>
+				<real>4.5</real>
+				<real>4</real>
+				<real>3.5</real>
+				<real>3</real>
+				<real>2.5</real>
+				<real>2</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Minimum polygon cluster size to be textured by a camera. This helps to filter sparse textured polygons.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Min Texture Cluster Size</string>
+			<key>Key</key>
+			<string>MinTextureClusterSize</string>
+			<key>DefaultValue</key>
+			<integer>50</integer>
+			<key>Titles</key>
+			<array>
+				<string>1000</string>
+				<string>500</string>
+				<string>200</string>
+				<string>100</string>
+				<string>50</string>
+				<string>10</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>1000</integer>
+				<integer>500</integer>
+				<integer>200</integer>
+				<integer>100</integer>
+				<integer>50</integer>
+				<integer>10</integer>
+				<integer>0</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>Title</key>
+			<string>Optimized</string>
+			<key>FooterText</key>
+			<string>Lowering this parameter decreases reconstruction time, but geometry precision is lower. Minimum polygon size: map length / 2^depth). &quot;Auto&quot; means that depth is chosen so that polygon size is just under 3 cm.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Reconstruction Depth</string>
+			<key>Key</key>
+			<string>ReconstructionDepth</string>
+			<key>DefaultValue</key>
+			<integer>0</integer>
+			<key>Titles</key>
+			<array>
+				<string>Auto</string>
+				<string>12</string>
+				<string>11</string>
+				<string>10</string>
+				<string>9</string>
+				<string>8</string>
+				<string>7</string>
+				<string>6</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>0</integer>
+				<integer>12</integer>
+				<integer>11</integer>
+				<integer>10</integer>
+				<integer>9</integer>
+				<integer>8</integer>
+				<integer>7</integer>
+				<integer>6</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Color Radius</string>
+			<key>Key</key>
+			<string>ColorRadius</string>
+			<key>DefaultValue</key>
+			<real>0.05</real>
+			<key>Titles</key>
+			<array>
+				<string>Nearest</string>
+				<string>2 m</string>
+				<string>1 m</string>
+				<string>0.5 m</string>
+				<string>0.2 m</string>
+				<string>0.1 m</string>
+				<string>0.05 m</string>
+				<string>0.025 m</string>
+				<string>0.02 m</string>
+				<string>0.015 m</string>
+				<string>0.01 m</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>0</real>
+				<real>2</real>
+				<real>1</real>
+				<real>0.5</real>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0.05</real>
+				<real>0.025</real>
+				<real>0.02</real>
+				<real>0.015</real>
+				<real>0.01</real>
+				<real>-1</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Clean mesh from textureless or colorless reconstructed polygons.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Clean Mesh</string>
+			<key>Key</key>
+			<string>CleanMesh</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>This can be used to filter polygons before texturing.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Polygon Filtering</string>
+			<key>Key</key>
+			<string>PolygonFiltering</string>
+			<key>DefaultValue</key>
+			<integer>0</integer>
+			<key>Titles</key>
+			<array>
+				<string>Only biggest cluster kept</string>
+				<string>Keep all polygons</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>-1</integer>
+				<integer>0</integer>
+			</array>
+		</dict>
+	</array>
+</dict>
+</plist>

app/ios/Settings.bundle/License.plist

@@ -0,0 +1,173 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>StringsTable</key>
+	<string>License</string>
+	<key>PreferenceSpecifiers</key>
+	<array>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>
+======= RTAB-Map =======
+RTAB-Map - https://github.com/introlab/rtabmap
+Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) XXX, contributors, 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 copyright holders nor the names of the 
+  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.
+
+======= OpenCV =======
+Copyright (C) 2000-2021, Intel Corporation, all rights reserved.
+Copyright (C) 2009-2011, Willow Garage Inc., all rights reserved.
+Copyright (C) 2009-2016, NVIDIA Corporation, all rights reserved.
+Copyright (C) 2010-2013, Advanced Micro Devices, Inc., all rights reserved.
+Copyright (C) 2015-2021, OpenCV Foundation, all rights reserved.
+Copyright (C) 2015-2016, Itseez Inc., all rights reserved.
+Copyright (C) 2019-2021, Xperience AI, all rights reserved.
+Third party copyrights are property of their respective owners.
+
+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 names of the copyright holders nor the names of the contributors
+    may be used to endorse or promote products derived from this software
+    without specific prior written permission.
+
+This software is provided by the copyright holders and contributors "as is" and
+any express or implied warranties, including, but not limited to, the implied
+warranties of merchantability and fitness for a particular purpose are disclaimed.
+In no event shall copyright holders or contributors be liable for any direct,
+indirect, incidental, special, exemplary, or consequential damages
+(including, but not limited to, procurement of substitute goods or services;
+loss of use, data, or profits; or business interruption) however caused
+and on any theory of liability, whether in contract, strict liability,
+or tort (including negligence or otherwise) arising in any way out of
+the use of this software, even if advised of the possibility of such damage.
+
+======= Point Cloud Library =======
+Point Cloud Library (PCL) - www.pointclouds.org
+Copyright (c) 2009-2012, Willow Garage, Inc.
+Copyright (c) 2012-, Open Perception, Inc.
+Copyright (c) XXX, respective authors.
+
+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 copyright holder(s) nor the names of its
+   contributors may be used to endorse or promote products derived
+   from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.
+
+======= g2o =======
+g2o - General Graph Optimization
+Copyright (C) 2011 R. Kuemmerle, G. Grisetti, W. Burgard
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are
+met:
+
+* Redistributions of source code must retain the above copyright notice,
+  this list of conditions and the following disclaimer.
+* Redistributions in binary form must reproduce the above copyright
+  notice, this list of conditions and the following disclaimer in the
+  documentation and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
+IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
+TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
+PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
+HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
+TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
+PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
+LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
+NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+======= GTSAM =======
+Copyright (c) 2010, Georgia Tech Research Corporation
+Atlanta, Georgia 30332-0415
+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 copyright holders nor the names of its contributors 
+      may be used to endorse or promote products derived from this software without 
+      specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY 
+EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES 
+OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT 
+SHALL 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.
+
+
+			</string>
+		</dict>
+	</array>
+</dict>
+</plist>

app/ios/Settings.bundle/Mapping.plist

@@ -0,0 +1,761 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>StringsTable</key>
+	<string>Mapping</string>
+	<key>PreferenceSpecifiers</key>
+	<array>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Append Mode</string>
+			<key>Key</key>
+			<string>AppendMode</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Save HD images of the color camera if you want very detailed textures. More memory will be required.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>HD Mode</string>
+			<key>Key</key>
+			<string>HDMode</string>
+			<key>DefaultValue</key>
+			<false/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>When this is disabled (or the phone doesn&apos;t have LiDAR), only tracked features from ARKit will be mapped into a point cloud.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>LiDAR Mode</string>
+			<key>Key</key>
+			<string>LidarMode</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Smooth the point clouds.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Smoothing</string>
+			<key>Key</key>
+			<string>Smoothing</string>
+			<key>DefaultValue</key>
+			<false/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>Title</key>
+			<string>Core</string>
+			<key>FooterText</key>
+			<string>Rate at which a new node is added to map.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Update Rate</string>
+			<key>Key</key>
+			<string>UpdateRate</string>
+			<key>DefaultValue</key>
+			<string>1</string>
+			<key>Titles</key>
+			<array>
+				<string>Max</string>
+				<string>5 Hz</string>
+				<string>4 Hz</string>
+				<string>3 Hz</string>
+				<string>2 Hz</string>
+				<string>1 Hz</string>
+				<string>0.5 Hz</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>5</string>
+				<string>4</string>
+				<string>3</string>
+				<string>2</string>
+				<string>1</string>
+				<string>0.5</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Images taken when the camera is moving too fast are ignored to avoid blurry textures.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Maximum Motion Speed</string>
+			<key>Key</key>
+			<string>MaximumMotionSpeed</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>High</string>
+				<string>Medium</string>
+				<string>Low</string>
+				<string>Very Low</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>0.4</string>
+				<string>0.3</string>
+				<string>0.2</string>
+				<string>0.1</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Time Limit</string>
+			<key>Key</key>
+			<string>TimeLimit</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>1500 ms</string>
+				<string>1400 ms</string>
+				<string>1300 ms</string>
+				<string>1200 ms</string>
+				<string>1100 ms</string>
+				<string>1000 ms</string>
+				<string>900 ms</string>
+				<string>800 ms</string>
+				<string>700 ms</string>
+				<string>600 ms</string>
+				<string>500 ms</string>
+				<string>400 ms</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>1500</string>
+				<string>1400</string>
+				<string>1300</string>
+				<string>1200</string>
+				<string>1100</string>
+				<string>1000</string>
+				<string>900</string>
+				<string>800</string>
+				<string>700</string>
+				<string>600</string>
+				<string>500</string>
+				<string>400</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Maximum nodes kept in working memory.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Memory Limit</string>
+			<key>Key</key>
+			<string>MemoryLimit</string>
+			<key>DefaultValue</key>
+			<string>0</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>500 nodes</string>
+				<string>400 nodes</string>
+				<string>300 nodes</string>
+				<string>200 nodes</string>
+				<string>100 nodes</string>
+				<string>50 nodes</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>500</string>
+				<string>400</string>
+				<string>300</string>
+				<string>200</string>
+				<string>100</string>
+				<string>50</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Proximity detections are loop closures found by comparing current frame with frames inside a radius of the current pose. Unlike global loop closure detection, this approach is dependent of odometry.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Proximity Detection</string>
+			<key>Key</key>
+			<string>ProximityDetection</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Threshold at which loop closure hypotheses are accepted. Higher means more robust to false loop closures while rejecting more good loop closures.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Loop Closure Threshold</string>
+			<key>Key</key>
+			<string>LoopClosureThreshold</string>
+			<key>DefaultValue</key>
+			<real>0.11</real>
+			<key>Titles</key>
+			<array>
+				<string>0.90</string>
+				<string>0.80</string>
+				<string>0.70</string>
+				<string>0.60</string>
+				<string>0.50</string>
+				<string>0.40</string>
+				<string>0.30</string>
+				<string>0.20</string>
+				<string>0.15</string>
+				<string>0.11</string>
+				<string>0.10</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>0.9</real>
+				<real>0.8</real>
+				<real>0.7</real>
+				<real>0.6</real>
+				<real>0.5</real>
+				<real>0.4</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.15</real>
+				<real>0.11</real>
+				<real>0.1</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Threshold at which consecutive images are considered the same, so the corresponding node&apos;s weight is increased. The background turns dark blue when this happens.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Similarity Threshold</string>
+			<key>Key</key>
+			<string>SimilarityThreshold</string>
+			<key>DefaultValue</key>
+			<real>0.3</real>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>0.60</string>
+				<string>0.50</string>
+				<string>0.40</string>
+				<string>0.30</string>
+				<string>0.20</string>
+				<string>0.10</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>1</real>
+				<real>0.6</real>
+				<real>0.5</real>
+				<real>0.4</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.1</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Minimum visual inliers to accept a loop closure.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Min Inliers</string>
+			<key>Key</key>
+			<string>MinInliers</string>
+			<key>DefaultValue</key>
+			<string>25</string>
+			<key>Titles</key>
+			<array>
+				<string>35</string>
+				<string>30</string>
+				<string>25</string>
+				<string>20</string>
+				<string>15</string>
+				<string>10</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>35</string>
+				<string>30</string>
+				<string>25</string>
+				<string>20</string>
+				<string>15</string>
+				<string>10</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Reject any loop closures causing error corrections in the map higher than this factor of the link&apos;s variance.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Max Optimization Error</string>
+			<key>Key</key>
+			<string>MaxOptimizationError</string>
+			<key>DefaultValue</key>
+			<integer>1</integer>
+			<key>Titles</key>
+			<array>
+				<string>4x</string>
+				<string>3.5x</string>
+				<string>3x</string>
+				<string>2.5x</string>
+				<string>2x</string>
+				<string>1.5x</string>
+				<string>1x</string>
+				<string>0.5x</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>4</real>
+				<real>3.5</real>
+				<real>3</real>
+				<real>2.5</real>
+				<real>2</real>
+				<real>1.5</real>
+				<real>1</real>
+				<real>0.5</real>
+				<real>0</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Extracting more features per image would result in better loop closure hypotheses but more processing time is required.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Max Features Extracted (Vocabulary)</string>
+			<key>Key</key>
+			<string>MaxFeaturesExtractedVocabulary</string>
+			<key>DefaultValue</key>
+			<string>400</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>1000</string>
+				<string>900</string>
+				<string>800</string>
+				<string>700</string>
+				<string>600</string>
+				<string>500</string>
+				<string>400</string>
+				<string>300</string>
+				<string>200</string>
+				<string>100</string>
+				<string>Disabled</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>1000</string>
+				<string>900</string>
+				<string>800</string>
+				<string>700</string>
+				<string>600</string>
+				<string>500</string>
+				<string>400</string>
+				<string>300</string>
+				<string>200</string>
+				<string>100</string>
+				<string>-1</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Extracting more features per image would result in better loop closure transforms but more processing time is required.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Max Features Extracted (Loop Closure)</string>
+			<key>Key</key>
+			<string>MaxFeaturesExtractedLoopClosure</string>
+			<key>DefaultValue</key>
+			<string>400</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>1000</string>
+				<string>900</string>
+				<string>800</string>
+				<string>700</string>
+				<string>600</string>
+				<string>500</string>
+				<string>400</string>
+				<string>300</string>
+				<string>200</string>
+				<string>100</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>1000</string>
+				<string>900</string>
+				<string>800</string>
+				<string>700</string>
+				<string>600</string>
+				<string>500</string>
+				<string>400</string>
+				<string>300</string>
+				<string>200</string>
+				<string>100</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map!</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Feature Type</string>
+			<key>Key</key>
+			<string>FeatureType</string>
+			<key>DefaultValue</key>
+			<string>6</string>
+			<key>Titles</key>
+			<array>
+				<string>BRIEF</string>
+				<string>FREAK</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>6</string>
+				<string>5</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Graph optimization approach.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Graph Optimizer</string>
+			<key>Key</key>
+			<string>GraphOptimizer</string>
+			<key>DefaultValue</key>
+			<string>2</string>
+			<key>Titles</key>
+			<array>
+				<string>GTSAM</string>
+				<string>g2o</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>2</string>
+				<string>1</string>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>The map&apos;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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Optimization from Graph End</string>
+			<key>Key</key>
+			<string>OptimizationfromGraphEnd</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>ArUco markers can be detected for localization and graph optimization.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>ArUco Marker Detection</string>
+			<key>Key</key>
+			<string>ArUcoMarkerDetection</string>
+			<key>DefaultValue</key>
+			<integer>-1</integer>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>4X4_50</string>
+				<string>4X4_100</string>
+				<string>4X4_250</string>
+				<string>4X4_1000</string>
+				<string>5X5_50</string>
+				<string>5X5_100</string>
+				<string>5X5_250</string>
+				<string>5X5_1000</string>
+				<string>6X6_50</string>
+				<string>6X6_100</string>
+				<string>6X6_250</string>
+				<string>6X6_1000</string>
+				<string>7X7_50</string>
+				<string>7X7_100</string>
+				<string>7X7_250</string>
+				<string>7X7_1000</string>
+				<string>ARUCO_ORIGINAL</string>
+				<string>APRILTAG_16h5</string>
+				<string>APRILTAG_25h9</string>
+				<string>APRILTAG_36h10</string>
+				<string>APRILTAG_36h11</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<integer>-1</integer>
+				<integer>0</integer>
+				<integer>1</integer>
+				<integer>2</integer>
+				<integer>3</integer>
+				<integer>4</integer>
+				<integer>5</integer>
+				<integer>6</integer>
+				<integer>7</integer>
+				<integer>8</integer>
+				<integer>9</integer>
+				<integer>10</integer>
+				<integer>11</integer>
+				<integer>12</integer>
+				<integer>13</integer>
+				<integer>14</integer>
+				<integer>15</integer>
+				<integer>16</integer>
+				<integer>17</integer>
+				<integer>18</integer>
+				<integer>19</integer>
+				<integer>20</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Marker Depth Error Estimation</string>
+			<key>Key</key>
+			<string>MarkerDepthErrorEstimation</string>
+			<key>DefaultValue</key>
+			<real>0.04</real>
+			<key>Titles</key>
+			<array>
+				<string>1 cm</string>
+				<string>2 cm</string>
+				<string>3 cm</string>
+				<string>4 cm</string>
+				<string>5 cm</string>
+				<string>6 cm</string>
+				<string>7 cm</string>
+				<string>8 cm</string>
+				<string>9 cm</string>
+				<string>10 cm</string>
+				<string>15 cm</string>
+				<string>20 cm</string>
+				<string>30 cm</string>
+				<string>40 cm</string>
+				<string>50 cm</string>
+				<string>100 cm</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<real>0.01</real>
+				<real>0.02</real>
+				<real>0.03</real>
+				<real>0.04</real>
+				<real>0.05</real>
+				<real>0.06</real>
+				<real>0.07000000000000001</real>
+				<real>0.08</real>
+				<real>0.09</real>
+				<real>0.1</real>
+				<real>0.15</real>
+				<real>0.2</real>
+				<real>0.3</real>
+				<real>0.4</real>
+				<real>0.5</real>
+				<real>1</real>
+			</array>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>If marker size is 0, marker size is estimated using depth camera on first detection, then the same size is assumed for next detections, thus all markers should have the same size (markers could then be detected farther from the camera). When the marker size is set &gt;0, all markers are estimated with this parameter (this can be useful on devices without depth camera). Finally, if marker size is &lt;0, the size of each marker is estimated the first time they are detected using the depth camera (thus markers can have different size). Note that if the marker is already in the database with a known size, the size contained in the database is used directly.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSTextFieldSpecifier</string>
+			<key>Title</key>
+			<string>Marker Size (cm)</string>
+			<key>Key</key>
+			<string>MarkerSize</string>
+			<key>DefaultValue</key>
+			<string>-1</string>
+			<key>KeyboardType</key>
+			<string>NumberPad</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>Title</key>
+			<string>Database</string>
+			<key>FooterText</key>
+			<string>Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Save All Frames in Database</string>
+			<key>Key</key>
+			<string>SaveAllFramesInDatabase</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Save raw point clouds in database.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Save Raw Scan</string>
+			<key>Key</key>
+			<string>SaveRawScan</string>
+			<key>DefaultValue</key>
+			<true/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Save GPS to database.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Save GPS</string>
+			<key>Key</key>
+			<string>SaveGPS</string>
+			<key>DefaultValue</key>
+			<false/>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>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>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+			<key>Title</key>
+			<string>Database in Memory</string>
+			<key>Key</key>
+			<string>DatabaseInMemory</string>
+			<key>DefaultValue</key>
+			<false/>
+		</dict>
+	</array>
+</dict>
+</plist>

app/ios/Settings.bundle/Root.plist

@@ -0,0 +1,487 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
+<plist version="1.0">
+<dict>
+	<key>PreferenceSpecifiers</key>
+	<array>
+		<dict>
+			<key>FooterText</key>
+			<string>Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density. Points over the maximum depth, under minimum depth or under the depth confidence are not rendered.</string>
+			<key>Title</key>
+			<string>Rendering</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>2</integer>
+			<key>Key</key>
+			<string>PointCloudDensity</string>
+			<key>Title</key>
+			<string>Point Cloud Density</string>
+			<key>Titles</key>
+			<array>
+				<string>Maximum</string>
+				<string>High</string>
+				<string>Low</string>
+				<string>Very Low</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>0</integer>
+				<integer>1</integer>
+				<integer>2</integer>
+				<integer>3</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>3</integer>
+			<key>Key</key>
+			<string>MaxDepth</string>
+			<key>Title</key>
+			<string>Max Depth</string>
+			<key>Titles</key>
+			<array>
+				<string>No Limit</string>
+				<string>5 m</string>
+				<string>4.5 m</string>
+				<string>4 m</string>
+				<string>3.5 m</string>
+				<string>3 m</string>
+				<string>2.5 m</string>
+				<string>2 m</string>
+				<string>1.5 m</string>
+				<string>1 m</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.0</real>
+				<real>5</real>
+				<real>4.5</real>
+				<real>4</real>
+				<real>3.5</real>
+				<real>3</real>
+				<real>2.5</real>
+				<real>2</real>
+				<real>1.5</real>
+				<real>1</real>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.0</real>
+			<key>Key</key>
+			<string>MinDepth</string>
+			<key>Title</key>
+			<string>Min Depth</string>
+			<key>Titles</key>
+			<array>
+				<string>0 m</string>
+				<string>0.3 m</string>
+				<string>0.5 m</string>
+				<string>0.75 m</string>
+				<string>1 m</string>
+				<string>1.5 m</string>
+				<string>2 m</string>
+				<string>2.5 m</string>
+				<string>3 m</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.0</real>
+				<real>0.29999999999999999</real>
+				<real>0.5</real>
+				<real>0.75</real>
+				<real>1</real>
+				<real>1.5</real>
+				<real>2</real>
+				<real>2.5</real>
+				<real>3</real>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>2</integer>
+			<key>Key</key>
+			<string>DepthConfidence</string>
+			<key>Title</key>
+			<string>Depth Confidence</string>
+			<key>Titles</key>
+			<array>
+				<string>High</string>
+				<string>Medium</string>
+				<string>Low</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>2</integer>
+				<integer>1</integer>
+				<integer>0</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>FooterText</key>
+			<string>Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don't affect post-processing.</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>20</integer>
+			<key>Key</key>
+			<string>MeshAngleTolerance</string>
+			<key>Title</key>
+			<string>Mesh Angle Tolerance (deg)</string>
+			<key>Titles</key>
+			<array>
+				<string>60</string>
+				<string>45</string>
+				<string>35</string>
+				<string>30</string>
+				<string>25</string>
+				<string>20</string>
+				<string>15</string>
+				<string>10</string>
+				<string>5</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>60</integer>
+				<integer>45</integer>
+				<integer>35</integer>
+				<integer>30</integer>
+				<integer>25</integer>
+				<integer>20</integer>
+				<integer>15</integer>
+				<integer>10</integer>
+				<integer>5</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>2</integer>
+			<key>Key</key>
+			<string>MeshTriangleSize</string>
+			<key>Title</key>
+			<string>Mesh Triangle Size</string>
+			<key>Titles</key>
+			<array>
+				<string>6 pix</string>
+				<string>5 pix</string>
+				<string>4 pix</string>
+				<string>3 pix</string>
+				<string>2 pix</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>6</integer>
+				<integer>5</integer>
+				<integer>4</integer>
+				<integer>3</integer>
+				<integer>2</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.0</real>
+			<key>Key</key>
+			<string>MeshDecimationFactor</string>
+			<key>Title</key>
+			<string>Mesh Decimation Factor</string>
+			<key>Titles</key>
+			<array>
+				<string>99%</string>
+				<string>95%</string>
+				<string>90%</string>
+				<string>80%</string>
+				<string>70%</string>
+				<string>60%</string>
+				<string>50%</string>
+				<string>40%</string>
+				<string>30%</string>
+				<string>20%</string>
+				<string>10%</string>
+				<string>Disabled</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.98999999999999999</real>
+				<real>0.94999999999999996</real>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
+				<real>0.5</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.0</real>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>4</integer>
+			<key>Key</key>
+			<string>TextureResolution</string>
+			<key>Title</key>
+			<string>Texture Resolution</string>
+			<key>Titles</key>
+			<array>
+				<string>Maximum</string>
+				<string>High</string>
+				<string>Low</string>
+				<string>Very Low</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>1</integer>
+				<integer>2</integer>
+				<integer>4</integer>
+				<integer>8</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>FooterText</key>
+			<string>Enable Blending to get smoother colors on overlapping surfaces. When Nodes Filetring is enabled, only the newest point clouds of loop closures are rendered.</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.20000000000000001</real>
+			<key>Key</key>
+			<string>BackgroundColor</string>
+			<key>Title</key>
+			<string>Background Color</string>
+			<key>Titles</key>
+			<array>
+				<string>0.9</string>
+				<string>Light Gray</string>
+				<string>0.7</string>
+				<string>0.6</string>
+				<string>Gray</string>
+				<string>0.4</string>
+				<string>0.3</string>
+				<string>Dark Gray</string>
+				<string>0.1</string>
+				<string>Black</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
+				<real>0.5</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.0</real>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<integer>10</integer>
+			<key>Key</key>
+			<string>PointSize</string>
+			<key>Title</key>
+			<string>Point Size</string>
+			<key>Titles</key>
+			<array>
+				<string>50</string>
+				<string>30</string>
+				<string>25</string>
+				<string>15</string>
+				<string>10</string>
+				<string>5</string>
+				<string>1</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<integer>50</integer>
+				<integer>30</integer>
+				<integer>25</integer>
+				<integer>15</integer>
+				<integer>10</integer>
+				<integer>5</integer>
+				<integer>1</integer>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<true/>
+			<key>Key</key>
+			<string>Blending</string>
+			<key>Title</key>
+			<string>Blending</string>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<false/>
+			<key>Key</key>
+			<string>NodesFiltering</string>
+			<key>Title</key>
+			<string>Nodes Filtering</string>
+			<key>Type</key>
+			<string>PSToggleSwitchSpecifier</string>
+		</dict>
+		<dict>
+			<key>Title</key>
+			<string>General</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>File</key>
+			<string>Mapping</string>
+			<key>Title</key>
+			<string>Mapping...</string>
+			<key>Type</key>
+			<string>PSChildPaneSpecifier</string>
+		</dict>
+		<dict>
+			<key>File</key>
+			<string>Assembling</string>
+			<key>Title</key>
+			<string>Assembling...</string>
+			<key>Type</key>
+			<string>PSChildPaneSpecifier</string>
+		</dict>
+		<dict>
+			<key>FooterText</key>
+			<string>Color CorrectionRadius is used to find pixel correspondences for Adjust Colors optimization. Noise Filtering Ratio is used to remove polygon clusters smaller than this ratio of the largest cluster during Noise Filtering optimization.</string>
+			<key>Title</key>
+			<string>Optimization</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.02</real>
+			<key>Key</key>
+			<string>ColorCorrectionRadius</string>
+			<key>Title</key>
+			<string>Color Correction Radius</string>
+			<key>Titles</key>
+			<array>
+				<string>30 cm</string>
+				<string>20 cm</string>
+				<string>10 cm</string>
+				<string>5 cm</string>
+				<string>2.5 cm</string>
+				<string>2 cm</string>
+				<string>1.5 cm</string>
+				<string>1 cm</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.050000000000000003</real>
+				<real>0.025000000000000001</real>
+				<real>0.02</real>
+				<real>0.014999999999999999</real>
+				<real>0.01</real>
+			</array>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<real>0.050000000000000003</real>
+			<key>Key</key>
+			<string>NoiseFilteringRatio</string>
+			<key>Title</key>
+			<string>Noise Filtering Ratio</string>
+			<key>Titles</key>
+			<array>
+				<string>Keep Largest Only</string>
+				<string>0.9</string>
+				<string>0.8</string>
+				<string>0.7</string>
+				<string>0.6</string>
+				<string>0.5</string>
+				<string>0.4</string>
+				<string>0.3</string>
+				<string>0.2</string>
+				<string>0.1</string>
+				<string>0.05</string>
+				<string>0.01</string>
+			</array>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Values</key>
+			<array>
+				<real>1</real>
+				<real>0.90000000000000002</real>
+				<real>0.80000000000000004</real>
+				<real>0.69999999999999996</real>
+				<real>0.59999999999999998</real>
+				<real>0.5</real>
+				<real>0.40000000000000002</real>
+				<real>0.29999999999999999</real>
+				<real>0.20000000000000001</real>
+				<real>0.10000000000000001</real>
+				<real>0.050000000000000003</real>
+				<real>0.01</real>
+			</array>
+		</dict>
+		<dict>
+			<key>FooterText</key>
+			<string>Copyright (c) 2010-2021, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<key>Title</key>
+			<string>About</string>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+		</dict>
+		<dict>
+			<key>DefaultValue</key>
+			<string>0.20.12</string>
+			<key>Key</key>
+			<string>Version</string>
+			<key>Title</key>
+			<string>Version</string>
+			<key>Type</key>
+			<string>PSTitleValueSpecifier</string>
+		</dict>
+		<dict>
+			<key>File</key>
+			<string>License</string>
+			<key>Key</key>
+			<string></string>
+			<key>Title</key>
+			<string>License</string>
+			<key>Type</key>
+			<string>PSChildPaneSpecifier</string>
+		</dict>
+	</array>
+	<key>StringsTable</key>
+	<string>Root</string>
+</dict>
+</plist>

app/src/main.cpp

@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UFile.h"
 #include "rtabmap/utilite/UConversion.h"
 
+#include <vtkObject.h>
+
 #ifdef RTABMAP_PYTHON
 #include "rtabmap/core/PythonInterface.h"
 #endif
@@ -48,10 +50,18 @@ int main(int argc, char* argv[])
 	ULogger::setType(ULogger::kTypeConsole);
 	ULogger::setLevel(ULogger::kWarning);
 
+#ifdef WIN32
+	CoInitialize(nullptr);
+#endif
+
 #ifdef RTABMAP_PYTHON
 	PythonInterface python; // Make sure we initialize python in main thread
 #endif
 
+#if VTK_MAJOR_VERSION >= 8
+	vtkObject::GlobalWarningDisplayOff();
+#endif
+
 	/* Create tasks */
 	QApplication * app = new QApplication(argc, argv);
 	app->setStyleSheet("QMessageBox { messagebox-text-interaction-flags: 5; }"); // selectable message box

cmake_modules/FindG2O.cmake

@@ -1,5 +1,5 @@
 #Pre-requisites: Look for csparse
-FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse
+FIND_PATH(CSPARSE_INCLUDE_DIR NAMES cs.h PATH_SUFFIXES suitesparse csparse EXTERNAL/suitesparse EXTERNAL/csparse g2o/EXTERNAL/suitesparse g2o/EXTERNAL/csparse
   PATHS "C:\\Program Files\\g2o\\include\\EXTERNAL")
 FIND_LIBRARY(CSPARSE_LIBRARY NAMES cxsparse g2o_ext_csparse 
   PATHS "C:\\Program Files\\g2o\\lib")

corelib/include/rtabmap/core/Camera.h

@@ -58,6 +58,7 @@ public:
 	virtual bool isCalibrated() const = 0;
 	virtual std::string getSerial() const = 0;
 	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }
 
 	//getters
 	float getImageRate() const {return _imageRate;}
@@ -72,9 +73,10 @@ protected:
 	/**
 	 * Constructor
 	 *
-	 * @param imageRate : image/second , 0 for fast as the camera can
+	 * @param imageRate the frame rate (Hz), 0 for fast as the camera can
+	 * @param localTransform the transform from base frame to camera frame (without optical rotation)
 	 */
-	Camera(float imageRate = 0, const Transform & localTransform = CameraModel::opticalRotation());
+	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());
 
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded

corelib/include/rtabmap/core/CameraModel.h

@@ -81,7 +81,7 @@ public:
 
 	virtual ~CameraModel() {}
 
-	void initRectificationMap();
+	bool 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;}

corelib/include/rtabmap/core/CameraThread.h

@@ -30,6 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 
 #include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
 #include <rtabmap/utilite/UThread.h>
 #include <rtabmap/utilite/UEventsSender.h>
 
@@ -58,6 +59,21 @@ class RTABMAP_EXP CameraThread :
 public:
 	// ownership transferred
 	CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose
+	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
+	 */
+	CameraThread(Camera * camera,
+			Camera * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			bool odomAsGt = false,
+			const ParametersMap & parameters = ParametersMap());
+	CameraThread(Camera * camera,
+				bool odomAsGt,
+				const ParametersMap & parameters = ParametersMap());
 	virtual ~CameraThread();
 
 	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
@@ -96,8 +112,10 @@ public:
 	//getters
 	bool isPaused() const {return !this->isRunning();}
 	bool isCapturing() const {return this->isRunning();}
+	bool odomProvided() const;
 
 	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
 
 private:
 	virtual void mainLoopBegin();
@@ -106,6 +124,11 @@ private:
 
 private:
 	Camera * _camera;
+	Camera * _odomSensor;
+	Transform _extrinsicsOdomToCamera;
+	bool _odomAsGt;
+	double _poseTimeOffset;
+	float _poseScaleFactor;
 	bool _mirroring;
 	bool _stereoExposureCompensation;
 	bool _colorOnly;

corelib/include/rtabmap/core/DBDriver.h

@@ -173,7 +173,7 @@ public:
 	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 getAllNodeIds(std::set<int> & ids, bool ignoreChildren = false, bool ignoreBadSignatures = false, bool ignoreIntermediateNodes = false) 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;
@@ -276,7 +276,7 @@ protected:
 	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 getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildren, bool ignoreBadSignatures, bool ignoreIntermediateNodes) 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 std::string & fieldName="id") const = 0;
 	virtual void getInvertedIndexNiQuery(int signatureId, int & ni) const = 0;