tango: fixed api19 build with latest master

README.md

@@ -7,7 +7,7 @@ rtabmap ![Analytics](https://ga-beacon-279122.nn.r.appspot.com/UA-56986679-3/git
 [![License][license-image]][license]
 Linux: [![Build Status](https://travis-ci.org/introlab/rtabmap.svg?branch=master)](https://travis-ci.org/introlab/rtabmap) Windows: [![Build status](https://ci.appveyor.com/api/projects/status/hr73xspix9oqa26h/branch/master?svg=true)](https://ci.appveyor.com/project/matlabbe/rtabmap/branch/master)
 
-[release-image]: https://img.shields.io/badge/release-0.18.0-green.svg?style=flat
+[release-image]: https://img.shields.io/badge/release-0.20.2-green.svg?style=flat
 [releases]: https://github.com/introlab/rtabmap/releases
 
 [license-image]: https://img.shields.io/badge/license-BSD-green.svg?style=flat

app/android/AndroidManifest.xml.in

@@ -57,7 +57,7 @@
             android:excludeFromRecents="true"
             android:exported="false"
             android:launchMode="singleTop"
-            android:theme="@android:style/Theme.Material.Light.Dialog.Alert" />
+            android:theme="@style/ThemeApp" />
             
         <provider
             android:name="android.support.v4.content.FileProvider"

app/android/jni/background_renderer.h

@@ -21,7 +21,6 @@
 #include <GLES2/gl2ext.h>
 #include <cstdlib>
 
-#include <arcore_c_api.h>
 #include "util.h"
 
 static const GLfloat BackgroundRenderer_kVertices[] = {

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

@@ -1,588 +0,0 @@
-package com.introlab.rtabmap;
-
-import java.nio.ByteBuffer;
-import java.nio.FloatBuffer;
-import java.util.ArrayList;
-import java.util.Arrays;
-import java.util.EnumSet;
-import java.util.List;
-import java.util.concurrent.atomic.AtomicBoolean;
-
-import com.google.ar.core.Camera;
-import com.google.ar.core.CameraIntrinsics;
-import com.google.ar.core.Config;
-import com.google.ar.core.Frame;
-import com.google.ar.core.ImageMetadata;
-import com.google.ar.core.PointCloud;
-import com.google.ar.core.Pose;
-import com.google.ar.core.Session;
-import com.google.ar.core.SharedCamera;
-import com.google.ar.core.TrackingState;
-import com.google.ar.core.exceptions.CameraNotAvailableException;
-import com.google.ar.core.exceptions.NotYetAvailableException;
-import com.google.ar.core.exceptions.UnavailableException;
-
-import android.content.Context;
-import android.graphics.ImageFormat;
-import android.hardware.camera2.CameraAccessException;
-import android.hardware.camera2.CameraCaptureSession;
-import android.hardware.camera2.CameraCharacteristics;
-import android.hardware.camera2.CameraDevice;
-import android.hardware.camera2.CameraManager;
-import android.hardware.camera2.CaptureFailure;
-import android.hardware.camera2.CaptureRequest;
-import android.hardware.camera2.TotalCaptureResult;
-import android.media.Image;
-import android.opengl.GLES20;
-import android.opengl.GLSurfaceView;
-import android.os.Handler;
-import android.os.HandlerThread;
-import android.support.annotation.NonNull;
-import android.util.Log;
-import android.view.Surface;
-
-public class ARCoreSharedCamera {
-
-	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
-
-
-	private static RTABMapActivity mActivity;
-	public ARCoreSharedCamera(RTABMapActivity c) {
-		mActivity = c;
-	}
-
-	// Depth TOF Image.
-	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
-	private static final int DEPTH_WIDTH = 240;
-	private static final int DEPTH_HEIGHT = 180;
-
-	// GL Surface used to draw camera preview image.
-	public GLSurfaceView surfaceView;
-
-	// ARCore session that supports camera sharing.
-	private Session sharedSession;
-
-	// Camera capture session. Used by both non-AR and AR modes.
-	private CameraCaptureSession captureSession;
-
-	// Reference to the camera system service.
-	private CameraManager cameraManager;
-
-	// Camera device. Used by both non-AR and AR modes.
-	private CameraDevice cameraDevice;
-
-	// Looper handler thread.
-	private HandlerThread backgroundThread;
-	// Looper handler.
-	private Handler backgroundHandler;
-
-	// ARCore shared camera instance, obtained from ARCore session that supports sharing.
-	private SharedCamera sharedCamera;
-
-	// Camera ID for the camera used by ARCore.
-	private String cameraId;
-	
-	private AtomicBoolean mReady = new AtomicBoolean(false);
-
-	// Camera preview capture request builder
-	private CaptureRequest.Builder previewCaptureRequestBuilder;
-
-	private int cameraTextureId = -1;
-
-	// Image reader that continuously processes CPU images.
-	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
-	private boolean mTOFAvailable = false;
-	
-	public boolean isDepthSupported() {return mTOFAvailable;}
-
-	// Camera device state callback.
-	private final CameraDevice.StateCallback cameraDeviceCallback =
-			new CameraDevice.StateCallback() {
-		@Override
-		public void onOpened(@NonNull CameraDevice cameraDevice) {
-			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " opened.");
-			ARCoreSharedCamera.this.cameraDevice = cameraDevice;
-			createCameraPreviewSession();
-		}
-
-		@Override
-		public void onClosed(@NonNull CameraDevice cameraDevice) {
-			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-
-		@Override
-		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
-			Log.w(TAG, "Camera device ID " + cameraDevice.getId() + " disconnected.");
-			cameraDevice.close();
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-
-		@Override
-		public void onError(@NonNull CameraDevice cameraDevice, int error) {
-			Log.e(TAG, "Camera device ID " + cameraDevice.getId() + " error " + error);
-			cameraDevice.close();
-			ARCoreSharedCamera.this.cameraDevice = null;
-		}
-	};
-
-	// Repeating camera capture session state callback.
-	CameraCaptureSession.StateCallback cameraCaptureCallback =
-			new CameraCaptureSession.StateCallback() {
-
-		// Called when the camera capture session is first configured after the app
-		// is initialized, and again each time the activity is resumed.
-		@Override
-		public void onConfigured(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session configured.");
-			captureSession = session;
-			setRepeatingCaptureRequest();
-		}
-
-		@Override
-		public void onSurfacePrepared(
-				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
-			Log.d(TAG, "Camera capture surface prepared.");
-		}
-
-		@Override
-		public void onReady(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session ready.");
-		}
-
-		@Override
-		public void onActive(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session active.");
-			resumeARCore();
-		}
-
-		@Override
-		public void onClosed(@NonNull CameraCaptureSession session) {
-			Log.d(TAG, "Camera capture session closed.");
-		}
-
-		@Override
-		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
-			Log.e(TAG, "Failed to configure camera capture session.");
-		}
-	};
-
-	// Repeating camera capture session capture callback.
-	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
-			new CameraCaptureSession.CaptureCallback() {
-
-		@Override
-		public void onCaptureCompleted(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull TotalCaptureResult result) {
-			Log.i(TAG, "onCaptureCompleted");
-		}
-
-		//@Override // android 23 
-		public void onCaptureBufferLost(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull Surface target,
-				long frameNumber) {
-			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
-		}
-
-		@Override
-		public void onCaptureFailed(
-				@NonNull CameraCaptureSession session,
-				@NonNull CaptureRequest request,
-				@NonNull CaptureFailure failure) {
-			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
-		}
-
-		@Override
-		public void onCaptureSequenceAborted(
-				@NonNull CameraCaptureSession session, int sequenceId) {
-			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
-		}
-	};
-
-	private void resumeARCore() {
-		// Ensure that session is valid before triggering ARCore resume. Handles the case where the user
-		// manually uninstalls ARCore while the app is paused and then resumes.
-		if (sharedSession == null) {
-			return;
-		}
-
-		try {
-			Log.i(TAG, "Resume ARCore.");
-			// Resume ARCore.
-			sharedSession.resume();
-			// Set capture session callback while in AR mode.
-			sharedCamera.setCaptureCallback(captureSessionCallback, backgroundHandler);
-		} catch (CameraNotAvailableException e) {
-			Log.e(TAG, "Failed to resume ARCore session", e);
-			return;
-		}
-
-	}
-
-
-	// Called when starting non-AR mode or switching to non-AR mode.
-	// Also called when app starts in AR mode, or resumes in AR mode.
-	private void setRepeatingCaptureRequest() {
-		try {
-			captureSession.setRepeatingRequest(
-					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "Failed to set repeating request", e);
-		}
-	}
-
-
-	private void createCameraPreviewSession() {
-		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
-		try {
-			// Note that isGlAttached will be set to true in AR mode in onDrawFrame().
-			sharedSession.setCameraTextureName(cameraTextureId);
-
-			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
-			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
-
-			// Build surfaces list, starting with ARCore provided surfaces.
-			List<Surface> surfaceList = sharedCamera.getArCoreSurfaces();
-			Log.e(TAG, " createCameraPreviewSession: " + "surfaceList: sharedCamera.getArCoreSurfaces(): " + surfaceList.size());
-
-			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
-			// may arrive significantly later, or not arrive at all.
-			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
-			// Surface list should now contain three surfacemReadymReadys:
-			// 0. sharedCamera.getSurfaceTexture()
-			// 1. …
-			// 2. depthImageReader.getSurface()
-
-			// Add ARCore surfaces and CPU image surface targets.
-			for (Surface surface : surfaceList) {
-				previewCaptureRequestBuilder.addTarget(surface);
-			}
-
-			// Wrap our callback in a shared camera callback.
-			CameraCaptureSession.StateCallback wrappedCallback = sharedCamera.createARSessionStateCallback(cameraCaptureCallback, backgroundHandler);
-
-			// Create camera capture session for camera preview using ARCore wrapped callback.
-			cameraDevice.createCaptureSession(surfaceList, wrappedCallback, backgroundHandler);
-			
-			mReady.set(true);
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "CameraAccessException", e);
-		}
-
-
-	}
-
-	// Start background handler thread, used to run callbacks without blocking UI thread.
-	private void startBackgroundThread() {
-		backgroundThread = new HandlerThread("sharedCameraBackground");
-		backgroundThread.start();
-		backgroundHandler = new Handler(backgroundThread.getLooper());
-		mTOFImageReader.startBackgroundThread();
-	}
-
-	// Stop background handler thread.
-	private void stopBackgroundThread() {
-		if (backgroundThread != null) {
-			backgroundThread.quitSafely();
-			try {
-				backgroundThread.join();
-				backgroundThread = null;
-				backgroundHandler = null;
-			} catch (InterruptedException e) {
-				Log.e(TAG, "Interrupted while trying to join background handler thread", e);
-			}
-		}
-		mTOFImageReader.stopBackgroundThread();
-	}
-	
-	private long mPreviousTime = 0;
-
-	// Perform various checks, then open camera device and create CPU image reader.
-	public boolean openCamera() {
-
-		close();
-		
-		startBackgroundThread();
-		
-		mPreviousTime = System.currentTimeMillis();
-
-		if(cameraTextureId == -1)
-		{
-			int[] textures = new int[1];
-			GLES20.glGenTextures(1, textures, 0);
-			cameraTextureId = textures[0];
-		}
-
-		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
-		// Don't open camera if already opened.
-		if (cameraDevice != null) {
-			return false;
-		}
-
-		if (sharedSession == null) {
-			try {
-				// Create ARCore session that supports camera sharing.
-				sharedSession = new Session(mActivity, EnumSet.of(Session.Feature.SHARED_CAMERA));
-			} catch (UnavailableException e) {
-				Log.e(TAG, "Failed to create ARCore session that supports camera sharing", e);
-				return false;
-			}
-
-			// Enable auto focus mode while ARCore is running.
-			Config config = sharedSession.getConfig();
-			config.setFocusMode(Config.FocusMode.FIXED);
-			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
-			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
-			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
-			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
-			sharedSession.configure(config);
-
-		}
-
-		// Store the ARCore shared camera reference.
-		sharedCamera = sharedSession.getSharedCamera();
-		// Store the ID of the camera used by ARCore.
-		cameraId = sharedSession.getCameraConfig().getCameraId();
-		initCamera(mActivity, cameraId, 1);
-		ArrayList<String> resolutions;
-
-		mTOFAvailable = false;
-
-		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
-		if (resolutions != null) {
-			for( String temp : resolutions) {
-				Log.e(TAG + "DEPTH16 resolution: ", temp);
-			};
-			if (resolutions.size()>0) mTOFAvailable = true;
-		}
-
-		// Color CPU Image.
-		// Use the currently configured CPU image size.
-		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
-
-		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
-
-		// When ARCore is running, make sure it also updates our CPU image surface.
-		if (mTOFAvailable) {
-			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
-		}
-
-		try {
-
-			// Wrap our callback in a shared camera callback.
-			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
-
-			// Store a reference to the camera system service.
-			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
-
-			// Get the characteristics for the ARCore camera.
-			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
-
-			// Open the camera device using the ARCore wrapped callback.
-			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
-			
-		} catch (CameraAccessException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} catch (IllegalArgumentException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} catch (SecurityException e) {
-			Log.e(TAG, "Failed to open camera", e);
-			return false;
-		} 
-
-		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
-		return true;
-	}
-
-
-	// Close the camera device.
-	public void close() {
-		
-		if (sharedSession != null)  {
-			sharedSession.pause();
-		}
-
-		if (captureSession != null) {
-			captureSession.close();
-			captureSession = null;
-		}
-		if (cameraDevice != null) {
-			cameraDevice.close();
-		}
-
-		if (mTOFImageReader.imageReader != null) {
-			mTOFImageReader.imageReader.close();
-			mTOFImageReader.imageReader = null;
-		}
-
-		if(cameraTextureId>=0)
-		{
-			GLES20.glDeleteTextures(1, new int[] {cameraTextureId}, 0);
-		}
-		
-		stopBackgroundThread();
-	}
-
-	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
-
-	// Draw frame when in AR mode. Called on the GL thread.
-	public void updateGL() throws CameraNotAvailableException {
-
-		if(!mReady.get())
-		{
-			return;
-		}
-		
-		if (mTOFAvailable && mTOFImageReader.frameCount == 0) return;
-		
-		// Perform ARCore per-frame update.
-		Frame frame = null;
-		try {
-			frame = sharedSession.update();
-		} catch (Exception e) {
-			e.printStackTrace();
-			return;
-		}
-
-		Camera camera = null;
-		if (frame != null) {
-			camera = frame.getCamera();
-		}else
-		{
-			return;
-		}
-
-		if (camera == null) return;
-		// If not tracking, don't draw 3D objects.
-		if (camera.getTrackingState() == TrackingState.PAUSED) return;
-
-		if (frame.getTimestamp() != 0) {
-			
-			Pose pose = camera.getPose();
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
-			
-			int rateMs = 100; // send images at most 10 Hz
-			if(System. currentTimeMillis() - mPreviousTime < rateMs)
-			{
-				return;
-			}
-			mPreviousTime = System. currentTimeMillis();
-			
-			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
-			try{
-				Image image = frame.acquireCameraImage();
-				PointCloud cloud = frame.acquirePointCloud();
-				FloatBuffer points = cloud.getPoints();
-
-				if (image.getFormat() != ImageFormat.YUV_420_888) {
-					throw new IllegalArgumentException(
-							"Expected image in YUV_420_888 format, got format " + image.getFormat());
-				}
-				
-				if(!RTABMapActivity.DISABLE_LOG)
-				{
-					for(int i =0;i<image.getPlanes().length;++i)
-					{
-						Log.d(TAG, String.format("Plane[%d] pixel stride = %d,  row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
-					}
-				}
-				
-				float[] fl = intrinsics.getFocalLength();
-				float[] pp = intrinsics.getPrincipalPoint();
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
-				
-				ByteBuffer y = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
-				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
-				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
-
-				double stamp = (double)image.getTimestamp()/10e8;
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
-						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
-
-				if(mTOFAvailable)
-				{
-					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
-							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
-					
-					RTABMapLib.postOdometryEvent(
-							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
-							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
-							points, points.limit()/4);
-				}
-				else
-				{
-					ByteBuffer bb = ByteBuffer.allocate(0);
-					RTABMapLib.postOdometryEvent(
-							RTABMapActivity.nativeApplication,
-							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
-							fl[0], fl[1], pp[0], pp[1], stamp, 
-							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							bb, 0, 0, 0, ImageFormat.DEPTH16,
-							points, points.limit()/4);
-				}
-				
-				image.close();
-				cloud.close();
-				
-			} catch (NotYetAvailableException e) {
-
-			}
-		}
-	}
-
-	/********************************************************************************************************************* */
-	/*************************************************** End ************************************************************* */
-	/********************************************************************************************************************* */
-
-
-	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
-		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
-
-		ArrayList<String> output = new ArrayList<String>();
-		try {
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(imageFormat)) {
-				output.add(s.getWidth() + "x" + s.getHeight());
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		return output;
-	}
-
-
-	public void initCamera (Context context, String cameraId,int index){
-		boolean ok = false;
-		try {
-			int current = 0;
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
-				ok = true;
-				if (current == index)
-					break;
-				else ;
-				current++;
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		if (!ok) {
-			Log.e(TAG + " initCamera", "Depth sensor not found!");
-		}
-	}
-
-
-}

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

@@ -90,9 +90,9 @@ import android.widget.SeekBar.OnSeekBarChangeListener;
 import android.widget.Toast;
 import android.widget.ToggleButton;
 
-import com.google.ar.core.ArCoreApk;
+//import com.google.ar.core.ArCoreApk;
 import com.google.atap.tangoservice.Tango;
-import com.huawei.hiar.AREnginesApk;
+//import com.huawei.hiar.AREnginesApk;
 
 
 // The main activity of the application. This activity shows debug information
@@ -254,7 +254,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	GestureDetector mGesDetect = null;
 
-	ARCoreSharedCamera mArCoreCamera = null;
+	//ARCoreSharedCamera mArCoreCamera = null;
 	int mCameraDriver = 0;
 
 	//Tango Service connection.
@@ -586,8 +586,8 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		String cameraDriverStr = sharedPref.getString(getString(R.string.pref_key_camera_driver), getString(R.string.pref_default_camera_driver));
 		mCameraDriver = Integer.parseInt(cameraDriverStr);
 		
-		isArCoreAvailable();
-		isArEngineAvailable();
+		//isArCoreAvailable();
+		//isArEngineAvailable();
 	}
 	
 	// Should be called only if read/write permissions are granted!
@@ -613,7 +613,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		Log.i(TAG, String.format("updateCameraDriverSettings() mCameraDriver=%d RTABMapLib.isBuiltWith(%d)=%d", mCameraDriver, mCameraDriver, RTABMapLib.isBuiltWith(nativeApplication, mCameraDriver)?1:0));
 		
 		SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this);
-		
+		/*
 		if(mCameraDriver == 0 && (!CheckTangoCoreVersion(MIN_TANGO_CORE_VERSION) || !RTABMapLib.isBuiltWith(nativeApplication, 0)))
 		{
 			if(mIsAREngineAvailable && RTABMapLib.isBuiltWith(nativeApplication, 2))
@@ -659,9 +659,9 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				editor.putString(getString(R.string.pref_key_camera_driver), "3");
 				editor.commit();
 			}
-		}
+		}*/
 	}
-
+/*
 	private void isArCoreAvailable() {
 		ArCoreApk.Availability availability = ArCoreApk.getInstance().checkAvailability(this);
 		if (availability.isTransient()) {
@@ -713,7 +713,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 	}
 
-
+*/
 	@Override
 	public void onDestroy() {
 		super.onDestroy();
@@ -1199,7 +1199,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 					mToast.makeText(this, "Current camera driver selected is Tango, but Tango service binding failed. Abort scanning...", mToast.LENGTH_LONG).show();
 				}
 			}
-		}
+		}/*
 		else if(mCameraDriver == 1 || mCameraDriver == 2 || mCameraDriver == 3)
 		{
 			if((mCameraDriver == 1 || mCameraDriver == 3) && !mIsARCoreAvailable)
@@ -1281,7 +1281,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 				}
 			});
 			bindThread.start();
-		}
+		}*/
 		else
 		{
 			mToast.makeText(this, "Supported camera driver not found! Cannot start a new scan.", mToast.LENGTH_LONG).show();
@@ -2274,14 +2274,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			}
 		}
 
