bump ros package version

* gui: fixed wrongly showing landmark rejected when it was not (because a loop closure was rejected at the same time)

* Added Vis/PnPMaxVariance and RGBD/InvertedReg parameters. Implemented inlier distribution computation for multicam.

* On loc/small displacement: don't remove from odom cache if loop is rejected (maybe first loc)

* Loc: don't prune odom cache on small movement if delayed loc is enabled

* loc/small movement: cleanup bidirectional links

* Cov/PnP: fixed objPt transform to estimate depth

Co-authored-by: mathieu86 <mathieu@robust.ai>

.dockerignore

@@ -0,0 +1 @@
+build/*

.github/workflows/cmake-ros.yml

@@ -0,0 +1,67 @@
+name: CMake-ROS
+
+on:
+  push:
+    branches:
+      - '**'
+  pull_request:
+    branches:
+      - '**'
+
+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}}/build/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/cmake.yml

@@ -2,59 +2,42 @@ name: CMake
 
 on:
   push:
-    branches: [ master ]
+    branches: 
+      - '**'
   pull_request:
-    branches: [ master ]
+    branches: 
+      - '**'
 
 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
+    name: ${{ matrix.os }}
-    # 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]
+        os: [ubuntu-22.04, ubuntu-20.04, ubuntu-18.04]
-        include:
-        - os: ubuntu-20.04
-          ros_distro: 'noetic'
-        - os: ubuntu-18.04
-          ros_distro: 'melodic'
 
-    steps:
+    steps:    
-    - uses: ros-tooling/setup-ros@v0.2
-      with:
-        required-ros-distributions: ${{ matrix.ros_distro }}
-    
     - name: Install dependencies
       run: |
+        DEBIAN_FRONTEND=noninteractive
         sudo apt-get update
-        sudo apt-get -y install ros-${{ matrix.ros_distro }}-rtabmap-ros
+        sudo apt-get -y install libopencv-dev libpcl-dev git cmake software-properties-common
-        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}}/build/bin
       run: |
-        source /opt/ros/${{ matrix.ros_distro }}/setup.bash
         ./rtabmap-console --version
 
 #    - name: Test

.github/workflows/docker.yml

@@ -41,27 +41,35 @@ jobs:
           docker_tags: |
             introlab3it/rtabmap:android23
             introlab3it/rtabmap:tango
+          docker_args: |
+            API_VERSION=23
           docker_platforms: |
             linux/amd64
-          docker_path: 'bionic/android/rtabmap_api23'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android24
           docker_tags: |
             introlab3it/rtabmap:android24
+          docker_args: |
+            API_VERSION=24
           docker_platforms: |
             linux/amd64
-          docker_path: 'bionic/android/rtabmap_api24'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android26
           docker_tags: |
             introlab3it/rtabmap:android26
+          docker_args: |
+            API_VERSION=26
           docker_platforms: |
             linux/amd64
-          docker_path: 'bionic/android/rtabmap_api26'
+          docker_path: 'bionic/android/rtabmap_apiXX'
         - docker_tag: android30
           docker_tags: |
             introlab3it/rtabmap:android30
+          docker_args: |
+            API_VERSION=30
           docker_platforms: |
             linux/amd64
-          docker_path: 'bionic/android/rtabmap_api30'
+          docker_path: 'bionic/android/rtabmap_apiXX'
     
     steps:
       -
@@ -85,11 +93,12 @@ jobs:
         name: Build and push
         uses: docker/build-push-action@v2
         with:
-          context: ./docker/${{ matrix.docker_path }}
+          context: .
           push: true
           platforms: ${{ matrix.docker_platforms }}
+          file: ./docker/${{ matrix.docker_path }}/Dockerfile
           build-args: |
-            CACHE_DATE=${{ github.head_ref }}.${{ github.sha }}
+            ${{ matrix.docker_args }}
           tags: ${{ matrix.docker_tags }}
           cache-from: type=registry,ref=introlab3it/rtabmap:${{ matrix.docker_tag }}
           cache-to: type=inline

CMakeLists.txt

@@ -20,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 18)
+SET(RTABMAP_PATCH_VERSION 21)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -212,6 +212,7 @@ option(WITH_OPENVINS      "Include OpenVINS support"             OFF)
 option(WITH_MADGWICK      "Include Madgwick IMU filtering support" ON)
 option(WITH_FASTCV        "Include FastCV support"               ON)
 option(WITH_OPENMP        "Include OpenMP support"               ON)
+option(WITH_OPENGV        "Include OpenGV support"               OFF)
 IF(MOBILE_BUILD)
 option(PCL_OMP            "With PCL OMP implementations"         OFF)
 ELSE()
@@ -221,7 +222,7 @@ ENDIF()
 set(RTABMAP_QT_VERSION AUTO CACHE STRING "Force a specific Qt version.")
 set_property(CACHE RTABMAP_QT_VERSION PROPERTY STRINGS AUTO 4 5)
 
-FIND_PACKAGE(OpenCV REQUIRED QUIET)
+FIND_PACKAGE(OpenCV REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video OPTIONAL_COMPONENTS aruco xfeatures2d nonfree gpu cudafeatures2d)
 
 IF(WITH_QT)
 FIND_PACKAGE(PCL 1.7 REQUIRED QUIET COMPONENTS common io kdtree search surface filters registration sample_consensus segmentation visualization)
@@ -349,6 +350,13 @@ IF(WITH_QT)
     ENDIF(QT4_FOUND OR Qt5_FOUND)
 ENDIF(WITH_QT)
 
+IF(NOT VTK_FOUND)
+    # Newest PCL versions won't set -DDISABLE_VTK
+    IF(NOT "${PCL_DEFINITIONS}" MATCHES "-DDISABLE_VTK")
+        SET(PCL_DEFINITIONS "${PCL_DEFINITIONS};-DDISABLE_VTK")
+    ENDIF()
+ENDIF(NOT VTK_FOUND)
+
 IF(WITH_TORCH)
     FIND_PACKAGE(Torch QUIET)
     IF(TORCH_FOUND)
@@ -357,7 +365,7 @@ IF(WITH_TORCH)
 ENDIF(WITH_TORCH)
 
 IF(WITH_PYTHON)
-    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development)
+    FIND_PACKAGE(Python3 COMPONENTS Interpreter Development NumPy)
     IF(Python3_FOUND)
         MESSAGE(STATUS "Found Python3")
     ENDIF(Python3_FOUND)
@@ -714,6 +722,13 @@ IF(WITH_FASTCV)
     ENDIF(FastCV_FOUND)
 ENDIF(WITH_FASTCV)
 
+IF(WITH_OPENGV)
+    FIND_PACKAGE(opengv QUIET)
+    IF(opengv_FOUND)
+       MESSAGE(STATUS "Found OpenGV: ${opengv_INCLUDE_DIRS}")
+    ENDIF(opengv_FOUND)
+ENDIF(WITH_OPENGV)
+
 IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
     FIND_PACKAGE(ORB_SLAM QUIET)
     IF(ORB_SLAM_FOUND)
@@ -722,8 +737,8 @@ IF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 ENDIF(WITH_ORB_SLAM AND NOT G2O_FOUND)
 
 IF(NOT MSVC)
-	IF(loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
+	IF((NOT WITH_MSCKF_VIO OR NOT msckf_vio_FOUND) AND (loam_velodyne_FOUND OR floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND))
-	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14
+	 #LOAM, PCL>=1.10, latest g2o and CCCoreLib require c++14, but MSCKF_VIO requires c++11
 	 include(CheckCXXCompilerFlag)
 	  CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
 	  IF(COMPILER_SUPPORTS_CXX14)
@@ -732,7 +747,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 floam_FOUND OR PCL_VERSION VERSION_GREATER "1.9.1" OR TORCH_FOUND OR G2O_FOUND OR CCCoreLib_FOUND OR Open3D_FOUND)
+	ENDIF()
 
 	IF( (NOT (${CMAKE_CXX_STANDARD} STREQUAL "14")) AND (
 	       G2O_FOUND OR
@@ -829,9 +844,9 @@ IF(NOT GTSAM_FOUND)
 ELSE()
    SET(CONF_DEPENDENCIES ${CONF_DEPENDENCIES} ${GTSAM_LIBRARIES})
 ENDIF()
-IF(NOT WITH_CERES OR NOT CERES_FOUND)
+IF(NOT CERES_FOUND)
    SET(CERES "//")
-ENDIF(NOT WITH_CERES OR NOT CERES_FOUND)
+ENDIF(NOT CERES_FOUND)
 IF(NOT WITH_TORO)
    SET(TORO "//")
 ENDIF(NOT WITH_TORO)
@@ -855,6 +870,9 @@ ENDIF(NOT Open3D_FOUND)
 IF(NOT FastCV_FOUND)
    SET(FASTCV "//")
 ENDIF(NOT FastCV_FOUND)
+IF(NOT opengv_FOUND)
+   SET(OPENGV "//")
+ENDIF(NOT opengv_FOUND)
 IF(NOT PDAL_FOUND)
    SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
@@ -1322,8 +1340,12 @@ ELSE()
 MESSAGE(STATUS " *With GTSAM                = NO (GTSAM not found)")
 ENDIF()
 
-IF(WITH_CERES AND CERES_FOUND)
+IF(CERES_FOUND)
+IF(WITH_CERES)
 MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD)")
+ELSE()
+MESSAGE(STATUS " *With Ceres ${Ceres_VERSION}          = YES (License: BSD, WITH_CERES=OFF but it is enabled by okvis or floam dependencies)")
+ENDIF()
 ELSEIF(NOT WITH_CERES)
 MESSAGE(STATUS " *With Ceres                = NO (WITH_CERES=OFF)")
 ELSE()
@@ -1374,6 +1396,14 @@ ELSE()
 MESSAGE(STATUS "  With Open3D               = NO (Open3D not found)")
 ENDIF()
 
+IF(opengv_FOUND)
+MESSAGE(STATUS "  With OpenGV               = YES (License: BSD)")
+ELSEIF(NOT WITH_OPENGV)
+MESSAGE(STATUS "  With OpenGV               = NO (WITH_OPENGV=OFF)")
+ELSE()
+MESSAGE(STATUS "  With OpenGV               = NO (OpenGV not found)")
+ENDIF()
+
 MESSAGE(STATUS "")
 MESSAGE(STATUS " Reconstruction Approaches:")
 IF(octomap_FOUND)

README.md

@@ -5,7 +5,9 @@ rtabmap
 
 [![Release][release-image]][releases]
 [![License][license-image]][license]
-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)
+
+ * Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake.yml) [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.yml/badge.svg)](https://github.com/introlab/rtabmap/actions/workflows/cmake-ros.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.16-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
@@ -15,7 +17,8 @@ Linux: [![Build Status](https://github.com/introlab/rtabmap/actions/workflows/cm
 
 RTAB-Map library and standalone application.
 
-For more information, visit the [RTAB-Map's home page](http://introlab.github.io/rtabmap) or the [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
+ * For more information (e.g., papers, major updates), visit [RTAB-Map's home page](http://introlab.github.io/rtabmap).
+ * For installation instructions and examples, visit [RTAB-Map's wiki](https://github.com/introlab/rtabmap/wiki).
 
 To use RTAB-Map under ROS, visit the [rtabmap](http://wiki.ros.org/rtabmap) page on the ROS wiki.
 

Version.h.in

@@ -54,6 +54,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @CCCORELIB@#define RTABMAP_CCCORELIB
 @OPEN3D@#define RTABMAP_OPEN3D
 @FASTCV@#define RTABMAP_FASTCV
+@OPENGV@#define RTABMAP_OPENGV
 @PDAL@#define RTABMAP_PDAL
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM

app/android/AndroidManifest.xml.in

@@ -8,7 +8,6 @@
     <uses-permission android:name="android.permission.CAMERA" />
     <uses-permission android:name="android.permission.READ_EXTERNAL_STORAGE" />
     <uses-permission android:name="android.permission.WRITE_EXTERNAL_STORAGE" />
-    <uses-permission android:name="android.permission.ACCESS_MEDIA_LOCATION" />
     <uses-permission android:name="android.permission.INTERNET" />
     <uses-permission android:name="android.permission.ACCESS_NETWORK_STATE" />
     <uses-permission android:name="android.permission.ACCESS_FINE_LOCATION" />

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

@@ -125,7 +125,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 	public static final String RTABMAP_TMP_DB = "rtabmap.tmp.db";
 	public static final String RTABMAP_TMP_DIR = "tmp";
 	public static final String RTABMAP_TMP_FILENAME = "map";
-	public static final String RTABMAP_SDCARD_PATH = "/sdcard/";
+	public static final String RTABMAP_SDCARD_PATH = "/Internal storage/";
 	public static final String RTABMAP_EXPORT_DIR = "Export/";
 
 	public static final String RTABMAP_AUTH_TOKEN_KEY = "com.introlab.rtabmap.AUTH_TOKEN";
@@ -303,7 +303,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 
 		public void onServiceDisconnected(ComponentName name) {
-			// Handle this if you need to gracefully shutsaveDatabasedown/retry
+			// Handle this if you need to gracefully shutdown/retry
 			// in the event that Tango itself crashes/gets upgraded while running.
 			mToast.makeText(getApplicationContext(), 
 					String.format("Tango disconnected!"), mToast.LENGTH_LONG).show();
@@ -504,10 +504,28 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		mTotalLoopClosures = 0;
 		mLastFastMovementNotificationStamp = System.currentTimeMillis()/1000;
 		
+		
+		int targetSdkVersion= 0;
+		try {
+	        ApplicationInfo app = this.getPackageManager().getApplicationInfo("com.introlab.rtabmap", 0);        
+	        targetSdkVersion = app.targetSdkVersion;
+	    } catch (NameNotFoundException e) {
+	        e.printStackTrace();
+	    }
+	
 		if(Environment.getExternalStorageState().compareTo(Environment.MEDIA_MOUNTED)==0 && 
-				getActivity().getExternalFilesDirs(null).length >=1)
+			(targetSdkVersion < 30 || getActivity().getExternalFilesDirs(null).length >=1))
 		{
-			File extStore = getActivity().getExternalFilesDirs(null)[0];
+			File extStore;
+			if(targetSdkVersion < 30)
+			{
+				extStore = Environment.getExternalStorageDirectory();
+			}
+			else // >= android30
+			{
+				extStore = getActivity().getExternalFilesDirs(null)[0];
+			}
+			
 			mWorkingDirectory = extStore.getAbsolutePath() + "/" + getString(R.string.app_name) + "/";
 			extStore = new File(mWorkingDirectory);
 			extStore.mkdirs();
@@ -3603,22 +3621,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				File exportDir = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR);
 				exportDir.mkdirs();
 				
-				// cleanup old zip
+				final String pathHuman = mWorkingDirectoryHuman + RTABMAP_EXPORT_DIR + fileName + ".zip";
-				fileNames = Util.loadFileList(mWorkingDirectory + RTABMAP_EXPORT_DIR, false);
-				if(!DISABLE_LOG) Log.i(TAG, String.format("Deleting %d files in \"%s\"", fileNames.length, mWorkingDirectory + RTABMAP_EXPORT_DIR));
-				for(int i=0; i<fileNames.length; ++i)
-				{
-					File f = new File(mWorkingDirectory + RTABMAP_EXPORT_DIR + "/" + fileNames[i]);
-					if(f.delete())
-					{
-						if(!DISABLE_LOG) Log.i(TAG, String.format("Deleted \"%s\"", f.getPath()));
-					}
-					else
-					{
-						if(!DISABLE_LOG) Log.i(TAG, String.format("Failed deleting \"%s\"", f.getPath()));
-					}
-				}
-
 				final String zipOutput = mWorkingDirectory+RTABMAP_EXPORT_DIR+fileName+".zip";
 				if(RTABMapLib.writeExportedMesh(nativeApplication, mWorkingDirectory + RTABMAP_TMP_DIR, RTABMAP_TMP_FILENAME))
 				{							
@@ -3660,41 +3663,30 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							final File f = new File(zipOutput);
 							final int fileSizeMB = (int)f.length()/(1024 * 1024);
 
-							// Save to public Documents/RTAB-Map folder
+							AlertDialog d = new AlertDialog.Builder(getActivity())
-							/*ContentValues values = new ContentValues();
+									.setCancelable(false)
-				            values.put(MediaStore.MediaColumns.DISPLAY_NAME, fileName); //file name                     
+									.setTitle("Mesh Saved!")
-				            values.put(MediaStore.MediaColumns.MIME_TYPE, "application/zip");        //file extension, will automatically add to file
+									.setMessage(String.format("Mesh \"%s\" (%d MB) successfully exported! Share it?", pathHuman, fileSizeMB))
-				            values.put(MediaStore.MediaColumns.RELATIVE_PATH, Environment.DIRECTORY_DOCUMENTS + "/RTAB-Map");     //end "/" is not mandatory
+									.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
-				            Uri uri = getContentResolver().insert(MediaStore.Files.getContentUri("external"),values);
+										public void onClick(DialogInterface dialog, int which) {
-				            if (uri != null) {
+											// Send to...
-				            	OutputStream out;
+											Intent shareIntent = new Intent();
-								try {
+											shareIntent.setAction(Intent.ACTION_SEND);
-									out = getApplicationContext().getContentResolver().openOutputStream(uri);
+											shareIntent.putExtra(Intent.EXTRA_STREAM, FileProvider.getUriForFile(getActivity(), getActivity().getApplicationContext().getPackageName() + ".provider", f));
-									
+											shareIntent.addFlags(Intent.FLAG_GRANT_READ_URI_PERMISSION);
-									InputStream in = new FileInputStream(zipOutput);
+											shareIntent.setType("application/zip");
-					        		byte[] buf = new byte[1024];
+											startActivity(Intent.createChooser(shareIntent, "Sharing..."));
-					        		int len;
-					        		while ((len = in.read(buf)) > 0) {
-					        			out.write(buf, 0, len);
-					        		}
-					        		in.close();
-					        		out.close();
-					        		
-					        		f.delete(); // remove private file
-								} catch (IOException e) {
-									Log.e(TAG, e.getMessage());
-								}
-				            } */
 
-							// Send to...
+											resetNoTouchTimer(true);
-							Intent shareIntent = new Intent();
+										}
-							shareIntent.setAction(Intent.ACTION_SEND);
+									})
-							shareIntent.putExtra(Intent.EXTRA_STREAM, FileProvider.getUriForFile(getActivity(), getActivity().getApplicationContext().getPackageName() + ".provider", f));
+									.setNegativeButton("No", new DialogInterface.OnClickListener() {
-							shareIntent.addFlags(Intent.FLAG_GRANT_READ_URI_PERMISSION);
+										public void onClick(DialogInterface dialog, int which) {
-							shareIntent.setType("application/zip");
+											resetNoTouchTimer(true);
-							startActivity(Intent.createChooser(shareIntent, "Sharing..."));
+										}
-
+									}).create();
-							resetNoTouchTimer(true);
+							d.setCanceledOnTouchOutside(false);
+							d.show();
 						}
 					});
 				}

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

@@ -7,7 +7,11 @@ import java.io.FileInputStream;
 import java.io.FileOutputStream;
 import java.io.FilenameFilter;
 import java.io.IOException;
+import java.util.ArrayList;
 import java.util.Arrays;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
 import java.util.zip.ZipEntry;
 import java.util.zip.ZipOutputStream;
 
@@ -55,7 +59,7 @@ public class Util {
 		}
 	}
 
-	public static String[] loadFileList(String directory, final boolean databasesOnly) {
+	public static String[] loadFileList(final String directory, final boolean databasesOnly) {
 		File path = new File(directory); 
 		String fileList[];
 		try {
@@ -83,6 +87,25 @@ public class Util {
 			};
 			fileList = path.list(filter);
 			Arrays.sort(fileList);
+			List<String> fileListt = new ArrayList<String>(Arrays.asList(fileList));
+			Collections.sort(fileListt, new Comparator<String>() {
+
+		        @Override
+		        public int compare(String filename1, String filename2) {
+		        	File file1 = new File(directory+"/"+filename1);
+		        	File file2 = new File(directory+"/"+filename2);
+		            long k = file1.lastModified() - file2.lastModified();
+		            if(k > 0){
+		               return -1;
+		            }else if(k == 0){
+		               return 0;
+		            }else{
+		              return 1;
+		           }
+		        }
+		    });
+			fileListt.toArray(fileList);
+			
 		}
 		else {
 			fileList = new String[0];

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -982,7 +982,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 13;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				EXCLUDED_ARCHS = "";
@@ -1007,7 +1007,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.17;
+				MARKETING_VERSION = 0.20.19;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1039,7 +1039,7 @@
 				CLANG_USE_OPTIMIZATION_PROFILE = NO;
 				CODE_SIGN_IDENTITY = "Apple Development";
 				CODE_SIGN_STYLE = Automatic;
-				CURRENT_PROJECT_VERSION = 13;
+				CURRENT_PROJECT_VERSION = 1;
 				DEFINES_MODULE = YES;
 				DEVELOPMENT_TEAM = 3RRB6NV8U9;
 				FRAMEWORK_SEARCH_PATHS = (
@@ -1064,7 +1064,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.20.17;
+				MARKETING_VERSION = 0.20.19;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

app/ios/RTABMapApp/ViewController.swift

@@ -115,6 +115,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     @IBOutlet weak var toastLabel: UILabel!
     
     let RTABMAP_TMP_DB = "rtabmap.tmp.db"
+    let RTABMAP_RECOVERY_DB = "rtabmap.tmp.recovery.db"
     let RTABMAP_EXPORT_DIR = "Export"
 
     func getDocumentDirectory() -> URL {
@@ -1437,7 +1438,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         mMapNodes = 0;
         self.openedDatabasePath = nil
-        let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+        let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
         let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
         if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
            FileManager.default.fileExists(atPath: tmpDatabase.path) &&
@@ -1642,7 +1643,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             alert.addAction(yes)
             self.present(alert, animated: true, completion: nil)
             do {
-                let tmpDatabase = self.getTmpDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
+                let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
                 try FileManager.default.removeItem(at: tmpDatabase)
             }
             catch {
@@ -2199,7 +2200,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     }
                     .sorted(by: { $0.1 > $1.1 }) // sort descending modification dates
                     .map { $0.0 } // extract file names
-            databases = data.filter{ $0.pathExtension == "db" }
+            databases = data.filter{ $0.pathExtension == "db" && $0.lastPathComponent != RTABMAP_TMP_DB && $0.lastPathComponent != RTABMAP_RECOVERY_DB }
             
         } catch {
             print("Error while enumerating files : \(error.localizedDescription)")

app/ios/Settings.bundle/Root.plist

@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.20.17</string>
+			<string>0.20.19</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

app/src/main.cpp

@@ -38,10 +38,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include <vtkObject.h>
 
-#ifdef RTABMAP_PYTHON
-#include "rtabmap/core/PythonInterface.h"
-#endif
-
 using namespace rtabmap;
 
 int main(int argc, char* argv[])
@@ -54,10 +50,6 @@ int main(int argc, char* argv[])
 	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

archive/2020-IlluminationInvariant/README.md

@@ -1,22 +0,0 @@
-
-## Multi-Session Visual SLAM for Illumination Invariant Localization in Indoor Environments
-
-* Paper: https://arxiv.org/abs/2103.03827
-
-* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
-![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_overview.jpg)]
-
-
-## Description
-
-This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using Multi-Session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_single_percentage.jpg)]
-
-The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_merged_percentage.jpg)]
-
-
-![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2020-IlluminationInvariant/images/fig_map_merged_999.jpg)]
-

archive/2020-IlluminationInvariant/scripts/export_stats.sh

@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14) #0 1 6 7 8 9 11 12 13 14
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  $REPORT_TOOL --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m "$SKIP/$d/loc"
-  $REPORT_TOOL --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ "$SKIP/$d/consecutive_loc"
-done
-

archive/2020-IlluminationInvariant/scripts/reprocess_maps.sh

@@ -1,42 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-[ ! -d "$SKIP" ] && mkdir $SKIP
-[ ! -d "$SKIP/$TYPE" ] && mkdir $SKIP/$TYPE
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
-DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
-
-PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
-
-if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
-then
-  # binary descriptors
-  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
-else
-  # float descriptors
-  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
-if
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 4 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --uerror $PARAMS $db $SKIP/$TYPE/$db
-  $DETECT_MORE_LOOP_CLOSURE_TOOL --uwarn $SKIP/$TYPE/$db
-done
-

archive/2020-IlluminationInvariant/scripts/reprocess_maps_all.sh

@@ -1,16 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./reprocess_maps.sh $d $SKIP
-  ./run_merge.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_all.sh

@@ -1,13 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-./reprocess_maps_all.sh $SKIP
-./run_merge.sh $SKIP
-./run_localization_single_all.sh $SKIP
-./run_consecutive_localization_all.sh $SKIP
-

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization.sh

@@ -1,31 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
-TARGETS=($SKIP/$TYPE'/map_190321-172717.db;'$SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-175428.db;'$SKIP/$TYPE'/map_190321-182709.db;' $SKIP/$TYPE'/map_190321-182709.db;'$SKIP/$TYPE'/map_190321-185608.db' $SKIP/$TYPE'/map_190321-185608.db;'$SKIP/$TYPE'/map_190321-193556.db' $SKIP/$TYPE'/map_190321-193556.db' )
-
-
-[ ! -d "$SKIP/$TYPE/consecutive_loc" ] && mkdir $SKIP/$TYPE/consecutive_loc
-
-for i in ${!SOURCE[@]}
-do
-    db=${SOURCE[$i]}
-    loc_dbs=${TARGETS[$i]}
-    $REPROCESS_TOOL --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --uwarn "$SKIP/$TYPE/$db;$loc_dbs" $SKIP/$TYPE/consecutive_loc/loc_$db
-done
-

archive/2020-IlluminationInvariant/scripts/run_consecutive_localization_all.sh

@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_consecutive_localization.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_localization_accuracy.sh

@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/accuracy
-
-db=merged_9999.db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
-
-$REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
-
-

archive/2020-IlluminationInvariant/scripts/run_localization_single.sh

@@ -1,32 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-
-# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_9999_reduced.db'
-DATABASES=(  'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_9999.db' 'merged_135.db' 'merged_246.db' 'merged_16.db'  )
-# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
-LOCALIZATION_DATABASES="loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db"
-
-[ ! -d "$SKIP/$TYPE/loc" ] && mkdir $SKIP/$TYPE/loc
-
-echo $PARAMS
-for db in "${DATABASES[@]}"
-do
-  $REPROCESS_TOOL --skip $SKIP --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$SKIP/$TYPE/$db;$LOCALIZATION_DATABASES" $SKIP/$TYPE/loc/loc_$db
-done
-
-

archive/2020-IlluminationInvariant/scripts/run_localization_single_all.sh

@@ -1,15 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 9 11 12 14)
-
-for d in "${DETECTOR[@]}"
-do
-  ./run_localization_single.sh $d $SKIP
-done
-

archive/2020-IlluminationInvariant/scripts/run_merge.sh

