tango: fixed api19 build with latest master

Update AndroidManifest.xml.in
patch for tango-api19
2026-09-10 21:40:19 +08:00 · 2020-07-10 15:08:27 -04:00 · 2020-07-10 09:48:06 -04:00 · 2020-07-10 09:48:06 -04:00 · 2020-07-10 09:04:38 -04:00 · 2020-07-10 08:58:56 -04:00
14 changed files with 207 additions and 764 deletions
@@ -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
@@ -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"
@@ -21,7 +21,6 @@
 #include <GLES2/gl2ext.h>
 #include <cstdlib>
 #include <arcore_c_api.h>
 #include "util.h"
 static const GLfloat BackgroundRenderer_kVertices[] = {
@@ -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!");
 		}
 	}
 }
@@ -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();
+		//isArCoreAvailable();
-		isArEngineAvailable();
+		//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() {
@@ -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)
+	//public void setCamera(ARCoreSharedCamera camera)
-	{
+	//{
-		mCamera = 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)
+				//	if(mCamera!=null)
-					{
+				//	{
-						mCamera.updateGL();
+				//		mCamera.updateGL();
-					}
+				//	}
 					final int value = RTABMapLib.render(mActivity.nativeApplication);
@@ -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) {
@@ -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);
            }
        }
    }
 }
@@ -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];
 					}
 				}
@@ -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
+				if(particleFilters_.size())
 				UASSERT(particleFilters_.size()==6);
 				if(velocityGuess_.isNull())
 				{
-					particleFilters_[0]->init(vx);
+					// Particle filtering
-					particleFilters_[1]->init(vy);
+					UASSERT(particleFilters_.size()==6);
-					particleFilters_[2]->init(vz);
+					if(velocityGuess_.isNull())
 					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)
 					{
-						// arc trajectory around ICR
+						particleFilters_[0]->init(vx);
-						float tmpY = vyaw!=0.0f ? vx / tan((CV_PI-vyaw)/2.0f) : 0.0f;
+						particleFilters_[1]->init(vy);
-						if(fabs(tmpY) < fabs(vy) || (tmpY<=0 && vy >=0) || (tmpY>=0 && vy<=0))
+						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);
+						info->timeParticleFiltering = time.ticks();
 						vroll = particleFilters_[3]->filter(vroll);
 						vpitch = particleFilters_[4]->filter(vpitch);
 					}
 				}
-
+				else if(!_holonomic)
 				if(info)
 				{
-					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)
 			{
@@ -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 arcore1_18/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+rm -r arcore1_18
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip
@@ -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 arcore1_18/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+rm -r arcore1_18
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip
@@ -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 arcore1_18/include/* $prefix/arm64-v8a/include/.
-cp -r arcore/arm64-v8a/* $prefix/arm64-v8a/lib/.
+cp -r arcore1_18/arm64-v8a/* $prefix/arm64-v8a/lib/.
-cp arcore/*.jar rtabmap-tango/app/android/libs/.
+cp arcore1_18/*.jar rtabmap-tango/app/android/libs/.
-rm -r arcore
+rm -r arcore1_18
 # AREngine
 wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip
@@ -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());
Author	SHA1	Message	Date
matlabbe	e4873e770a	tango: fixed api19 build with latest master	2020-07-10 15:08:27 -04:00
matlabbe	3148ae2d76	Update AndroidManifest.xml.in	2020-07-10 09:48:06 -04:00
matlabbe	8819b42f24	patch for tango-api19	2020-07-10 09:48:06 -04:00
matlabbe	bf45a0cba2	docker: fixed arcore not correctly extracted	2020-07-10 09:04:38 -04:00
matlabbe	4be07fc3e9	fixed tango build without arcore support	2020-07-10 08:58:56 -04:00
matlabbe	4e32e8846c	fixed build error	2020-07-08 21:34:41 -04:00
matlabbe	6468245317	Reprocess: added loc stats in motion, added option to export odom and loc poses in g2o format	2020-07-08 21:25:14 -04:00
matlabbe	169b79215d	Memory::computeIcpTransformMulti() added support for 3d scans with RGB channel (L515)	2020-07-08 13:43:28 -04:00
matlabbe	4a09c4bdcf	Odometry: fixed Reg/Force3DoF ignored if filters are not used	2020-07-08 10:03:54 -04:00
matlabbe	3505611fb5	reprocess: set Mem/UseOdomFeatures to false if Kp/DetectorStrategy or Vis/FeatureType is overwritten (#566 )	2020-07-06 13:31:01 -04:00
matlabbe	55228aa574	Update README.md	2020-07-02 21:43:10 -04:00