-		if(mArCoreCamera != null)
+/*		if(mArCoreCamera != null)
 		{
 			synchronized (this) {
 				mRenderer.setCamera(null);
 				mArCoreCamera.close();
 				mArCoreCamera = null;
 			}
-		}
+		}*/
 
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {
@@ -2314,14 +2314,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 		updateState(State.STATE_IDLE);
 
-		if(mArCoreCamera != null)
+		/*if(mArCoreCamera != null)
 		{
 			synchronized (this) {
 				mRenderer.setCamera(null);
 				mArCoreCamera.close();
 				mArCoreCamera = null;
 			}
-		}
+		}*/
 		
 		Thread stopThread = new Thread(new Runnable() {
 			public void run() {

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

@@ -43,7 +43,7 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mSurfaceHeight = 0.0f;
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
-	private ARCoreSharedCamera mCamera = null;
+	//private ARCoreSharedCamera mCamera = null;
 	
 	private Vector<TextObject> mTexts;
 
@@ -73,10 +73,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 		mOffset = offset;
 	}
 	
-	public void setCamera(ARCoreSharedCamera camera)
-	{
-		mCamera = camera;
-	}
+	//public void setCamera(ARCoreSharedCamera camera)
+	//{
+	//	mCamera = camera;
+	//}
 
 	// Render loop of the Gl context.
 	public void onDrawFrame(GL10 useGLES20instead) {
@@ -86,10 +86,10 @@ public class Renderer implements GLSurfaceView.Renderer {
 			{
 				try
 				{
-					if(mCamera!=null)
-					{
-						mCamera.updateGL();
-					}
+				//	if(mCamera!=null)
+				//	{
+				//		mCamera.updateGL();
+				//	}
 					
 					final int value = RTABMapLib.render(mActivity.nativeApplication);
 		

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

@@ -361,7 +361,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
 		ed.commit(); //save it.
     }
     
-    @Override
+    
 	public void onRequestPermissionsResult(int requestCode, String[] permissions, int[] results) {
 
 		switch (requestCode) {

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

@@ -1,87 +0,0 @@
-package com.introlab.rtabmap;
-
-
-import android.graphics.ImageFormat;
-import android.media.Image;
-import android.media.ImageReader;
-import android.os.Handler;
-import android.os.HandlerThread;
-import android.util.Log;
-
-import java.nio.ByteBuffer;
-
-public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
-
-    public int WIDTH;
-    public int HEIGHT;
-    public ImageReader imageReader;
-    public int frameCount = 0;
-    public long timestamp;
-    
-    // Looper handler thread.
-    private HandlerThread backgroundThread;
-    // Looper handler.
-    private Handler backgroundHandler;
-
-    public ByteBuffer depth16_raw;
-
-    TOF_ImageReader(){
-    }
-
-    public void createImageReader(int width, int height){
-        this.WIDTH = width;
-        this.HEIGHT = height;
-        this.imageReader =
-                ImageReader.newInstance(
-                        width,
-                        height,
-                        ImageFormat.DEPTH16,
-                        2);
-        this.imageReader.setOnImageAvailableListener(this, this.backgroundHandler);
-    }
-
-    // CPU image reader callback.
-    @Override
-    public void onImageAvailable(ImageReader imageReader) {
-        Image image  = imageReader.acquireLatestImage();
-        if (image == null) {
-            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
-            return;
-        }
-        else{
-            if(image.getFormat() == ImageFormat.DEPTH16){
-                this.timestamp = image.getTimestamp();
-                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
-                // copy raw undecoded DEPTH16 format depth data to NativeBuffer
-                frameCount++;
-            }
-            else{
-                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
-            }
-        }
-        image.close();
-    }
-    
- // Start background handler thread, used to run callbacks without blocking UI thread.
-    public void startBackgroundThread() {
-        this.backgroundThread = new HandlerThread("DepthDecoderThread");
-        this.backgroundThread.start();
-        this.backgroundHandler = new Handler(backgroundThread.getLooper());
-    }
-
-    // Stop background handler thread.
-    public void stopBackgroundThread() {
-        if (this.backgroundThread != null) {
-            this.backgroundThread.quitSafely();
-            try {
-                this.backgroundThread.join();
-                this.backgroundThread = null;
-                this.backgroundHandler = null;
-            } catch (InterruptedException e) {
-                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
-            }
-        }
-    }
-
-}
-

corelib/src/Memory.cpp

@@ -3086,6 +3086,8 @@ Transform Memory::computeIcpTransformMulti(
 		pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
 		pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
 		pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
+		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
 		UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
 		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
 		{
@@ -3111,6 +3113,19 @@ Transform Memory::computeIcpTransformMulti(
 										toPoseInv * iter->second * scan.localTransform());
 							}
 						}
+						else if(scan.hasRGB())
+						{
+							if(scan.hasNormals())
+							{
+								*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+							else
+							{
+								*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
+										toPoseInv * iter->second * scan.localTransform());
+							}
+						}
 						else
 						{
 							if(scan.hasNormals())
@@ -3160,6 +3175,28 @@ Transform Memory::computeIcpTransformMulti(
 		{
 			assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
 		}
+		else if(assembledToNormalRGBClouds->size())
+		{
+			if(fromScan.is2d())
+			{
+				UERROR("Cannot handle 2d scan with RGB format.");
+			}
+			else
+			{
+				assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
+			}
+		}
+		else if(assembledToRGBClouds->size())
+		{
+			if(fromScan.is2d())
+			{
+				UERROR("Cannot handle 2d scan with RGB format.");
+			}
+			else
+			{
+				assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
+			}
+		}
 		UDEBUG("assembledScan=%d points", assembledScan.cols);
 
 		// scans are in base frame but for 2d scans, set the height so that correspondences matching works
@@ -4439,7 +4476,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 					keypoints3D.resize(keypoints.size());
 					for(size_t i=0; i<keypoints.size(); ++i)
 					{
-						UASSERT(keypoints[i].class_id < data.keypoints3D().size());
+						UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
 						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
 					}
 				}

corelib/src/Odometry.cpp

@@ -592,50 +592,58 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					updateKalmanFilter(vx,vy,vz,vroll,vpitch,vyaw);
 				}
 			}
-			else if(particleFilters_.size())
+			else
 			{
-				// Particle filtering
-				UASSERT(particleFilters_.size()==6);
-				if(velocityGuess_.isNull())
+				if(particleFilters_.size())
 				{
-					particleFilters_[0]->init(vx);
-					particleFilters_[1]->init(vy);
-					particleFilters_[2]->init(vz);
-					particleFilters_[3]->init(vroll);
-					particleFilters_[4]->init(vpitch);
-					particleFilters_[5]->init(vyaw);
-				}
-				else
-				{
-					vx = particleFilters_[0]->filter(vx);
-					vy = particleFilters_[1]->filter(vy);
-					vyaw = particleFilters_[5]->filter(vyaw);
-
-					if(!_holonomic)
+					// Particle filtering
+					UASSERT(particleFilters_.size()==6);
+					if(velocityGuess_.isNull())
 					{
-						// arc trajectory around ICR
-						float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-						if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
+						particleFilters_[0]->init(vx);
+						particleFilters_[1]->init(vy);
+						particleFilters_[2]->init(vz);
+						particleFilters_[3]->init(vroll);
+						particleFilters_[4]->init(vpitch);
+						particleFilters_[5]->init(vyaw);
+					}
+					else
+					{
+						vx = particleFilters_[0]->filter(vx);
+						vy = particleFilters_[1]->filter(vy);
+						vyaw = particleFilters_[5]->filter(vyaw);
+
+						if(!_holonomic)
 						{
-							vy = tmpY;
+							// arc trajectory around ICR
+							float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
+							if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
+							{
+								vy = tmpY;
+							}
+							else
+							{
+								vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
+							}
 						}
-						else
+
+						if(!_force3DoF)
 						{
-							vyaw = (atan(vx/vy)*2.0f-CV_PI)*-1;
+							vz = particleFilters_[2]->filter(vz);
+							vroll = particleFilters_[3]->filter(vroll);
+							vpitch = particleFilters_[4]->filter(vpitch);
 						}
 					}
 
-					if(!_force3DoF)
+					if(info)
 					{
-						vz = particleFilters_[2]->filter(vz);
-						vroll = particleFilters_[3]->filter(vroll);
-						vpitch = particleFilters_[4]->filter(vpitch);
+						info->timeParticleFiltering = time.ticks();
 					}
 				}
-
-				if(info)
+				else if(!_holonomic)
 				{
-					info->timeParticleFiltering = time.ticks();
+					// arc trajectory around ICR
+					vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
 				}
 
 				if(_force3DoF)
@@ -645,17 +653,6 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 					vpitch = 0.0f;
 				}
 			}
-			else if(!_holonomic)
-			{
-				// arc trajectory around ICR
-				vy = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
-				if(_force3DoF)
-				{
-					vz = 0.0f;
-					vroll = 0.0f;
-					vpitch = 0.0f;
-				}
-			}
 
 			if(dt)
 			{

docker/bionic/android/rtabmap_api23/rtabmap.bash

@@ -34,10 +34,10 @@ mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/bionic/android/rtabmap_api24/rtabmap.bash

@@ -34,10 +34,10 @@ mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

docker/bionic/android/rtabmap_api26/rtabmap.bash

@@ -34,10 +34,10 @@ mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 wget 'https://docs.google.com/uc?authuser=0&id=1VsibeqRYpS5pjmrG-vYTXyiPg8kbIfVN&export=download' -O arcore.zip
 unzip -qq arcore.zip
 rm arcore.zip
-cp -r arcore/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+cp -r arcore1_18/include/* $prefix/arm64-v8a/include/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore1_18
 
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip

tools/Reprocess/main.cpp

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/OctoMap.h>
 #endif
 #include <rtabmap/core/OccupancyGrid.h>
+#include <rtabmap/core/Graph.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -64,6 +65,7 @@ void showUsage()
 			"     -g3         Assemble 3D cloud map and save it to \"[output]_map.pcd\".\n"
 			"     -o2         Assemble OctoMap 2D projection and save it to \"[output]_octomap.pgm\".\n"
 			"     -o3         Assemble OctoMap 3D cloud and save it to \"[output]_octomap.pcd\".\n"
+			"     -p          Save odometry and localization poses (*.g2o).\n"
 			"%s\n"
 			"\n", Parameters::showUsage());
 	exit(1);
@@ -79,15 +81,22 @@ void sighandler(int sig)
 
 int loopCount = 0;
 int proxCount = 0;
+int loopCountMotion = 0;
 int totalFrames = 0;
+int totalFramesMotion = 0;
 std::vector<float> previousLocalizationDistances;
 std::vector<float> odomDistances;
 std::vector<float> localizationVariations;
 std::vector<float> localizationAngleVariations;
 std::vector<float> localizationTime;
-void showLocalizationStats()
+std::map<int, Transform> odomTrajectoryPoses;
+std::multimap<int, Link> odomTrajectoryLinks;
+std::map<int, Transform> localizationPoses;
+bool exportPoses = false;
+int sessionCount = 0;
+void showLocalizationStats(const std::string & outputDatabasePath)
 {
-	printf("Total localizations on previous session = %d/%d (Loop=%d, Prox=%d)\n", loopCount+proxCount, totalFrames, loopCount, proxCount);
+	printf("Total localizations on previous session = %d/%d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, totalFrames, loopCount, proxCount, loopCountMotion, totalFramesMotion);
 	{
 		float m = uMean(localizationTime);
 		float var = uVariance(localizationTime, m);
@@ -145,14 +154,30 @@ void showLocalizationStats()
 		printf("Average odometry distances = %f m (stddev=%f m)\n", m, stddev);
 	}
 
+	if(exportPoses)
+	{
+		std::string outputPath = outputDatabasePath.substr(0, outputDatabasePath.size()-3);
+		std::string oName = outputPath+uFormat("_session_%d_odom.g2o", sessionCount);
+		std::string lName = outputPath+uFormat("_session_%d_loc.g2o", sessionCount);
+		graph::exportPoses(oName, 4, odomTrajectoryPoses, odomTrajectoryLinks);
+		graph::exportPoses(lName, 4, localizationPoses, odomTrajectoryLinks);
+		printf("Exported %s and %s\n", oName.c_str(), lName.c_str());
+	}
+
 	loopCount = 0;
 	proxCount = 0;
 	totalFrames = 0;
+	loopCountMotion = 0;
+	totalFramesMotion = 0;
 	previousLocalizationDistances.clear();
 	odomDistances.clear();
 	localizationVariations.clear();
 	localizationAngleVariations.clear();
 	localizationTime.clear();
+	odomTrajectoryPoses.clear();
+	odomTrajectoryLinks.clear();
+	localizationPoses.clear();
+	++sessionCount;
 }
 
 int main(int argc, char * argv[])
@@ -221,6 +246,11 @@ int main(int argc, char * argv[])
 				printf("Stop at node ID = %d.\n", stopId);
 			}
 		}
+		else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
+		{
+			exportPoses = true;
+			printf("Odometry trajectory and localization poses will be exported in g2o format (-p option).\n");
+		}
 		else if(strcmp(argv[i], "-g2") == 0 || strcmp(argv[i], "--g2") == 0)
 		{
 			assemble2dMap = true;
@@ -308,6 +338,27 @@ int main(int argc, char * argv[])
 			printf("  %s\t= %s\n", iter->first.c_str(), iter->second.c_str());
 		}
 	}
+	if((configParameters.find(Parameters::kKpDetectorStrategy())!=configParameters.end() ||
+		configParameters.find(Parameters::kVisFeatureType())!=configParameters.end() ||
+		customParameters.find(Parameters::kKpDetectorStrategy())!=customParameters.end() ||
+		customParameters.find(Parameters::kVisFeatureType())!=customParameters.end()) &&
+			configParameters.find(Parameters::kMemUseOdomFeatures())==configParameters.end() &&
+			customParameters.find(Parameters::kMemUseOdomFeatures())==customParameters.end())
+	{
+		bool useOdomFeatures = Parameters::defaultMemUseOdomFeatures();
+		Parameters::parse(parameters, Parameters::kMemUseOdomFeatures(), useOdomFeatures);
+		if(useOdomFeatures)
+		{
+			printf("[Warning] %s and/or %s are overwritten but parameter %s is true in the opened database. "
+					"Setting it to false for convenience to use the new selected feature detector. Set %s "
+					"explicitly to suppress this warning.\n",
+					Parameters::kKpDetectorStrategy().c_str(),
+					Parameters::kVisFeatureType().c_str(),
+					Parameters::kMemUseOdomFeatures().c_str(),
+					Parameters::kMemUseOdomFeatures().c_str());
+			uInsert(parameters, ParametersPair(Parameters::kMemUseOdomFeatures(), "false"));
+		}
+	}
 	uInsert(parameters, configParameters);
 	uInsert(parameters, customParameters);
 
@@ -375,12 +426,18 @@ int main(int argc, char * argv[])
 	OctoMap octomap(parameters);
 #endif
 
+	float linearUpdate = Parameters::defaultRGBDLinearUpdate();
+	float angularUpdate = Parameters::defaultRGBDAngularUpdate();
+	Parameters::parse(parameters, Parameters::kRGBDLinearUpdate(), linearUpdate);
+	Parameters::parse(parameters, Parameters::kRGBDAngularUpdate(), angularUpdate);
+
 	printf("Reprocessing data of \"%s\"...\n", inputDatabasePath.c_str());
 	std::map<std::string, float> globalMapStats;
 	int processed = 0;
 	CameraInfo info;
 	SensorData data = dbReader.takeImage(&info);
 	Transform lastLocalizationOdomPose = info.odomPose;
+	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
 	{
 		UTimer iterationTime;
@@ -396,10 +453,11 @@ int main(int argc, char * argv[])
 				printf("High variance detected, triggering a new map...\n");
 				if(!incrementalMemory && processed>0)
 				{
-					showLocalizationStats();
+					showLocalizationStats(outputDatabasePath);
 					lastLocalizationOdomPose = info.odomPose;
 				}
 				rtabmap.triggerNewMap();
+				inMotion = true;
 			}
 			UTimer t;
 			if(!rtabmap.process(data, info.odomPose, info.odomCovariance, info.odomVelocity, globalMapStats))
@@ -494,6 +552,11 @@ int main(int argc, char * argv[])
 		int landmarkId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
 		int refMapId = stats.refImageMapId();
 		++totalFrames;
+
+		if(inMotion)
+		{
+			++totalFramesMotion;
+		}
 		if (loopId>0)
 		{
 			if(stats.loopClosureId()>0)
@@ -504,6 +567,10 @@ int main(int argc, char * argv[])
 			{
 				++proxCount;
 			}
+			if(inMotion)
+			{
+				++loopCountMotion;
+			}
 			int loopMapId = stats.loopClosureId() > 0? stats.loopClosureMapId(): stats.proximityDetectionMapId();
 			printf("Processed %d/%d nodes [id=%d map=%d]... %dms %s on %d [%d]\n", ++processed, totalIds, refId, refMapId, int(iterationTime.ticks() * 1000), stats.loopClosureId() > 0?"Loop":"Prox", loopId, loopMapId);
 		}
@@ -536,26 +603,44 @@ int main(int argc, char * argv[])
 				localizationVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_norm()));
 				localizationAngleVariations.push_back(stats.data().at(Statistics::kLoopOdom_correction_angle()));
 			}
+
+			if(exportPoses && !info.odomPose.isNull())
+			{
+				if(!odomTrajectoryPoses.empty())
+				{
+					int previousId = odomTrajectoryPoses.rbegin()->first;
+					odomTrajectoryLinks.insert(std::make_pair(previousId, Link(previousId, refId, Link::kNeighbor, odomTrajectoryPoses.rbegin()->second.inverse()*info.odomPose, info.odomCovariance)));
+				}
+				odomTrajectoryPoses.insert(std::make_pair(refId, info.odomPose));
+				localizationPoses.insert(std::make_pair(refId, stats.mapCorrection()*info.odomPose));
+			}
 		}
 
 		Transform odomPose = info.odomPose;
 		data = dbReader.takeImage(&info);
 
+		inMotion = true;
 		if(!incrementalMemory &&
 		   !odomPose.isNull() &&
 		   !info.odomPose.isNull())
 		{
-			odomDistances.push_back(odomPose.getDistance(info.odomPose));
+			float distance = odomPose.getDistance(info.odomPose);
+			float angle = (odomPose.inverse()*info.odomPose).getAngle();
+			odomDistances.push_back(distance);
+			if(distance < linearUpdate && angle <= angularUpdate)
+			{
+				inMotion = false;
+			}
 		}
 	}
 
 	if(!incrementalMemory)
 	{
-		showLocalizationStats();
+		showLocalizationStats(outputDatabasePath);
 	}
 	else
 	{
-		printf("Total loop closures = %d (Loop=%d, Prox=%d)\n", loopCount+proxCount, loopCount, proxCount);
+		printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
 	}
 
 	printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());