@@ -1,38 +0,0 @@
-#!/bin/bash
-
-if [ $# -eq 0 ]
-then
-  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint)."
-  exit
-fi
-TYPE=$1
-
-SKIP=0
-if [ $# -eq 2 ]
-then
-  SKIP=$2
-fi
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPROCESS_TOOL="${PREFIX}rtabmap-reprocess"
-DETECT_MORE_LOOP_CLOSURE_TOOL="${PREFIX}rtabmap-detectMoreLoopClosures"
-
-DATABASES="$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-185608.db;$SKIP/$TYPE/map_190321-193556.db"
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$DATABASES" $SKIP/$TYPE/merged_9999.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999.db
-
-#$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 --Mem/ReduceGraph true --Vis/MinInliers 60 "$DATABASES" $SKIP/$TYPE/merged_9999_reduced.db
-#$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_9999_reduced.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_16.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_16.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-164651.db;$SKIP/$TYPE/map_190321-175428.db;$SKIP/$TYPE/map_190321-185608.db" $SKIP/$TYPE/merged_135.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_135.db
-
-$REPROCESS_TOOL --uwarn --RGBD/OptimizeMaxError 0 "$SKIP/$TYPE/map_190321-172717.db;$SKIP/$TYPE/map_190321-182709.db;$SKIP/$TYPE/map_190321-193556.db" $SKIP/$TYPE/merged_246.db
-$DETECT_MORE_LOOP_CLOSURE_TOOL $SKIP/$TYPE/merged_246.db
-
-
-

archive/2020-IlluminationInvariant/scripts/valgrind.sh

@@ -1,19 +0,0 @@
-#!/bin/bash
-
-SKIP=0
-if [ $# -eq 1 ]
-then
-  SKIP=$1
-fi
-
-DETECTOR=(0 1 6 7 8 9 11 12 14)
-
-PREFIX="/home/mathieu/workspace/rtabmap_cv_latest/bin/"
-REPORT_TOOL="${PREFIX}rtabmap-report"
-
-for d in "${DETECTOR[@]}"
-do
-  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 ${PREFIX}rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${SKIP}/${d}/merged_9999.db;map_190321-164651.db" output.db
-  rm output.db
-done
-

archive/2022-IlluminationInvariant/README.md

@@ -0,0 +1,106 @@
+
+## Multi-Session Visual SLAM for Illumination Invariant Re-Localization in Indoor Environments
+
+* Paper: https://doi.org/10.3389/frobt.2022.801886
+
+* The setup: we did 6 mapping sessions at dusk to evaluate how well RTAB-Map can localize (only by vision) on maps taken at different illumination conditions. The data has been collected with [RTAB-Map Tango](https://play.google.com/store/apps/details?id=com.introlab.rtabmap&hl=en_CA&gl=US).
+![Overview](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_overview.jpg)
+
+
+## Description
+
+This folder contains scripts to re-generate results from the paper. The main idea behind this work is that using multi-session mapping can help to localize visually in illumination changing environments even with features that are not very robust to such conditions. We compared common hand-made visual features like SIFT, SURF, BRIEF, BRISK, FREAK, DAISY, KAZE with learned descriptor SuperPoint. The following picture show how robust are the visual features tested when localizing against single session recorded at different time. For example, the bottom-left and top-right cells are when the robot tries to localize the night on a map taken the day or vice-versa. The diagonal is localization performance when the localization session is about the same time than when the map was recorded. SuperPoint has clearly an advantage on this single-session experiment.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_single_percentage.jpg)
+
+The following image shows when we do the same localization experiment at different hours, but against maps created by assembling maps taken at different hours. In this case, we can see that even binary features like BRIEF can work relatively well in illumination-variant environments. See the paper for more detailled results and comments. The line `1+2+3+4+5+6` refers to the assembled map shown below containing all mapping sessions linked together in same database.
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_merged_percentage.jpg)
+
+
+![All sessions](https://github.com/introlab/rtabmap/raw/master/archive/2022-IlluminationInvariant/images/fig_map_merged_999.jpg)
+
+
+## Dataset
+
+We provide two formats: the first one is more general and the second one is used to produce the results in this paper with RTAB-Map. Please open issue if the links are outdated.
+
+* [Images](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EV8F4PZUxOxLhwAyEehlzKwBjF-9xNuxR32Q4mUjx5u-rA?e=eCJ3TW):
+  * `rgb`: folder containing *.jpg color camera images
+  * `depth`: folder containing *.png 16bits mm depth images
+  * `calib`: folder containing calibration for each color image. Each calibration contains also the transform between `device` and `camera` frames as `local_transform`.
+  * `device_poses.txt`: VIO poses of each image in `device` frame
+  * `camera_poses.txt`: VIO poses of each image in `camera` frame
+* [RTAB-Map Databases](https://usherbrooke-my.sharepoint.com/:u:/g/personal/labm2414_usherbrooke_ca/EU5fb0jEKzlGhPK3OWjMGLUBnDo1BRAoZwtB2czyeVLE_A?e=Y0JyXY)
+
+
+
+## How reproduce results shown in the paper
+
+1. RTAB-Map should be built from source with those dependencies (don't need to "install" it, we will launch it from build directory in the scripts below to avoid conflicting with another rtabmap already installed): 
+    * Use Ubuntu 20.04+ to avoid any python2/python3 conflicts.
+    * OpenCV built with **xfeatures2d** and **nonfree** modules
+    * [torchlib c++](https://pytorch.org/get-started/locally/) (tested on v1.10.2) to enable [SuperPoint](https://github.com/magicleap/SuperPointPretrainedNetwork)
+    * Git clone [SuperGlue](https://github.com/magicleap/SuperGluePretrainedNetwork) into scripts directory.
+    * Generate `superpoint_v1.pt` in the scripts directory (can also be downloaded from [here](https://github.com/KinglittleQ/SuperPoint_SLAM/blob/master/superpoint.pt) but may not be compatible with more recent pytorch versions):
+      ```bash
+      cd rtabmap/archive/2022-IlluminationInvariant/scripts
+      wget https://github.com/magicleap/SuperPointPretrainedNetwork/raw/master/superpoint_v1.pth
+      wget https://raw.githubusercontent.com/magicleap/SuperPointPretrainedNetwork/master/demo_superpoint.py
+      python trace.py
+      ```
+
+2. Download databases of the dataset and extract them.
+3. Adjust the path inside `rtabmap_latest.sh` script to match where you just built rtabmap with right dependencies.
+4. Run `run_all.sh DATABASES_PATH OUTPUT_PATH`, this script will do the following steps (warning, this could take hours to do...):
+    * Recreate the map databases for each feature type
+    * Create the merged databases
+    * Run localization databases over all map/merged databases
+    * Run consecutive localization experiment
+  
+5. Export statistics with `export_stats.sh` script.
+
+6. Use the MatLab/Octave scripts in this folder to show results you want. Set `dataDir` to directory containing the exported statistics. 
+```
+sudo apt install install octave liboctave-dev
+
+# In octave:
+pkg install -forge control signal
+```
+
+### Docker
+
+1. Create the docker image:
+```
+cd rtabmap
+docker build -t rtabmap_frontiers -f docker/frontiers2022/Dockerfile .
+```
+
+2. Assuming you extracted the databases of the dataset in `~/Downloads/Illumination_invariant_databases`, create an output directory for results:
+```
+mkdir ~/Downloads/Illumination_invariant_databases/results
+```
+
+3. Run script:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/scripts \
+    -v ~/Downloads/Illumination_invariant_databases:/workspace/databases \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/run_all.sh /workspace/databases /workspace/results
+```
+
+4. Export statistics:
+```
+docker run --gpus all -it --rm --ipc=host --runtime=nvidia \
+    --env="DISPLAY=$DISPLAY" \
+    --env="QT_X11_NO_MITSHM=1" \
+    --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \
+    --env="XAUTHORITY=$XAUTH" \
+    --volume="$XAUTH:$XAUTH" \
+    --user $(id -u):$(id -g) \
+    -w=/workspace/results \
+    -v ~/Downloads/Illumination_invariant_databases/results:/workspace/results \
+    rtabmap_frontiers /workspace/scripts/export_stats.sh /workspace/results
+```

archive/2022-IlluminationInvariant/results_common.m

@@ -2,24 +2,35 @@
 clear all
 close all
 
+# sudo apt install octave-signal
 pkg load signal
 
-# rtabmap-report --loc 32 Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms . Keypoint/Current_frame/words
+# Use with files generated by export_stats.sh
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in correction#.txt, inliers#.txt and time#.txt where # is the 
-# number of the descriptor used
 
-skipFrameDir = '0';
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
-prefix = 'Stat';
+resultsToShow = 1; % 1=single 2=Consecutive
-RAMaddOverhead = 1;
+
+prefix = 'Stat'; 
+RAMaddOverhead = 0;
 % Inliers_ratio = 'Loop/Visual_inliers/' ./ 'Keypoint/Current_frame/words'
 % Odometry_average = 'Memory/Distance_travelled/m'(2:end) - 'Memory/Distance_travelled/m'(1:end-1)
-statNames = {'Loop/Odom_correction_norm/m', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/'}; % 'Odometry_average'
 
-datasets = [ 0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+statNames = {'Loop/Odom_correction_norm/m', 'Loop/Visual_inliers/', 'Inliers_ratio_%', 'Timing/Total/ms', 'Memory/RAM_usage/MB', 'Memory/RAM_estimated/MB', 'Keypoint/Current_frame/words', 'Loop/Map_id/', 'Memory/Local_graph_size/', 'Keypoint/Dictionary_size/words', 'Loop/Distance_since_last_loc/'}; % 'Odometry_average'
+
+ 
+datasets = [ 0 1 6 7 9 14 11 111 ]; % 0 1 6 7 9 12 14 11
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
 
+if resultsToShow == 2
+  sep = [0, 1000, 3000, 5000, 7000, 9000];
+  sepName = {'17:27', '17:54', '18:27', '18:56', '19:35'};
+  prefix = 'Consecutive';
+  statNames = {'Loop/Distance_since_last_loc/', 'Distance_since_last_loc_under_50cm'};
+endif
+
+MapsN = length(sepName);
+
 allCumResults = {};
 allMaxResults = {};
 
@@ -35,13 +46,15 @@ statName = strrep(statNames{s},'/','-');
 for d=1:length(datasets)
 
 if strcmp(statName,'Inliers_ratio_%')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Visual_inliers-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
-  dataWords = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  dataWords = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Keypoint-Current_frame-words' '.txt'], '\t', 1, 0, "emptyvalue", 0);
   data(:, 2:end) = data(:, 2:end) ./ dataWords(:, 2:end) * 100;
 elseif strcmp(statName, 'Odometry_average')
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Memory-Distance_travelled-m' '.txt'], '\t', 1, 0, "emptyvalue", 0);
+elseif strcmp(statName, 'Distance_since_last_loc_under_50cm')
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' 'Loop-Distance_since_last_loc-' '.txt'], '\t', 1, 0, "emptyvalue", 0);
 else
-  data = dlmread([skipFrameDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+  data = dlmread([dataDir '/' prefix num2str(datasets(d)) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
 endif
 sessions = size(data,2)-1;
 
@@ -69,7 +82,7 @@ for i = 1:sessions
       if datasets(d) == 7
         %% 135 MB overhead for BRISK kernel
         y = y + 135;
-      elseif datasets(d) == 11
+      elseif datasets(d) == 11 || datasets(d) == 111
         %% 645 MB (library cuda) + 800 MB (network) for SuperPoint
         y = y + 645+800;
       elseif datasets(d) == 13 || datasets(d) == 14
@@ -81,6 +94,12 @@ for i = 1:sessions
     if strcmp(statName, 'Loop-Map_id-')
       nonzeros = y;
     end
+    if strcmp(statName, 'Distance_since_last_loc_under_50cm')
+      y(y>0.55) = 0;
+      y(isnan(y)) = 0;
+      y(y>0) = 1;
+      nonzeros = y;
+    end
     if length(nonzeros) > 0
       avgValue = sum(nonzeros)/length(nonzeros);
       avgResultsTmp(i,j) = avgValue;
@@ -103,7 +122,7 @@ for d=1:length(datasets)
   if sum(totalResults{1,d}, 2)
     cumResults(2:end-1,d+1) = sum(absResults{1,d}, 2) ./ sum(totalResults{1,d}, 2);
   endif
-  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:6,1:6).*eye(6,6))) / sum(sum(totalResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumResults(end,d+1) = sum(sum(absResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN))) / sum(sum(totalResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumResults(2:end-1,1) = 1:sessions;
 
@@ -123,7 +142,7 @@ for d=1:length(datasets)
   if sum(totalResults{1,d}, 2)
     cumMaxResults(2:end-1,d+1) = max(maxResults{1,d}, [], 2);
   endif
-  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:6,1:6).*eye(6,6)));
+  cumMaxResults(end,d+1) = max(max(maxResults{1,d}(1:MapsN,1:MapsN).*eye(MapsN,MapsN)));
 end
 cumMaxResults(2:end-1,1) = 1:sessions;
 
@@ -133,3 +152,18 @@ allMaxResults{2,s} = cumMaxResults;
 endfor % statNames
 
 
+if resultsToShow == 2
+  disp('30min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* eye(5,5)) / sum(totalResults{1,d} .*eye(5,5)) * 100)
+  endfor
+  disp('60min') 
+  for d=1:length(datasets)
+    round(sum(absResults{1,d} .* [[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) / sum(totalResults{1,d} .*[[0;0;0;0] eye(4,4) ; [0 0 0 0 0]]) * 100)
+  endfor
+  disp('120min') 
+  for d=1:length(datasets)
+    round(avgResults{1,d}(1,5) * 100)
+  endfor
+endif
+

archive/2022-IlluminationInvariant/results_loc_over_time.m

@@ -4,16 +4,15 @@ clear all
 
 pkg load signal
 
-# rtabmap-report --loc 32 Loop/Map_id/ loc
+# Use with files generated by export_stats.sh
-# Right-click on thr legend of the figure, copy all data to clipboard
-# Paste in data#.txt where # is the number of the descriptor used
 
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
 resultsToShow = 1; % 1=single loc, 2=merged loc, 3=consecutive
-skipFrameDir = '0';
 
 datasetPrefix = 'Stat';
-datasets = [0 1 6 7 9 12 14 11]; % 0 1 6 7 8 9 11 12
+datasets = [0 1 6 7 9 14 11 111]; % 0 1 6 7 8 9 11 12
 datasetsName = {'SURF' 'SIFT' 'ORB' 'FAST/FREAK' 'FAST/BRIEF' 'GFTT/FREAK' 'GFTT/BRIEF' 'BRISK' 'GFTT/ORB' 'KAZE' 'ORB-OCTREE' 'SuperPoint' 'SURF/FREAK' 'GFTT/DAISY' 'SURF/DAISY'};
+datasetsName{112} = 'SuperGlue'
 sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
 sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
 
@@ -41,13 +40,13 @@ globalc = [];
 
 for d=1:length(datasets)
 
-data = dlmread([skipFrameDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
+data = dlmread([dataDir '/' datasetPrefix  num2str(datasets(d)) '-Loop-Map_id-' '.txt'], '\t', 1, 0, "emptyvalue", NaN);
 
 curvesBeg = 2;
-curvesEnd = size(data,2)-4;
+curvesEnd = size(data,2)-5;  % -4 for '0', -5 for '1'
 
 if resultsToShow == 2
-  curvesBeg = 8;
+  curvesBeg = 8; % 2 if only 4 merged_reduced in stats, 8 to skip first 6
   curvesEnd = size(data,2);
 elseif resultsToShow == 3
   curvesEnd = size(data,2);
@@ -198,7 +197,24 @@ for d=1:length(datasets)
   data=percentResults{1,d}*100;
   data(isnan(data)) = 0;
   hAxes = gca;
-  imagesc( hAxes, data, [0, 100])
+  % Upscaling the image to reduce anti-aliasing effect in pfd viewers
+  scale = 50;
+  tickXStep = zeros(1, size(data, 2));
+  tickYStep = zeros(1, size(data, 1));
+  dataUp = upsample(upsample(data',scale)',scale);
+  for x=0:size(data, 2)-1
+    for y=1:scale-1
+       dataUp(:,(x*scale+1)+y) = dataUp(:,x*scale+1);
+    endfor
+    tickXStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  for x=0:size(data, 1)-1
+    for y=1:scale-1
+       dataUp((x*scale+1)+y,:) = dataUp(x*scale+1,:);
+    endfor
+    tickYStep(1,x+1) = scale/2 + scale*x;
+  endfor
+  imagesc( hAxes, dataUp, [0, 100])
   %title({"",datasetsName{datasets(d)+1}})
   colors = [ones(100,1) [1:100]'*0.01 [1:100]'*0];
   colors(1,:) = 1;
@@ -211,12 +227,14 @@ for d=1:length(datasets)
     ylabel("Map")
   endif
   xlabel([datasetsName{datasets(d)+1} " Localization"])
-  set (gca, "xaxislocation", "top");
+  set(gca, "xaxislocation", "top");
+  set(gca, 'XTick', tickXStep)
   set(gca, 'XTickLabel', sepName, 'fontsize',7)
+  set(gca, 'YTick', tickYStep)
   if resultsToShow == 3
     set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56'}, 'fontsize',7)
   elseif resultsToShow == 2
-    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+6', 'bundle', 'reduced'}, 'fontsize',7)
+    set(gca, 'YTickLabel', {'1+6', '1+3+5', '2+4+6', '1+2+3+4+5+6', '1-2-3-4-5-6', 'not set'}, 'fontsize',7)
   else
     set(gca, 'YTickLabel', {'16:46', '17:27', '17:54', '18:27', '18:56', '19:35'}, 'fontsize',7)
   endif
@@ -232,4 +250,4 @@ for d=1:length(datasets)
   endif
 end
 cumResults(2:end-1,1) = 1:curves;
-cumResults
+cumResults

archive/2022-IlluminationInvariant/results_loc_prob_by_dist.m

@@ -0,0 +1,109 @@
+
+clear all
+close all
+
+# sudo apt install octave-signal
+pkg load signal
+
+# Use with files generated by export_stats.sh
+
+dataDir = 'SET_PATH_TO_RESULTS_DIR';
+prefix = 'Stat';
+
+dataset = 6
+sep = [0, 1000, 3000, 5000, 7000, 9000, 12000];
+sepName = {'16:51', '17:31', '17:58', '18:30', '18:59', '19:42'};
+
+statName = strrep('Loop/Distance_since_last_loc/','/','-');
+
+data = dlmread([dataDir '/' prefix num2str(dataset) '-' statName '.txt'], '\t', 1, 0, "emptyvalue", 0);
+
+sessions = size(data,2)-1
+
+j = 1 % session #
+x3 = data(:,1);
+y3 = data(:,11);
+y3 = y3(x3>=sep(j) & x3<=sep(j+1), :);
+x3 = x3(x3>=sep(j) & x3<=sep(j+1), :);
+
+max = 0;
+for i=2:length(y3)
+  if isfinite(y3(i)) && isfinite(y3(i-1)) && (x3(i) - x3(i-1) < 1.5)
+    v = y3(i);
+    if v>max
+      max = v
+    endif
+  endif
+endfor
+xb = max/2:max:5;
+[nn3, xx3] = hist (y3, xb);
+nn3 = nn3/length(x3);
+nn3(nn3==0) = NaN;
+
+for j=1:6
+
+  x{j} = data(:,1);
+  y{j} = data(:,2);
+  y{j} = y{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  x{j} = x{j}(x{j}>=sep(j) & x{j}<=sep(j+1), :);
+  [nn{j}, xx{j}] = hist (y{j}, xb);
+  %nn{j} = nn{j}/length(x{j});
+  %nn{j} = nn{j}/sum(nn{j});
+
+  for k=length(nn{j}):-1:1
+    if nn{j}(k) != 0
+      break;
+    else
+      nn{j}(k) = NaN;
+    endif
+  endfor
+
+endfor
+
+j = 6 % session #
+x4 = data(:,1);
+y4 = data(:,12);
+y4 = y4(x4>=sep(j) & x4<=sep(j+1), :);
+x4 = x4(x4>=sep(j) & x4<=sep(j+1), :);
+[nn4, xx4] = hist (y4, xb);
+nn4 = nn4/length(x4);
+nn4(nn4==0) = NaN;
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x3-x3(1),y3, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1+2+3+4+5+6 -> LocA')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+%plot(x1,y1, '.-');
+%plot(x2-x2(1),y2, '.-');
+plot(x{6}-x{6}(1),y{6}, '.-');
+%plot(x4-x4(1),y4, '.-');
+%legend('Map1 -> LocA', 'Map1 -> LocF', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+legend('Map1 -> LocF')
+xlabel('Time')
+ylabel('m')
+title('Distance since last loc')
+
+figure
+hold on
+for j=1:6
+plot(xx{j},nn{j}, '-', 'linewidth', 3)
+endfor
+%plot(xx3,nn3, '.-', 'linewidth', 3)
+%plot(xx4,nn4, '.-', 'linewidth', 3)
+%legend('Loc-F', 'Loc-E', 'Loc-D', 'Loc-C', 'Loc-B', 'Loc-A')
+legend('A-16:51', 'B-17:31', 'C-17:58', 'D-18:30', 'E-18:59', 'F-19:42', 'Map1+2+3+4+5+6 -> LocF', 'Map1-2-3-4-5-6 -> LocF')
+%h = get(gca,'Children');
+%set(gca,'Children',[h(6) h(5) h(4) h(3) h(2) h(1)])
+%set(gca, 'YScale', 'log')
+xlabel(['Distance not localized (m) Step '  num2str(max) ' m'])
+ylabel('Re-Localization Probability on Map 1 (16:46)')

archive/2022-IlluminationInvariant/scripts/export_stats.sh

@@ -0,0 +1,19 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases are saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111) 
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  rtabmap-report --export --export_prefix "Stat$d" --loc 32  Loop/Odom_correction_norm/m Loop/Visual_inliers/ Timing/Total/ms Timing/Proximity_by_space_visual/ms Timing/Likelihood_computation/ms Timing/Posterior_computation/ms TimingMem/Keypoints_detection/ms TimingMem/Descriptors_extraction/ms TimingMem/Add_new_words/ms Loop/Map_id/ Keypoint/Current_frame/words Memory/RAM_usage/MB Memory/RAM_estimated/MB Memory/Distance_travelled/m Loop/Distance_since_last_loc/ Memory/Local_graph_size/ Keypoint/Dictionary_size/words "$DATA/$d/loc"
+  rtabmap-report --export --export_prefix "Consecutive$d" --loc 32 Loop/Map_id/ Loop/Distance_since_last_loc/ "$DATA/$d/consecutive_loc"
+done
+

archive/2022-IlluminationInvariant/scripts/reprocess_maps.sh

@@ -0,0 +1,41 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+[ ! -d "$OUTPUT" ] && mkdir $OUTPUT
+[ ! -d "$OUTPUT/$TYPE" ] && mkdir $OUTPUT/$TYPE
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db'
+DATABASES=( 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' )
+
+PARAMS="--Kp/DetectorStrategy $TYPE --Vis/FeatureType $TYPE"
+
+if [ $TYPE -eq 2 ] || [ $TYPE -eq 3 ] || [ $TYPE -eq 4 ] || [ $TYPE -eq 5 ] || [ $TYPE -eq 6 ] || [ $TYPE -eq 7 ] || [ $TYPE -eq 8 ] || [ $TYPE -eq 10 ] || [ $TYPE -eq 12 ]
+then
+  # binary descriptors
+  PARAMS="--Vis/CorNNDR 0.8 $PARAMS"
+else
+  # float descriptors
+  PARAMS="--Vis/CorNNDR 0.6 $PARAMS"
+fi
+
+if [ $TYPE -eq 111 ] 
+then
+  PARAMS="--Vis/CorNNType 6 --SuperGlue/Path SuperGluePretrainedNetwork/rtabmap_superglue.py --Reg/RepeatOnce false --Vis/CorGuessWinSize 0 $PARAMS --Kp/DetectorStrategy 11 --Vis/FeatureType 11"
+fi
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess --RGBD/MarkerDetection false --RGBD/ProximityBySpace true --RGBD/LocalRadius 1 --Mem/InitWMWithAllNodes true --Rtabmap/TimeThr 0 --Mem/UseOdomFeatures false --Optimizer/GravitySigma 0.1 --Mem/UseOdomGravity true --RGBD/OptimizeFromGraphEnd false --Mem/DepthAsMask false --RGBD/OptimizeMaxError 0 --RGBD/ProximityOdomGuess false --Vis/MaxFeatures 1000 --Kp/MaxFeatures 400 --Vis/EpipolarGeometryVar 0.1 --Vis/EstimationType 1 --Vis/MinInliers 20 --Rtabmap/MaxRetrieved 2 --Optimizer/Iterations 20 --Mem/CompressionParallelized true --Kp/Parallelized true --Kp/MaxDepth 0 --Kp/BadSignRatio 0.2 --BRIEF/Bytes 32 --Kp/ByteToFloat true --SURF/HessianThreshold 100 --SIFT/ContrastThreshold 0.02 --BRISK/Thresh 10 --SuperPoint/ModelPath superpoint_v1.pt --Rtabmap/PublishRAMUsage true --ORB/EdgeThreshold 19 --ORB/ScaleFactor 2 --ORB/NLevels 3 --Db/TargetVersion "" --Icp/CorrespondenceRatio 0.1 --RGBD/MaxOdomCacheSize 0 --uwarn $PARAMS $INPUT/$db $OUTPUT/$TYPE/$db
+  rtabmap-detectMoreLoopClosures --uwarn $OUTPUT/$TYPE/$db
+done
+

archive/2022-IlluminationInvariant/scripts/reprocess_maps_all.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./reprocess_maps.sh $d $INPUT $OUTPUT
+  ./run_merge.sh $d $OUTPUT
+done
+

archive/2022-IlluminationInvariant/scripts/rtabmap_latest.bash

@@ -0,0 +1,5 @@
+#!/bin/bash
+
+export PATH=~/workspace/rtabmap/build/bin:$PATH
+export LD_LIBRARY_PATH=~/workspace/rtabmap/build/lib:$LD_LIBRARY_PATH
+

archive/2022-IlluminationInvariant/scripts/run_all.sh

@@ -0,0 +1,14 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+./reprocess_maps_all.sh $INPUT $OUTPUT
+./run_localization_single_all.sh $INPUT $OUTPUT
+./run_consecutive_localization_all.sh $OUTPUT
+

archive/2022-IlluminationInvariant/scripts/run_consecutive_localization.sh

@@ -0,0 +1,25 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+source rtabmap_latest.bash
+
+SOURCE=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db')
+TARGETS=($DATA/$TYPE'/map_190321-172717.db;'$DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-175428.db;'$DATA/$TYPE'/map_190321-182709.db;' $DATA/$TYPE'/map_190321-182709.db;'$DATA/$TYPE'/map_190321-185608.db' $DATA/$TYPE'/map_190321-185608.db;'$DATA/$TYPE'/map_190321-193556.db' $DATA/$TYPE'/map_190321-193556.db' )
+
+
+[ ! -d "$DATA/$TYPE/consecutive_loc" ] && mkdir $DATA/$TYPE/consecutive_loc
+
+for i in ${!SOURCE[@]}
+do
+    db=${SOURCE[$i]}
+    loc_dbs=${TARGETS[$i]}
+    rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$DATA/$TYPE/$db;$loc_dbs" $DATA/$TYPE/consecutive_loc/loc_$db
+done
+

archive/2022-IlluminationInvariant/scripts/run_consecutive_localization_all.sh

@@ -0,0 +1,16 @@
+#!/bin/bash
+
+if [ $# -lt 1 ]
+then
+  echo "No arguments supplied. It should be the data directory (where the reprocessed map databases will be saved)."
+  exit
+fi
+DATA=$1
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_consecutive_localization.sh $d $DATA
+done
+

archive/2022-IlluminationInvariant/scripts/run_localization_accuracy.sh

@@ -0,0 +1,33 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+source rtabmap_latest.bash
+
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/accuracy
+
+db=merged_123456.db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOn_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --Reg/RepeatOnce false --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOff_DoubleRegOff_BaOff_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 1 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOn_$db
+
+rtabmap-reprocess --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess true --Reg/RepeatOnce true --Vis/BundleAdjustment 0 --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/accuracy/ProxOn_DoubleRegOn_BaOff_$db
+
+

archive/2022-IlluminationInvariant/scripts/run_localization_single.sh

@@ -0,0 +1,28 @@
+#!/bin/bash
+
+if [ $# -lt 3 ]
+then
+  echo "No arguments supplied. They should be 3: the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint), the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+TYPE=$1
+INPUT=$2
+OUTPUT=$3
+
+
+source rtabmap_latest.bash
+
+# 'map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db'
+DATABASES=('map_190321-164651.db' 'map_190321-172717.db' 'map_190321-175428.db' 'map_190321-182709.db' 'map_190321-185608.db' 'map_190321-193556.db' 'merged_123456.db' 'merged_135.db' 'merged_246.db' 'merged_16.db' 'merged_123456_reduced.db')
+# loc_190321-165128.db;loc_190321-173134.db;loc_190321-175823.db;loc_190321-183051.db;loc_190321-185950.db;loc_190321-194226.db
+LOCALIZATION_DATABASES="$INPUT/loc_190321-165128.db;$INPUT/loc_190321-173134.db;$INPUT/loc_190321-175823.db;$INPUT/loc_190321-183051.db;$INPUT/loc_190321-185950.db;$INPUT/loc_190321-194226.db"
+
+[ ! -d "$OUTPUT/$TYPE/loc" ] && mkdir $OUTPUT/$TYPE/loc
+
+echo $PARAMS
+for db in "${DATABASES[@]}"
+do
+  rtabmap-reprocess -loc_null --Mem/IncrementalMemory false --RGBD/ProximityBySpace true --RGBD/ProximityMaxPaths 1 --Mem/LocalizationDataSaved true --Mem/BinDataKept false --RGBD/SavedLocalizationIgnored true --Kp/IncrementalFlann false --Vis/MinInliers 20 --Rtabmap/PublishRAMUsage true --RGBD/ProximityOdomGuess false --uwarn "$OUTPUT/$TYPE/$db;$LOCALIZATION_DATABASES" $OUTPUT/$TYPE/loc/loc_$db
+done
+
+

archive/2022-IlluminationInvariant/scripts/run_localization_single_all.sh

@@ -0,0 +1,17 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+for d in "${DETECTOR[@]}"
+do
+  ./run_localization_single.sh $d $INPUT $OUTPUT
+done
+

archive/2022-IlluminationInvariant/scripts/run_merge.sh

@@ -0,0 +1,34 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. It should be the detector number type (0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint) and the data directory (where map databases have been reprocessed)."
+  exit
+fi
+TYPE=$1
+DATA=$2
+
+MIN_INLIERS=20 #20 40 60 80
+
+source rtabmap_latest.bash
+
+DATABASES="$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-185608.db;$DATA/$TYPE/map_190321-193556.db"
+
+# To compute "Ground truth"
+rtabmap-reprocess --uwarn "$DATABASES" $DATA/$TYPE/merged_123456.db
+
+cp $DATA/$TYPE/merged_123456.db $DATA/$TYPE/merged_123456_gt.db
+rtabmap-detectMoreLoopClosures -r 0.5 -i 5 $DATA/$TYPE/merged_123456_gt.db
+
+rtabmap-reprocess --uwarn -gt $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_16.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-164651.db;$DATA/$TYPE/map_190321-175428.db;$DATA/$TYPE/map_190321-185608.db" $DATA/$TYPE/merged_135.db
+
+rtabmap-reprocess --uwarn "$DATA/$TYPE/map_190321-172717.db;$DATA/$TYPE/map_190321-182709.db;$DATA/$TYPE/map_190321-193556.db" $DATA/$TYPE/merged_246.db
+
+# Reduced graph
+rtabmap-reprocess --uwarn -gt --Mem/ReduceGraph true --Vis/MinInliers $MIN_INLIERS $DATA/$TYPE/merged_123456_gt.db $DATA/$TYPE/merged_123456_reduced.db
+
+

archive/2022-IlluminationInvariant/scripts/trace.py

@@ -0,0 +1,9 @@
+import torch
+import torchvision
+from demo_superpoint  import SuperPointNet
+model = SuperPointNet()
+model.load_state_dict(torch.load("superpoint_v1.pth"))
+model.eval()
+example = torch.rand(1, 1, 640, 480)
+traced_script_module = torch.jit.trace(model, example)
+traced_script_module.save("superpoint_v1.pt")

archive/2022-IlluminationInvariant/scripts/valgrind.sh

@@ -0,0 +1,20 @@
+#!/bin/bash
+
+if [ $# -lt 2 ]
+then
+  echo "No arguments supplied. They should be 2: the input directory (original maps) and the output data directory (where reprocessed map databases will be saved)."
+  exit
+fi
+INPUT=$1
+OUTPUT=$2
+
+DETECTOR=(0 1 6 7 9 14 11 111)
+
+source rtabmap_latest.bash
+
+for d in "${DETECTOR[@]}"
+do
+  valgrind --tool=massif --time-unit=ms --detailed-freq=1 --max-snapshots=100 rtabmap-reprocess --Mem/IncrementalMemory false --Kp/IncrementalFlann false  "${OUTPUT}/${d}/map_190321-164651.db;${INPUT}/loc_190321-165128.db" output.db
+  rm output.db
+done
+

archive/2022-IlluminationInvariant/showLoopOverTime.m

@@ -0,0 +1,108 @@
+##close all
+##clear all
+
+%% Use Export Poses in TORO format, then copy columns
+load vertexes.txt;
+load edges.txt;
+
+set(0,'defaultAxesFontName', 'Times')
+set(0,'defaultTextFontName', 'Times')
+
+%matlab indexes  % rtabmap indexes
+endMap1 = 201; % ID=206 
+endMap2 = 401; % ID=411 
+endMap3 = 604; % ID=621  
+endMap4 = 794; % ID=814
+endMap5 = 968; % ID=990
+endMap6 = 1201; % ID=1230
+
+%% 3D
+
+t = vertexes(:,1);
+
+##figure
+##plot3(vertexes(1:endMap1,2), vertexes(1:endMap1,3), vertexes(1:endMap1,1))
+##hold on
+##plot3(vertexes(endMap1+1:endMap2,2), vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1))
+##plot3(vertexes(endMap2+1:endMap3,2), vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1))
+##plot3(vertexes(endMap3+1:endMap4,2), vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1))
+##plot3(vertexes(endMap4+1:endMap5,2), vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1))
+##plot3(vertexes(endMap5+1:end,2), vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1))
+
+mapIds = zeros(vertexes(end,1), 2); % matlab index to vertexes, map id
+for i=1:size(vertexes,1)
+    mapIds(vertexes(i,1),1) = i;
+    if i <= endMap1
+        mapIds(vertexes(i,1),2) = 1;
+    elseif i<=endMap2
+        mapIds(vertexes(i,1),2) = 2;
+    elseif i<=endMap3
+        mapIds(vertexes(i,1),2) = 3;
+    elseif i<=endMap4
+        mapIds(vertexes(i,1),2) = 4;
+    elseif i<=endMap5
+        mapIds(vertexes(i,1),2) = 5;
+    else
+        mapIds(vertexes(i,1),2) = 6;
+    end
+        
+end
+
+##interLoopClosures = 0;
+##intraLoopClosures = 0;
+##
+##for i=1:size(edges, 1)
+##    if edges(i,2) > edges(i,1)+1
+##        x = [vertexes(mapIds(edges(i,1),1), 2) vertexes(mapIds(edges(i,2),1), 2)];
+##        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+##        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+##        if mapIds(edges(i,1),2) ~= mapIds(edges(i,2),2)
+##            plot3(x,y,t, 'g')
+##            interLoopClosures = interLoopClosures+1;
+##        else
+##            plot3(x,y,t, 'r')
+##            intraLoopClosures = intraLoopClosures + 1;
+##        end
+##    end
+##end
+##xlabel('x')
+##ylabel('y')
+##zlabel('Node indexes')
+##
+##interLoopClosures
+##intraLoopClosures
+
+%% 2D
+figure
+hold on
+plot([-8 6], [vertexes(endMap1,1) vertexes(endMap1,1)], 'k:')
+plot([-8 6], [vertexes(endMap2,1) vertexes(endMap2,1)], 'k:')
+plot([-8 6], [vertexes(endMap3,1) vertexes(endMap3,1)], 'k:')
+plot([-8 6], [vertexes(endMap4,1) vertexes(endMap4,1)], 'k:')
+plot([-8 6], [vertexes(endMap5,1) vertexes(endMap5,1)], 'k:')
+
+colors = {'r:', 'g:', 'c:', 'y:', 'm:', 'c'};
+
+for i=1:size(edges, 1)
+    if edges(i,2) > edges(i,1)+1
+        y = [vertexes(mapIds(edges(i,1),1), 3) vertexes(mapIds(edges(i,2),1), 3)];
+        t = [vertexes(mapIds(edges(i,1),1), 1) vertexes(mapIds(edges(i,2),1), 1)];
+        mapId = mapIds(edges(i,1),2);
+        if mapId ~= mapIds(edges(i,2),2) && (mapId == 1 || mapIds(edges(i,2),2) == 1)
+            plot(y,t, 'r')
+        else
+            %plot(y,t, 'r')
+        end
+    end
+end
+
+curveColor = 'b'
+plot(vertexes(1:endMap1,3), vertexes(1:endMap1,1), curveColor)
+plot(vertexes(endMap1+1:endMap2,3), vertexes(endMap1+1:endMap2,1), curveColor)
+plot(vertexes(endMap2+1:endMap3,3), vertexes(endMap2+1:endMap3,1), curveColor)
+plot(vertexes(endMap3+1:endMap4,3), vertexes(endMap3+1:endMap4,1), curveColor)
+plot(vertexes(endMap4+1:endMap5,3), vertexes(endMap4+1:endMap5,1), curveColor)
+plot(vertexes(endMap5+1:end,3), vertexes(endMap5+1:end,1), 'k')
+
+xlabel('y')
+ylabel('Node indexes')

corelib/include/rtabmap/core/DBDriver.h

@@ -167,7 +167,7 @@ public:
 	void loadNodeData(Signature * signature, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void loadNodeData(std::list<Signature *> & signatures, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
 	void getNodeData(int signatureId, SensorData & data, bool images = true, bool scan = true, bool userData = true, bool occupancyGrid = true) const;
-	bool getCalibration(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	bool getCalibration(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	bool getLaserScanInfo(int signatureId, LaserScan & info) const;
 	bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	void loadLinks(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
@@ -272,7 +272,7 @@ protected:
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const = 0;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const = 0;
-	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const = 0;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const = 0;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const = 0;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const = 0;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -137,7 +137,7 @@ protected:
 	virtual void loadLinksQuery(int signatureId, std::multimap<int, Link> & links, Link::Type type = Link::kUndef) const;
 
 	virtual void loadNodeDataQuery(std::list<Signature *> & signatures, bool images=true, bool scan=true, bool userData=true, bool occupancyGrid=true) const;
-	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, StereoCameraModel & stereoModel) const;
+	virtual bool getCalibrationQuery(int signatureId, std::vector<CameraModel> & models, std::vector<StereoCameraModel> & stereoModels) const;
 	virtual bool getLaserScanInfoQuery(int signatureId, LaserScan & info) const;
 	virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose, std::vector<float> & velocity, GPS & gps, EnvSensors & sensors) const;
 	virtual void getLastNodeIdsQuery(std::set<int> & ids) const;

corelib/include/rtabmap/core/DBReader.h

@@ -54,7 +54,8 @@ public:
 			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
-			 bool landmarksIgnored = false);
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false);
 	DBReader(const std::list<std::string> & databasePaths,
 			 float frameRate = 0.0f, // -1 = use Database stamps, 0 = inf
 			 bool odometryIgnored = false,
@@ -64,7 +65,8 @@ public:
 			 int cameraIndex = -1,
 			 int stopId = 0,
 			 bool intermediateNodesIgnored = false,
-			 bool landmarksIgnored = false);
+			 bool landmarksIgnored = false,
+			 bool featuresIgnored = false);
 	virtual ~DBReader();
 
 	virtual bool init(
@@ -91,6 +93,7 @@ private:
 	int _cameraIndex;
 	bool _intermediateNodesIgnored;
 	bool _landmarksIgnored;
+	bool _featuresIgnored;
 
 	DBDriver * _dbDriver;
 	UTimer _timer;

corelib/include/rtabmap/core/Graph.h

@@ -159,14 +159,14 @@ std::list<Link> RTABMAP_EXP findLinks(
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
  */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
 		Link::Type filteredType,
 		bool inverted = false);
 /**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
+ * Return links not of type "filteredType". If inverted=true, return links of type "filteredType".
  */
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,

corelib/include/rtabmap/core/MarkerDetector.h

@@ -65,16 +65,22 @@ public:
 
     RTABMAP_DEPRECATED(
     MapIdPose detect(const cv::Mat & image,
-                                    const CameraModel & model,
+			const CameraModel & model,
-                                    const cv::Mat & depth = cv::Mat(),
+			const cv::Mat & depth = cv::Mat(),
-                                    float * estimatedMarkerLength = 0,
+			float * estimatedMarkerLength = 0,
-                                    cv::Mat * imageWithDetections = 0), "Use the other constructor, in which the returned map contains the length of each marker detected.");
+			cv::Mat * imageWithDetections = 0), "Use the other detect(), in which the returned map contains the length of each marker detected.");
 
     std::map<int, MarkerInfo> detect(const cv::Mat & image,
-                               const CameraModel & model,
+		    const std::vector<CameraModel> & models,
-                               const cv::Mat & depth = cv::Mat(),
+		    const cv::Mat & depth = cv::Mat(),
-                               const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
-                               cv::Mat * imageWithDetections = 0);
+		    cv::Mat * imageWithDetections = 0);
+
+    std::map<int, MarkerInfo> detect(const cv::Mat & image,
+		    const CameraModel & model,
+		    const cv::Mat & depth = cv::Mat(),
+		    const std::map<int, float> & markerLengths = std::map<int, float>(),
+		    cv::Mat * imageWithDetections = 0);
 
 private:
 #ifdef HAVE_OPENCV_ARUCO

corelib/include/rtabmap/core/Memory.h

@@ -55,6 +55,7 @@ class Statistics;
 class Registration;
 class RegistrationInfo;
 class RegistrationIcp;
+class RegistrationVis;
 class Stereo;
 class OccupancyGrid;
 class MarkerDetector;
@@ -205,7 +206,7 @@ public:
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
-			StereoCameraModel & stereoModel) const;
+			std::vector<StereoCameraModel> & stereoModels) const;
 	std::set<int> getAllSignatureIds(bool ignoreChildren = true) const;
 	bool memoryChanged() const {return _memoryChanged;}
 	bool isIncremental() const {return _incrementalMemory;}
@@ -323,6 +324,7 @@ private:
 	float _laserScanGroundNormalsUp;
 	bool _reextractLoopClosureFeatures;
 	bool _localBundleOnLoopClosure;
+	bool _invertedReg;
 	float _rehearsalMaxDistance;
 	float _rehearsalMaxAngle;
 	bool _rehearsalWeightIgnoredWhileMoving;
@@ -347,7 +349,7 @@ private:
 	bool _allNodesInWM;
 	GPS _gpsOrigin;
 	std::vector<CameraModel> _rectCameraModels;
-	StereoCameraModel _rectStereoCameraModel;
+	std::vector<StereoCameraModel> _rectStereoCameraModels;
 	std::vector<double> _odomMaxInf;
 
 	std::map<int, Signature *> _signatures; // TODO : check if a signature is already added? although it is not supposed to occur...
@@ -367,6 +369,7 @@ private:
 
 	Registration * _registrationPipeline;
 	RegistrationIcp * _registrationIcpMulti;
+	RegistrationVis * _registrationVis;
 
 	OccupancyGrid * _occupancy;
 

corelib/include/rtabmap/core/Odometry.h

@@ -122,7 +122,7 @@ private:
 
 	std::vector<ParticleFilter *> particleFilters_;
 	cv::KalmanFilter kalmanFilter_;
-	StereoCameraModel stereoModel_;
+	std::vector<StereoCameraModel> stereoModels_;
 	std::vector<CameraModel> models_;
 	std::map<double, Transform> imus_;
 

corelib/include/rtabmap/core/OdometryInfo.h

@@ -103,7 +103,7 @@ public:
 	int localBundleConstraints;
 	float localBundleTime;
 	std::map<int, Transform> localBundlePoses;
-	std::map<int, CameraModel> localBundleModels;
+	std::map<int, std::vector<CameraModel> > localBundleModels;
 	bool keyFrameAdded;
 	float timeEstimation;
 	float timeParticleFiltering;

corelib/include/rtabmap/core/Optimizer.h

@@ -41,14 +41,18 @@ namespace rtabmap {
 class FeatureBA
 {
 public:
-	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat()):
+	FeatureBA(const cv::KeyPoint & kptIn, const float & depthIn = 0.0f, const cv::Mat & descriptorIn = cv::Mat(), int cameraIndexIn = 0):
 		kpt(kptIn),
 		depth(depthIn),
-		descriptor(descriptorIn)
+		descriptor(descriptorIn),
-	{}
+		cameraIndex(cameraIndexIn)
+	{
+		//UDEBUG("kpt=(%f,%f) depth=%f, camIndex=%d", kpt.pt.x, kpt.pt.y, depth, cameraIndex);
+	}
 	cv::KeyPoint kpt;
 	float depth;
 	cv::Mat descriptor;
+	int cameraIndex;
 };
 
 ////////////////////////////////////////////
@@ -134,7 +138,7 @@ public:
 			int rootId, // if negative, all other poses are fixed
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/depth/descriptor>
 			std::set<int> * outliers = 0);

corelib/include/rtabmap/core/Parameters.h

@@ -370,6 +370,7 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(RGBD, LoopClosureIdentityGuess,     bool, false,  uFormat("Use Identity matrix as guess when computing loop closure transform, otherwise no guess is used, thus assuming that registration strategy selected (%s) can deal with transformation estimation without guess.", kRegStrategy().c_str()));
     RTABMAP_PARAM(RGBD, LoopClosureReextractFeatures, bool, false,  "Extract features even if there are some already in the nodes. Raw features are not saved in database.");
     RTABMAP_PARAM(RGBD, LocalBundleOnLoopClosure,     bool, false,  "Do local bundle adjustment with neighborhood of the loop closure.");
+    RTABMAP_PARAM(RGBD, InvertedReg,                  bool, false,  "On loop closure, do registration from the target to reference instead of reference to target.");
     RTABMAP_PARAM(RGBD, CreateOccupancyGrid,          bool, false,  "Create local occupancy grid maps. See \"Grid\" group for parameters.");
     RTABMAP_PARAM(RGBD, MarkerDetection,              bool, false,  "Detect static markers to be added as landmarks for graph optimization. If input data have already landmarks, this will be ignored. See \"Marker\" group for parameters.");
     RTABMAP_PARAM(RGBD, LoopCovLimited,               bool, false,  "Limit covariance of non-neighbor links to minimum covariance of neighbor links. In other words, if covariance of a loop closure link is smaller than the minimum covariance of odometry links, its covariance is set to minimum covariance of odometry links.");
@@ -594,6 +595,7 @@ class RTABMAP_EXP Parameters
 #else
     RTABMAP_PARAM(Vis, PnPRefineIterations,      int, 1,        uFormat("[%s = 1] Refine iterations. Set to 0 if \"%s\" is also used.", kVisEstimationType().c_str(), kVisBundleAdjustment().c_str()));
 #endif
+    RTABMAP_PARAM(Vis, PnPMaxVariance,           float, 0.0,    uFormat("[%s = 1] Max linear variance between 3D point correspondences after PnP. 0 means disabled.", kVisEstimationType().c_str()));
 
     RTABMAP_PARAM(Vis, EpipolarGeometryVar,      float, 0.1,    uFormat("[%s = 2] Epipolar geometry maximum variance to accept the transformation.", kVisEstimationType().c_str()));
     RTABMAP_PARAM(Vis, MinInliers,               int, 20,       "Minimum feature correspondences to compute/accept the transformation.");
@@ -780,8 +782,11 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Marker, VarianceLinear,         float, 0.001, "Linear variance to set on marker detections.");
     RTABMAP_PARAM(Marker, VarianceAngular,        float, 0.01,  "Angular variance to set on marker detections. Set to >=9999 to use only position (xyz) constraint in graph optimization.");
     RTABMAP_PARAM(Marker, CornerRefinementMethod, int,   0,     "Corner refinement method (0: None, 1: Subpixel, 2:contour, 3: AprilTag2). For OpenCV <3.3.0, this is \"doCornerRefinement\" parameter: set 0 for false and 1 for true.");
-    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,    "Maximum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MaxRange,               float, 0.0,   "Maximum range in which markers will be detected. <=0 for unlimited range.");
-    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,    "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM(Marker, MinRange,               float, 0.0,   "Miniminum range in which markers will be detected. <=0 for unlimited range.");
+    RTABMAP_PARAM_STR(Marker, Priors,             "",           "World prior locations of the markers. The map will be transformed in marker's world frame when a tag is detected. Format is the marker's ID followed by its position (angles in rad), markers are separated by vertical line (\"id1 x y z roll pitch yaw|id2 x y z roll pitch yaw\"). Example:  \"1 0 0 1 0 0 0|2 1 0 1 0 0 1.57\" (marker 2 is 1 meter forward than marker 1 with 90 deg yaw rotation).");
+    RTABMAP_PARAM(Marker, PriorsVarianceLinear,   float, 0.001, "Linear variance to set on marker priors.");
+    RTABMAP_PARAM(Marker, PriorsVarianceAngular,  float, 0.001, "Angular variance to set on marker priors.");
 
     RTABMAP_PARAM(ImuFilter, MadgwickGain,                  double, 0.1,  "Gain of the filter. Higher values lead to faster convergence but more noise. Lower values lead to slower convergence but smoother signal, belongs in [0, 1].");
     RTABMAP_PARAM(ImuFilter, MadgwickZeta,                  double, 0.0,  "Gyro drift gain (approx. rad/s), belongs in [-1, 1].");

corelib/include/rtabmap/core/Registration.h

@@ -45,7 +45,8 @@ public:
 		kTypeIcp = 1,
 		kTypeVisIcp = 2
 	};
-	static double COVARIANCE_EPSILON;
+	static double COVARIANCE_LINEAR_EPSILON;
+	static double COVARIANCE_ANGULAR_EPSILON;
 
 public:
 	static Registration * create(const ParametersMap & parameters);

corelib/include/rtabmap/core/RegistrationVis.h

@@ -82,6 +82,7 @@ private:
 	float _PnPReprojError;
 	int _PnPFlags;
 	int _PnPRefineIterations;
+	float _PnPMaxVar;
 	int _correspondencesApproach;
 	int _flowWinSize;
 	int _flowIterations;

corelib/include/rtabmap/core/Rtabmap.h

@@ -49,6 +49,7 @@ class Memory;
 class BayesFilter;
 class Signature;
 class Optimizer;
+class PythonInterface;
 
 class RTABMAP_EXP Rtabmap
 {
@@ -323,6 +324,8 @@ private:
 	bool _loopGPS;
 	int _maxOdomCacheSize;
 	bool _createGlobalScanMap;
+	float _markerPriorsLinearVariance;
+	float _markerPriorsAngularVariance;
 
 	std::pair<int, float> _loopClosureHypothesis;
 	std::pair<int, float> _highestHypothesis;
@@ -363,6 +366,7 @@ private:
 	std::map<int, Transform> _odomCachePoses;       // used in localization mode to reject loop closures
 	std::multimap<int, Link> _odomCacheConstraints; // used in localization mode to reject loop closures
 	std::vector<float> _odomCorrectionAcc;
+	std::map<int, Transform> _markerPriors;
 
 	// Planning stuff
 	int _pathStatus;
@@ -374,6 +378,10 @@ private:
 	int _pathStuckCount;
 	float _pathStuckDistance;
 
+#ifdef RTABMAP_PYTHON
+	PythonInterface * _python;
+#endif
+
 };
 
 } // namespace rtabmap

corelib/include/rtabmap/core/SensorData.h

@@ -126,6 +126,25 @@ public:
 			double stamp = 0.0,
 			const cv::Mat & userData = cv::Mat());
 
+	// Multi-cameras stereo constructor
+	SensorData(
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
+	// Multi-cameras stereo constructor + laser scan
+	SensorData(
+			const LaserScan & laserScan,
+			const cv::Mat & rgb,
+			const cv::Mat & depth,
+			const std::vector<StereoCameraModel> & cameraModels,
+			int id = 0,
+			double stamp = 0.0,
+			const cv::Mat & userData = cv::Mat());
+
 	// IMU constructor
 	SensorData(
 			const IMU & imu,
@@ -143,8 +162,8 @@ public:
 			_depthOrRightCompressed.empty() &&
 			_laserScanRaw.isEmpty() &&
 			_laserScanCompressed.isEmpty() &&
-			_cameraModels.size() == 0 &&
+			_cameraModels.empty() &&
-			!_stereoCameraModel.isValidForProjection() &&
+			_stereoCameraModels.empty() &&
 			_userDataRaw.empty() &&
 			_userDataCompressed.empty() &&
 			_keypoints.size() == 0 &&
@@ -173,6 +192,7 @@ public:
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const CameraModel & model, bool clearPreviousData = true);
 	void setRGBDImage(const cv::Mat & rgb, const cv::Mat & depth, const std::vector<CameraModel> & models, bool clearPreviousData = true);
 	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const StereoCameraModel & stereoCameraModel, bool clearPreviousData = true);
+	void setStereoImage(const cv::Mat & left, const cv::Mat & right, const std::vector<StereoCameraModel> & stereoCameraModels, bool clearPreviousData = true);
 
 	/**
 	 * Set laser scan data. Detect automatically if raw or compressed.
@@ -183,7 +203,8 @@ public:
 
 	void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
 	void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
-	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
+	void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModels.clear(); _stereoCameraModels.push_back(stereoCameraModel);}
+	void setStereoCameraModels(const std::vector<StereoCameraModel> & stereoCameraModels) {_stereoCameraModels = stereoCameraModels;}
 
 	//for convenience
 	cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
@@ -213,7 +234,7 @@ public:
 			cv::Mat * emptyCellsRaw = 0) const;
 
 	const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
-	const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
+	const std::vector<StereoCameraModel> & stereoCameraModels() const {return _stereoCameraModels;}
 
 	/**
 	 * Set user data. Detect automatically if raw or compressed. If raw, the data is
@@ -302,7 +323,7 @@ private:
 	LaserScan _laserScanRaw;
 
 	std::vector<CameraModel> _cameraModels;
-	StereoCameraModel _stereoCameraModel;
+	std::vector<StereoCameraModel> _stereoCameraModels;
 
 	// user data
 	cv::Mat _userDataCompressed;      // compressed data

corelib/include/rtabmap/core/Statistics.h

@@ -151,6 +151,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odom_cache_links,);
 	RTABMAP_STATS(Memory, Small_movement,);
 	RTABMAP_STATS(Memory, Fast_movement,);
+	RTABMAP_STATS(Memory, New_landmark,);
 	RTABMAP_STATS(Memory, Odometry_variance_ang,);
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);

corelib/include/rtabmap/core/Transform.h

@@ -143,6 +143,8 @@ public:
 	static Transform fromEigen3d(const Eigen::Affine3d & matrix);
 	static Transform fromEigen3f(const Eigen::Isometry3f & matrix);
 	static Transform fromEigen3d(const Eigen::Isometry3d & matrix);
+	static Transform fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix);
+	static Transform fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix);
 
 	static Transform opengl_T_rtabmap() {return Transform(
 			 0.0f, -1.0f, 0.0f, 0.0f,

corelib/include/rtabmap/core/camera/CameraDepthAI.h

@@ -58,6 +58,7 @@ public:
 	virtual ~CameraDepthAI();
 
 	void setOutputDepth(bool enabled, int confidence = 200);
+	void setIMUFirmwareUpdate(bool enabled);
 
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
@@ -74,6 +75,7 @@ private:
 	bool outputDepth_;
 	int depthConfidence_;
 	int resolution_;
+	bool imuFirmwareUpdate_;
 	std::shared_ptr<dai::Device> device_;
 	std::shared_ptr<dai::DataOutputQueue> leftQueue_;
 	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;

corelib/include/rtabmap/core/odometry/OdometryF2M.h

@@ -83,7 +83,7 @@ private:
 	std::map<int, Transform> bundlePoses_;
 	std::multimap<int, Link> bundleLinks_;
 	std::multimap<int, Link> bundleIMUOrientations_;
-	std::map<int, CameraModel> bundleModels_;
+	std::map<int, std::vector<CameraModel> > bundleModels_;
 	std::map<int, int> bundlePoseReferences_;
 	int bundleSeq_;
 	Optimizer * sba_;

corelib/include/rtabmap/core/odometry/OdometryMono.h

@@ -73,7 +73,7 @@ private:
 	std::map<int, std::map<int, cv::Point3f> > keyFrameWords3D_;
 	std::map<int, Transform> keyFramePoses_;
 	std::multimap<int, Link> keyFrameLinks_;
-	std::map<int, CameraModel> keyFrameModels_;
+	std::map<int, std::vector<CameraModel> > keyFrameModels_;
 	float maxVariance_;
 	float keyFrameThr_;
 };

corelib/include/rtabmap/core/optimizer/OptimizerCVSBA.h

@@ -55,7 +55,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models,
+			const std::map<int, std::vector<CameraModel> > & models,
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
 			std::set<int> * outliers = 0);

corelib/include/rtabmap/core/optimizer/OptimizerG2O.h

@@ -62,7 +62,7 @@ public:
 			int rootId,
 			const std::map<int, Transform> & poses,
 			const std::multimap<int, Link> & links,
-			const std::map<int, CameraModel> & models, // in case of stereo, Tx should be set
+			const std::map<int, std::vector<CameraModel> > & models, // in case of stereo, Tx should be set
 			std::map<int, cv::Point3f> & points3DMap,
 			const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint(x,y,depth)>
 			std::set<int> * outliers = 0);

corelib/include/rtabmap/core/util3d_motion_estimation.h

@@ -48,6 +48,23 @@ Transform RTABMAP_EXP estimateMotion3DTo2D(
 			double reprojError = 5.,
 			int flagsPnP = 0,
 			int pnpRefineIterations = 1,
+			float maxVariance = 0,
+			const Transform & guess = Transform::getIdentity(),
+			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
+			cv::Mat * covariance = 0, // mean reproj error if words3B is not set
+			std::vector<int> * matchesOut = 0,
+			std::vector<int> * inliersOut = 0);
+			
+Transform RTABMAP_EXP estimateMotion3DTo2D(
+			const std::map<int, cv::Point3f> & words3A,
+			const std::map<int, cv::KeyPoint> & words2B,
+			const std::vector<CameraModel> & cameraModels,
+			int minInliers = 10,
+			int iterations = 100,
+			double reprojError = 5.,
+			int flagsPnP = 0,
+			int pnpRefineIterations = 1,
+			float maxVariance = 0,
 			const Transform & guess = Transform::getIdentity(),
 			const std::map<int, cv::Point3f> & words3B = std::map<int, cv::Point3f>(),
 			cv::Mat * covariance = 0, // mean reproj error if words3B is not set

corelib/include/rtabmap/core/util3d_transforms.h

@@ -93,6 +93,9 @@ pcl::PointCloud<pcl::PointXYZINormal>::Ptr RTABMAP_EXP transformPointCloud(
 cv::Point3f RTABMAP_EXP transformPoint(
 		const cv::Point3f & pt,
 		const Transform & transform);
+cv::Point3d RTABMAP_EXP transformPoint(
+		const cv::Point3d & pt,
+		const Transform & transform);
 pcl::PointXYZ RTABMAP_EXP transformPoint(
 		const pcl::PointXYZ & pt,
 		const Transform & transform);

corelib/src/CMakeLists.txt

@@ -199,6 +199,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
 	SET(LIBRARIES
 		${LIBRARIES}
 		Python3::Python
+		Python3::NumPy
 	)
 	SET(SRC_FILES
 		${SRC_FILES}
@@ -399,9 +400,6 @@ IF(G2O_FOUND)
 		${G2O_LIBRARIES}
 	  )
 	ENDIF()
-	SET(SRC_FILES ${SRC_FILES}
-		optimizer/g2o/edge_se3_xyzprior.cpp
-	)
 	IF(WITH_VERTIGO)
 		SET(SRC_FILES ${SRC_FILES}
 			optimizer/vertigo/g2o/edge_se2Switchable.cpp
@@ -424,16 +422,16 @@ IF(cvsba_FOUND)
 	)
 ENDIF(cvsba_FOUND)
 
-IF(WITH_CERES AND CERES_FOUND)
+IF(CERES_FOUND)
-	SET(INCLUDE_DIRS 
+	SET(INCLUDE_DIRS
-		${INCLUDE_DIRS} 
+	  ${INCLUDE_DIRS}
-		${CERES_INCLUDE_DIRS}
+	  ${CERES_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
-		${LIBRARIES}
+	  ${LIBRARIES}
-		${CERES_LIBRARIES}
+	  ${CERES_LIBRARIES}
 	)
-ENDIF(WITH_CERES AND CERES_FOUND)
+ENDIF(CERES_FOUND)
 
 IF(libpointmatcher_FOUND)
 	SET(INCLUDE_DIRS 
@@ -471,6 +469,13 @@ IF(FastCV_FOUND)
 	)
 ENDIF(FastCV_FOUND)
 
+IF(opengv_FOUND)
+    SET(LIBRARIES
+        ${LIBRARIES}
+        opengv
+    )
+ENDIF(opengv_FOUND)
+
 IF(PDAL_FOUND)
 	SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
@@ -720,29 +725,12 @@ endforeach(arg ${RESOURCES})
 #MESSAGE(STATUS "RESOURCES = ${RESOURCES}")
 #MESSAGE(STATUS "RESOURCES_HEADERS = ${RESOURCES_HEADERS}")
 
-IF(ANDROID OR IOS)
+ADD_CUSTOM_COMMAND(
-
+	OUTPUT ${RESOURCES_HEADERS}
-   IF(NOT RTABMAP_RES_TOOL)
+	COMMAND res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
-      find_host_program(RTABMAP_RES_TOOL rtabmap-res_tool PATHS ${PROJECT_BINARY_DIR}/../bin)
+	COMMENT "[Creating resources]"
-      IF(NOT RTABMAP_RES_TOOL)
+	DEPENDS ${RESOURCES}
-         MESSAGE( FATAL_ERROR "RTABMAP_RES_TOOL is not defined (it is the path to \"rtabmap-res_tool\" application created by a non-Android build)." )
+)
-      ENDIF(NOT RTABMAP_RES_TOOL)
-   ENDIF(NOT RTABMAP_RES_TOOL)
-
-   ADD_CUSTOM_COMMAND(
-      OUTPUT ${RESOURCES_HEADERS}
-      COMMAND ${RTABMAP_RES_TOOL} -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
-      COMMENT "[Creating resources]"
-      DEPENDS ${RESOURCES}
-   )
-ELSE()
-   ADD_CUSTOM_COMMAND(
-      OUTPUT ${RESOURCES_HEADERS}
-      COMMAND ${CMAKE_RUNTIME_OUTPUT_DIRECTORY}/rtabmap-res_tool -n rtabmap -p ${CMAKE_CURRENT_BINARY_DIR} ${RESOURCES}
-      COMMENT "[Creating resources]"
-      DEPENDS ${RESOURCES} res_tool
-   )
-ENDIF()
 
 ####################################
 # Generate resources files END

corelib/src/CameraModel.cpp

@@ -37,7 +37,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-CameraModel::CameraModel()
+CameraModel::CameraModel() :
+		localTransform_(opticalRotation())
 {
 
 }
@@ -339,6 +340,25 @@ bool CameraModel::load(const std::string & filePath)
 				UWARN("Missing \"projection_matrix\" field in \"%s\"", filePath.c_str());
 			}
 
+			n = fs["local_transform"];
+			if(n.type() != cv::FileNode::NONE)
+			{
+				int rows = (int)n["rows"];
+				int cols = (int)n["cols"];
+				std::vector<float> data;
+				n["data"] >> data;
+				UASSERT(rows*cols == (int)data.size());
+				UASSERT(rows == 3 && cols == 4);
+				localTransform_ = Transform(
+						data[0], data[1], data[2], data[3],
+						data[4], data[5], data[6], data[7],
+						data[8], data[9], data[10], data[11]);
+			}
+			else
+			{
+				UWARN("Missing \"local_transform\" field in \"%s\"", filePath.c_str());
+			}
+
 			fs.release();
 
 			if(isValidForRectification())
@@ -448,6 +468,15 @@ bool CameraModel::save(const std::string & directory) const
 			fs << "}";
 		}
 
+		if(!localTransform_.isNull())
+		{
+			fs << "local_transform" << "{";
+			fs << "rows" << 3;
+			fs << "cols" << 4;
+			fs << "data" << std::vector<float>((float*)localTransform_.data(), ((float*)localTransform_.data())+12);
+			fs << "}";
+		}
+
 		fs.release();
 
 		return true;

corelib/src/CameraThread.cpp

@@ -287,9 +287,10 @@ void CameraThread::mainLoop()
 				model.setLocalTransform(_extrinsicsOdomToCamera);
 				data.setCameraModel(model);
 			}
-			else
+			else if(!data.stereoCameraModels().empty())
 			{
-				StereoCameraModel model = data.stereoCameraModel();
+				UASSERT(data.stereoCameraModels().size()==1);
+				StereoCameraModel model = data.stereoCameraModels()[0];
 				model.setLocalTransform(_extrinsicsOdomToCamera);
 				data.setStereoCameraModel(model);
 			}
@@ -358,7 +359,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 		}
 		else if(!data.rightRaw().empty())
 		{
-			data.setRGBDImage(data.imageRaw(), cv::Mat(), data.stereoCameraModel().left());
+			std::vector<CameraModel> models;
+			for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+			{
+				models.push_back(data.stereoCameraModels()[i].left());
+			}
+			data.setRGBDImage(data.imageRaw(), cv::Mat(), models);
 		}
 	}
 
@@ -435,91 +441,116 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
 			{
 				data.setRGBDImage(image, depthOrRight, models);
 			}
-			else
+
+
+			std::vector<StereoCameraModel> stereoModels = data.stereoCameraModels();
+			for(unsigned int i=0; i<stereoModels.size(); ++i)
 			{
-				StereoCameraModel stereoModel = data.stereoCameraModel();
+				if(stereoModels[i].isValidForProjection())
-				if(stereoModel.isValidForProjection())
 				{
-					stereoModel.scale(1.0/double(_imageDecimation));
+					stereoModels[i].scale(1.0/double(_imageDecimation));
 				}
-				data.setStereoImage(image, depthOrRight, stereoModel);
+			}
+			if(!stereoModels.empty())
+			{
+				data.setStereoImage(image, depthOrRight, stereoModels);
 			}
 		}
 		if(info) info->timeImageDecimation = timer.ticks();
 	}
-	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
+	if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1)
 	{
-		UDEBUG("");
+		if(data.cameraModels().size() == 1)
-		UTimer timer;
+		{
-		cv::Mat tmpRgb;
+			UDEBUG("");
-		cv::flip(data.imageRaw(), tmpRgb, 1);
+			UTimer timer;
+			cv::Mat tmpRgb;
+			cv::flip(data.imageRaw(), tmpRgb, 1);
 
-		UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
+			CameraModel tmpModel = data.cameraModels()[0];
-		CameraModel tmpModel = data.cameraModels()[0];
+			if(data.cameraModels()[0].cx())
-		if(data.cameraModels()[0].cx())
+			{
-		{
+				tmpModel = CameraModel(
-			tmpModel = CameraModel(
+						data.cameraModels()[0].fx(),
-					data.cameraModels()[0].fx(),
+						data.cameraModels()[0].fy(),
-					data.cameraModels()[0].fy(),
+						float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
-					float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
+						data.cameraModels()[0].cy(),
-					data.cameraModels()[0].cy(),
+						data.cameraModels()[0].localTransform(),
-					data.cameraModels()[0].localTransform(),
+						data.cameraModels()[0].Tx(),
-					data.cameraModels()[0].Tx(),
+						data.cameraModels()[0].imageSize());
-					data.cameraModels()[0].imageSize());
+			}
+			cv::Mat tmpDepth = data.depthOrRightRaw();
+			if(!data.depthRaw().empty())
+			{
+				cv::flip(data.depthRaw(), tmpDepth, 1);
+			}
+			data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
+			if(info) info->timeMirroring = timer.ticks();
 		}
-		cv::Mat tmpDepth = data.depthOrRightRaw();
+		else
-		if(!data.depthRaw().empty())
 		{
-			cv::flip(data.depthRaw(), tmpDepth, 1);
+			UWARN("Mirroring is not implemented for multiple cameras or stereo...");
 		}
-		data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
-		if(info) info->timeMirroring = timer.ticks();
 	}
 
 	if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
 	{
+		if(data.stereoCameraModels().size()==1)
+		{
 #if CV_MAJOR_VERSION < 3
-		UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
+			UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
 #else
-		UDEBUG("");
+			UDEBUG("");
-		UTimer timer;
+			UTimer timer;
-		cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
+			cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
-		std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
+			std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
-		std::vector<cv::UMat> images;
+			std::vector<cv::UMat> images;
-		std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1,  cv::Scalar(255)));
+			std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1,  cv::Scalar(255)));
-		images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
+			images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
-		images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
+			images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
-		compensator->feed(topLeftCorners, images, masks);
+			compensator->feed(topLeftCorners, images, masks);
-		cv::Mat imgLeft = data.imageRaw().clone();
+			cv::Mat imgLeft = data.imageRaw().clone();
-		compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
+			compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
-		cv::Mat imgRight = data.rightRaw().clone();
+			cv::Mat imgRight = data.rightRaw().clone();
-		compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
+			compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
-		data.setStereoImage(imgLeft, imgRight, data.stereoCameraModel());
+			data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
-		cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
+			cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
-		UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
+			UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
-		if(info) info->timeStereoExposureCompensation = timer.ticks();
+			if(info) info->timeStereoExposureCompensation = timer.ticks();
 #endif
+		}
+		else
+		{
+			UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras...");
+		}
 	}
 
-	if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
+	if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty())
 	{
-		UDEBUG("");
+		if(data.stereoCameraModels().size()==1)
-		UTimer timer;
+		{
-		cv::Mat depth = util2d::depthFromDisparity(
+			UDEBUG("");
-				_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
+			UTimer timer;
-				data.stereoCameraModel().left().fx(),
+			cv::Mat depth = util2d::depthFromDisparity(
-				data.stereoCameraModel().baseline());
+					_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
-		// set Tx for stereo bundle adjustment (when used)
+					data.stereoCameraModels()[0].left().fx(),
-		CameraModel model = CameraModel(
+					data.stereoCameraModels()[0].baseline());
-				data.stereoCameraModel().left().fx(),
+			// set Tx for stereo bundle adjustment (when used)
-				data.stereoCameraModel().left().fy(),
+			CameraModel model = CameraModel(
-				data.stereoCameraModel().left().cx(),
+					data.stereoCameraModels()[0].left().fx(),
-				data.stereoCameraModel().left().cy(),
+					data.stereoCameraModels()[0].left().fy(),
-				data.stereoCameraModel().localTransform(),
+					data.stereoCameraModels()[0].left().cx(),
-				-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
+					data.stereoCameraModels()[0].left().cy(),
-				data.stereoCameraModel().left().imageSize());
+					data.stereoCameraModels()[0].localTransform(),
-		data.setRGBDImage(data.imageRaw(), depth, model);
+					-data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(),
-		if(info) info->timeDisparity = timer.ticks();
+					data.stereoCameraModels()[0].left().imageSize());
+			data.setRGBDImage(data.imageRaw(), depth, model);
+			if(info) info->timeDisparity = timer.ticks();
+		}
+		else
+		{
+			UWARN("Stereo to depth is not implemented for multiple stereo cameras...");
+		}
 	}
 	if(_scanFromDepth &&
 		data.cameraModels().size() &&

corelib/src/DBDriver.cpp

@@ -726,7 +726,7 @@ void DBDriver::getNodeData(
 bool DBDriver::getCalibration(
 		int signatureId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	UDEBUG("");
 	bool found = false;
@@ -735,7 +735,7 @@ bool DBDriver::getCalibration(
 	if(uContains(_trashSignatures, signatureId))
 	{
 		models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
-		stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
+		stereoModels = _trashSignatures.at(signatureId)->sensorData().stereoCameraModels();
 		found = true;
 	}
 	_trashesMutex.unlock();
@@ -743,7 +743,7 @@ bool DBDriver::getCalibration(
 	if(!found)
 	{
 		_dbSafeAccessMutex.lock();
-		found = this->getCalibrationQuery(signatureId, models, stereoModel);
+		found = this->getCalibrationQuery(signatureId, models, stereoModels);
 		_dbSafeAccessMutex.unlock();
 	}
 	return found;

corelib/src/DBDriverSqlite3.cpp

@@ -1448,7 +1448,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 				cv::Mat imageCompressed;
 				cv::Mat depthOrRightCompressed;
 				std::vector<CameraModel> models;
-				StereoCameraModel stereoModel;
+				std::vector<StereoCameraModel> stereoModels;
 				Transform localTransform = Transform::getIdentity();
 				cv::Mat scanCompressed;
 				cv::Mat userDataCompressed;
@@ -1515,8 +1515,16 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									}
 									else if(type == 1) // stereo
 									{
-										int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
+										StereoCameraModel model;
-										UASSERT(bytesRead == dataSize);
+										int bytesReadTotal = 0;
+										unsigned int bytesRead = 0;
+										while(bytesReadTotal < dataSize &&
+											  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+										{
+											bytesReadTotal+=bytesRead;
+											stereoModels.push_back(model);
+										}
+										UASSERT(bytesReadTotal == dataSize);
 									}
 									else
 									{
@@ -1589,14 +1597,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									{
 										localTransform.normalizeRotation();
 									}
-									stereoModel = StereoCameraModel(
+									stereoModels.push_back(StereoCameraModel(
 											dataFloat[0],  // fx
 											dataFloat[1],  // fy
 											dataFloat[2],  // cx
 											dataFloat[3],  // cy
 											dataFloat[4], // baseline
 											localTransform,
-											cv::Size(dataFloat[5],dataFloat[6]));
+											cv::Size(dataFloat[5],dataFloat[6])));
 								}
 								else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 								{
@@ -1606,13 +1614,13 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 									{
 										localTransform.normalizeRotation();
 									}
-									stereoModel = StereoCameraModel(
+									stereoModels.push_back(StereoCameraModel(
 											dataFloat[0],  // fx
 											dataFloat[1],  // fy
 											dataFloat[2],  // cx
 											dataFloat[3],  // cy
 											dataFloat[4], // baseline
-											localTransform);
+											localTransform));
 								}
 								else
 								{
@@ -1632,7 +1640,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 						if(fyOrBaseline < 1.0)
 						{
 							//it is a baseline
-							stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+							stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
 						}
 						else
 						{
@@ -1818,7 +1826,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 					}
 					else
 					{
-						(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModel);
+						(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModels);
 					}
 				}
 				if(userData)
@@ -1850,7 +1858,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
 bool DBDriverSqlite3::getCalibrationQuery(
 		int signatureId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	bool found = false;
 	if(_ppDb && signatureId)
@@ -1936,8 +1944,16 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							}
 							else if(type == 1) // stereo
 							{
-								int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
+								StereoCameraModel model;
-								UASSERT(bytesRead == dataSize);
+								int bytesReadTotal = 0;
+								unsigned int bytesRead = 0;
+								while(bytesReadTotal < dataSize &&
+									  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+								{
+									bytesReadTotal+=bytesRead;
+									stereoModels.push_back(model);
+								}
+								UASSERT(bytesReadTotal == dataSize);
 							}
 							else
 							{
@@ -2010,14 +2026,14 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							{
 								localTransform.normalizeRotation();
 							}
-							stereoModel = StereoCameraModel(
+							stereoModels.push_back(StereoCameraModel(
 									dataFloat[0],  // fx
 									dataFloat[1],  // fy
 									dataFloat[2],  // cx
 									dataFloat[3],  // cy
 									dataFloat[4], // baseline
 									localTransform,
-									cv::Size(dataFloat[5],dataFloat[6]));
+									cv::Size(dataFloat[5],dataFloat[6])));
 						}
 						else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 						{
@@ -2027,13 +2043,13 @@ bool DBDriverSqlite3::getCalibrationQuery(
 							{
 								localTransform.normalizeRotation();
 							}
-							stereoModel = StereoCameraModel(
+							stereoModels.push_back((StereoCameraModel(
 									dataFloat[0],  // fx
 									dataFloat[1],  // fy
 									dataFloat[2],  // cx
 									dataFloat[3],  // cy
 									dataFloat[4], // baseline
-									localTransform);
+									localTransform)));
 						}
 						else
 						{
@@ -2054,7 +2070,7 @@ bool DBDriverSqlite3::getCalibrationQuery(
 				if(fyOrBaseline < 1.0)
 				{
 					//it is a baseline
-					stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
+					stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
 				}
 				else
 				{
@@ -3278,7 +3294,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					int dataSize = 0;
 					Transform localTransform;
 					std::vector<CameraModel> models;
-					StereoCameraModel stereoModel;
+					std::vector<StereoCameraModel> stereoModels;
 
 					// calibration
 					data = sqlite3_column_blob(ppStmt, index);
@@ -3308,8 +3324,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								}
 								else if(type == 1) // stereo
 								{
-									int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
+									StereoCameraModel model;
-									UASSERT(bytesRead == dataSize);
+									int bytesReadTotal = 0;
+									unsigned int bytesRead = 0;
+									while(bytesReadTotal < dataSize &&
+										  (bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
+									{
+										bytesReadTotal+=bytesRead;
+										stereoModels.push_back(model);
+									}
+									UASSERT(bytesReadTotal == dataSize);
 								}
 								else
 								{
@@ -3383,14 +3407,14 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								{
 									localTransform.normalizeRotation();
 								}
-								stereoModel = StereoCameraModel(
+								stereoModels.push_back(StereoCameraModel(
 										dataFloat[0],  // fx
 										dataFloat[1],  // fy
 										dataFloat[2],  // cx
 										dataFloat[3],  // cy
 										dataFloat[4], // baseline
 										localTransform,
-										cv::Size(dataFloat[5], dataFloat[6]));
+										cv::Size(dataFloat[5], dataFloat[6])));
 							}
 							else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
 							{
@@ -3400,13 +3424,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 								{
 									localTransform.normalizeRotation();
 								}
-								stereoModel = StereoCameraModel(
+								stereoModels.push_back(StereoCameraModel(
 										dataFloat[0],  // fx
 										dataFloat[1],  // fy
 										dataFloat[2],  // cx
 										dataFloat[3],  // cy
 										dataFloat[4], // baseline
-										localTransform);
+										localTransform));
 							}
 							else
 							{
@@ -3415,7 +3439,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 						}
 
 						(*iter)->sensorData().setCameraModels(models);
-						(*iter)->sensorData().setStereoCameraModel(stereoModel);
+						(*iter)->sensorData().setStereoCameraModels(stereoModels);
 					}
 					rc = sqlite3_step(ppStmt);
 				}
@@ -4382,8 +4406,8 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 				   !(*i)->sensorData().depthOrRightCompressed().empty() ||
 				   !(*i)->sensorData().laserScanCompressed().isEmpty() ||
 				   !(*i)->sensorData().userDataCompressed().empty() ||
-				   !(*i)->sensorData().cameraModels().size() ||
+				   !(*i)->sensorData().cameraModels().empty() ||
-				   !(*i)->sensorData().stereoCameraModel().isValidForProjection())
+				   !(*i)->sensorData().stereoCameraModels().empty())
 				{
 					UASSERT((*i)->id() == (*i)->sensorData().id());
 					stepSensorData(ppStmt, (*i)->sensorData());
@@ -5691,13 +5715,15 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
 		cy = sensorData.cameraModels()[0].cy();
 		localTransform = sensorData.cameraModels()[0].localTransform();
 	}
-	else if(sensorData.stereoCameraModel().isValidForProjection())
+	else if(sensorData.stereoCameraModels().size())
 	{
-		fx = sensorData.stereoCameraModel().left().fx();
+		UASSERT_MSG(sensorData.stereoCameraModels().size() == 1,
-		fyOrBaseline = sensorData.stereoCameraModel().baseline();
+				uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
-		cx = sensorData.stereoCameraModel().left().cx();
+		fx = sensorData.stereoCameraModels()[0].left().fx();
-		cy = sensorData.stereoCameraModel().left().cy();
+		fyOrBaseline = sensorData.stereoCameraModels()[0].baseline();
-		localTransform = sensorData.stereoCameraModel().left().localTransform();
+		cx = sensorData.stereoCameraModels()[0].left().cx();
+		cy = sensorData.stereoCameraModels()[0].left().cy();
+		localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
 	}
 
 	if(uStrNumCmp(_version, "0.7.0") >= 0)
@@ -6040,24 +6066,32 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
 			}
 		}
 	}
-	else if(sensorData.stereoCameraModel().isValidForProjection())
+	else if(sensorData.stereoCameraModels().size() && sensorData.stereoCameraModels()[0].isValidForProjection())
 	{
 		if(uStrNumCmp(_version, "0.18.0") >= 0)
 		{
-			calibrationData = sensorData.stereoCameraModel().serialize();
+			for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
-			UASSERT(!calibrationData.empty());
+			{
+				UASSERT(sensorData.stereoCameraModels()[i].isValidForProjection());
+				std::vector<unsigned char> data = sensorData.stereoCameraModels()[i].serialize();
+				UASSERT(!data.empty());
+				unsigned int oldSize = calibrationData.size();
+				calibrationData.resize(calibrationData.size() + data.size());
+				memcpy(calibrationData.data()+oldSize, data.data(), data.size());
+			}
 		}
 		else
 		{
-			const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
+			UASSERT_MSG(sensorData.stereoCameraModels().size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
+			const Transform & localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
 			calibration.resize(7+localTransform.size());
-			calibration[0] = sensorData.stereoCameraModel().left().fx();
+			calibration[0] = sensorData.stereoCameraModels()[0].left().fx();
-			calibration[1] = sensorData.stereoCameraModel().left().fy();
+			calibration[1] = sensorData.stereoCameraModels()[0].left().fy();
-			calibration[2] = sensorData.stereoCameraModel().left().cx();
+			calibration[2] = sensorData.stereoCameraModels()[0].left().cx();
-			calibration[3] = sensorData.stereoCameraModel().left().cy();
+			calibration[3] = sensorData.stereoCameraModels()[0].left().cy();
-			calibration[4] = sensorData.stereoCameraModel().baseline();
+			calibration[4] = sensorData.stereoCameraModels()[0].baseline();
-			calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
+			calibration[5] = sensorData.stereoCameraModels()[0].left().imageWidth();
-			calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
+			calibration[6] = sensorData.stereoCameraModels()[0].left().imageHeight();
 			memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
 		}
 	}

corelib/src/DBReader.cpp

@@ -51,7 +51,8 @@ DBReader::DBReader(const std::string & databasePath,
 				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
-				   bool landmarksIgnored) :
+				   bool landmarksIgnored,
+				   bool featuresIgnored) :
 	Camera(frameRate),
 	_paths(uSplit(databasePath, ';')),
 	_odometryIgnored(odometryIgnored),
@@ -62,6 +63,7 @@ DBReader::DBReader(const std::string & databasePath,
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
+	_featuresIgnored(featuresIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -84,7 +86,8 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 				   int cameraIndex,
 				   int stopId,
 				   bool intermediateNodesIgnored,
-				   bool landmarksIgnored) :
+				   bool landmarksIgnored,
+				   bool featuresIgnored) :
 	Camera(frameRate),
    _paths(databasePaths),
 	_odometryIgnored(odometryIgnored),
@@ -95,6 +98,7 @@ DBReader::DBReader(const std::list<std::string> & databasePaths,
 	_cameraIndex(cameraIndex),
 	_intermediateNodesIgnored(intermediateNodesIgnored),
 	_landmarksIgnored(landmarksIgnored),
+	_featuresIgnored(featuresIgnored),
 	_dbDriver(0),
 	_currentId(_ids.end()),
 	_previousMapId(-1),
@@ -182,8 +186,8 @@ bool DBReader::init(
 	if(_ids.size())
 	{
 		std::vector<CameraModel> models;
-		StereoCameraModel stereoModel;
+		std::vector<StereoCameraModel> stereoModels;
-		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
+		if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModels))
 		{
 			if(models.size())
 			{
@@ -204,7 +208,7 @@ bool DBReader::init(
 					}
 				}
 			}
-			else if(stereoModel.isValidForProjection())
+			else if(stereoModels.size() && stereoModels.at(0).isValidForProjection())
 			{
 				_calibrated = true;
 			}
@@ -433,20 +437,31 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					infMatrix = links.begin()->second.infMatrix();
 					_previousInfMatrix = infMatrix;
 				}
-				else if(_previousMapId != s->mapId())
-				{
-					// first node, set high variance to make rtabmap trigger a new map
-					infMatrix /= 9999.0;
-					UDEBUG("First node of map %d, variance set to 9999", s->mapId());
-				}
 				else
 				{
-					if(_previousInfMatrix.empty())
+					// if localization data saved in database, covariance will be set in a prior link
+					_dbDriver->loadLinks(*_currentId, links, Link::kPosePrior);
+					if(links.size())
 					{
-						_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+						// assume the first is the backward neighbor, take its variance
+						infMatrix = links.begin()->second.infMatrix();
+						_previousInfMatrix = infMatrix;
+					}
+					else if(_previousMapId != s->mapId())
+					{
+						// first node, set high variance to make rtabmap trigger a new map
+						infMatrix /= 9999.0;
+						UDEBUG("First node of map %d, variance set to 9999", s->mapId());
+					}
+					else
+					{
+						if(_previousInfMatrix.empty())
+						{
+							_previousInfMatrix = cv::Mat::eye(6,6,CV_64FC1);
+						}
+						// we have a node not linked to map, use last variance
+						infMatrix = _previousInfMatrix;
 					}
-					// we have a node not linked to map, use last variance
-					infMatrix = _previousInfMatrix;
 				}
 				_previousMapId = s->mapId();
 			}
@@ -558,13 +573,14 @@ SensorData DBReader::getNextData(CameraInfo * info)
 			cv::Mat descriptors = s->getWordsDescriptors().clone();
 			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
 			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
-			if(!keypoints.empty() &&
+			if(!_featuresIgnored &&
+				!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))
 			{
 				data.setFeatures(keypoints, keypoints3D, descriptors);
 			}
-			else if(!keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
+			else if(!_featuresIgnored && !keypoints.empty() && (!keypoints3D.empty() || !descriptors.empty()))
 			{
 				UERROR("Missing feature data, features won't be published.");
 			}

corelib/src/Features2d.cpp

@@ -792,7 +792,9 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 	std::vector<cv::Point3f> keypoints3D;
 	if(keypoints.size())
 	{
-		if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
+		if(!data.rightRaw().empty() && !data.imageRaw().empty() &&
+			!data.stereoCameraModels().empty() &&
+			data.stereoCameraModels()[0].isValidForProjection())
 		{
 			//stereo
 			cv::Mat imageMono;
@@ -808,22 +810,121 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
 
 			std::vector<cv::Point2f> leftCorners;
 			cv::KeyPoint::convert(keypoints, leftCorners);
-			std::vector<unsigned char> status;
 
 			std::vector<cv::Point2f> rightCorners;
-			rightCorners = _stereo->computeCorrespondences(
-					imageMono,
-					data.rightRaw(),
-					leftCorners,
-					status);
 
-			keypoints3D = util3d::generateKeypoints3DStereo(
+			if(data.stereoCameraModels().size() == 1)
-					leftCorners,
+			{
-					rightCorners,
+				std::vector<unsigned char> status;
-					data.stereoCameraModel(),
+				rightCorners = _stereo->computeCorrespondences(
-					status,
+						imageMono,
-					_minDepth,
+						data.rightRaw(),
-					_maxDepth);
+						leftCorners,
+						status);
+
+				if(ULogger::level() >= ULogger::kWarning)
+				{
+					int rejected = 0;
+					for(size_t i=0; i<status.size(); ++i)
+					{
+						if(status[i]==0)
+						{
+							++rejected;
+						}
+					}
+					if(rejected > (int)status.size()/2)
+					{
+						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
+								"Optical flow may have failed because images are not calibrated, "
+								"the background is too far (no disparity between the images), "
+								"maximum disparity may be too small (%f) or that exposure between "
+								"left and right images is too different.",
+								rejected,
+								(int)status.size(),
+								_stereo->maxDisparity());
+					}
+				}
+
+				keypoints3D = util3d::generateKeypoints3DStereo(
+						leftCorners,
+						rightCorners,
+						data.stereoCameraModels()[0],
+						status,
+						_minDepth,
+						_maxDepth);
+			}
+			else
+			{
+				int subImageWith = imageMono.cols / data.stereoCameraModels().size();
+				UASSERT(imageMono.cols % subImageWith == 0);
+				std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
+				std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
+				// Assign keypoints per camera
+				for(size_t i=0; i<leftCorners.size(); ++i)
+				{
+					int cameraIndex = int(leftCorners[i].x / subImageWith);
+					leftCorners[i].x -= cameraIndex*subImageWith;
+					subLeftCorners[cameraIndex].push_back(leftCorners[i]);
+					subIndex[cameraIndex].push_back(i);
+				}
+
+				keypoints3D.resize(keypoints.size());
+				int total = 0;
+				int rejected = 0;
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
+				{
+					if(!subLeftCorners[i].empty())
+					{
+						std::vector<unsigned char> status;
+						rightCorners = _stereo->computeCorrespondences(
+								imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
+								subLeftCorners[i],
+								status);
+
+						std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
+								subLeftCorners[i],
+								rightCorners,
+								data.stereoCameraModels()[i],
+								status,
+								_minDepth,
+								_maxDepth);
+
+						if(ULogger::level() >= ULogger::kWarning)
+						{
+							for(size_t i=0; i<status.size(); ++i)
+							{
+								if(status[i]==0)
+								{
+									++rejected;
+								}
+							}
+							total+=status.size();
+						}
+
+						UASSERT(subIndex[i].size() == subKeypoints3D.size());
+						for(size_t j=0; j<subKeypoints3D.size(); ++j)
+						{
+							keypoints3D[subIndex[i][j]] = subKeypoints3D[j];
+						}
+					}
+				}
+
+				if(ULogger::level() >= ULogger::kWarning)
+				{
+					if(rejected > total/2)
+					{
+						UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
+								"Optical flow may have failed because images are not calibrated, "
+								"the background is too far (no disparity between the images), "
+								"maximum disparity may be too small (%f) or that exposure between "
+								"left and right images is too different.",
+								rejected,
+								total,
+								_stereo->maxDisparity());
+					}
+				}
+			}
 		}
 		else if(!data.depthRaw().empty() && data.cameraModels().size())
 		{

corelib/src/Graph.cpp

@@ -130,11 +130,11 @@ bool exportPoses(
 						// header
 						if(format == 11)
 						{
-							fprintf(fout, "# timestamp x y z qx qy qz qw id\n");
+							fprintf(fout, "#timestamp x y z qx qy qz qw id\n");
 						}
 						else
 						{
-							fprintf(fout, "# timestamp x y z qx qy qz qw\n");
+							fprintf(fout, "#timestamp x y z qx qy qz qw\n");
 						}
 					}
 
@@ -523,7 +523,10 @@ bool exportGPS(
 			std::string values;
 			for(std::map<int, GPS>::const_iterator iter=gpsValues.begin(); iter!=gpsValues.end(); ++iter)
 			{
-				values += uFormat("%f,%f,%f ", iter->second.longitude(), iter->second.latitude(), iter->second.altitude());
+				values += uFormat("%s,%s,%s ",
+                                  uReplaceChar(uNumber2Str(iter->second.longitude()), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.latitude()), ',', '.').c_str(),
+                                  uReplaceChar(uNumber2Str(iter->second.altitude()), ',', '.').c_str());
 			}
 
 			// switch argb (Qt format) -> abgr

corelib/src/MarkerDetector.cpp

@@ -130,12 +130,70 @@ std::map<int, Transform> MarkerDetector::detect(const cv::Mat & image, const Cam
     return detections;
 }
 
+std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
+                           const std::vector<CameraModel> & models,
+                           const cv::Mat & depth,
+                           const std::map<int, float> & markerLengths,
+                           cv::Mat * imageWithDetections)
+{
+	UASSERT(!models.empty() && !image.empty());
+	UASSERT(int((image.cols/models.size())*models.size()) == image.cols);
+	UASSERT(int((depth.cols/models.size())*models.size()) == depth.cols);
+	int subRGBWidth = image.cols/models.size();
+	int subDepthWidth = depth.cols/models.size();
+
+	std::map<int, MarkerInfo> allInfo;
+	for(size_t i=0; i<models.size(); ++i)
+	{
+		cv::Mat subImage(image, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows));
+		cv::Mat subDepth;
+		if(!depth.empty())
+			subDepth = cv::Mat(depth, cv::Rect(subDepthWidth*i, 0, subDepthWidth, depth.rows));
+		CameraModel model = models[i];
+		cv::Mat subImageWithDetections;
+		std::map<int, MarkerInfo> subInfo = detect(subImage, model, subDepth, markerLengths, imageWithDetections?&subImageWithDetections:0);
+		if(ULogger::level() >= ULogger::kWarning)
+		{
+			for(std::map<int, MarkerInfo>::iterator iter=subInfo.begin(); iter!=subInfo.end(); ++iter)
+			{
+				std::pair<std::map<int, MarkerInfo>::iterator, bool> inserted = allInfo.insert(*iter);
+				if(!inserted.second)
+				{
+					UWARN("Marker %d already added by another camera, ignoring detection from camera %d", iter->first, i);
+				}
+			}
+		}
+		else
+		{
+			allInfo.insert(subInfo.begin(), subInfo.end());
+		}
+		if(imageWithDetections)
+		{
+			if(i==0)
+			{
+				*imageWithDetections = image.clone();
+			}
+			if(!subImageWithDetections.empty())
+			{
+				subImageWithDetections.copyTo(cv::Mat(*imageWithDetections, cv::Rect(subRGBWidth*i, 0, subRGBWidth, image.rows)));
+			}
+		}
+	}
+	return allInfo;
+}
+
 std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                            const CameraModel & model,
                            const cv::Mat & depth,
                            const std::map<int, float> & markerLengths,
                            cv::Mat * imageWithDetections)
 {
+	if(!image.empty() && image.cols != model.imageWidth())
+	{
+		UERROR("This method cannot handle multi-camera marker detection, use the other function version supporting it.");
+		return std::map<int, MarkerInfo>();
+	}
+
     std::map<int, MarkerInfo> detections;
 
 #ifdef HAVE_OPENCV_ARUCO
@@ -257,7 +315,7 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
 								 R.at<double>(2,0), R.at<double>(2,1), R.at<double>(2,2), tvecs[i].val[2]);
 				Transform pose = model.localTransform() * t;
 				detections.insert(std::make_pair(ids[i], MarkerInfo(ids[i], length, pose)));
-				UDEBUG("Marker %d detected at %s (%s)", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str());
+				UDEBUG("Marker %d detected in base_link: %s, optical_link=%s, local transform=%s", ids[i], pose.prettyPrint().c_str(), t.prettyPrint().c_str(), model.localTransform().prettyPrint().c_str());
 			}
 		}
 		if(markerLength_ == 0 && !scales.empty())
@@ -304,7 +362,11 @@ std::map<int, MarkerInfo> MarkerDetector::detect(const cv::Mat & image,
                 std::map<int, MarkerInfo>::iterator iter = detections.find(ids[i]);
                 if(iter!=detections.end())
                 {
+#if CV_MAJOR_VERSION > 4 || (CV_MAJOR_VERSION == 4 && (CV_MINOR_VERSION >1 || (CV_MINOR_VERSION==1 && CV_PATCH_VERSION>=1)))
+                    cv::drawFrameAxes(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+#else
                     cv::aruco::drawAxis(*imageWithDetections, model.K(), model.D(), rvecs[i], tvecs[i], iter->second.length() * 0.5f);
+#endif
                 }
 			}
 		}

corelib/src/Memory.cpp

@@ -101,6 +101,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_laserScanGroundNormalsUp(Parameters::defaultIcpPointToPlaneGroundNormalsUp()),
 	_reextractLoopClosureFeatures(Parameters::defaultRGBDLoopClosureReextractFeatures()),
 	_localBundleOnLoopClosure(Parameters::defaultRGBDLocalBundleOnLoopClosure()),
+	_invertedReg(Parameters::defaultRGBDInvertedReg()),
 	_rehearsalMaxDistance(Parameters::defaultRGBDLinearUpdate()),
 	_rehearsalMaxAngle(Parameters::defaultRGBDAngularUpdate()),
 	_rehearsalWeightIgnoredWhileMoving(Parameters::defaultMemRehearsalWeightIgnoredWhileMoving()),
@@ -122,14 +123,22 @@ Memory::Memory(const ParametersMap & parameters) :
 	_linksChanged(false),
 	_signaturesAdded(0),
 	_allNodesInWM(true),
-
 	_badSignRatio(Parameters::defaultKpBadSignRatio()),
 	_tfIdfLikelihoodUsed(Parameters::defaultKpTfIdfLikelihoodUsed()),
-	_parallelized(Parameters::defaultKpParallelized())
+	_parallelized(Parameters::defaultKpParallelized()),
+	_registrationVis(0)
 {
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
+	if(!_registrationPipeline->isImageRequired())
+	{
+		// make sure feature matching is used instead of optical flow to compute the guess
+		ParametersMap tmp = parameters;
+		uInsert(tmp, ParametersPair(Parameters::kVisCorType(), "0"));
+		uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+		_registrationVis = new RegistrationVis(tmp);
+	}
 
 	// for local scan matching, correspondences ratio should be two times higher as we expect more matches
 	float corRatio = Parameters::defaultIcpCorrespondenceRatio();
@@ -531,6 +540,7 @@ Memory::~Memory()
 	delete _vwd;
 	delete _registrationPipeline;
 	delete _registrationIcpMulti;
+	delete _registrationVis;
 	delete _occupancy;
 }
 
@@ -569,6 +579,15 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kIcpPointToPlaneGroundNormalsUp(), _laserScanGroundNormalsUp);
 	Parameters::parse(params, Parameters::kRGBDLoopClosureReextractFeatures(), _reextractLoopClosureFeatures);
 	Parameters::parse(params, Parameters::kRGBDLocalBundleOnLoopClosure(), _localBundleOnLoopClosure);
+	Parameters::parse(params, Parameters::kRGBDInvertedReg(), _invertedReg);
+	if(_invertedReg && _localBundleOnLoopClosure)
+	{
+		UWARN("%s and %s cannot be used at the same time, disabling %s...",
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str(),
+				Parameters::kRGBDInvertedReg().c_str(),
+				Parameters::kRGBDLocalBundleOnLoopClosure().c_str());
+		_localBundleOnLoopClosure = false;
+	}
 	Parameters::parse(params, Parameters::kRGBDLinearUpdate(), _rehearsalMaxDistance);
 	Parameters::parse(params, Parameters::kRGBDAngularUpdate(), _rehearsalMaxAngle);
 	Parameters::parse(params, Parameters::kMemRehearsalWeightIgnoredWhileMoving(), _rehearsalWeightIgnoredWhileMoving);
@@ -647,12 +666,28 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	uInsert(parameters_, ParametersPair(Parameters::kVisCorType(), "0"));
 	uInsert(params, ParametersPair(Parameters::kVisCorType(), "0"));
 
+	Registration::Type currentStrategy = Registration::kTypeUndef;
+	if(_registrationPipeline)
+	{
+		if(_registrationPipeline->isImageRequired() && _registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeVisIcp;
+		}
+		else if(_registrationPipeline->isImageRequired())
+		{
+			currentStrategy = Registration::kTypeVis;
+		}
+		else if(_registrationPipeline->isScanRequired())
+		{
+			currentStrategy = Registration::kTypeIcp;
+		}
+	}
 	Registration::Type regStrategy = Registration::kTypeUndef;
 	if((iter=params.find(Parameters::kRegStrategy())) != params.end())
 	{
 		regStrategy = (Registration::Type)std::atoi((*iter).second.c_str());
 	}
-	if(regStrategy!=Registration::kTypeUndef)
+	if(regStrategy!=Registration::kTypeUndef && regStrategy != currentStrategy)
 	{
 		UDEBUG("new registration strategy %d", int(regStrategy));
 		if(_registrationPipeline)
@@ -662,10 +697,29 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		}
 
 		_registrationPipeline = Registration::create(regStrategy, parameters_);
+
+		if(!_registrationPipeline->isImageRequired() && _registrationVis == 0)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis = new RegistrationVis(tmp);
+		}
+		else if(_registrationPipeline->isImageRequired() && _registrationVis)
+		{
+			delete _registrationVis;
+			_registrationVis = 0;
+		}
 	}
 	else if(_registrationPipeline)
 	{
 		_registrationPipeline->parseParameters(params);
+
+		if(_registrationVis)
+		{
+			ParametersMap tmp = params;
+			uInsert(tmp, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
+			_registrationVis->parseParameters(tmp);
+		}
 	}
 
 	if(_registrationIcpMulti)
@@ -1768,7 +1822,7 @@ void Memory::clear()
 	_linksChanged = false;
 	_gpsOrigin = GPS();
 	_rectCameraModels.clear();
-	_rectStereoCameraModel = StereoCameraModel();
+	_rectStereoCameraModels.clear();
 	_odomMaxInf.clear();
 	_groundTruths.clear();
 	_labels.clear();
@@ -2811,8 +2865,21 @@ Transform Memory::computeTransform(
 		(fromS.getWords().size() && toS.getWords().size()) ||
 		(!guess.isNull() && !_registrationPipeline->isImageRequired()))
 	{
-		Signature tmpFrom = fromS;
+		Signature tmpFrom, tmpTo;
-		Signature tmpTo = toS;
+		if(_invertedReg)
+		{
+			tmpFrom = toS;
+			tmpTo = fromS;
+			if(!guess.isNull())
+			{
+				guess = guess.inverse();
+			}
+		}
+		else
+		{
+			tmpFrom = fromS;
+			tmpTo = toS;
+		}
 
 		if(_reextractLoopClosureFeatures && (_registrationPipeline->isImageRequired() || guess.isNull()))
 		{
@@ -2835,12 +2902,8 @@ Transform Memory::computeTransform(
 		{
 			UDEBUG("");
 			// no visual in the pipeline, make visual registration for guess
-			// make sure feature matching is used instead of optical flow to compute the guess
+			UASSERT(_registrationVis!=0);
-			ParametersMap parameters = parameters_;
+			guess = _registrationVis->computeTransformation(tmpFrom, tmpTo, guess, info);
-			uInsert(parameters, ParametersPair(Parameters::kVisCorType(), "0"));
-			uInsert(parameters, ParametersPair(Parameters::kRegRepeatOnce(), "false"));
-			RegistrationVis regVis(parameters);
-			guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info);
 			if(!guess.isNull())
 			{
 				transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
@@ -2851,6 +2914,7 @@ Transform Memory::computeTransform(
 				_registrationPipeline->isImageRequired() &&
 			   !_registrationPipeline->isScanRequired() &&
 			   !_registrationPipeline->isUserDataRequired() &&
+			   !_invertedReg &&
 			   !tmpTo.getWordsDescriptors().empty() &&
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
@@ -2923,7 +2987,7 @@ Transform Memory::computeTransform(
 				}
 				std::map<int, Transform> bundlePoses;
 				std::multimap<int, Link> bundleLinks;
-				std::map<int, CameraModel> bundleModels;
+				std::map<int, std::vector<CameraModel> > bundleModels;
 				std::map<int, std::map<int, FeatureBA> > wordReferences;
 
 				std::multimap<int, Link> links = fromS.getLinks();
@@ -2949,28 +3013,32 @@ Transform Memory::computeTransform(
 						}
 						if(s)
 						{
-							CameraModel model;
+							std::vector<CameraModel> models;
-							if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
+							if(s->sensorData().cameraModels().size() >= 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
 							{
-								model = s->sensorData().cameraModels()[0];
+								models = s->sensorData().cameraModels();
 							}
-							else if(s->sensorData().stereoCameraModel().isValidForProjection())
+							else if(s->sensorData().stereoCameraModels().size() >= 1 && s->sensorData().stereoCameraModels().at(0).isValidForProjection())
 							{
-								model = s->sensorData().stereoCameraModel().left();
+								for(size_t i=0; i<s->sensorData().stereoCameraModels().size(); ++i)
-								// Set Tx for stereo BA
+								{
-								model = CameraModel(model.fx(),
+									CameraModel model = s->sensorData().stereoCameraModels()[i].left();
-										model.fy(),
+									// Set Tx for stereo BA
-										model.cx(),
+									model = CameraModel(model.fx(),
-										model.cy(),
+											model.fy(),
-										model.localTransform(),
+											model.cx(),
-										-s->sensorData().stereoCameraModel().baseline()*model.fx());
+											model.cy(),
+											model.localTransform(),
+											-s->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
+											model.imageSize());
+									models.push_back(model);
+								}
 							}
 							else
 							{
 								UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
 							}
-							bundleModels.insert(std::make_pair(id, model));
+							bundleModels.insert(std::make_pair(id, models));
-							Transform invLocalTransform = model.localTransform().inverse();
 							UASSERT(iter->second.isValid() || iter->first == fromS.id());
 
 							if(iter->second.transform().isNull())
@@ -2990,16 +3058,27 @@ Transform Memory::computeTransform(
 								if(points3DMap.find(jter->first)!=points3DMap.end() &&
 										(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
 								{
+									cv::KeyPoint kpts = s->getWordsKpts()[jter->second];
+									int cameraIndex = 0;
+									if(models.size()>1)
+									{
+										UASSERT(models[0].imageWidth()>0);
+										float subImageWidth = models[0].imageWidth();
+										cameraIndex = int(kpts.pt.x / subImageWidth);
+										kpts.pt.x = kpts.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									//get depth
 									float d = 0.0f;
 									if( !s->getWords3().empty() &&
 										util3d::isFinite(s->getWords3()[jter->second]))
 									{
 										//move back point in camera frame (to get depth along z)
+										Transform invLocalTransform = models[cameraIndex].localTransform().inverse();
 										d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
 									}
 									wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
-									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
+									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(kpts, d, cv::Mat(), cameraIndex)));
 									++totalWordReferences;
 								}
 							}
@@ -3060,6 +3139,10 @@ Transform Memory::computeTransform(
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}
+		if(_invertedReg && !transform.isNull())
+		{
+			transform = transform.inverse();
+		}
 	}
 	return transform;
 }
@@ -4070,19 +4153,19 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 
 void Memory::getNodeCalibration(int nodeId,
 		std::vector<CameraModel> & models,
-		StereoCameraModel & stereoModel) const
+		std::vector<StereoCameraModel> & stereoModels) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
 	Signature * s = this->_getSignature(nodeId);
 	if(s)
 	{
 		models = s->sensorData().cameraModels();
-		stereoModel = s->sensorData().stereoCameraModel();
+		stereoModels = s->sensorData().stereoCameraModels();
 	}
 	else if(_dbDriver)
 	{
 		// load from database
-		_dbDriver->getCalibration(nodeId, models, stereoModel);
+		_dbDriver->getCalibration(nodeId, models, stereoModels);
 	}
 }
 
@@ -4405,15 +4488,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						CV_16UC1, CV_32FC1, CV_8UC1).c_str());
 
 	if(!data.depthOrRightRaw().empty() &&
-		data.cameraModels().size() == 0 &&
+		data.cameraModels().empty() &&
-		!data.stereoCameraModel().isValidForProjection() &&
+		data.stereoCameraModels().empty() &&
 		!pose.isNull())
 	{
-		UERROR("Camera calibration not valid, calibrate your camera!");
+		UERROR("No camera calibration found, calibrate your camera!");
-		if(data.cameraModels().empty())
-			std::cout << data.stereoCameraModel() << std::endl;
-		else
-			std::cout << data.cameraModels()[0] << std::endl;
 		return 0;
 	}
 	UASSERT(_feature2D != 0);
@@ -4464,6 +4543,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		// we assume that once rtabmap is receiving data, the calibration won't change over time
 		if(data.cameraModels().size())
 		{
+			UDEBUG("Monocular rectification");
 			// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
 			UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
 			int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
@@ -4505,25 +4585,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
 		}
-		else if(data.stereoCameraModel().isValidForRectification())
+		else if(data.stereoCameraModels().size())
 		{
-			if(!_rectStereoCameraModel.isValidForRectification())
+			UDEBUG("Stereo rectification");
+			UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
+			int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
+			UASSERT(subImageWidth == data.rightRaw().cols/(int)data.stereoCameraModels().size());
+			cv::Mat rectifiedLefts(data.imageRaw().size(), data.imageRaw().type());
+			cv::Mat rectifiedRights(data.rightRaw().size(), data.rightRaw().type());
+			bool initRectMaps = _rectStereoCameraModels.empty();
+			if(initRectMaps)
 			{
-				_rectStereoCameraModel = data.stereoCameraModel();
+				_rectStereoCameraModels.resize(data.stereoCameraModels().size());
-				if(!_rectStereoCameraModel.isRectificationMapInitialized())
+			}
+
+			for(unsigned int i=0; i<data.stereoCameraModels().size(); ++i)
+			{
+				if(data.stereoCameraModels()[i].isValidForRectification())
 				{
-					UWARN("Initializing rectification maps (only done for the first image received)...");
+					if(initRectMaps)
-					_rectStereoCameraModel.initRectificationMap();
+					{
-					UWARN("Initializing rectification maps (only done for the first image received)...done!");
+						_rectStereoCameraModels[i] = data.stereoCameraModels()[i];
+						if(!_rectStereoCameraModels[i].isRectificationMapInitialized())
+						{
+							UWARN("Initializing rectification maps (only done for the first image received)...");
+							_rectStereoCameraModels[i].initRectificationMap();
+							UWARN("Initializing rectification maps (only done for the first image received)...done!");
+						}
+					}
+					UASSERT(_rectStereoCameraModels[i].left().imageWidth() == data.stereoCameraModels()[i].left().imageWidth());
+					UASSERT(_rectStereoCameraModels[i].left().imageHeight() == data.stereoCameraModels()[i].left().imageHeight());
+
+					cv::Mat rectifiedLeft = _rectStereoCameraModels[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					cv::Mat rectifiedRight = _rectStereoCameraModels[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+					rectifiedLeft.copyTo(cv::Mat(rectifiedLefts, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					rectifiedRight.copyTo(cv::Mat(rectifiedRights, cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+					imagesRectified = true;
+				}
+				else
+				{
+					UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
+						"full calibration. If images are already rectified, set %s parameter back to true.",
+						(int)i,
+						Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					std::cout << data.stereoCameraModels()[i] << std::endl;
+					return 0;
 				}
 			}
-			UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
+
-			UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
 			data.setStereoImage(
-					_rectStereoCameraModel.left().rectifyImage(data.imageRaw()),
+					rectifiedLefts,
-					_rectStereoCameraModel.right().rectifyImage(data.rightRaw()),
+					rectifiedRights,
-					data.stereoCameraModel());
+					data.stereoCameraModels());
-			imagesRectified = true;
 		}
 		else
 		{
@@ -4596,17 +4709,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 							util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
 							cameraModels);
 				}
-				else
+
+				std::vector<StereoCameraModel> stereoCameraModels = decimatedData.stereoCameraModels();
+				for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
+				{
+					stereoCameraModels[i].scale(1.0/double(_imagePreDecimation));
+				}
+				if(!stereoCameraModels.empty())
 				{
-					StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
-					if(stereoModel.isValidForProjection())
-					{
-						stereoModel.scale(1.0/double(_imagePreDecimation));
-					}
 					decimatedData.setStereoImage(
 							util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
 							util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation),
-							stereoModel);
+							stereoCameraModels);
 				}
 			}
 
@@ -4845,7 +4959,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					keypoints3D = data.keypoints3D();
 				}
 				else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
-				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
+				   (!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
 				{
 					keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
 					t = timer.ticks();
@@ -5050,7 +5164,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 		if(keypoints3D.empty() &&
 			((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
-		   (!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
+		   (!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
 		{
 			keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
 		}
@@ -5223,40 +5337,37 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("Detecting markers...");
 		if(landmarks.empty())
 		{
-			std::map<int, MarkerInfo> markers;
+			std::vector<CameraModel> models = data.cameraModels();
-			if(!data.cameraModels().empty() && data.cameraModels()[0].isValidForProjection())
+			if(models.empty())
 			{
-				if(data.cameraModels().size() > 1)
+				for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
 				{
-					static bool warned = false;
+					models.push_back(data.stereoCameraModels()[i].left());
-					if(!warned)
+				}
+			}
+
+			if(!models.empty() && models[0].isValidForProjection())
+			{
+				std::map<int, MarkerInfo> markers = _markerDetector->detect(data.imageRaw(), models, data.depthRaw(), _landmarksSize);
+
+				for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
+				{
+					if(iter->first <= 0)
 					{
-						UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
+						UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
+						continue;
 					}
-					warned = true;
+					cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-				}
+					covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
-				else
+					covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
-				{
+					landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
-					markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw(), _landmarksSize);
 				}
+				UDEBUG("Markers detected = %d", (int)markers.size());
 			}
-			else if(data.stereoCameraModel().isValidForProjection())
+			else
 			{
-				markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left(), cv::Mat(), _landmarksSize);
+				UWARN("No valid camera calibration for marker detection");
 			}
-			for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
-			{
-				if(iter->first <= 0)
-				{
-					UERROR("Invalid marker received! IDs should be > 0 (it is %d). Ignoring this marker.", iter->first);
-					continue;
-				}
-				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
-				covariance(cv::Range(0,3), cv::Range(0,3)) *= _markerLinVariance;
-				covariance(cv::Range(3,6), cv::Range(3,6)) *= _markerAngVariance;
-				landmarks.insert(std::make_pair(iter->first, Landmark(iter->first, iter->second.length(), iter->second.pose(), covariance)));
-			}
-			UDEBUG("Markers detected = %d", (int)markers.size());
 		}
 		else
 		{
@@ -5270,7 +5381,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	cv::Mat image = data.imageRaw();
 	cv::Mat depthOrRightImage = data.depthOrRightRaw();
 	std::vector<CameraModel> cameraModels = data.cameraModels();
-	StereoCameraModel stereoCameraModel = data.stereoCameraModel();
+	std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
 
 	// apply decimation?
 	if(_imagePostDecimation > 1 && !isIntermediateNode)
@@ -5280,7 +5391,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			image = decimatedData.imageRaw();
 			depthOrRightImage = decimatedData.depthOrRightRaw();
 			cameraModels = decimatedData.cameraModels();
-			stereoCameraModel = decimatedData.stereoCameraModel();
+			stereoCameraModels = decimatedData.stereoCameraModels();
 		}
 		else
 		{
@@ -5308,9 +5419,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			{
 				cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
 			}
-			if(stereoCameraModel.isValidForProjection())
+			for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
 			{
-				stereoCameraModel.scale(1.0/double(_imagePostDecimation));
+				stereoCameraModels[i].scale(1.0/double(_imagePostDecimation));
 			}
 		}
 
@@ -5558,7 +5669,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			"",
 			pose,
 			data.groundTruth(),
-			stereoCameraModel.isValidForProjection()?
+			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
 							LaserScan(compressedScan,
@@ -5576,7 +5687,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 								laserScan.localTransform()),
 						compressedImage,
 						compressedDepth,
-						stereoCameraModel,
+						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
@@ -5642,7 +5753,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			"",
 			pose,
 			data.groundTruth(),
-			stereoCameraModel.isValidForProjection()?
+			!stereoCameraModels.empty()?
 				SensorData(
 						laserScan.angleIncrement() == 0.0f?
 								LaserScan(compressedScan,
@@ -5660,7 +5771,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 									laserScan.localTransform()),
 						cv::Mat(),
 						cv::Mat(),
-						stereoCameraModel,
+						stereoCameraModels,
 						id,
 						0,
 						compressedUserData):
@@ -5698,7 +5809,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 	else
 	{
-		s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModel, false);
+		s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
 	}
 	s->sensorData().setLaserScan(laserScan, false);
 	s->sensorData().setUserData(data.userDataRaw(), false);
@@ -5774,7 +5885,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
 			}
 			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
-			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
+			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*M_PI/180.0);
 			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
 
 			UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());

corelib/src/OccupancyGrid.cpp

@@ -430,8 +430,25 @@ void OccupancyGrid::createLocalMap(
 			}
 			else
 			{
-				const Transform & t = node.sensorData().stereoCameraModel().localTransform();
+				// average of all local transforms
-				viewPoint = cv::Point3f(t.x(), t.y(), t.z());
+				float sum = 0;
+				for(unsigned int i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
+				{
+					const Transform & t = node.sensorData().stereoCameraModels()[i].localTransform();
+					if(!t.isNull())
+					{
+						viewPoint.x += t.x();
+						viewPoint.y += t.y();
+						viewPoint.z += t.z();
+						sum += 1.0f;
+					}
+				}
+				if(sum > 0.0f)
+				{
+					viewPoint.x /= sum;
+					viewPoint.y /= sum;
+					viewPoint.z /= sum;
+				}
 			}
 
 			cv::Mat scanGroundCells;

corelib/src/Odometry.cpp

@@ -328,35 +328,73 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 
 	if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
 	{
-		if(data.stereoCameraModel().isValidForRectification())
+		if(!data.stereoCameraModels().empty())
 		{
-			if(!stereoModel_.isRectificationMapInitialized() ||
+			bool valid = true;
-				stereoModel_.left().imageSize() != data.stereoCameraModel().left().imageSize())
+			if(data.stereoCameraModels().size() != stereoModels_.size())
 			{
-				stereoModel_ = data.stereoCameraModel();
+				stereoModels_.clear();
-				stereoModel_.initRectificationMap();
+				valid = false;
-				if(stereoModel_.isRectificationMapInitialized())
+			}
+			else
+			{
+				for(size_t i=0; i<data.stereoCameraModels().size() && valid; ++i)
+				{
+					valid = stereoModels_[i].isRectificationMapInitialized() &&
+							stereoModels_[i].left().imageSize() == data.stereoCameraModels()[i].left().imageSize();
+				}
+			}
+
+			if(!valid)
+			{
+				stereoModels_ = data.stereoCameraModels();
+				valid = true;
+				for(size_t i=0; i<stereoModels_.size() && valid; ++i)
+				{
+					stereoModels_[i].initRectificationMap();
+					valid = stereoModels_[i].isRectificationMapInitialized();
+				}
+				if(valid)
 				{
 					UWARN("%s parameter is set to false but the selected odometry approach cannot "
-							"process raw images. We will rectify them for convenience.",
+							"process raw stereo images. We will rectify them for convenience.",
 							Parameters::kRtabmapImagesAlreadyRectified().c_str());
 				}
 				else
 				{
-					UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
+					UERROR("Odometry approach chosen cannot process raw stereo images (not rectified images) "
 							"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
-							"Make sure images are rectified and set %s parameter back to true, or make sure "
+							"Make sure images are rectified and set %s parameter back to true, or "
-							"calibration is valid for rectification.",
+							"make sure calibration is valid for rectification",
 							Parameters::kRtabmapImagesAlreadyRectified().c_str());
+					stereoModels_.clear();
 				}
 			}
-			if(stereoModel_.isRectificationMapInitialized())
+			if(valid)
 			{
-				data.setStereoImage(
+				if(stereoModels_.size()==1)
-						stereoModel_.left().rectifyImage(data.imageRaw()),
+				{
-						stereoModel_.right().rectifyImage(data.rightRaw()),
+					data.setStereoImage(
-						stereoModel_,
+							stereoModels_[0].left().rectifyImage(data.imageRaw()),
-						false);
+							stereoModels_[0].right().rectifyImage(data.rightRaw()),
+							stereoModels_,
+							false);
+				}
+				else
+				{
+					UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
+					int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
+					cv::Mat rectifiedLeftImages = data.imageRaw().clone();
+					cv::Mat rectifiedRightImages = data.imageRaw().clone();
+					for(size_t i=0; i<stereoModels_.size() && valid; ++i)
+					{
+						cv::Mat rectifiedLeft = stereoModels_[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+						cv::Mat rectifiedRight = stereoModels_[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
+						rectifiedLeft.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+						rectifiedRight.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
+					}
+					data.setStereoImage(rectifiedLeftImages, rectifiedRightImages, stereoModels_, false);
+				}
 			}
 		}
 		else if(!data.cameraModels().empty())
@@ -599,12 +637,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		}
 		else
 		{
-			StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
+			std::vector<StereoCameraModel> stereoModels = decimatedData.stereoCameraModels();
-			if(stereoModel.isValidForProjection())
+			for(unsigned int i=0; i<stereoModels.size(); ++i)
 			{
-				stereoModel.scale(1.0/double(_imageDecimation));
+				stereoModels[i].scale(1.0/double(_imageDecimation));
+			}
+			if(!stereoModels.empty())
+			{
+				decimatedData.setStereoImage(rgbLeft, depthRight, stereoModels);
 			}
-			decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
 		}
 
 

corelib/src/OdometryThread.cpp

@@ -134,7 +134,7 @@ void OdometryThread::addData(const SensorData & data)
 	{
 		if(dynamic_cast<OdometryMono*>(_odometry) == 0)
 		{
-			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) &&
+			if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
 					data.laserScanRaw().empty())
 			{
 				ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
@@ -144,7 +144,7 @@ void OdometryThread::addData(const SensorData & data)
 		else
 		{
 			// Mono can accept RGB only
-			if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
+			if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
 			{
 				ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
 				return;

corelib/src/Optimizer.cpp

@@ -438,7 +438,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		int rootId,
 		const std::map<int, Transform> & poses,
 		const std::multimap<int, Link> & links,
-		const std::map<int, CameraModel> & models,
+		const std::map<int, std::vector<CameraModel> > & models,
 		std::map<int, cv::Point3f> & points3DMap,
 		const std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		std::set<int> * outliers)
@@ -457,37 +457,35 @@ std::map<int, Transform> Optimizer::optimizeBA(
 		bool rematchFeatures)
 {
 	UDEBUG("");
-	std::map<int, CameraModel> models;
+	std::map<int, std::vector<CameraModel> > multiModels;
 	std::map<int, Transform> poses;
 	for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
 	{
 		// Get camera model
-		CameraModel model;
+		std::vector<CameraModel> models;
 		if(uContains(signatures, iter->first))
 		{
-			if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
+			const SensorData & s = signatures.at(iter->first).sensorData();
+			if(s.cameraModels().size() >= 1 && s.cameraModels().at(0).isValidForProjection())
 			{
-				model = signatures.at(iter->first).sensorData().cameraModels()[0];
+				models = s.cameraModels();
 			}
-			else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
+			else if(!s.stereoCameraModels().empty() && s.stereoCameraModels()[0].isValidForProjection())
 			{
-				model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
+				for(size_t i=0; i<s.stereoCameraModels().size(); ++i)
+				{
+					CameraModel model = s.stereoCameraModels()[i].left();
 
-				// Set Tx = -baseline*fx for stereo BA
+					// Set Tx = -baseline*fx for stereo BA
-				model = CameraModel(
+					models.push_back(CameraModel(
-						model.fx(),
+							model.fx(),
-						model.fy(),
+							model.fy(),
-						model.cx(),
+							model.cx(),
-						model.cy(),
+							model.cy(),
-						model.localTransform(),
+							model.localTransform(),
-						-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
+							-s.stereoCameraModels()[i].baseline()*model.fx(),
-			}
+							model.imageSize()));
-			else if(signatures.at(iter->first).sensorData().cameraModels().size() > 1)
+				}
-			{
-				UERROR("Multi-cameras (%d) is not supported (id=%d).",
-						signatures.at(iter->first).sensorData().cameraModels().size(),
-						iter->first);
-				return std::map<int, Transform>();
 			}
 			else
 			{
@@ -501,16 +499,14 @@ std::map<int, Transform> Optimizer::optimizeBA(
 			return std::map<int, Transform>();
 		}
 
-		UASSERT(model.isValidForProjection());
+		multiModels.insert(std::make_pair(iter->first, models));
-
-		models.insert(std::make_pair(iter->first, model));
 		poses.insert(*iter);
 	}
 
 	// compute correspondences
 	this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
 
-	return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
+	return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
 }
 
 std::map<int, Transform> Optimizer::optimizeBA(
@@ -537,9 +533,11 @@ Transform Optimizer::optimizeBA(
 	poses.insert(std::make_pair(link.to(), link.transform()));
 	std::multimap<int, Link> links;
 	links.insert(std::make_pair(link.from(), link));
-	std::map<int, CameraModel> models;
+	std::map<int, std::vector<CameraModel> > models;
-	models.insert(std::make_pair(link.from(), model));
+	std::vector<CameraModel> tmp;
-	models.insert(std::make_pair(link.to(), model));
+	tmp.push_back(model);
+	models.insert(std::make_pair(link.from(), tmp));
+	models.insert(std::make_pair(link.to(), tmp));
 	poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
 	if(poses.size() == 2)
 	{
@@ -567,7 +565,7 @@ void Optimizer::computeBACorrespondences(
 		std::map<int, std::map<int, FeatureBA> > & wordReferences,
 		bool rematchFeatures)
 {
-	UDEBUG("");
+	UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
 	int wordCount = 0;
 	int edgeWithWordsAdded = 0;
 	std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
@@ -587,6 +585,14 @@ void Optimizer::computeBACorrespondences(
 			if(sFrom.getWeight() >= 0) // ignore intermediate links
 			{
 				Signature sTo = signatures.at(link.to());
+
+				if((sFrom.sensorData().cameraModels().empty() && sFrom.sensorData().stereoCameraModels().empty()) ||
+				   (sTo.sensorData().cameraModels().empty() && sTo.sensorData().stereoCameraModels().empty()))
+				{
+					UERROR("No camera models found");
+					continue;
+				}
+
 				if(sTo.getWeight() < 0)
 				{
 					for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
@@ -675,8 +681,7 @@ void Optimizer::computeBACorrespondences(
 									wordId = ++wordCount;
 									wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
 
-									p = util3d::transformPoint(p, pose);
+									points3DMap.insert(std::make_pair(wordId, util3d::transformPoint(p, pose)));
-									points3DMap.insert(std::make_pair(wordId, p));
 								}
 								else
 								{
@@ -692,7 +697,27 @@ void Optimizer::computeBACorrespondences(
 										UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
 										descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
 									}
-									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
+									int cameraIndex = 0;
+									if(sFrom.sensorData().cameraModels().size()>1 || sFrom.sensorData().stereoCameraModels().size()>1)
+									{
+										float subImageWidth = sFrom.sensorData().cameraModels().size()>1?sFrom.sensorData().cameraModels()[0].imageWidth():sFrom.sensorData().stereoCameraModels()[0].left().imageWidth();
+										cameraIndex = int(ptFrom.pt.x / subImageWidth);
+										ptFrom.pt.x = ptFrom.pt.x - (subImageWidth*float(cameraIndex));
+									}
+									
+									float depth = 0.0f;
+									if(!sFrom.sensorData().cameraModels().empty())
+									{
+										depth = util3d::transformPoint(p, sFrom.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
+									}
+									else
+									{
+										UASSERT(!sFrom.sensorData().stereoCameraModels().empty());
+										depth = util3d::transformPoint(p, sFrom.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
+									}
+
+
+									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, depth, descriptorFrom, cameraIndex)));
 									frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
 									frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
 								}
@@ -705,17 +730,32 @@ void Optimizer::computeBACorrespondences(
 										UASSERT(indexTo < sTo.getWordsDescriptors().rows);
 										descriptorTo = sTo.getWordsDescriptors().row(indexTo);
 									}
+
+									int cameraIndex = 0;
+									if(sTo.sensorData().cameraModels().size()>1 || sTo.sensorData().stereoCameraModels().size()>1)
+									{
+										float subImageWidth = sTo.sensorData().cameraModels().size()>1?sTo.sensorData().cameraModels()[0].imageWidth():sTo.sensorData().stereoCameraModels()[0].left().imageWidth();
+										cameraIndex = int(ptTo.pt.x / subImageWidth);
+										ptTo.pt.x = ptTo.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									float depth = 0.0f;
 									if(!sTo.getWords3().empty())
 									{
 										UASSERT(indexTo < (int)sTo.getWords3().size());
 										const cv::Point3f & pt = sTo.getWords3()[indexTo];
-										if( pt.x > 0)
+										if(!sTo.sensorData().cameraModels().empty())
 										{
-											depth = pt.x;
+											depth = util3d::transformPoint(pt, sTo.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
+										}
+										else
+										{
+											UASSERT(!sTo.sensorData().stereoCameraModels().empty());
+											depth = util3d::transformPoint(pt, sTo.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
 										}
 									}
-									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
+
+									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo, cameraIndex)));
 									frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
 									frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
 								}
@@ -732,6 +772,14 @@ void Optimizer::computeBACorrespondences(
 		}
 	}
 	UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
+	if(links.empty())
+	{
+		UERROR("No links found for BA?!");
+	}
+	else if(wordCount == 0)
+	{
+		UERROR("No words added for BA?!");
+	}
 }
 
 } /* namespace rtabmap */

corelib/src/Parameters.cpp

@@ -188,7 +188,8 @@ rtabmap::ParametersMap Parameters::getDefaultOdometryParameters(bool stereo, boo
 			group.compare("GTSAM") == 0 ||
 			(vis && (group.compare("Vis") == 0 || group.compare("PyMatcher") == 0 || group.compare("GMS") == 0)) ||
 			iter->first.compare(kRtabmapPublishRAMUsage())==0 ||
-			iter->first.compare(kRtabmapImagesAlreadyRectified())==0)
+			iter->first.compare(kRtabmapImagesAlreadyRectified())==0 ||
+			iter->first.compare(kKpByteToFloat())==0)
 		{
 			odomParameters.insert(*iter);
 		}
@@ -660,6 +661,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
+#endif
+				str = "With OpenGV:";
+#ifdef RTABMAP_OPENGV
+				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
+#else
+				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With Madgwick:";
 #ifdef RTABMAP_MADGWICK

corelib/src/Recovery.cpp

@@ -52,7 +52,6 @@ bool databaseRecovery(
 		return false;
 	}
 
-	std::string backupPath;
 	if(UFile::getExtension(databasePath).compare("db") != 0)
 	{
 		if(errorMsg)
@@ -61,12 +60,17 @@ bool databaseRecovery(
 	}
 	std::list<std::string> strList = uSplit(databasePath, '.');
 	strList.pop_back();
-	backupPath = uJoin(strList, ".") + ".backup.db";
+
-	if(UFile::exists(backupPath))
+	std::string recoveryPath;
+	recoveryPath = uJoin(strList, ".") + ".recovery.db";
+	if(UFile::exists(recoveryPath))
 	{
-		if(errorMsg)
+		if(UFile::erase(recoveryPath) != 0)
-			*errorMsg = uFormat("Backup file \"%s\" already exists!", backupPath.c_str());
+		{
-		return false;
+			if(errorMsg)
+				*errorMsg = uFormat("Failed to remove temporary recovery database \"%s\", is it opened by another app?", recoveryPath.c_str());
+			return false;
+		}
 	}
 
 	DBDriver * dbDriver = DBDriver::create();
@@ -125,41 +129,42 @@ bool databaseRecovery(
 	dbDriver->closeConnection(false);
 	delete dbDriver;
 
-	if(progressState)
-		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"...", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
-	if(UFile::rename(databasePath, backupPath) != 0)
-	{
-		if(errorMsg)
-			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
-		return false;
-	}
-
 	bool incrementalMemory = true;
+	bool dbInMemory = false;
 	Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
+	Parameters::parse(parameters, Parameters::kDbSqlite3InMemory(), dbInMemory);
 	if(!incrementalMemory)
 	{
 		if(progressState)
 		{
-			progressState->callback("Database is in localization mode, setting it to mapping mode to recover...");
+			progressState->callback("Database is in localization mode, setting it to mapping mode to recover.");
 		}
 		uInsert(parameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
 	}
+	if(dbInMemory)
+	{
+		if(progressState)
+		{
+			progressState->callback(uFormat("Database has %s=true, setting it to false to avoid RAM problems during recovery.", Parameters::kDbSqlite3InMemory().c_str()));
+		}
+		uInsert(parameters, ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
+	}
 
 	Rtabmap rtabmap;
-	rtabmap.init(parameters, databasePath);
+	rtabmap.init(parameters, recoveryPath);
 
 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
 	{
-		DBReader dbReader(backupPath, 0, odometryIgnored);
+		DBReader dbReader(databasePath, 0, odometryIgnored);
 		dbReader.init();
 
 		CameraInfo info;
 		SensorData data = dbReader.takeImage(&info);
 		int processed = 0;
 		if (progressState)
-			progressState->callback(uFormat("Recovering data of \"%s\"...", backupPath.c_str()));
+			progressState->callback(uFormat("Recovering data of \"%s\"...", databasePath.c_str()));
 		while (data.isValid() && (progressState == 0 || !progressState->isCanceled()))
 		{
 			std::string status;
@@ -198,25 +203,60 @@ bool databaseRecovery(
 		{
 			rtabmap.close(false);
 			if(errorMsg)
-				*errorMsg = uFormat("Recovery canceled, renaming back \"%s\" to \"%s\".", backupPath.c_str(), databasePath.c_str());
+				*errorMsg = uFormat("Recovery canceled, removing temporary recovery database \"%s\".", recoveryPath.c_str());
 
-			// put back the file as before
+			UFile::erase(recoveryPath);
-			UFile::erase(databasePath);
-			UFile::rename(backupPath, databasePath);
 			return false;
 		}
 	}
 
 	if(progressState)
-		progressState->callback(uFormat("Closing database \"%s\"...", databasePath.c_str()));
+		progressState->callback(uFormat("Closing database \"%s\"...", recoveryPath.c_str()));
 	rtabmap.close(true);
 	if(progressState)
-		progressState->callback(uFormat("Closing database \"%s\"... done!", databasePath.c_str()));
+		progressState->callback(uFormat("Closing database \"%s\"... done!", recoveryPath.c_str()));
 
-	if(!keepCorruptedDatabase)
+	if(keepCorruptedDatabase)
 	{
-		UFile::erase(backupPath);
+		std::string backupPath;
+		backupPath = uJoin(strList, ".") + ".backup.db";
+
+		if(!UFile::exists(backupPath))
+		{
+			if(progressState)
+				progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... (keep corrupted database backup option is enabled).", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
+			if(UFile::rename(databasePath, backupPath) != 0)
+			{
+				if(errorMsg)
+					*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
+				return false;
+			}
+			if(progressState)
+				progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... done!", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str()));
+		}
+		else
+		{
+			if(progressState)
+				progressState->callback(uFormat("Backup \"%s\" already exists, won't copy again.", UFile::getName(backupPath).c_str()));
+		}
 	}
+	else if(UFile::erase(databasePath) != 0)
+	{
+		if(errorMsg)
+			*errorMsg = uFormat("Failed remove original database file \"%s\". Is it opened by another app? The recovered database cannot be copied back to original name.", UFile::getName(databasePath).c_str(), UFile::getName(recoveryPath).c_str());
+		return false;
+	}
+
+	if(progressState)
+		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"...", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str()));
+	if(UFile::rename(recoveryPath, databasePath) != 0)
+	{
+		if(errorMsg)
+			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str());
+		return false;
+	}
+	if(progressState)
+		progressState->callback(uFormat("Renaming \"%s\" to \"%s\"... done!", UFile::getName(recoveryPath).c_str(), UFile::getName(databasePath).c_str()));
 
 	return true;
 }

corelib/src/Registration.cpp

@@ -33,7 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-double Registration::COVARIANCE_EPSILON = 0.000000001;
+double Registration::COVARIANCE_LINEAR_EPSILON = 0.00000001;   // 0.1 mm
+double Registration::COVARIANCE_ANGULAR_EPSILON = 0.00000003; // 0.01 deg
 
 Registration * Registration::create(const ParametersMap & parameters)
 {
@@ -237,18 +238,18 @@ Transform Registration::computeTransformationMod(
 		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
 	}
 
-	if(info.covariance.at<double>(0,0)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
-		info.covariance.at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(1,1)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
-		info.covariance.at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(2,2)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
-		info.covariance.at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(3,3)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
-		info.covariance.at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(4,4)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
-		info.covariance.at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-	if(info.covariance.at<double>(5,5)<=COVARIANCE_EPSILON)
+	if(info.covariance.at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
-		info.covariance.at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
+		info.covariance.at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
 
 
 	if(infoOut)

corelib/src/RegistrationVis.cpp

@@ -69,6 +69,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 		_PnPReprojError(Parameters::defaultVisPnPReprojError()),
 		_PnPFlags(Parameters::defaultVisPnPFlags()),
 		_PnPRefineIterations(Parameters::defaultVisPnPRefineIterations()),
+		_PnPMaxVar(Parameters::defaultVisPnPMaxVariance()),
 		_correspondencesApproach(Parameters::defaultVisCorType()),
 		_flowWinSize(Parameters::defaultVisCorFlowWinSize()),
 		_flowIterations(Parameters::defaultVisCorFlowIterations()),
@@ -124,6 +125,7 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kVisPnPReprojError(), _PnPReprojError);
 	Parameters::parse(parameters, Parameters::kVisPnPFlags(), _PnPFlags);
 	Parameters::parse(parameters, Parameters::kVisPnPRefineIterations(), _PnPRefineIterations);
+	Parameters::parse(parameters, Parameters::kVisPnPMaxVariance(), _PnPMaxVar);
 	Parameters::parse(parameters, Parameters::kVisCorType(), _correspondencesApproach);
 	Parameters::parse(parameters, Parameters::kVisCorFlowWinSize(), _flowWinSize);
 	Parameters::parse(parameters, Parameters::kVisCorFlowIterations(), _flowIterations);
@@ -287,6 +289,7 @@ Transform RegistrationVis::computeTransformationImpl(
 	UDEBUG("%s=%f", Parameters::kVisEpipolarGeometryVar().c_str(), _epipolarGeometryVar);
 	UDEBUG("%s=%f", Parameters::kVisPnPReprojError().c_str(), _PnPReprojError);
 	UDEBUG("%s=%d", Parameters::kVisPnPFlags().c_str(), _PnPFlags);
+	UDEBUG("%s=%f", Parameters::kVisPnPMaxVariance().c_str(), _PnPMaxVar);
 	UDEBUG("%s=%d", Parameters::kVisCorType().c_str(), _correspondencesApproach);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowWinSize().c_str(), _flowWinSize);
 	UDEBUG("%s=%d", Parameters::kVisCorFlowIterations().c_str(), _flowIterations);
@@ -310,7 +313,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.sensorData().imageRaw().cols,
 			fromSignature.sensorData().imageRaw().rows,
 			(int)fromSignature.sensorData().cameraModels().size(),
-			fromSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
+			(int)fromSignature.sensorData().stereoCameraModels().size());
 
 	UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
 			toSignature.id(),
@@ -323,7 +326,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			toSignature.sensorData().imageRaw().cols,
 			toSignature.sensorData().imageRaw().rows,
 			(int)toSignature.sensorData().cameraModels().size(),
-			toSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
+			(int)toSignature.sensorData().stereoCameraModels().size());
 
 	std::string msg;
 	info.projectedIDs.clear();
@@ -487,17 +490,24 @@ Transform RegistrationVis::computeTransformationImpl(
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
 				if(guessSet)
 				{
-					Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
+					if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1)
-					Transform guessCameraRef = (guess * localTransform).inverse();
+					{
-					cv::Mat R = (cv::Mat_<double>(3,3) <<
+						Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform();
-							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+						Transform guessCameraRef = (guess * localTransform).inverse();
-							(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
+						cv::Mat R = (cv::Mat_<double>(3,3) <<
-							(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
+								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
-					cv::Mat rvec(1,3, CV_64FC1);
+								(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
-					cv::Rodrigues(R, rvec);
+								(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat rvec(1,3, CV_64FC1);
-					cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
+						cv::Rodrigues(R, rvec);
-					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
+						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K();
+						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
+					}
+					else
+					{
+						UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
+					}
 				}
 
 				// Find features in the new left image
@@ -735,7 +745,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 			if(!kptsFrom3D.empty() &&
 			   (_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f) &&
-			   (!fromSignature.sensorData().cameraModels().empty() || fromSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
+			   (!fromSignature.sensorData().cameraModels().empty() || !fromSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
 			{
 				_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
 			}
@@ -770,7 +780,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 			if(kptsTo3D.size() &&
 		       (_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f) &&
-			   (!toSignature.sensorData().cameraModels().empty() || toSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
+			   (!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
 			{
 				_detectorTo->filterKeypointsByDepth(kptsTo, descriptorsTo, kptsTo3D, _detectorTo->getMinDepth(), _detectorTo->getMaxDepth());
 			}
@@ -787,15 +797,37 @@ Transform RegistrationVis::computeTransformationImpl(
 			// We have all data we need here, so match!
 			if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
 			{
-				cv::Size imageSize = imageTo.size();
+				std::vector<CameraModel> models;
-				bool isCalibrated = false; // multiple cameras not supported.
+				if(!toSignature.sensorData().stereoCameraModels().empty())
-				if(imageSize.height == 0 || imageSize.width == 0)
 				{
-					imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
+					for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+					{
+						models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+					}
+				}
+				else
+				{
+					models = toSignature.sensorData().cameraModels();
 				}
 
-				isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
+				bool isCalibrated = !models.empty();
-						(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
+				for(size_t i=0; i<models.size() && isCalibrated; ++i)
+				{
+					isCalibrated = models[i].isValidForProjection();
+
+					// For old database formats
+					if(isCalibrated && (models[i].imageWidth()==0 || models[i].imageHeight()==0))
+					{
+						if(!toSignature.sensorData().imageRaw().empty())
+						{
+							models[i].setImageSize(cv::Size(toSignature.sensorData().imageRaw().cols/models.size(), toSignature.sensorData().imageRaw().rows));
+						}
+						else
+						{
+							isCalibrated = false;
+						}
+					}
+				}
 
 				// If guess is set, limit the search of matches using optical flow window size
 				bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -803,52 +835,62 @@ Transform RegistrationVis::computeTransformationImpl(
 						isCalibrated &&  // needed for projection
 						_estimationType != 2) // To make sure we match all features for 2D->2D
 				{
+					// Use guess to project 3D "from" keypoints into "to" image
 					UDEBUG("");
 					UASSERT((int)kptsTo.size() == descriptorsTo.rows);
 					UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
 
-					// Use guess to project 3D "from" keypoints into "to" image
+					std::vector<cv::Point2f> cornersProjected;
-					if(toSignature.sensorData().cameraModels().size() > 1)
+					std::vector<int> projectedIndexToDescIndex;
+					float subImageWidth = models[0].imageWidth();
+					std::set<int> added;
+					int duplicates=0;
+					for(size_t m=0; m<models.size(); ++m)
 					{
-						UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
+						Transform guessCameraRef = (guess * models[m].localTransform()).inverse();
-					}
+						cv::Mat R = (cv::Mat_<double>(3,3) <<
+								(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+								(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
+								(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
+						cv::Mat rvec(1,3, CV_64FC1);
+						cv::Rodrigues(R, rvec);
+						cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
+						cv::Mat K = models[m].K();
+						std::vector<cv::Point2f> projected;
+						cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
+						UDEBUG("Projected points=%d", (int)projected.size());
 
-					Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
+						//remove projected points outside of the image
-					Transform guessCameraRef = (guess * localTransform).inverse();
+						UASSERT((int)projected.size() == descriptorsFrom.rows);
-					cv::Mat R = (cv::Mat_<double>(3,3) <<
+						int cornersInFrame = 0;
-							(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
+						for(unsigned int i=0; i<projected.size(); ++i)
-							(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
-							(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
-					cv::Mat rvec(1,3, CV_64FC1);
-					cv::Rodrigues(R, rvec);
-					cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
-					cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
-					std::vector<cv::Point2f> projected;
-					cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
-					UDEBUG("Projected points=%d", (int)projected.size());
-					//remove projected points outside of the image
-					UASSERT((int)projected.size() == descriptorsFrom.rows);
-					std::vector<cv::Point2f> cornersProjected(projected.size());
-					std::vector<int> projectedIndexToDescIndex(projected.size());
-					int oi=0;
-					for(unsigned int i=0; i<projected.size(); ++i)
-					{
-						if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
-						   uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
-						   util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
 						{
-							projectedIndexToDescIndex[oi] = i;
+							if(uIsInBounds(projected[i].x, 0.0f, float(models[m].imageWidth()-1)) &&
-							cornersProjected[oi++] = projected[i];
+							   uIsInBounds(projected[i].y, 0.0f, float(models[m].imageHeight()-1)) &&
+							   util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
+							{
+								if(added.find(i) != added.end())
+								{
+									++duplicates;
+									continue;
+								}
+
+								projectedIndexToDescIndex.push_back(i);
+								projected[i].x += subImageWidth*float(m); // Convert in multicam stitched image
+								cornersProjected.push_back(projected[i]);
+								++cornersInFrame;
+								added.insert(i);
+							}
 						}
+						UDEBUG("corners in frame=%d (camera index=%ld)", cornersInFrame, m);
 					}
-					projectedIndexToDescIndex.resize(oi);
-					cornersProjected.resize(oi);
-					UDEBUG("corners in frame=%d", (int)cornersProjected.size());
 
 					// For each projected feature guess of "from" in "to", find its matching feature in
 					// the radius around the projected guess.
 					// TODO: do cross-check?
-					UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size());
+					UDEBUG("guessMatchToProjection=%d, cornersProjected=%d orignalWordsFromIds=%d (added=%ld, duplicates=%d)",
+							_guessMatchToProjection?1:0, (int)cornersProjected.size(), (int)orignalWordsFromIds.size(),
+							added.size(), duplicates);
 					if(cornersProjected.size())
 					{
 						if(_guessMatchToProjection)
@@ -1147,19 +1189,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
 					{
-						if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
+						UWARN("Calibration not found! Finding correspondences "
-						{
+							   "with the guess cannot be done, global matching is "
-							UWARN("Finding correspondences with the guess cannot "
+							   "done instead.");
-								  "be done with multiple cameras, global matching is "
-								   "done instead. Please set \"%s\" to 0 to avoid this warning.",
-								   Parameters::kVisCorGuessWinSize().c_str());
-						}
-						else
-						{
-							UWARN("Calibration not found! Finding correspondences "
-								   "with the guess cannot be done, global matching is "
-								   "done instead.");
-						}
 					}
 
 					UDEBUG("");
@@ -1194,16 +1226,16 @@ Transform RegistrationVis::computeTransformationImpl(
 								descriptorsTo.type() == CV_32F &&
 								descriptorsFrom.type() == CV_32F &&
 								descriptorsFrom.rows == (int)kptsFrom.size() &&
-								imageSize.width > 0 && imageSize.height > 0)
+								models.size() == 1)
 							{
 								UDEBUG("Python matching");
-								matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, imageSize);
+								matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, models[0].imageSize());
 							}
 							else
 							{
 								if(_nnType == 6 && _pyMatcher)
 								{
-									UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported), doing bruteforce matching instead.", descriptorsFrom.type());
+									UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported, multicam not supported), doing bruteforce matching instead.", descriptorsFrom.type());
 								}
 #else
 							{
@@ -1215,11 +1247,11 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(_nnType == 7)
 								{
 									imageSizeFrom = imageFrom.size();
-									if(imageSizeFrom.height == 0 || imageSizeFrom.width == 0)
+									if((imageSizeFrom.height == 0 || imageSizeFrom.width == 0) && (fromSignature.sensorData().cameraModels().size() || fromSignature.sensorData().stereoCameraModels().size()))
 									{
-										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
+										imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModels()[0].left().imageSize();
 									}
-									if(imageSize.height > 0 && imageSize.width > 0 &&
+									if(!models.empty() && models[0].imageSize().height > 0 && models[0].imageSize().width > 0 &&
 									   imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
 									{
 										doCrossCheck = false;
@@ -1239,8 +1271,9 @@ Transform RegistrationVis::computeTransformationImpl(
 #if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
 								if(!doCrossCheck)
 								{
+									UASSERT(!models.empty());
 									std::vector<cv::DMatch> matchesGMS;
-									cv::xfeatures2d::matchGMS(imageSize, imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
+									cv::xfeatures2d::matchGMS(models[0].imageSize(), imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
 									matches = matchesGMS;
 								}
 #endif
@@ -1385,7 +1418,8 @@ Transform RegistrationVis::computeTransformationImpl(
 			if(_estimationType == 2) // Epipolar Geometry
 			{
 				UDEBUG("");
-				if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
+				if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
+					!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
 				   (signatureB->sensorData().cameraModels().size() != 1 ||
 					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
 				{
@@ -1394,8 +1428,8 @@ Transform RegistrationVis::computeTransformationImpl(
 				else if((int)signatureA->getWords().size() >= _minInliers &&
 						(int)signatureB->getWords().size() >= _minInliers)
 				{
-					UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
+					UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
-					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
+					const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
 
 					// we only need the camera transform, send guess words3 for scale estimation
 					Transform cameraTransform;
@@ -1479,16 +1513,22 @@ Transform RegistrationVis::computeTransformationImpl(
 			else if(_estimationType == 1) // PnP
 			{
 				UDEBUG("");
-				if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
+				if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
-				   (signatureB->sensorData().cameraModels().size() != 1 ||
+				   (signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
-					!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
 				{
-					UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
+					UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
 							signatureB->id(),
 							(int)signatureB->sensorData().cameraModels().size(),
-							signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
+							signatureB->sensorData().stereoCameraModels().size(),
 							signatureB->getWeight());
 				}
+#ifndef RTABMAP_OPENGV
+				else if(signatureB->sensorData().cameraModels().size() > 1)
+				{
+					UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
+							"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
+				}
+#endif
 				else
 				{
 					UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
@@ -1496,9 +1536,6 @@ Transform RegistrationVis::computeTransformationImpl(
 					if((int)signatureA->getWords3().size() >= _minInliers &&
 					   (int)signatureB->getWords().size() >= _minInliers)
 					{
-						UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
-						const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
-
 						std::vector<int> inliersV;
 						std::vector<int> matchesV;
 						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
@@ -1518,22 +1555,65 @@ Transform RegistrationVis::computeTransformationImpl(
 								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
 							}
 						}
-						transforms[dir] = util3d::estimateMotion3DTo2D(
+
-								words3A,
+						std::vector<CameraModel> models;
-								wordsB,
+						if(signatureB->sensorData().stereoCameraModels().size())
-								cameraModel,
+						{
-								_minInliers,
+							for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
-								_iterations,
+							{
-								_PnPReprojError,
+								models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
-								_PnPFlags,
+							}
-								_PnPRefineIterations,
+						}
-								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+						else
-								words3B,
+						{
-								&covariances[dir],
+							models = signatureB->sensorData().cameraModels();
-								&matchesV,
+						}
-								&inliersV);
+
-						inliers[dir] = inliersV;
+						if(models.size()>1)
-						matches[dir] = matchesV;
+						{
+							// Multi-Camera
+							UASSERT(models[0].isValidForProjection());
+
+							transforms[dir] = util3d::estimateMotion3DTo2D(
+									words3A,
+									wordsB,
+									models,
+									_minInliers,
+									_iterations,
+									_PnPReprojError,
+									_PnPFlags,
+									_PnPRefineIterations,
+									_PnPMaxVar,
+									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+									words3B,
+									&covariances[dir],
+									&matchesV,
+									&inliersV);
+							inliers[dir] = inliersV;
+							matches[dir] = matchesV;
+						}
+						else
+						{
+							UASSERT(models.size() == 1 && models[0].isValidForProjection());
+
+							transforms[dir] = util3d::estimateMotion3DTo2D(
+									words3A,
+									wordsB,
+									models[0],
+									_minInliers,
+									_iterations,
+									_PnPReprojError,
+									_PnPFlags,
+									_PnPRefineIterations,
+									_PnPMaxVar,
+									dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
+									words3B,
+									&covariances[dir],
+									&matchesV,
+									&inliersV);
+							inliers[dir] = inliersV;
+							matches[dir] = matchesV;
+						}
 						UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
 						if(transforms[dir].isNull())
 						{
@@ -1652,8 +1732,8 @@ Transform RegistrationVis::computeTransformationImpl(
 			allInliers.size() &&
 			fromSignature.getWords3().size() &&
 			toSignature.getWords().size() &&
-			fromSignature.sensorData().cameraModels().size() <= 1 &&
+			(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
-			toSignature.sensorData().cameraModels().size() <= 1)
+			(toSignature.sensorData().stereoCameraModels().size() >= 1 || toSignature.sensorData().cameraModels().size() >= 1))
 		{
 			UDEBUG("Refine with bundle adjustment");
 			Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
@@ -1668,18 +1748,18 @@ Transform RegistrationVis::computeTransformationImpl(
 			for(int i=0;i<2;++i)
 			{
 				UASSERT(covariances[i].cols==6 && covariances[i].rows == 6 && covariances[i].type() == CV_64FC1);
-				if(covariances[i].at<double>(0,0)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(0,0)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(0,0) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(0,0) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(1,1)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(1,1)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(1,1) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(1,1) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(2,2)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(2,2)<=COVARIANCE_LINEAR_EPSILON)
-					covariances[i].at<double>(2,2) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(2,2) = COVARIANCE_LINEAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(3,3)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(3,3)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(3,3) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(3,3) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(4,4)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(4,4)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(4,4) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(4,4) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
-				if(covariances[i].at<double>(5,5)<=COVARIANCE_EPSILON)
+				if(covariances[i].at<double>(5,5)<=COVARIANCE_ANGULAR_EPSILON)
-					covariances[i].at<double>(5,5) = COVARIANCE_EPSILON; // epsilon if exact transform
+					covariances[i].at<double>(5,5) = COVARIANCE_ANGULAR_EPSILON; // epsilon if exact transform
 			}
 
 			cv::Mat cov = covariances[0].clone();
@@ -1693,60 +1773,61 @@ Transform RegistrationVis::computeTransformationImpl(
 
 			std::map<int, Transform> optimizedPoses;
 
-			UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
+			UASSERT((toSignature.sensorData().stereoCameraModels().size()  >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
-					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
+					(toSignature.sensorData().cameraModels().size() >= 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
 
-			std::map<int, CameraModel> models;
+			std::map<int, std::vector<CameraModel> > models;
 
-			Transform invLocalTransformFrom;
+			std::vector<CameraModel> cameraModelsFrom;
-			CameraModel cameraModelFrom;
+			if(fromSignature.sensorData().stereoCameraModels().size())
-			if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
+				for(size_t i=0; i<fromSignature.sensorData().stereoCameraModels().size(); ++i)
-				// Set Tx=-baseline*fx for Stereo BA
+				{
-				cameraModelFrom = CameraModel(cameraModelFrom.fx(),
+					CameraModel cameraModel = fromSignature.sensorData().stereoCameraModels()[i].left();
-						cameraModelFrom.fy(),
+					// Set Tx=-baseline*fx for Stereo BA
-						cameraModelFrom.cx(),
+					cameraModel = CameraModel(cameraModel.fx(),
-						cameraModelFrom.cy(),
+							cameraModel.fy(),
-						cameraModelFrom.localTransform(),
+							cameraModel.cx(),
-						-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
+							cameraModel.cy(),
-				invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
+							cameraModel.localTransform(),
+							-fromSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
+							cameraModel.imageSize());
+					cameraModelsFrom.push_back(cameraModel);
+				}
 			}
-			else if(fromSignature.sensorData().cameraModels().size() == 1)
+			else
 			{
-				cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
+				cameraModelsFrom = fromSignature.sensorData().cameraModels();
-				invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
 			}
 
-			Transform invLocalTransformTo = Transform::getIdentity();
+			std::vector<CameraModel> cameraModelsTo;
-			CameraModel cameraModelTo;
+			if(toSignature.sensorData().stereoCameraModels().size())
-			if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
 			{
-				cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
+				for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
-				// Set Tx=-baseline*fx for Stereo BA
+				{
-				cameraModelTo = CameraModel(cameraModelTo.fx(),
+					CameraModel cameraModel = toSignature.sensorData().stereoCameraModels()[i].left();
-						cameraModelTo.fy(),
+					// Set Tx=-baseline*fx for Stereo BA
-						cameraModelTo.cx(),
+					cameraModel = CameraModel(cameraModel.fx(),
-						cameraModelTo.cy(),
+							cameraModel.fy(),
-						cameraModelTo.localTransform(),
+							cameraModel.cx(),
-						-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
+							cameraModel.cy(),
-				invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
+							cameraModel.localTransform(),
+							-toSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
+							cameraModel.imageSize());
+					cameraModelsTo.push_back(cameraModel);
+				}
 			}
-			else if(toSignature.sensorData().cameraModels().size() == 1)
+			else
 			{
-				cameraModelTo = toSignature.sensorData().cameraModels()[0];
+				cameraModelsTo = toSignature.sensorData().cameraModels();
-				invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
-			}
-			if(invLocalTransformFrom.isNull())
-			{
-				invLocalTransformFrom = invLocalTransformTo;
 			}
 
-			models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
+			models.insert(std::make_pair(1, cameraModelsFrom));
-			models.insert(std::make_pair(2, cameraModelTo));
+			models.insert(std::make_pair(2, cameraModelsTo));
 
 			std::map<int, std::map<int, FeatureBA> > wordReferences;
 			std::set<int> sbaOutliers;
+			UDEBUG("");
 			for(unsigned int i=0; i<allInliers.size(); ++i)
 			{
 				int wordId = allInliers[i];
@@ -1762,22 +1843,50 @@ Transform RegistrationVis::computeTransformationImpl(
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
 				std::map<int, FeatureBA> ptMap;
-				if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
+				if(!fromSignature.getWordsKpts().empty())
 				{
-					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
+					cv::KeyPoint kpt = fromSignature.getWordsKpts()[indexFrom];
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
+
-					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
+					int cameraIndex = 0;
+					const std::vector<CameraModel> & cam = models.at(1);
+					if(cam.size()>1)
+					{
+						UASSERT(cam[0].imageWidth()>0);
+						float subImageWidth = cam[0].imageWidth();
+						cameraIndex = int(kpt.pt.x / subImageWidth);
+						kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+					}
+
+					UASSERT(cam[cameraIndex].isValidForProjection());
+
+					float depthFrom = util3d::transformPoint(pt3D, cam[cameraIndex].localTransform().inverse()).z;
+					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom, cv::Mat(), cameraIndex)));
 				}
-				if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
+
+				if(!toSignature.getWordsKpts().empty())
 				{
 					int indexTo = toSignature.getWords().find(wordId)->second;
+					cv::KeyPoint kpt = toSignature.getWordsKpts()[indexTo];
+
+					int cameraIndex = 0;
+					const std::vector<CameraModel> & cam = models.at(2);
+					if(cam.size()>1)
+					{
+						UASSERT(cam[0].imageWidth()>0);
+						float subImageWidth = cam[0].imageWidth();
+						cameraIndex = int(kpt.pt.x / subImageWidth);
+						kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+					}
+
+					UASSERT(cam[cameraIndex].isValidForProjection());
+
 					float depthTo = 0.0f;
 					if(!toSignature.getWords3().empty())
 					{
-						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
+						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], cam[cameraIndex].localTransform().inverse()).z;
 					}
-					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
+
-					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
+					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo, cv::Mat(), cameraIndex)));
 				}
 
 				wordReferences.insert(std::make_pair(wordId, ptMap));
@@ -1873,31 +1982,31 @@ Transform RegistrationVis::computeTransformationImpl(
 		if(!transform.isNull() && !allInliers.empty() && (_minInliersDistributionThr>0.0f || _maxInliersMeanDistance>0.0f))
 		{
 			cv::Mat pcaData;
-			float cx=0, cy=0, w=0, h=0;
+			std::vector<CameraModel> cameraModelsTo;
+			if(toSignature.sensorData().stereoCameraModels().size())
+			{
+				for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
+				{
+					cameraModelsTo.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
+				}
+			}
+			else
+			{
+				cameraModelsTo = toSignature.sensorData().cameraModels();
+			}
 			if(_minInliersDistributionThr > 0)
 			{
-				if(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
+				if(cameraModelsTo.size() >= 1 && cameraModelsTo[0].isValidForReprojection())
-					(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
 				{
-					const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
+					if(cameraModelsTo[0].imageWidth()>0 && cameraModelsTo[0].imageHeight()>0)
-					cx = cameraModel.cx();
-					cy = cameraModel.cy();
-					w = cameraModel.imageWidth();
-					h = cameraModel.imageHeight();
-
-					if(w>0 && h>0)
 					{
 						pcaData = cv::Mat(allInliers.size(), 2, CV_32FC1);
 					}
 					else
 					{
-						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
+						UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", cameraModelsTo[0].imageWidth(), cameraModelsTo[0].imageHeight(), Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
 					}
 				}
-				else if(toSignature.sensorData().cameraModels().size() > 1)
-				{
-					UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
-				}
 				else
 				{
 					UERROR("Calibration not valid, cannot compute inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
@@ -1927,12 +2036,14 @@ Transform RegistrationVis::computeTransformationImpl(
 
 				if(!pcaData.empty())
 				{
-					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
+					std::multimap<int, int>::const_iterator wordsIter = toSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
+					UASSERT(wordsIter != fromSignature.getWords().end() && !toSignature.getWordsKpts().empty());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
+					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[wordsIter->second];
-					ptr[0] = (kpt.pt.x-cx) / w;
+					int cameraIndex = (int)(kpt.pt.x / cameraModelsTo[0].imageWidth());
-					ptr[1] = (kpt.pt.y-cy) / h;
+					UASSERT_MSG(cameraIndex < (int)cameraModelsTo.size(), uFormat("cameraIndex=%d (x=%f models=%d camera width = %d)", cameraIndex, kpt.pt.x, (int)cameraModelsTo.size(), cameraModelsTo[0].imageWidth()).c_str());
+					ptr[0] = (kpt.pt.x-cameraIndex*cameraModelsTo[cameraIndex].imageWidth()-cameraModelsTo[cameraIndex].cx()) / cameraModelsTo[cameraIndex].imageWidth();
+					ptr[1] = (kpt.pt.y-cameraModelsTo[cameraIndex].cy()) / cameraModelsTo[cameraIndex].imageHeight();
 				}
 			}
 

corelib/src/Rtabmap.cpp

@@ -53,6 +53,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UMath.h>
 #include <rtabmap/utilite/UProcessInfo.h>
 
+#ifdef RTABMAP_PYTHON
+#include "rtabmap/core/PythonInterface.h"
+#endif
+
 #include <pcl/search/kdtree.h>
 #include <pcl/filters/crop_box.h>
 #include <pcl/io/pcd_io.h>
@@ -137,6 +141,8 @@ Rtabmap::Rtabmap() :
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
 	_maxOdomCacheSize(Parameters::defaultRGBDMaxOdomCacheSize()),
 	_createGlobalScanMap(Parameters::defaultRGBDProximityGlobalScanMap()),
+	_markerPriorsLinearVariance(Parameters::defaultMarkerPriorsVarianceLinear()),
+	_markerPriorsAngularVariance(Parameters::defaultMarkerPriorsVarianceAngular()),
 	_loopClosureHypothesis(0,0.0f),
 	_highestHypothesis(0,0.0f),
 	_lastProcessTime(0.0),
@@ -161,6 +167,9 @@ Rtabmap::Rtabmap() :
 	_pathTransformToGoal(Transform::getIdentity()),
 	_pathStuckCount(0),
 	_pathStuckDistance(0.0f)
+#ifdef RTABMAP_PYTHON
+	,_python(new PythonInterface())
+#endif
 {
 }
 
@@ -584,13 +593,6 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 		_optimizeFromGraphEndChanged = true;
 	}
 	Parameters::parse(parameters, Parameters::kRGBDOptimizeMaxError(), _optimizationMaxError);
-	if(_optimizationMaxError > 0.0 && _optimizationMaxError < 1.0)
-	{
-		UWARN("RGBD/OptimizeMaxError (value=%f) is smaller than 1.0, setting to default %f "
-			  "instead (for backward compatibility issues when this parameter was previously "
-			  "an absolute error value).", _optimizationMaxError, Parameters::defaultRGBDOptimizeMaxError());
-		_optimizationMaxError = Parameters::defaultRGBDOptimizeMaxError();
-	}
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnLoopClosure(), _startNewMapOnLoopClosure);
 	Parameters::parse(parameters, Parameters::kRtabmapStartNewMapOnGoodSignature(), _startNewMapOnGoodSignature);
 	Parameters::parse(parameters, Parameters::kRGBDGoalReachedRadius(), _goalReachedRadius);
@@ -604,6 +606,44 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDMaxOdomCacheSize(), _maxOdomCacheSize);
 	Parameters::parse(parameters, Parameters::kRGBDProximityGlobalScanMap(), _createGlobalScanMap);
 
+	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceLinear(), _markerPriorsLinearVariance);
+	UASSERT(_markerPriorsLinearVariance>0.0f);
+	Parameters::parse(parameters, Parameters::kMarkerPriorsVarianceAngular(), _markerPriorsAngularVariance);
+	UASSERT(_markerPriorsAngularVariance>0.0f);
+	std::string markerPriorsStr;
+	if(Parameters::parse(parameters, Parameters::kMarkerPriors(), markerPriorsStr))
+	{
+		_markerPriors.clear();
+		std::list<std::string> strList = uSplit(markerPriorsStr, '|');
+		for(std::list<std::string>::iterator iter=strList.begin(); iter!=strList.end(); ++iter)
+		{
+			std::string markerStr = *iter;
+			while(!markerStr.empty() && !uIsDigit(markerStr[0]))
+			{
+				markerStr.erase(markerStr.begin());
+			}
+			if(!markerStr.empty())
+			{
+				std::string idStr = uSplitNumChar(markerStr).front();
+				int id = uStr2Int(idStr);
+				Transform prior = Transform::fromString(markerStr.substr(idStr.size()));
+				if(!prior.isNull() && id>0)
+				{
+					_markerPriors.insert(std::make_pair(-id, prior));
+					UDEBUG("Added landmark prior %d: %s", id, prior.prettyPrint().c_str());
+				}
+				else
+				{
+					UERROR("Failed to parse element \"%s\" in parameter %s", markerStr.c_str(), Parameters::kMarkerPriors().c_str());
+				}
+			}
+			else if(!iter->empty())
+			{
+				UERROR("Failed to parse parameter %s, value=\"%s\"", Parameters::kMarkerPriors().c_str(), iter->c_str());
+			}
+		}
+	}
+
 	UASSERT(_rgbdLinearUpdate >= 0.0f);
 	UASSERT(_rgbdAngularUpdate >= 0.0f);
 	UASSERT(_rgbdLinearSpeedUpdate >= 0.0f);
@@ -1038,23 +1078,32 @@ void Rtabmap::resetMemory()
 class NearestPathKey
 {
 public:
-	NearestPathKey(float l, int i) :
+	NearestPathKey(float l, int i, float d) :
 		likelihood(l),
-		id(i){}
+		id(i),
+		distance(d){}
 	bool operator<(const NearestPathKey & k) const
 	{
 		if(likelihood < k.likelihood)
 		{
 			return true;
 		}
-		else if(likelihood == k.likelihood && id < k.id)
+		else if(likelihood == k.likelihood)
 		{
-			return true;
+			if(distance > k.distance)
+			{
+				return true;
+			}
+			else if(distance == k.distance && id < k.id)
+			{
+				return true;
+			}
 		}
 		return false;
 	}
 	float likelihood;
 	int id;
+	float distance;
 };
 
 //============================================================
@@ -1366,6 +1415,7 @@ bool Rtabmap::process(
 	bool tooFastMovement = false;
 	std::list<int> signaturesRemoved;
 	bool neighborLinkRefined = false;
+	bool addedNewLandmark = false;
 	if(_rgbdSlamMode)
 	{
 		statistics_.addStatistic(Statistics::kMemoryOdometry_variance_lin(), odomCovariance.empty()?1.0f:(float)odomCovariance.at<double>(0,0));
@@ -1384,32 +1434,45 @@ bool Rtabmap::process(
 				//============================================================
 				// Minimum displacement required to add to Memory
 				//============================================================
-				const std::multimap<int, Link> & links = signature->getLinks();
+				Transform t;
-				if(links.size() && links.begin()->second.type() == Link::kNeighbor)
+
+				if(_memory->isIncremental())
 				{
-					const Signature * s = _memory->getSignature(links.begin()->second.to());
+					const std::multimap<int, Link> & links = signature->getLinks();
-					UASSERT(s!=0);
+					if(links.size() && links.begin()->second.type() == Link::kNeighbor)
-					// don't filter if the new node is not intermediate but previous one is
-					if(signature->getWeight() < 0 || s->getWeight() >= 0)
 					{
-						float x,y,z, roll,pitch,yaw;
+						const Signature * s = _memory->getSignature(links.begin()->second.to());
-						links.begin()->second.transform().getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+						UASSERT(s!=0);
-						bool isMoving = fabs(x) > _rgbdLinearUpdate ||
+						// don't filter if the new node is not intermediate but previous one is
-										fabs(y) > _rgbdLinearUpdate ||
+						if(signature->getWeight() < 0 || s->getWeight() >= 0)
-										fabs(z) > _rgbdLinearUpdate ||
-									    (_rgbdAngularUpdate>0.0f && (
-											fabs(roll) > _rgbdAngularUpdate ||
-											fabs(pitch) > _rgbdAngularUpdate ||
-											fabs(yaw) > _rgbdAngularUpdate));
-						if(!isMoving)
 						{
-							// This will disable global loop closure detection, only retrieval will be done.
+							t = links.begin()->second.transform();
-							// The location will also be deleted at the end.
-							smallDisplacement = true;
-							UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
 						}
 					}
 				}
+				else if(!_odomCachePoses.empty())
+				{
+					t = _odomCachePoses.rbegin()->second.inverse() * signature->getPose();
+				}
+				if(!t.isNull())
+				{
+					float x,y,z, roll,pitch,yaw;
+					t.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+					bool isMoving = fabs(x) > _rgbdLinearUpdate ||
+									fabs(y) > _rgbdLinearUpdate ||
+									fabs(z) > _rgbdLinearUpdate ||
+									(_rgbdAngularUpdate>0.0f && (
+										fabs(roll) > _rgbdAngularUpdate ||
+										fabs(pitch) > _rgbdAngularUpdate ||
+										fabs(yaw) > _rgbdAngularUpdate));
+					if(!isMoving)
+					{
+						// This will disable global loop closure detection, only retrieval will be done.
+						// The location will also be deleted at the end.
+						smallDisplacement = true;
+						UDEBUG("smallDisplacement: %f %f %f %f %f %f", x,y,z, roll,pitch,yaw);
+					}
+				}
 			}
 			if(odomVelocity.size() == 6)
 			{
@@ -1428,7 +1491,8 @@ bool Rtabmap::process(
 			signature->getLinks().size() &&
 			signature->getLinks().begin()->second.type() == Link::kNeighbor &&
 		   _memory->isIncremental() && // ignore pose matching in localization mode
-		   rehearsedId == 0) // don't do it if rehearsal happened
+		   rehearsedId == 0 && // don't do it if rehearsal happened
+		   !tooFastMovement) // ignore if too fast movement has been detected
 		{
 			int oldId = signature->getLinks().begin()->first;
 			const Signature * oldS = _memory->getSignature(oldId);
@@ -1543,6 +1607,8 @@ bool Rtabmap::process(
 				if(_optimizedPoses.find(iter->first) == _optimizedPoses.end())
 				{
 					_optimizedPoses.insert(std::make_pair(iter->first, newPose*iter->second.transform()));
+					UDEBUG("Added landmark %d : %s", iter->first, (newPose*iter->second.transform()).prettyPrint().c_str());
+					addedNewLandmark = true;
 				}
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
@@ -1952,6 +2018,11 @@ bool Rtabmap::process(
 	else if(!signature->isBadSignature() && (smallDisplacement || tooFastMovement))
 	{
 		_highestHypothesis = lastHighestHypothesis;
+		UDEBUG("smallDisplacement=%d tooFastMovement=%d", smallDisplacement?1:0, tooFastMovement?1:0);
+	}
+	else
+	{
+		UDEBUG("Ignoring likelihood and loop closure hypotheses as current signature doesn't have enough visual features.");
 	}
 
 	//============================================================
@@ -2348,23 +2419,6 @@ bool Rtabmap::process(
 		ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
 	}
 
-	//============================================================
-	// Landmark
-	//============================================================
-	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
-	if(!signature->getLandmarks().empty())
-	{
-		for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
-		{
-			if(uContains(_memory->getLandmarksIndex(), iter->first) &&
-					_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
-			{
-				UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
-				landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
-			}
-		}
-	}
-
 	//============================================================
 	// Proximity detections
 	//============================================================
@@ -2446,21 +2500,25 @@ bool Rtabmap::process(
 				UDEBUG("got %d paths", (int)nearestPathsNotSorted.size());
 				// sort nearest paths by highest likelihood (if two have same likelihood, sort by id)
 				std::map<NearestPathKey, std::map<int, Transform> > nearestPaths;
+				Transform currentPoseInv = _optimizedPoses.at(signature->id());
 				for(std::map<int, std::map<int, Transform> >::const_iterator iter=nearestPathsNotSorted.begin();iter!=nearestPathsNotSorted.end(); ++iter)
 				{
 					const std::map<int, Transform> & path = iter->second;
 					float highestLikelihood = 0.0f;
 					int highestLikelihoodId = iter->first;
+					float smallestDistanceSqr = -1;
 					for(std::map<int, Transform>::const_iterator jter=path.begin(); jter!=path.end(); ++jter)
 					{
 						float v = uValue(likelihood, jter->first, 0.0f);
-						if(v > highestLikelihood)
+						float distance = (currentPoseInv * jter->second).getNormSquared();
+						if(v > highestLikelihood || (v == highestLikelihood && (smallestDistanceSqr < 0 || distance < smallestDistanceSqr)))
 						{
 							highestLikelihood = v;
 							highestLikelihoodId = jter->first;
+							smallestDistanceSqr = distance;
 						}
 					}
-					nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId), path));
+					nearestPaths.insert(std::make_pair(NearestPathKey(highestLikelihood, highestLikelihoodId, smallestDistanceSqr), path));
 				}
 				UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);
 
@@ -2538,17 +2596,20 @@ bool Rtabmap::process(
 									if(_loopClosureHypothesis.first>0 &&
 										nearestIds.find(_loopClosureHypothesis.first)!=nearestIds.end())
 									{
+										// Avoid transform computation on the global loop closure if a visual proximity
+										// one has been detected close (inside proximity radius) to that hypothesis.
 										UDEBUG("Proximity detection on %d is close to loop closure %d, ignoring loop closure transform estimation...",
 												nearestId, _loopClosureHypothesis.first);
+
 										if(nearestId == _loopClosureHypothesis.first)
 										{
 											type = Link::kGlobalClosure;
+											loopIdSuppressedByProximity = nearestId;
+										}
+										else if(loopIdSuppressedByProximity == 0)
+										{
+											loopIdSuppressedByProximity = nearestId;
 										}
-										// In localization mode, avoid transform
-										// computation on the global loop closure if a visual proximity
-										// one has been detected close (inside proximity radius) to that hypothesis.
-										loopIdSuppressedByProximity = _loopClosureHypothesis.first;
-										_loopClosureHypothesis.first = 0;
 									}
 
 									_memory->addLink(Link(signature->id(), nearestId, type, transform, information));
@@ -2764,56 +2825,63 @@ bool Rtabmap::process(
 	//=============================================================
 	if(_loopClosureHypothesis.first>0)
 	{
-		//Compute transform if metric data are present
+		if(loopIdSuppressedByProximity==0)
-		Transform transform;
-		RegistrationInfo info;
-		info.covariance = cv::Mat::eye(6,6,CV_64FC1);
-		if(_rgbdSlamMode)
 		{
-			transform = _memory->computeTransform(
+			//Compute transform if metric data are present
-					_loopClosureHypothesis.first,
+			Transform transform;
-					signature->id(),
+			RegistrationInfo info;
-					_loopClosureIdentityGuess?Transform::getIdentity():Transform(),
+			info.covariance = cv::Mat::eye(6,6,CV_64FC1);
-					&info);
+			if(_rgbdSlamMode)
+			{
+				transform = _memory->computeTransform(
+						_loopClosureHypothesis.first,
+						signature->id(),
+						_loopClosureIdentityGuess?Transform::getIdentity():Transform(),
+						&info);
 
-			loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
+				loopClosureVisualInliersMeanDist = info.inliersMeanDistance;
-			loopClosureVisualInliersDistribution = info.inliersDistribution;
+				loopClosureVisualInliersDistribution = info.inliersDistribution;
 
-			loopClosureVisualInliers = info.inliers;
+				loopClosureVisualInliers = info.inliers;
-			loopClosureVisualInliersRatio = info.inliersRatio;
+				loopClosureVisualInliersRatio = info.inliersRatio;
-			loopClosureVisualMatches = info.matches;
+				loopClosureVisualMatches = info.matches;
-			rejectedGlobalLoopClosure = transform.isNull();
+				rejectedGlobalLoopClosure = transform.isNull();
-			if(rejectedGlobalLoopClosure)
+				if(rejectedGlobalLoopClosure)
-			{
+				{
-				UWARN("Rejected loop closure %d -> %d: %s",
+					UWARN("Rejected loop closure %d -> %d: %s",
-						_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
+							_loopClosureHypothesis.first, signature->id(), info.rejectedMsg.c_str());
+				}
+				else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
+				{
+					rejectedGlobalLoopClosure = true;
+					UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
+							_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
+				}
+				else
+				{
+					transform = transform.inverse();
+				}
 			}
-			else if(_maxLoopClosureDistance>0.0f && transform.getNorm() > _maxLoopClosureDistance)
-			{
-				rejectedGlobalLoopClosure = true;
-				UWARN("Rejected localization %d -> %d because distance to map (%fm) is over %s=%fm.",
-						_loopClosureHypothesis.first, signature->id(), transform.getNorm(), Parameters::kRGBDMaxLoopClosureDistance().c_str(), _maxLoopClosureDistance);
-			}
-			else
-			{
-				transform = transform.inverse();
-			}
-		}
-		if(!rejectedGlobalLoopClosure)
-		{
-			// Make the new one the parent of the old one
-			UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
-			cv::Mat information = getInformation(info.covariance);
-			loopClosureLinearVariance = 1.0/information.at<double>(0,0);
-			loopClosureAngularVariance = 1.0/information.at<double>(5,5);
-			rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
 			if(!rejectedGlobalLoopClosure)
 			{
-				loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
+				// Make the new one the parent of the old one
+				UASSERT(info.covariance.at<double>(0,0) > 0.0 && info.covariance.at<double>(5,5) > 0.0);
+				cv::Mat information = getInformation(info.covariance);
+				loopClosureLinearVariance = 1.0/information.at<double>(0,0);
+				loopClosureAngularVariance = 1.0/information.at<double>(5,5);
+				rejectedGlobalLoopClosure = !_memory->addLink(Link(signature->id(), _loopClosureHypothesis.first, Link::kGlobalClosure, transform, information));
+				if(!rejectedGlobalLoopClosure)
+				{
+					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), _loopClosureHypothesis.first));
+				}
+			}
+
+			if(rejectedGlobalLoopClosure)
+			{
+				_loopClosureHypothesis.first = 0;
 			}
 		}
-
+		else if(loopIdSuppressedByProximity != _loopClosureHypothesis.first)
-		if(rejectedGlobalLoopClosure)
 		{
 			_loopClosureHypothesis.first = 0;
 		}
@@ -2822,6 +2890,42 @@ bool Rtabmap::process(
 	timeAddLoopClosureLink = timer.ticks();
 	ULOGGER_INFO("timeAddLoopClosureLink=%fs", timeAddLoopClosureLink);
 
+	//============================================================
+	// Landmark
+	//============================================================
+	std::map<int, std::set<int> > landmarksDetected; // <Landmark ID, list of nodes that saw this landmark>
+	if(!signature->getLandmarks().empty())
+	{
+		bool hasGlobalLoopClosuresInOdomCache = !graph::filterLinks(_odomCacheConstraints, Link::kGlobalClosure, true).empty() || _loopClosureHypothesis.first != 0;
+		UDEBUG("hasGlobalLoopClosuresInOdomCache=%d", hasGlobalLoopClosuresInOdomCache?1:0);
+		for(std::map<int, Link>::const_iterator iter=signature->getLandmarks().begin(); iter!=signature->getLandmarks().end(); ++iter)
+		{
+			if(uContains(_memory->getLandmarksIndex(), iter->first) &&
+					_memory->getLandmarksIndex().find(iter->first)->second.size()>1)
+			{
+				if(!_memory->isIncremental() &&          // In localization mode
+					!hasGlobalLoopClosuresInOdomCache && // If there are global loop closures in odom cache, we can keep far landmarks
+					_localRadius>0.0 &&
+					iter->second.transform().getNormSquared() > _localRadius*_localRadius)
+				{
+					// Ignore landmark detections over local radius
+					UWARN("Ignoring landmark %d for localization as it is too far (%fm > %s=%f) "
+							"and odom cache doesn't contain global loop closure(s).",
+							iter->first,
+							iter->second.transform().getNorm(),
+							Parameters::kRGBDLocalRadius().c_str(),
+							_localRadius);
+				}
+				else
+				{
+					UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
+					landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
+					rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
+				}
+			}
+		}
+	}
+
 	//============================================================
 	// Add virtual links if a path is activated
 	//============================================================
@@ -2860,6 +2964,7 @@ bool Rtabmap::process(
 	cv::Mat localizationCovariance;
 	Transform previousMapCorrection;
 	bool rejectedLandmark = false;
+	bool delayedLocalization = false;
 	UDEBUG("RGB-D SLAM mode: %d", _rgbdSlamMode?1:0);
 	UDEBUG("Incremental: %d", _memory->isIncremental());
 	UDEBUG("Loop hyp: %d", _loopClosureHypothesis.first);
@@ -2906,6 +3011,16 @@ bool Rtabmap::process(
 			}
 		}
 
+		bool allLocalizationLinksInGraph = !localizationLinks.empty();
+		for(std::multimap<int, Link>::iterator iter=localizationLinks.begin(); iter!=localizationLinks.end(); ++iter)
+		{
+			if(!uContains(_optimizedPoses, iter->first))
+			{
+				allLocalizationLinksInGraph = false;
+				break;
+			}
+		}
+
 		// Note that in localization mode, we don't re-optimize the graph
 		// if:
 		//  1- there are no signatures retrieved,
@@ -2913,7 +3028,7 @@ bool Rtabmap::process(
 		if(!_memory->isIncremental() &&
 		   signaturesRetrieved.empty() &&
 		   !localizationLinks.empty() &&
-		   uContains(_optimizedPoses, localizationLinks.rbegin()->first))
+		   allLocalizationLinksInGraph)
 		{
 			bool rejectLocalization = _odomCachePoses.empty();
 			if(!_odomCachePoses.empty())
@@ -3054,7 +3169,7 @@ bool Rtabmap::process(
 				}
 
 				bool hasGlobalLoopClosuresOrLandmarks = false;
-				if(rejectLocalization)
+				if(rejectLocalization && !graph::filterLinks(constraints, Link::kLocalSpaceClosure, true).empty())
 				{
 					// Let's try again without local loop closures
 					localizationLinks = graph::filterLinks(localizationLinks, Link::kLocalSpaceClosure);
@@ -3215,8 +3330,13 @@ bool Rtabmap::process(
 						UDEBUG("  to %s", newT.prettyPrint().c_str());
 						iter->second.setTransform(newT);
 
+						// Update link in the referred signatures
+						if(iter->first > 0)
+							_memory->updateLink(iter->second, false);
+
 						_odomCacheConstraints.insert(std::make_pair(signature->id(), iter->second));
 					}
+
 					_odomCacheConstraints.insert(selfLinks.begin(), selfLinks.end());
 
 					// At least 2 localizations at 2 different time required
@@ -3246,7 +3366,7 @@ bool Rtabmap::process(
                                             !landmarksDetected.at(landmarkId).empty());
                                     loopId = *landmarksDetected.at(landmarkId).begin();
                                 }
-                                
+
                                 const Signature * loopS = _memory->getSignature(loopId);
                                 UASSERT(loopS !=0);
                                 std::multimap<int, Link>::const_iterator iterGravityLoop = graph::findLink(loopS->getLinks(), loopS->id(), loopS->id(), false, Link::kGravity);
@@ -3335,6 +3455,7 @@ bool Rtabmap::process(
 					else //delayed localization (wait for more than 1 link)
 					{
 						UWARN("Localization was good, but waiting for another one to be more accurate (%s>0)", Parameters::kRGBDMaxOdomCacheSize().c_str());
+						delayedLocalization = true;
 						rejectLocalization = true;
 					}
 				}
@@ -3726,6 +3847,8 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kMemorySmall_movement(), smallDisplacement?1.0f:0);
 			statistics_.addStatistic(Statistics::kMemoryDistance_travelled(), _distanceTravelled);
 			statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
+			statistics_.addStatistic(Statistics::kMemoryNew_landmark(), addedNewLandmark?1.0f:0);
+
 			if(_publishRAMUsage)
 			{
 				UTimer ramTimer;
@@ -3784,6 +3907,13 @@ bool Rtabmap::process(
 		_memory->removeRawData(signature->id(), true, !_neighborLinkRefining && !_proximityBySpace, true);
 	}
 
+	// Localization mode and saving localization data: save odometry covariance in a prior link
+	// so that DBReader can republish the covariance of localization data
+	if(!_memory->isIncremental() && _memory->isLocalizationDataSaved() && !odomCovariance.empty())
+	{
+		_memory->addLink(Link(signature->id(), signature->id(), Link::kPosePrior, odomPose, odomCovariance.inv()));
+	}
+
 	// remove last signature if the memory is not incremental or is a bad signature (if bad signatures are ignored)
 	int signatureRemoved = _memory->cleanup();
 	if(signatureRemoved)
@@ -3817,7 +3947,11 @@ bool Rtabmap::process(
 			signaturesRemoved.push_back(signature->id());
 			_memory->deleteLocation(signature->id());
 		}
-		else if((smallDisplacement || tooFastMovement) && _loopClosureHypothesis.first == 0 && lastProximitySpaceClosureId == 0)
+		else if((smallDisplacement || tooFastMovement) &&
+				_loopClosureHypothesis.first == 0 &&
+				lastProximitySpaceClosureId == 0 &&
+				(rejectedLandmark || landmarksDetected.empty()) &&
+				!addedNewLandmark)
 		{
 			// Don't delete the location if a loop closure is detected
 			UINFO("Ignoring location %d because the displacement is too small! (d=%f a=%f)",
@@ -3834,10 +3968,22 @@ bool Rtabmap::process(
 	else if(!_memory->isIncremental() &&
 			(smallDisplacement || tooFastMovement) &&
 			_loopClosureHypothesis.first == 0 &&
-			lastProximitySpaceClosureId == 0)
+			lastProximitySpaceClosureId == 0 &&
+			!delayedLocalization &&
+			(rejectedLandmark || landmarksDetected.empty()))
 	{
 		_odomCachePoses.erase(signatureRemoved);
-		_odomCacheConstraints.erase(signatureRemoved);
+		for(std::multimap<int, Link>::iterator iter=_odomCacheConstraints.begin(); iter!=_odomCacheConstraints.end();)
+		{
+			if(iter->second.from() == signatureRemoved || iter->second.to() == signatureRemoved)
+			{
+				_odomCacheConstraints.erase(iter++);
+			}
+			else
+			{
+				++iter;
+			}
+		}
 	}
 
 	// Pass this point signature should not be used, since it could have been transferred...
@@ -4671,6 +4817,20 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	_memory->getMetricConstraints(ids, poses, edgeConstraints, lookInDatabase, !_graphOptimizer->landmarksIgnored());
 	UINFO("get constraints (ids=%d, %d poses, %d edges) time %f s", (int)ids.size(), (int)poses.size(), (int)edgeConstraints.size(), timer.ticks());
 
+	// add landmark priors if there are some
+	for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end() && iter->first < 0; ++iter)
+	{
+		if(_markerPriors.find(iter->first) != _markerPriors.end())
+		{
+			cv::Mat infMatrix = cv::Mat::eye(6, 6, CV_64FC1);
+			infMatrix(cv::Range(0,3), cv::Range(0,3)) /= _markerPriorsLinearVariance;
+			infMatrix(cv::Range(3,6), cv::Range(3,6)) /= _markerPriorsAngularVariance;
+			edgeConstraints.insert(std::make_pair(iter->first, Link(iter->first, iter->first, Link::kPosePrior, _markerPriors.at(iter->first), infMatrix)));
+			UDEBUG("Added prior %d : %s (variance: lin=%f ang=%f)", iter->first, _markerPriors.at(iter->first).prettyPrint().c_str(),
+					_markerPriorsLinearVariance, _markerPriorsAngularVariance);
+		}
+	}
+
 	if(_graphOptimizer->iterations() > 0)
 	{
 		for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
@@ -4697,7 +4857,7 @@ std::map<int, Transform> Rtabmap::optimizeGraph(
 	}
 	else
 	{
-		bool hasLandmarks = edgeConstraints.begin()->first < 0;
+		bool hasLandmarks = !edgeConstraints.empty() && edgeConstraints.begin()->first < 0;
 		if(poses.size() != guessPoses.size() || hasLandmarks)
 		{
 			UDEBUG("recompute poses using only links (robust to multi-session)");
@@ -4885,10 +5045,10 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 		if(!images && withWords)
 		{
 			std::vector<CameraModel> models;
-			StereoCameraModel stereoModel;
+			std::vector<StereoCameraModel> stereoModels;
-			_memory->getNodeCalibration(id, models, stereoModel);
+			_memory->getNodeCalibration(id, models, stereoModels);
 			data.setCameraModels(models);
-			data.setStereoCameraModel(stereoModel);
+			data.setStereoCameraModels(stereoModels);
 		}
 
 		s=Signature(id,

corelib/src/SensorData.cpp

@@ -175,6 +175,40 @@ SensorData::SensorData(
 	setUserData(userData);
 }
 
+// Multi-Stereo constructor
+SensorData::SensorData(
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const std::vector<StereoCameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData):
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setStereoImage(left, right, cameraModels);
+	setUserData(userData);
+}
+
+// Multi-Stereo constructor + 2d laser scan
+SensorData::SensorData(
+		const LaserScan & laserScan,
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const std::vector<StereoCameraModel> & cameraModels,
+		int id,
+		double stamp,
+		const cv::Mat & userData) :
+		_id(id),
+		_stamp(stamp),
+		_cellSize(0.0f)
+{
+	setStereoImage(left, right, cameraModels);
+	setLaserScan(laserScan);
+	setUserData(userData);
+}
+
 SensorData::SensorData(
 	const IMU & imu,
 	int id,
@@ -206,16 +240,16 @@ void SensorData::setRGBDImage(
 		const std::vector<CameraModel> & models,
 		bool clearPreviousData)
 {
-	if(!clearPreviousData && _stereoCameraModel.isValidForProjection())
+	if(!clearPreviousData && !_stereoCameraModels.empty())
 	{
 		UERROR("Sensor data has previously stereo images "
 				"but clearPreviousData parameter is false. We "
 				"will still clear previous data to avoid incompatibilities "
 				"between raw and compressed data!");
 	}
-	bool clearData = clearPreviousData || _stereoCameraModel.isValidForProjection();
+	bool clearData = clearPreviousData || !_stereoCameraModels.empty();
 
-	_stereoCameraModel = StereoCameraModel();
+	_stereoCameraModels.clear();
 	_cameraModels = models;
 	if(rgb.rows == 1)
 	{
@@ -272,6 +306,16 @@ void SensorData::setStereoImage(
 		const cv::Mat & right,
 		const StereoCameraModel & stereoCameraModel,
 		bool clearPreviousData)
+{
+	std::vector<StereoCameraModel> models;
+	models.push_back(stereoCameraModel);
+	setStereoImage(left, right, models, clearPreviousData);
+}
+void SensorData::setStereoImage(
+		const cv::Mat & left,
+		const cv::Mat & right,
+		const std::vector<StereoCameraModel> & stereoCameraModels,
+		bool clearPreviousData)
 {
 	if(!clearPreviousData && !_cameraModels.empty())
 	{
@@ -283,7 +327,7 @@ void SensorData::setStereoImage(
 	bool clearData = clearPreviousData || !_cameraModels.empty();
 
 	_cameraModels.clear();
-	_stereoCameraModel = stereoCameraModel;
+	_stereoCameraModels = stereoCameraModels;
 
 	if(left.rows == 1)
 	{
@@ -842,15 +886,24 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
 			}
 		}
 	}
-	else if(_stereoCameraModel.isValidForProjection())
+	else if(_stereoCameraModels.size() >= 1)
 	{
-		cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
+		for(unsigned int i=0; i<_stereoCameraModels.size(); ++i)
-		if(ptInCameraFrame.z > 0.0f)
 		{
-			int u, v;
+			if(_stereoCameraModels[i].isValidForProjection() && !_stereoCameraModels[i].localTransform().isNull())
-			_stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
+			{
-			return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
+				cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModels[i].localTransform().inverse());
-				   uIsInBounds(v, 0, _stereoCameraModel.left().imageHeight());
+				if(ptInCameraFrame.z > 0.0f)
+				{
+					int u, v;
+					_stereoCameraModels[i].left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
+					if(uIsInBounds(u, 0, _stereoCameraModels[i].left().imageWidth()) &&
+					   uIsInBounds(v, 0, _stereoCameraModels[i].left().imageHeight()))
+					{
+						return true;
+					}
+				}
+			}
 		}
 	}
 	else

corelib/src/Stereo.cpp

@@ -148,18 +148,6 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
 	}
 	UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
 
-	if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
-	{
-		UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
-				"Optical flow may have failed because images are not calibrated, "
-				"the background is too far (no disparity between the images), "
-				"maximum disparity may be too small (%f) or that exposure between "
-				"left and right images is too different.",
-				countFlowRejected+countDisparityRejected,
-				(int)status.size(),
-				this->maxDisparity());
-	}
-
 	return rightCorners;
 }
 

corelib/src/Transform.cpp

@@ -442,6 +442,19 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
 					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
 }
 
+Transform Transform::fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+Transform Transform::fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix)
+{
+	return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
+					 matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
+					 matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
+}
+
 /**
  * Format (3 values): x y z
  * Format (6 values): x y z roll pitch yaw

corelib/src/camera/CameraDepthAI.cpp

@@ -55,7 +55,8 @@ CameraDepthAI::CameraDepthAI(
 	deviceSerial_(deviceSerial),
 	outputDepth_(false),
 	depthConfidence_(200),
-	resolution_(resolution)
+	resolution_(resolution),
+	imuFirmwareUpdate_(false)
 #endif
 {
 #ifdef RTABMAP_DEPTHAI
@@ -86,6 +87,15 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
 #endif
 }
 
+void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
+{
+#ifdef RTABMAP_DEPTHAI
+	imuFirmwareUpdate_ = enabled;
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
 bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	UDEBUG("");
@@ -140,7 +150,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
 	auto xoutIMU = p.create<dai::node::XLinkOut>();
 
-	 // XLinkOut
+	// XLinkOut
 	xoutLeft->setStreamName("rectified_left");
 	xoutDepthOrRight->setStreamName(outputDepth_?"depth":"rectified_right");
 	xoutIMU->setStreamName("imu");
@@ -158,9 +168,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	// StereoDepth
 	stereo->initialConfig.setConfidenceThreshold(depthConfidence_);
+	stereo->initialConfig.setLeftRightCheckThreshold(5);
 	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
-	stereo->setRectifyMirrorFrame(false);
+	stereo->setLeftRightCheck(true);
-	stereo->setLeftRightCheck(false);
 	stereo->setSubpixel(false);
 	stereo->setExtendedDisparity(false);
 
@@ -170,7 +180,11 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 
 	if(outputDepth_)
 	{
-		stereo->rectifiedLeft.link(xoutLeft->input);
+		// Depth is registered to right image by default, so subscribe to right image when depth is used
+		if(outputDepth_)
+			stereo->rectifiedRight.link(xoutLeft->input);
+		else
+			stereo->rectifiedLeft.link(xoutLeft->input);
 		stereo->depth.link(xoutDepthOrRight->input);
 	}
 	else
@@ -191,6 +205,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	// Link plugins IMU -> XLINK
 	imu->out.link(xoutIMU->input);
 
+	imu->enableFirmwareUpdate(imuFirmwareUpdate_);
+
 	device_.reset(new dai::Device(p, deviceToUse));
 
 	UINFO("Loading eeprom calibration data");
@@ -206,17 +222,17 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	stereoModel_ = StereoCameraModel(device_->getMxId(), fx, fy, cx, cy, baseline, this->getLocalTransform(), targetSize);
 
 	// Cannot test the following, I get "IMU calibration data is not available on device yet." with my camera
+	// Update: now (as March 6, 2022) it crashes in "dai::CalibrationHandler::getImuToCameraExtrinsics(dai::CameraBoardSocket, bool)"
 	//matrix = calibHandler.getImuToCameraExtrinsics(dai::CameraBoardSocket::LEFT);
 	//imuLocalTransform_ = Transform(
 	//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
 	//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
 	//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-	// Hard-coded acc: x->left, y->up, z->forward
+	// Hard-coded: x->down, y->left, z->forward
-	// Hard-coded gyro: x->down, y->left, z->forward
 	imuLocalTransform_ = Transform(
-			0, 0, 1, 0,
+			 0, 0, 1, 0,
-			1, 0, 0, 0,
+			 0, 1, 0, 0,
-			0 ,1, 0, 0);
+			-1 ,0, 0, 0);
 	UINFO("IMU local transform = %s", imuLocalTransform_.prettyPrint().c_str());
 
 	leftQueue_ = device_->getOutputQueue("rectified_left", 8, false);
@@ -279,7 +295,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			}
 			else
 			{
-				cv::flip(depthOrRight, depthOrRight, 1);
 				data = SensorData(left, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
 			}
 
@@ -408,10 +423,6 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 						UWARN("Could not find gyro data to interpolate at image time %f (between %f and %f). Are sensors synchronized?", stamp, iterA->first, iterB->first);
 					}
 				}
-				// Rotate gyro frame (x->down, y->left, z->forward) in acc frame (x->left, y->up, z->forward)
-				double tmp = gyro[0];
-				gyro[0] = gyro[1];
-				gyro[1] = -tmp;
 			}
 
 			if(valid)

corelib/src/camera/CameraImages.cpp

@@ -215,6 +215,16 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 						_model.fy(),
 						_model.cx(),
 						_model.cy());
+
+				cv::FileStorage fs(calibrationFolder+"/"+cameraName+".yaml", 0);
+				cv::FileNode poseNode = fs["local_transform"];
+				if(!poseNode.isNone())
+				{
+					UWARN("Using local transform from calibration file (%s) instead of the parameter one (%s).",
+							_model.localTransform().prettyPrint().c_str(),
+							this->getLocalTransform().prettyPrint().c_str());
+					this->setLocalTransform(_model.localTransform());
+				}
 			}
 		}
 		_model.setName(cameraName);
@@ -243,36 +253,18 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 			if(dirJson.getFileNames().size() == _dir->getFileNames().size())
 			{
 				bool modelsWarned = false;
-				bool firstFrame = true;
+				bool localTWarned = false;
 				for(std::list<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
 				{
-					// Assuming 3DScannerApp(iOS) format (only this one supported...)
 					std::string filePath = _path+"/"+*iter;
 					cv::FileStorage fs(filePath, 0);
-					cv::FileNode poseNode = fs["cameraPoseARFrame"];
+					cv::FileNode poseNode = fs["cameraPoseARFrame"]; // Check if it is 3DScannerApp(iOS) format
-					cv::FileNode timeNode = fs["time"];
+					if(poseNode.isNone())
-					cv::FileNode intrinsicsNode = fs["intrinsics"];
-					if(poseNode.isNone() || poseNode.size() != 16)
 					{
-						UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
+						cv::FileNode n = fs["local_transform"];
-						success = false;
+						bool hasLocalTransform = !n.isNone();
-						break;
+
-					}
+						fs.release();
-					else if(timeNode.isNone() || !timeNode.isReal())
-					{
-						UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str());
-						success = false;
-						break;
-					}
-					else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
-					{
-						UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
-						success = false;
-						break;
-					}
-					else
-					{
-						_stamps.push_back((double)timeNode);
 						if(_model.isValidForProjection() && !modelsWarned)
 						{
 							UWARN("Camera model loaded for each frame is overridden by "
@@ -283,31 +275,74 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 						}
 						else
 						{
-							_models.push_back(CameraModel(
+							CameraModel model;
-									(double)intrinsicsNode[0], //fx
+							model.load(filePath);
-									(double)intrinsicsNode[4], //fy
+
-									(double)intrinsicsNode[2], //cx
+							if(!hasLocalTransform)
-									(double)intrinsicsNode[5], //cy
+							{
-									CameraModel::opticalRotation()));
+								if(!localTWarned)
+								{
+									UWARN("Loaded calibration file doesn't have local_transform field, "
+											"the global local_transform parameter is used by default (%s).",
+											this->getLocalTransform().prettyPrint().c_str());
+									localTWarned = true;
+								}
+								model.setLocalTransform(this->getLocalTransform());
+							}
+
+							_models.push_back(model);
 						}
-						// we need to rotate from opengl world to rtabmap world
+					}
-						Transform pose(
+					else
-								(float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3],
+					{
-								(float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7],
+						cv::FileNode timeNode = fs["time"];
-								(float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]);
+						cv::FileNode intrinsicsNode = fs["intrinsics"];
-						pose =  Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
+						if(poseNode.isNone() || poseNode.size() != 16)
-						odometry_.push_back(pose);
-						// linear cov = 0.0001
-						cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-						if(!firstFrame)
 						{
-							// angular cov = 0.000001
+							UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
-							covariance.at<double>(3,3) *= 0.01;
+							success = false;
-							covariance.at<double>(4,4) *= 0.01;
+							break;
-							covariance.at<double>(5,5) *= 0.01;
+						}
+						else if(timeNode.isNone() || !timeNode.isReal())
+						{
+							UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str());
+							success = false;
+							break;
+						}
+						else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9)
+						{
+							UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str());
+							success = false;
+							break;
+						}
+						else
+						{
+							_stamps.push_back((double)timeNode);
+							if(_model.isValidForProjection() && !modelsWarned)
+							{
+								UWARN("Camera model loaded for each frame is overridden by "
+										"general calibration file provided. Remove general calibration "
+										"file to use camera model of each frame. This warning will "
+										"be shown only one time.");
+								modelsWarned = true;
+							}
+							else
+							{
+								_models.push_back(CameraModel(
+										(double)intrinsicsNode[0], //fx
+										(double)intrinsicsNode[4], //fy
+										(double)intrinsicsNode[2], //cx
+										(double)intrinsicsNode[5], //cy
+										CameraModel::opticalRotation()));
+							}
+							// we need to rotate from opengl world to rtabmap world
+							Transform pose(
+									(float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3],
+									(float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7],
+									(float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]);
+							pose =  Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
+							odometry_.push_back(pose);
 						}
-						firstFrame = false;
-						covariances_.push_back(covariance);
 					}
 				}
 				if(!success)
@@ -315,7 +350,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 					odometry_.clear();
 					_stamps.clear();
 					_models.clear();
-					covariances_.clear();
 				}
 			}
 			else
@@ -334,106 +368,110 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 				success = false;
 			}
 		}
-		else if(_filenamesAreTimestamps)
+
+		if(_stamps.empty())
 		{
-			std::list<std::string> filenames = _dir?_dir->getFileNames():_scanDir->getFileNames();
+			if(_filenamesAreTimestamps)
-			for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
 			{
-				// format is text_1223445645.12334_text.png or text_122344564512334_text.png
+				std::list<std::string> filenames = _dir?_dir->getFileNames():_scanDir->getFileNames();
-				// If no decimals, 10 first number are the seconds
+				for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
-				std::list<std::string> list = uSplit(*iter, '.');
-				if(list.size() == 3 || list.size() == 2)
 				{
-					list.pop_back(); // remove extension
+					// format is text_1223445645.12334_text.png or text_122344564512334_text.png
-					double stamp = 0.0;
+					// If no decimals, 10 first number are the seconds
-					if(list.size() == 1)
+					std::list<std::string> list = uSplit(*iter, '.');
+					if(list.size() == 3 || list.size() == 2)
 					{
-						std::list<std::string> numberList = uSplitNumChar(list.front());
+						list.pop_back(); // remove extension
-						for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
+						double stamp = 0.0;
+						if(list.size() == 1)
 						{
-							if(uIsNumber(*iter))
+							std::list<std::string> numberList = uSplitNumChar(list.front());
+							for(std::list<std::string>::iterator iter=numberList.begin(); iter!=numberList.end(); ++iter)
 							{
-								std::string decimals;
+								if(uIsNumber(*iter))
-								std::string sec;
-								if(iter->length()>10)
 								{
-									decimals = iter->substr(10, iter->size()-10);
+									std::string decimals;
-									sec = iter->substr(0, 10);
+									std::string sec;
+									if(iter->length()>10)
+									{
+										decimals = iter->substr(10, iter->size()-10);
+										sec = iter->substr(0, 10);
+									}
+									else
+									{
+										sec = *iter;
+									}
+									stamp = uStr2Double(sec + "." + decimals);
+									break;
 								}
-								else
-								{
-									sec = *iter;
-								}
-								stamp = uStr2Double(sec + "." + decimals);
-								break;
 							}
 						}
-					}
+						else
-					else
+						{
-					{
+							std::string decimals = uSplitNumChar(list.back()).front();
-						std::string decimals = uSplitNumChar(list.back()).front();
+							list.pop_back();
-						list.pop_back();
+							std::string sec = uSplitNumChar(list.back()).back();
-						std::string sec = uSplitNumChar(list.back()).back();
+							stamp = uStr2Double(sec + "." + decimals);
-						stamp = uStr2Double(sec + "." + decimals);
+						}
-					}
+						if(stamp > 0.0)
-					if(stamp > 0.0)
+						{
-					{
+							_stamps.push_back(stamp);
-						_stamps.push_back(stamp);
+						}
-					}
+						else
-					else
+						{
-					{
+							UERROR("Conversion filename to timestamp failed! (filename=%s)", iter->c_str());
-						UERROR("Conversion filename to timestamp failed! (filename=%s)", iter->c_str());
+						}
 					}
 				}
-			}
+				if(_stamps.size() != this->imagesCount())
-			if(_stamps.size() != this->imagesCount())
-			{
-				UERROR("The stamps count is not the same as the images (%d vs %d)! "
-					   "Converting filenames to timestamps is activated.",
-						(int)_stamps.size(), this->imagesCount());
-				_stamps.clear();
-				success = false;
-			}
-		}
-		else if(_timestampsPath.size())
-		{
-			std::ifstream file;
-			file.open(_timestampsPath.c_str(), std::ifstream::in);
-			while(file.good())
-			{
-				std::string str;
-				std::getline(file, str);
-
-				if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
 				{
-					continue;
+					UERROR("The stamps count is not the same as the images (%d vs %d)! "
+						   "Converting filenames to timestamps is activated.",
+							(int)_stamps.size(), this->imagesCount());
+					_stamps.clear();
+					success = false;
 				}
-
-				std::list<std::string> strList = uSplit(str, ' ');
-				std::string stampStr = strList.front();
-				if(strList.size() == 2)
-				{
-					// format "seconds millisec"
-					// the millisec str needs 0-padding if size < 6
-					std::string millisecStr = strList.back();
-					while(millisecStr.size() < 6)
-					{
-						millisecStr = "0" + millisecStr;
-					}
-					stampStr = stampStr+'.'+millisecStr;
-				}
-				_stamps.push_back(uStr2Double(stampStr));
 			}
-
+			else if(_timestampsPath.size())
-			file.close();
-
-			if(_stamps.size() != this->imagesCount())
 			{
-				UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
+				std::ifstream file;
-						"the timestamps file path if you don't want to use them (current file path=%s).",
+				file.open(_timestampsPath.c_str(), std::ifstream::in);
-						(int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
+				while(file.good())
-				_stamps.clear();
+				{
-				success = false;
+					std::string str;
+					std::getline(file, str);
+
+					if(str.empty() || str.at(0) == '#' || str.at(0) == '%')
+					{
+						continue;
+					}
+
+					std::list<std::string> strList = uSplit(str, ' ');
+					std::string stampStr = strList.front();
+					if(strList.size() == 2)
+					{
+						// format "seconds millisec"
+						// the millisec str needs 0-padding if size < 6
+						std::string millisecStr = strList.back();
+						while(millisecStr.size() < 6)
+						{
+							millisecStr = "0" + millisecStr;
+						}
+						stampStr = stampStr+'.'+millisecStr;
+					}
+					_stamps.push_back(uStr2Double(stampStr));
+				}
+
+				file.close();
+
+				if(_stamps.size() != this->imagesCount())
+				{
+					UERROR("The stamps count (%d) is not the same as the images (%d)! Please remove "
+							"the timestamps file path if you don't want to use them (current file path=%s).",
+							(int)_stamps.size(), this->imagesCount(), _timestampsPath.c_str());
+					_stamps.clear();
+					success = false;
+				}
 			}
 		}
 
@@ -446,6 +484,23 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		{
 			success = readPoses(groundTruth_, _stamps, _groundTruthPath, _groundTruthFormat, _maxPoseTimeDiff);
 		}
+
+		if(!odometry_.empty())
+		{
+			for(size_t i=0; i<odometry_.size(); ++i)
+			{
+				// linear cov = 0.0001
+				cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (i==0?9999.0:0.0001);
+				if(i!=0)
+				{
+					// angular cov = 0.000001
+					covariance.at<double>(3,3) *= 0.01;
+					covariance.at<double>(4,4) *= 0.01;
+					covariance.at<double>(5,5) *= 0.01;
+				}
+				covariances_.push_back(covariance);
+			}
+		}
 	}
 
 	_captureTimer.restart();

corelib/src/odometry/OdometryF2F.cpp

@@ -73,9 +73,10 @@ Transform OdometryF2F::computeTransform(
 {
 	UTimer timer;
 	Transform output;
-	if(!data.rightRaw().empty() && !data.stereoCameraModel().isValidForProjection())
+	if(!data.rightRaw().empty() &&
+	   (data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection()))
 	{
-		UERROR("Calibrated stereo camera required");
+		UERROR("Calibrated stereo camera required (multi-cameras not supported)");
 		return output;
 	}
 	if(!data.depthRaw().empty() &&

corelib/src/odometry/OdometryF2M.cpp

@@ -225,12 +225,6 @@ Transform OdometryF2M::computeTransform(
 	lastFrame_ = new Signature(data);
 	data.setId(id);
 
-	if(bundleAdjustment_ > 0 &&
-	   data.cameraModels().size() > 1)
-	{
-		UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
-		bundleAdjustment_ = 0;
-	}
 	bool addKeyFrame = false;
 	int totalBundleWordReferencesUsed = 0;
 	int totalBundleOutliers = 0;
@@ -238,6 +232,31 @@ Transform OdometryF2M::computeTransform(
 	bool visDepthAsMask = Parameters::defaultVisDepthAsMask();
 	Parameters::parse(parameters_, Parameters::kVisDepthAsMask(), visDepthAsMask);
 
+	std::vector<CameraModel> lastFrameModels;
+	if(!lastFrame_->sensorData().cameraModels().empty() &&
+		lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
+	{
+		lastFrameModels = lastFrame_->sensorData().cameraModels();
+	}
+	else if(!lastFrame_->sensorData().stereoCameraModels().empty() &&
+			lastFrame_->sensorData().stereoCameraModels().at(0).isValidForProjection())
+	{
+		for(size_t i=0; i<lastFrame_->sensorData().stereoCameraModels().size(); ++i)
+		{
+			CameraModel model = lastFrame_->sensorData().stereoCameraModels()[i].left();
+			// Set Tx for stereo BA
+			model = CameraModel(model.fx(),
+					model.fy(),
+					model.cx(),
+					model.cy(),
+					model.localTransform(),
+					-lastFrame_->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
+					model.imageSize());
+			lastFrameModels.push_back(model);
+		}
+	}
+	UDEBUG("lastFrameModels=%ld", lastFrameModels.size());
+
 	// Generate keypoints from the new data
 	if(lastFrame_->sensorData().isValid())
 	{
@@ -252,7 +271,7 @@ Transform OdometryF2M::computeTransform(
 			std::map<int, cv::Point3f> points3DMap;
 			std::map<int, Transform> bundlePoses;
 			std::multimap<int, Link> bundleLinks;
-			std::map<int, CameraModel> bundleModels;
+			std::map<int, std::vector<CameraModel> > bundleModels;
 
 			for(int guessIteration=0;
 					guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -315,7 +334,7 @@ Transform OdometryF2M::computeTransform(
 					// local bundle adjustment
 					if(bundleAdjustment_>0 && sba_ &&
 					   regPipeline_->isImageRequired() &&
-					   lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
+					   !lastFrameModels.empty() &&
 					   regInfo.inliersIDs.size())
 					{
 						UDEBUG("Local Bundle Adjustment");
@@ -326,7 +345,12 @@ Transform OdometryF2M::computeTransform(
 						   map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
 						   map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
 						{
-							UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
+							UERROR("Bundle Adjustment cannot be used with a registration approach recomputing "
+									"features from the \"from\" signature (e.g., Optical Flow) that would change "
+									"their ids (size=old=%ld new=%ld first/last: old=%d->%d new=%d->%d).",
+									map_->getWords().size(), tmpMap.getWords().size(),
+									map_->getWords().begin()->first, map_->getWords().rbegin()->first,
+									tmpMap.getWords().begin()->first, tmpMap.getWords().rbegin()->first);
 							bundleAdjustment_ = 0;
 						}
 						else
@@ -350,28 +374,7 @@ Transform OdometryF2M::computeTransform(
 								bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
 							}
 
-							CameraModel model;
+							bundleModels.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
-							if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-							{
-								model = lastFrame_->sensorData().cameraModels()[0];
-							}
-							else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-							{
-								model = lastFrame_->sensorData().stereoCameraModel().left();
-								// Set Tx for stereo BA
-								model = CameraModel(model.fx(),
-										model.fy(),
-										model.cx(),
-										model.cy(),
-										model.localTransform(),
-										-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
-							}
-							else
-							{
-								UFATAL("no valid camera model to do odometry bundle adjustment!");
-							}
-							bundleModels.insert(std::make_pair(lastFrame_->id(), model));
-							Transform invLocalTransform = model.localTransform().inverse();
 
 							UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
 							std::map<int, std::map<int, FeatureBA> > wordReferences;
@@ -416,15 +419,27 @@ Transform OdometryF2M::computeTransform(
 								if(iter2D!=lastFrame_->getWords().end())
 								{
 									UASSERT(!lastFrame_->getWordsKpts().empty());
+									cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter2D->second];
+
+									int cameraIndex = 0;
+									if(lastFrameModels.size()>1)
+									{
+										UASSERT(lastFrameModels[0].imageWidth()>0);
+										float subImageWidth = lastFrameModels[0].imageWidth();
+										cameraIndex = int(kpt.pt.x / subImageWidth);
+										UASSERT(cameraIndex < (int)lastFrameModels.size());
+										kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									//get depth
 									float d = 0.0f;
 									if( !lastFrame_->getWords3().empty() &&
 										util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
 									{
 										//move back point in camera frame (to get depth along z)
-										d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
+										d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
 									}
-									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
+									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
 								}
 								wordReferences.insert(std::make_pair(wordId, references));
 
@@ -494,6 +509,23 @@ Transform OdometryF2M::computeTransform(
 											info->gravityRollError = fabs(rollImu - roll);
 											info->gravityPitchError = fabs(pitchImu - pitch);
 										}
+
+										// With bundle adjustment, scale down covariance by 10
+										UASSERT(regInfo.covariance.cols==6 && regInfo.covariance.rows == 6 && regInfo.covariance.type() == CV_64FC1);
+										double thrLin = Registration::COVARIANCE_LINEAR_EPSILON*10.0;
+										double thrAng = Registration::COVARIANCE_ANGULAR_EPSILON*10.0;
+										if(regInfo.covariance.at<double>(0,0)>thrLin)
+											regInfo.covariance.at<double>(0,0) *= 0.1;
+										if(regInfo.covariance.at<double>(1,1)>thrLin)
+											regInfo.covariance.at<double>(1,1) *= 0.1;
+										if(regInfo.covariance.at<double>(2,2)>thrLin)
+											regInfo.covariance.at<double>(2,2) *= 0.1;
+										if(regInfo.covariance.at<double>(3,3)>thrAng)
+											regInfo.covariance.at<double>(3,3) *= 0.1;
+										if(regInfo.covariance.at<double>(4,4)>thrAng)
+											regInfo.covariance.at<double>(4,4) *= 0.1;
+										if(regInfo.covariance.at<double>(5,5)>thrAng)
+											regInfo.covariance.at<double>(5,5) *= 0.1;
 									}
 								}
 								UDEBUG("Local Bundle Adjustment After : %s", transform.prettyPrint().c_str());
@@ -609,30 +641,14 @@ Transform OdometryF2M::computeTransform(
 					}
 
 					// sort by feature response
-					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
+					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > > newIds;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
 					std::set<int> seenStatusUpdated;
-					Transform invLocalTransform;
-					if(bundleAdjustment_>0)
-					{
-						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-						{
-							invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
-						}
-						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-						{
-							invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
-						}
-						else
-						{
-							UFATAL("no valid camera model!");
-						}
-					}
 
 					// add points without depth only if the local map has reached its maximum size
 					bool addPointsWithoutDepth = false;
-					if(!visDepthAsMask && validDepthRatio_ < 1.0f)
+					if(!visDepthAsMask && validDepthRatio_ < 1.0f && !lastFrame_->getWords3().empty())
 					{
 						int ptsWithDepth = 0;
 						for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
@@ -653,63 +669,79 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
+					if(!lastFrameModels.empty())
 					{
-						const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
+						for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
-						const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
-						if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
 						{
-							if(util3d::isFinite(pt) || addPointsWithoutDepth)
+							const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
+							cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter->second];
+
+							int cameraIndex = 0;
+							if(lastFrameModels.size()>1)
 							{
-								newIds.insert(
+								UASSERT(lastFrameModels[0].imageWidth()>0);
-										std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
+								float subImageWidth = lastFrameModels[0].imageWidth();
-												std::make_pair(iter->first,
+								cameraIndex = int(kpt.pt.x / subImageWidth);
-														std::make_pair(kpt,
+								UASSERT(cameraIndex < (int)lastFrameModels.size());
-																std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
+								kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
 							}
-						}
+
-						else if(bundleAdjustment_>0)
+							if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
-						{
-							if(lastFrame_->getWords().count(iter->first) == 1)
 							{
-								std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
+								if(util3d::isFinite(pt) || addPointsWithoutDepth)
-								if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
 								{
-									mapWordsKpts[iterKpts->second].octave = kpt.octave;
+									newIds.insert(
+											std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
+													std::make_pair(iter->first,
+															std::make_pair(kpt,
+																	std::make_pair(pt,
+																			std::make_pair(lastFrame_->getWordsDescriptors().row(iter->second), cameraIndex))))));
 								}
+							}
+							else if(bundleAdjustment_>0)
+							{
+								if(lastFrame_->getWords().count(iter->first) == 1)
+								{
+									std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
+									if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
+									{
+										mapWordsKpts[iterKpts->second].octave = kpt.octave;
+									}
 
-								UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
+									UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
-								iterBundlePosesRef->second += 1;
+									iterBundlePosesRef->second += 1;
 
-								//move back point in camera frame (to get depth along z)
+									//move back point in camera frame (to get depth along z)
-								float depth = 0.0f;
+									float depth = 0.0f;
-								if(util3d::isFinite(pt))
+									if(util3d::isFinite(pt))
-								{
+									{
-									depth = util3d::transformPoint(pt, invLocalTransform).z;
+										depth = util3d::transformPoint(pt, lastFrameModels[cameraIndex].localTransform().inverse()).z;
-								}
+									}
-								if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
+									if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
-								{
+									{
-									std::map<int, FeatureBA> framePt;
+										std::map<int, FeatureBA> framePt;
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
+										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
-									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
+										bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
-								}
+									}
-								else
+									else
-								{
+									{
-									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
+										bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
+									}
 								}
 							}
 						}
+						UDEBUG("newIds=%d", (int)newIds.size());
 					}
-					UDEBUG("newIds=%d", (int)newIds.size());
 
 					int lastFrameOldestNewId = lastFrameOldestNewId_;
 					lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
-					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
+					for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > >::reverse_iterator iter=newIds.rbegin();
 						iter!=newIds.rend();
 						++iter)
 					{
 						if(maxNewFeatures_ == 0  || added < maxNewFeatures_)
 						{
+							int cameraIndex = iter->second.second.second.second.second;
 							if(bundleAdjustment_>0)
 							{
 								if(lastFrame_->getWords().count(iter->second.first) == 1)
@@ -721,17 +753,17 @@ Transform OdometryF2M::computeTransform(
 									float depth = 0.0f;
 									if(util3d::isFinite(iter->second.second.second.first))
 									{
-										depth = util3d::transformPoint(iter->second.second.second.first, invLocalTransform).z;
+										depth = util3d::transformPoint(iter->second.second.second.first, lastFrameModels[cameraIndex].localTransform().inverse()).z;
 									}
 									if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
 									{
 										std::map<int, FeatureBA> framePt;
-										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
+										framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
 										bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
 									}
 									else
 									{
-										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
+										bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
 									}
 								}
 							}
@@ -742,39 +774,21 @@ Transform OdometryF2M::computeTransform(
 							if(!util3d::isFinite(pt))
 							{
 								// get the ray instead
-								float x = iter->second.second.first.pt.x;
+								float x = iter->second.second.first.pt.x; //subImageWidth should be already removed
 								float y = iter->second.second.first.pt.y;
-								float subImageWidth = lastFrame_->sensorData().imageRaw().cols;
-								CameraModel model;
-								if(lastFrame_->sensorData().cameraModels().size() > 1)
-								{
-									subImageWidth = lastFrame_->sensorData().imageRaw().cols/lastFrame_->sensorData().cameraModels().size();
-									int cameraIndex = int(x / subImageWidth);
-									model = lastFrame_->sensorData().cameraModels()[cameraIndex];
-									x = x-subImageWidth*cameraIndex;
-								}
-								else if(lastFrame_->sensorData().cameraModels().size() == 1)
-								{
-									model = lastFrame_->sensorData().cameraModels()[0];
-								}
-								else
-								{
-									model = lastFrame_->sensorData().stereoCameraModel().left();
-								}
-
 								Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
-										model.imageSize(),
+										lastFrameModels[cameraIndex].imageSize(),
 										x,
 										y,
-										model.cx(),
+										lastFrameModels[cameraIndex].cx(),
-										model.cy(),
+										lastFrameModels[cameraIndex].cy(),
-										model.fx(),
+										lastFrameModels[cameraIndex].fx(),
-										model.fy());
+										lastFrameModels[cameraIndex].fy());
-								float scaleInf = (0.05 * model.fx()) / 0.01;
+								float scaleInf = (0.05 * lastFrameModels[cameraIndex].fx()) / 0.01;
-								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
+								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), lastFrameModels[cameraIndex].localTransform()); // in base_link frame
 							}
 							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
-							mapDescriptors.push_back(iter->second.second.second.second);
+							mapDescriptors.push_back(iter->second.second.second.second.first);
 							if(lastFrameOldestNewId_ > iter->second.first)
 							{
 								lastFrameOldestNewId_ = iter->second.first;
@@ -782,6 +796,7 @@ Transform OdometryF2M::computeTransform(
 							++added;
 						}
 					}
+					UDEBUG("");
 
 					// remove words in map if max size is reached
 					if((int)mapWords.size() > maximumMapSize_)
@@ -1170,7 +1185,7 @@ Transform OdometryF2M::computeTransform(
 					std::vector<cv::Point3f> transformedPoints;
 					std::multimap<int, int> mapPointWeights;
 					cv::Mat descriptors;
-					if(!lastFrame_->getWords3().empty())
+					if(!lastFrame_->getWords3().empty() && !lastFrameModels.empty())
 					{
 						for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
 							iter != lastFrame_->getWords().end();
@@ -1190,20 +1205,6 @@ Transform OdometryF2M::computeTransform(
 
 					if(bundleAdjustment_>0)
 					{
-						Transform invLocalTransform;
-						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-						{
-							invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
-						}
-						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-						{
-							invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
-						}
-						else
-						{
-							UFATAL("no valid camera model!");
-						}
-
 						// update bundleWordReferences_: used for bundle adjustment
 						if(!wordsKpts.empty())
 						{
@@ -1214,6 +1215,17 @@ Transform OdometryF2M::computeTransform(
 									UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
 									std::map<int, FeatureBA> framePt;
 
+									cv::KeyPoint kpt = wordsKpts[iter->second];
+
+									int cameraIndex = 0;
+									if(lastFrameModels.size()>1)
+									{
+										UASSERT(lastFrameModels[0].imageWidth()>0);
+										float subImageWidth = lastFrameModels[0].imageWidth();
+										cameraIndex = int(kpt.pt.x / subImageWidth);
+										kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
+									}
+
 									//get depth
 									float d = 0.0f;
 									if(lastFrame_->getWords().count(iter->first) == 1 &&
@@ -1221,39 +1233,18 @@ Transform OdometryF2M::computeTransform(
 										util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
 									{
 										//move back point in camera frame (to get depth along z)
-										d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
+										d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], lastFrameModels[cameraIndex].localTransform().inverse()).z;
 									}
 
 
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 							}
 						}
 
 						bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
-
+						bundleModels_.insert(std::make_pair(lastFrame_->id(), lastFrameModels));
-						CameraModel model;
-						if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
-						{
-							model = lastFrame_->sensorData().cameraModels()[0];
-						}
-						else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
-						{
-							model = lastFrame_->sensorData().stereoCameraModel().left();
-							// Set Tx for stereo BA
-							model = CameraModel(model.fx(),
-									model.fy(),
-									model.cx(),
-									model.cy(),
-									model.localTransform(),
-									-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
-						}
-						else
-						{
-							UFATAL("invalid camera model!");
-						}
-						bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
 						bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
 
 						if(!imuT.isNull())
@@ -1400,6 +1391,10 @@ Transform OdometryF2M::computeTransform(
 			}
 		}
 	}
+	else
+	{
+		UERROR("SensorData not valid!");
+	}
 
 	if(info)
 	{
@@ -1426,11 +1421,10 @@ Transform OdometryF2M::computeTransform(
 			nFeatures,
 			regInfo.inliers,
 			regInfo.matches,
-			regInfo.covariance.at<double>(0,0),
+			!regInfo.covariance.empty()?regInfo.covariance.at<double>(0,0):0,
-			regInfo.covariance.at<double>(5,5),
+			!regInfo.covariance.empty()?regInfo.covariance.at<double>(5,5):0,
 			regPipeline_->isImageRequired()?(int)map_->getWords3().size():0,
 			regPipeline_->isScanRequired()?(int)map_->sensorData().laserScanRaw().size():0);
-
 	return output;
 }
 

corelib/src/odometry/OdometryFovis.cpp

@@ -123,18 +123,14 @@ Transform OdometryFovis::computeTransform(
 		return t;
 	}
 
-	if(!((data.cameraModels().size() == 1 &&
+	if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForReprojection()) ||
-			data.cameraModels()[0].isValidForReprojection()) ||
+		(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
-		(data.stereoCameraModel().isValidForProjection() &&
-			data.stereoCameraModel().left().isValidForReprojection() &&
-			data.stereoCameraModel().right().isValidForReprojection())))
 	{
-		UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo camera=%d (reproj=%d|%d)",
+		UERROR("Invalid camera model! Mono cameras=%d (reproj=%d), Stereo cameras=%d (reproj=%d)",
 				(int)data.cameraModels().size(),
 				data.cameraModels().size() && data.cameraModels()[0].isValidForReprojection()?1:0,
-				data.stereoCameraModel().isValidForProjection()?1:0,
+				(int)data.stereoCameraModels().size(),
-				data.stereoCameraModel().left().isValidForReprojection()?1:0,
+				data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()?1:0);
-				data.stereoCameraModel().right().isValidForReprojection()?1:0);
 		return t;
 	}
 
@@ -254,18 +250,18 @@ Transform OdometryFovis::computeTransform(
 		depthImage_->setDepthImage((float*)depth.data);
 		depthSource = depthImage_;
 	}
-	else // stereo
+	else if(data.stereoCameraModels().size() == 1) // stereo
 	{
 		UDEBUG("");
 		// initialize left camera parameters
 		fovis::CameraIntrinsicsParameters left_parameters;
-		left_parameters.width = data.stereoCameraModel().left().imageWidth();
+		left_parameters.width = data.stereoCameraModels()[0].left().imageWidth();
-		left_parameters.height = data.stereoCameraModel().left().imageHeight();
+		left_parameters.height = data.stereoCameraModels()[0].left().imageHeight();
-		left_parameters.fx = data.stereoCameraModel().left().fx();
+		left_parameters.fx = data.stereoCameraModels()[0].left().fx();
-		left_parameters.fy = data.stereoCameraModel().left().fy();
+		left_parameters.fy = data.stereoCameraModels()[0].left().fy();
-		left_parameters.cx = data.stereoCameraModel().left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModel().left().cx();
+		left_parameters.cx = data.stereoCameraModels()[0].left().cx()==0.0?double(left_parameters.width) / 2.0:data.stereoCameraModels()[0].left().cx();
-		left_parameters.cy = data.stereoCameraModel().left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModel().left().cy();
+		left_parameters.cy = data.stereoCameraModels()[0].left().cy()==0.0?double(left_parameters.height) / 2.0:data.stereoCameraModels()[0].left().cy();
-		localTransform = data.stereoCameraModel().localTransform();
+		localTransform = data.stereoCameraModels()[0].localTransform();
 
 		if(rect_ == 0)
 		{
@@ -277,12 +273,12 @@ Transform OdometryFovis::computeTransform(
 		{
 			// initialize right camera parameters
 			fovis::CameraIntrinsicsParameters right_parameters;
-			right_parameters.width = data.stereoCameraModel().right().imageWidth();
+			right_parameters.width = data.stereoCameraModels()[0].right().imageWidth();
-			right_parameters.height = data.stereoCameraModel().right().imageHeight();
+			right_parameters.height = data.stereoCameraModels()[0].right().imageHeight();
-			right_parameters.fx = data.stereoCameraModel().right().fx();
+			right_parameters.fx = data.stereoCameraModels()[0].right().fx();
-			right_parameters.fy = data.stereoCameraModel().right().fy();
+			right_parameters.fy = data.stereoCameraModels()[0].right().fy();
-			right_parameters.cx = data.stereoCameraModel().right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModel().right().cx();
+			right_parameters.cx = data.stereoCameraModels()[0].right().cx()==0.0?double(right_parameters.width) / 2.0:data.stereoCameraModels()[0].right().cx();
-			right_parameters.cy = data.stereoCameraModel().right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModel().right().cy();
+			right_parameters.cy = data.stereoCameraModels()[0].right().cy()==0.0?double(right_parameters.height) / 2.0:data.stereoCameraModels()[0].right().cy();
 
 			// as we use rectified images, rotation is identity
 			// and translation is baseline only
@@ -293,7 +289,7 @@ Transform OdometryFovis::computeTransform(
 			stereo_parameters.right_to_left_rotation[1] = 0.0;
 			stereo_parameters.right_to_left_rotation[2] = 0.0;
 			stereo_parameters.right_to_left_rotation[3] = 0.0;
-			stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModel().baseline();
+			stereo_parameters.right_to_left_translation[0] = -data.stereoCameraModels()[0].baseline();
 			stereo_parameters.right_to_left_translation[1] = 0.0;
 			stereo_parameters.right_to_left_translation[2] = 0.0;