tango: fixed api19 build with latest master

CMakeLists.txt

@@ -21,7 +21,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 20)
-SET(RTABMAP_PATCH_VERSION 5)
+SET(RTABMAP_PATCH_VERSION 2)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -61,7 +61,7 @@ ELSE ()
 ENDIF()
 
 if(POLICY CMP0020)
-    cmake_policy(SET CMP0020 NEW)
+    cmake_policy(SET CMP0020 OLD)
 endif()
 if(POLICY CMP0043)
     cmake_policy(SET CMP0043 OLD)
@@ -427,10 +427,6 @@ ENDIF(WITH_CVSBA)
 IF(WITH_POINTMATCHER)
     find_package(libpointmatcher QUIET)
     IF(libpointmatcher_FOUND)
-       find_package(Boost COMPONENTS thread filesystem system program_options date_time REQUIRED)
-       if (Boost_MINOR_VERSION GREATER 47)
-           find_package(Boost COMPONENTS thread filesystem system program_options date_time chrono REQUIRED)
-       endif (Boost_MINOR_VERSION GREATER 47)
        MESSAGE(STATUS "Found libpointmatcher: ${libpointmatcher_INCLUDE_DIRS}")
     ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)

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/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/include/rtabmap/core/BayesFilter.h

@@ -61,8 +61,6 @@ public:
 
 	cv::Mat generatePrediction(const Memory * memory, const std::vector<int> & ids);
 
-	unsigned long getMemoryUsed() const;
-
 private:
 	cv::Mat updatePrediction(const cv::Mat & oldPrediction,
 			const Memory * memory,

corelib/include/rtabmap/core/CameraModel.h

@@ -158,7 +158,5 @@ private:
 	Transform localTransform_;
 };
 
-RTABMAP_EXP std::ostream& operator<<(std::ostream& os, const CameraModel& model);
-
 } /* namespace rtabmap */
 #endif /* CAMERAMODEL_H_ */

corelib/include/rtabmap/core/DBDriver.h

@@ -131,7 +131,7 @@ public:
 	bool openConnection(const std::string & url, bool overwritten = false);
 	void closeConnection(bool save = true, const std::string & outputUrl = "");
 	bool isConnected() const;
-	unsigned long getMemoryUsed() const; // In bytes
+	long getMemoryUsed() const; // In bytes
 	std::string getDatabaseVersion() const;
 	long getNodesMemoryUsed() const;
 	long getLinksMemoryUsed() const;
@@ -188,7 +188,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
 	virtual bool isConnectedQuery() const = 0;
-	virtual unsigned long getMemoryUsedQuery() const = 0; // In bytes
+	virtual long getMemoryUsedQuery() const = 0; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
 	virtual long getNodesMemoryUsedQuery() const = 0;
 	virtual long getLinksMemoryUsedQuery() const = 0;

corelib/include/rtabmap/core/DBDriverSqlite3.h

@@ -54,7 +54,7 @@ protected:
 	virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
 	virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
 	virtual bool isConnectedQuery() const;
-	virtual unsigned long getMemoryUsedQuery() const; // In bytes
+	virtual long getMemoryUsedQuery() const; // In bytes
 	virtual bool getDatabaseVersionQuery(std::string & version) const;
 	virtual long getNodesMemoryUsedQuery() const;
 	virtual long getLinksMemoryUsedQuery() const;
@@ -189,7 +189,7 @@ protected:
 	std::string _version;
 
 private:
-	unsigned long _memoryUsedEstimate;
+	long _memoryUsedEstimate;
 	bool _dbInMemory;
 	unsigned int _cacheSize;
 	int _journalMode;

corelib/include/rtabmap/core/EpipolarGeometry.h

@@ -29,7 +29,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #include "rtabmap/core/RtabmapExp.h" // DLL export/import defines
 #include "rtabmap/core/Parameters.h"
-#include "rtabmap/utilite/UStl.h"
 #include <opencv2/core/core.hpp>
 #include <opencv2/features2d/features2d.hpp>
 #include <pcl/point_cloud.h>
@@ -92,133 +91,41 @@ public:
 	 * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
 	 * realPairsCount = 4
 	 */
-	template<typename T>
 	static int findPairs(
-			const std::map<int, T> & wordsA,
-			const std::map<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true)
-	{
-		int realPairsCount = 0;
-		pairs.clear();
-		for(typename std::map<int, T>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
-		{
-			if(!ignoreNegativeIds || (ignoreNegativeIds && i->first>=0))
-			{
-				std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
-				if(ptB != wordsB.end())
-				{
-					pairs.push_back(std::pair<int, std::pair<T, T> >(i->first, std::make_pair(i->second, ptB->second)));
-					++realPairsCount;
-				}
-			}
-		}
-		return realPairsCount;
-	}
+			const std::map<int, cv::KeyPoint> & wordsA,
+			const std::map<int, cv::KeyPoint> & wordsB,
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
-	template<typename T>
 	static int findPairs(
-			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true)
-	{
-		const std::list<int> & ids = uUniqueKeys(wordsA);
-		typename std::multimap<int, T>::const_iterator iterA;
-		typename std::multimap<int, T>::const_iterator iterB;
-		pairs.clear();
-		int realPairsCount = 0;
-		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-		{
-			if(!ignoreNegativeIds || (ignoreNegativeIds && *i >= 0))
-			{
-				iterA = wordsA.find(*i);
-				iterB = wordsB.find(*i);
-				while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
-				{
-					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::make_pair((*iterA).second, (*iterB).second)));
-					++iterA;
-					++iterB;
-					++realPairsCount;
-				}
-			}
-		}
-		return realPairsCount;
-	}
+			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, cv::KeyPoint> & wordsB,
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
 	 * realPairsCount = 5
 	 */
-	template<typename T>
 	static int findPairsUnique(
-			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true)
-	{
-		const std::list<int> & ids = uUniqueKeys(wordsA);
-		int realPairsCount = 0;
-		pairs.clear();
-		for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
-		{
-			if(!ignoreNegativeIds || (ignoreNegativeIds && *i>=0))
-			{
-				std::list<T> ptsA = uValues(wordsA, *i);
-				std::list<T> ptsB = uValues(wordsB, *i);
-				if(ptsA.size() == 1 && ptsB.size() == 1)
-				{
-					pairs.push_back(std::pair<int, std::pair<T, T> >(*i, std::pair<T, T>(ptsA.front(), ptsB.front())));
-					++realPairsCount;
-				}
-				else if(ptsA.size()>1 && ptsB.size()>1)
-				{
-					// just update the count
-					realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-				}
-			}
-		}
-		return realPairsCount;
-	}
+			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, cv::KeyPoint> & wordsB,
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	/**
 	 * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
 	 * realPairsCount = 5
 	 */
-	template<typename T>
 	static int findPairsAll(
-			const std::multimap<int, T> & wordsA,
-			const std::multimap<int, T> & wordsB,
-			std::list<std::pair<int, std::pair<T, T> > > & pairs,
-			bool ignoreNegativeIds = true)
-	{
-		const std::list<int> & ids = uUniqueKeys(wordsA);
-		pairs.clear();
-		int realPairsCount = 0;;
-		for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
-		{
-			if(!ignoreNegativeIds || (ignoreNegativeIds && *iter>=0))
-			{
-				std::list<T> ptsA = uValues(wordsA, *iter);
-				std::list<T> ptsB = uValues(wordsB, *iter);
-
-				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
-
-				for(typename std::list<T>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
-				{
-					for(typename std::list<T>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
-					{
-						pairs.push_back(std::pair<int, std::pair<T, T> >(*iter, std::pair<T, T>(*jter, *kter)));
-					}
-				}
-			}
-		}
-		return realPairsCount;
-	}
+			const std::multimap<int, cv::KeyPoint> & wordsA,
+			const std::multimap<int, cv::KeyPoint> & wordsB,
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+			bool ignoreNegativeIds = true);
 
 	static cv::Mat linearLSTriangulation(
 			cv::Point3d u,    //homogenous image point (u,v,1)

corelib/include/rtabmap/core/Features2d.h

@@ -115,8 +115,7 @@ public:
 		kFeatureGfttOrb=8,  //new 0.10.11
 		kFeatureKaze=9,     //new 0.13.2
 		kFeatureOrbOctree=10, //new 0.19.2
-		kFeatureSuperPointTorch=11, //new 0.19.7
-		kFeatureSurfFreak=12}; //new 0.20.4
+		kFeatureSuperPointTorch=11}; //new 0.19.7
 	static std::string typeName(Type type)
 	{
 		switch(type){
@@ -144,8 +143,6 @@ public:
 			return "ORB-OCTREE";
 		case kFeatureSuperPointTorch:
 			return "SUPERPOINT";
-		case kFeatureSurfFreak:
-			return "SURF+Freak";
 		default:
 			return "Unknown";
 		}
@@ -458,28 +455,6 @@ private:
 	cv::Ptr<CV_FREAK> _freak;
 };
 
-//SURF_FREAK
-class RTABMAP_EXP SURF_FREAK : public SURF
-{
-public:
-	SURF_FREAK(const ParametersMap & parameters = ParametersMap());
-	virtual ~SURF_FREAK();
-
-	virtual void parseParameters(const ParametersMap & parameters);
-	virtual Feature2D::Type getType() const {return kFeatureSurfFreak;}
-
-private:
-	virtual cv::Mat generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const;
-
-private:
-	bool orientationNormalized_;
-	bool scaleNormalized_;
-	float patternScale_;
-	int nOctaves_;
-
-	cv::Ptr<CV_FREAK> _freak;
-};
-
 //GFTT_ORB
 class RTABMAP_EXP GFTT_ORB : public GFTT
 {

corelib/include/rtabmap/core/FlannIndex.h

@@ -43,8 +43,8 @@ public:
 	void release();
 	unsigned int indexedFeatures() const;
 
-	// return Bytes
-	unsigned long memoryUsed() const;
+	// return KB
+	unsigned int memoryUsed() const;
 
 	// Note that useDistanceL1 doesn't have any effect if LSH is used
 	void buildLinearIndex(
@@ -74,7 +74,7 @@ public:
 	int featuresType() const {return featuresType_;}
 	int featuresDim() const {return featuresDim_;}
 
-	std::vector<unsigned int> addPoints(const cv::Mat & features);
+	unsigned int addPoints(const cv::Mat & features);
 
 	void removePoint(unsigned int index);
 

corelib/include/rtabmap/core/Graph.h

@@ -155,20 +155,12 @@ std::list<Link> RTABMAP_EXP findLinks(
 
 std::multimap<int, Link> RTABMAP_EXP filterDuplicateLinks(
 		const std::multimap<int, Link> & links);
-/**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
- */
 std::multimap<int, Link> RTABMAP_EXP filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType,
-		bool inverted = false);
-/**
- * Return links not of type "filteredType". If inverted=true, return links of of type "filteredType".
- */
+		Link::Type filteredType);
 std::map<int, Link> RTABMAP_EXP filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType,
-		bool inverted = false);
+		Link::Type filteredType);
 
 //Note: This assumes a coordinate system where X is forward, * Y is up, and Z is right.
 std::map<int, Transform> RTABMAP_EXP frustumPosesFiltering(
@@ -263,26 +255,11 @@ std::list<std::pair<int, Transform> > RTABMAP_EXP computePath(
 		float linearVelocity = 0.0f,   // m/sec
 		float angularVelocity = 0.0f); // rad/sec
 
-/**
- * Get the nearest node of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param distance squared distance of the nearest node found (optional)
- * @return the node id.
- */
 int RTABMAP_EXP findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		float * distance = 0);
+		const rtabmap::Transform & targetPose);
 
-/**
- * Get K nearest nodes of the target pose
- * @param nodes the nodes to search for
- * @param targetPose the target pose to search around
- * @param k number of nearest neighbors to search for
- * @return the node ids with squared distance to target pose.
- */
-std::map<int, float> RTABMAP_EXP findNearestNodes(
+std::vector<int> RTABMAP_EXP findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k);
@@ -298,20 +275,11 @@ std::map<int, float> RTABMAP_EXP getNodesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius);
-std::map<int, float> RTABMAP_EXP getNodesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius);
 std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
 		int nodeId,
 		const std::map<int, Transform> & nodes,
 		float radius,
 		float angle = 0.0f);
-std::map<int, Transform> RTABMAP_EXP getPosesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle = 0.0f);
 
 float RTABMAP_EXP computePathLength(
 		const std::vector<std::pair<int, Transform> > & path,

corelib/include/rtabmap/core/Memory.h

@@ -199,10 +199,9 @@ public:
 	cv::Mat getImageCompressed(int signatureId) const;
 	SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const;
 	void getNodeWordsAndGlobalDescriptors(int nodeId,
-			std::multimap<int, int> & words,
-			std::vector<cv::KeyPoint> & wordsKpts,
-			std::vector<cv::Point3f> & words3,
-			cv::Mat & wordsDescriptors,
+			std::multimap<int, cv::KeyPoint> & words,
+			std::multimap<int, cv::Point3f> & words3,
+			std::multimap<int, cv::Mat> & wordsDescriptors,
 			std::vector<GlobalDescriptor> & globalDescriptors) const;
 	void getNodeCalibration(int nodeId,
 			std::vector<CameraModel> & models,
@@ -226,7 +225,6 @@ public:
 	virtual void dumpMemory(std::string directory) const;
 	virtual void dumpSignatures(const char * fileNameSign, bool words3D) const;
 	void dumpDictionary(const char * fileNameRef, const char * fileNameDesc) const;
-	unsigned long getMemoryUsed() const; //Bytes
 
 	void generateGraph(const std::string & fileName, const std::set<int> & ids = std::set<int>());
 

corelib/include/rtabmap/core/OccupancyGrid.h

@@ -104,8 +104,6 @@ public:
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapObstacles() const {return assembledObstacles_;}
 	const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & getMapEmptyCells() const {return assembledEmptyCells_;}
 
-	unsigned long getMemoryUsed() const;
-
 private:
 	ParametersMap parameters_;
 	int cloudDecimation_;

corelib/include/rtabmap/core/Parameters.h

@@ -236,7 +236,6 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, IncrementalDictionary,    bool, true,   "");
     RTABMAP_PARAM(Kp, IncrementalFlann,         bool, true,   uFormat("When using FLANN based strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor \"%s\" in size).", kKpFlannRebalancingFactor().c_str()));
     RTABMAP_PARAM(Kp, FlannRebalancingFactor,   float, 2.0,   uFormat("Factor used when rebuilding the incremental FLANN index (see \"%s\"). Set <=1 to disable.", kKpIncrementalFlann().c_str()));
-    RTABMAP_PARAM(Kp, ByteToFloat,              bool, false,  uFormat("For %s=1, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.", kKpNNStrategy().c_str()));
     RTABMAP_PARAM(Kp, MaxDepth,                 float, 0,     "Filter extracted keypoints by depth (0=inf).");
     RTABMAP_PARAM(Kp, MinDepth,                 float, 0,     "Filter extracted keypoints by depth.");
     RTABMAP_PARAM(Kp, MaxFeatures,              int, 500,     "Maximum features extracted from the images (0 means not bounded, <0 means no extraction).");
@@ -244,9 +243,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Kp, NndrRatio,                float, 0.8,   "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch 12=SURF/FREAK.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 8,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #else
-    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch 12=SURF/FREAK.");
+    RTABMAP_PARAM(Kp, DetectorStrategy,         int, 6,       "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #endif
     RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed,      bool, true,   "Use of the td-idf strategy to compute the likelihood.");
     RTABMAP_PARAM(Kp, Parallelized,             bool, true,   "If the dictionary update and signature creation were parallelized.");
@@ -587,9 +586,9 @@ class RTABMAP_EXP Parameters
     RTABMAP_PARAM(Vis, Iterations,               int, 300,      "Maximum iterations to compute the transform.");
 #if CV_MAJOR_VERSION > 2 && !defined(HAVE_OPENCV_XFEATURES2D)
     // OpenCV>2 without xFeatures2D module doesn't have BRIEF
-    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch 12=SURF/FREAK.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #else
-    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch 12=SURF/FREAK.");
+    RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=KAZE 10=ORB-OCTREE 11=SuperPoint Torch.");
 #endif
     RTABMAP_PARAM(Vis, MaxFeatures,              int, 1000,   "0 no limits.");
     RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,   "Max depth of the features (0 means no limit).");

corelib/include/rtabmap/core/RegistrationInfo.h

@@ -75,7 +75,6 @@ public:
 
 	// RegistrationVis
 	int inliers;
-	float inliersRatio;
 	float inliersMeanDistance;
 	float inliersDistribution;
 	std::vector<int> inliersIDs;

corelib/include/rtabmap/core/Rtabmap.h

@@ -136,9 +136,11 @@ public:
 	std::map<int, int> getWeights() const;
 	int getTotalMemSize() const;
 	double getLastProcessTime() const {return _lastProcessTime;};
+	std::multimap<int, cv::KeyPoint> getWords(int locationId) const;
 	bool isInSTM(int locationId) const;
 	bool isIDsGenerated() const;
 	const Statistics & getStatistics() const;
+	//bool getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const;
 	const std::map<int, Transform> & getLocalOptimizedPoses() const {return _optimizedPoses;}
 	const std::multimap<int, Link> & getLocalConstraints() const {return _constraints;}
 	Transform getPose(int locationId) const;
@@ -196,8 +198,6 @@ public:
 			bool withGrid = false,
 			bool withWords = true,
 			bool withGlobalDescriptors = true) const;
-	std::map<int, Transform> getNodesInRadius(const Transform & pose, float radius); // If radius=0, RGBD/LocalRadius is used. Can return landmarks.
-	std::map<int, Transform> getNodesInRadius(int nodeId, float radius); // If nodeId==0, return poses around latest node. If radius=0, RGBD/LocalRadius is used. Can return landmarks and use landmark id (negative) as request.
 	int detectMoreLoopClosures(
 			float clusterRadius = 0.5f,
 			float clusterAngle = M_PI/6.0f,
@@ -310,7 +310,6 @@ private:
 	double _lastProcessTime;
 	bool _someNodesHaveBeenTransferred;
 	float _distanceTravelled;
-	float _distanceTravelledSinceLastLocalization;
 	bool _optimizeFromGraphEndChanged;
 
 	// Abstract classes containing all loop closure

corelib/include/rtabmap/core/SensorData.h

@@ -275,7 +275,7 @@ public:
 	void setLandmarks(const Landmarks & landmarks) {_landmarks = landmarks;}
 	const Landmarks & landmarks() const {return _landmarks;}
 
-	unsigned long getMemoryUsed() const; // Return memory usage in Bytes
+	long getMemoryUsed() const; // Return memory usage in Bytes
 	/**
 	 * Clear compressed rgb/depth (left/right) images, compressed laser scan and compressed user data.
 	 * Raw data are kept is set.

corelib/include/rtabmap/core/Signature.h

@@ -104,18 +104,19 @@ public:
 
 	//visual words stuff
 	void removeAllWords();
+	void removeWord(int wordId);
 	void changeWordsRef(int oldWordId, int activeWordId);
-	void setWords(const std::multimap<int, int> & words, const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & words3, const cv::Mat & descriptors);
+	void setWords(const std::multimap<int, cv::KeyPoint> & words);
 	bool isEnabled() const {return _enabled;}
 	void setEnabled(bool enabled) {_enabled = enabled;}
-	const std::multimap<int, int> & getWords() const {return _words;}
-	const std::vector<cv::KeyPoint> & getWordsKpts() const {return _wordsKpts;}
+	const std::multimap<int, cv::KeyPoint> & getWords() const {return _words;}
 	int getInvalidWordsCount() const {return _invalidWordsCount;}
 	const std::map<int, int> & getWordsChanged() const {return _wordsChanged;}
-	const cv::Mat & getWordsDescriptors() const {return _wordsDescriptors;}
-	void setWordsDescriptors(const cv::Mat & descriptors);
+	const std::multimap<int, cv::Mat> & getWordsDescriptors() const {return _wordsDescriptors;}
+	void setWordsDescriptors(const std::multimap<int, cv::Mat> & descriptors) {_wordsDescriptors = descriptors;}
 
 	//metric stuff
+	void setWords3(const std::multimap<int, cv::Point3f> & words3) {_words3 = words3;}
 	void setPose(const Transform & pose) {_pose = pose;}
 	void setGroundTruthPose(const Transform & pose) {_groundTruthPose = pose;}
 	void setVelocity(float vx, float vy, float vz, float vroll, float vpitch, float vyaw) {
@@ -128,7 +129,7 @@ public:
 		_velocity[5]=vyaw;
 	}
 
-	const std::vector<cv::Point3f> & getWords3() const {return _words3;}
+	const std::multimap<int, cv::Point3f> & getWords3() const {return _words3;}
 	const Transform & getPose() const {return _pose;}
 	cv::Mat getPoseCovariance() const;
 	const Transform & getGroundTruthPose() const {return _groundTruthPose;}
@@ -137,7 +138,7 @@ public:
 	SensorData & sensorData() {return _sensorData;}
 	const SensorData & sensorData() const {return _sensorData;}
 
-	unsigned long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
+	long getMemoryUsed(bool withSensorData=true) const; // Return memory usage in Bytes
 
 private:
 	int _id;
@@ -154,10 +155,9 @@ private:
 	// Contains all words (Some can be duplicates -> if a word appears 2
 	// times in the signature, it will be 2 times in this list)
 	// Words match with the CvSeq keypoints and descriptors
-	std::multimap<int, int> _words; // word <id, keypoint index>
-	std::vector<cv::KeyPoint> _wordsKpts;
-	std::vector<cv::Point3f> _words3; // in base_link frame (localTransform applied))
-	cv::Mat _wordsDescriptors;
+	std::multimap<int, cv::KeyPoint> _words; // word <id, keypoint>
+	std::multimap<int, cv::Point3f> _words3; // word <id, point> // in base_link frame (localTransform applied))
+	std::multimap<int, cv::Mat> _wordsDescriptors;
 	std::map<int, int> _wordsChanged; // <oldId, newId>
 	bool _enabled;
 	int _invalidWordsCount;

corelib/include/rtabmap/core/Statistics.h

@@ -65,9 +65,7 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Loop, Map_id,);
 	RTABMAP_STATS(Loop, Visual_words,);
 	RTABMAP_STATS(Loop, Visual_inliers,);
-	RTABMAP_STATS(Loop, Visual_inliers_ratio,);
 	RTABMAP_STATS(Loop, Visual_matches,);
-	RTABMAP_STATS(Loop, Distance_since_last_loc,);
 	RTABMAP_STATS(Loop, Last_id,);
 	RTABMAP_STATS(Loop, Optimization_max_error, m);
 	RTABMAP_STATS(Loop, Optimization_max_error_ratio, );
@@ -150,7 +148,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Memory, Odometry_variance_lin,);
 	RTABMAP_STATS(Memory, Distance_travelled, m);
 	RTABMAP_STATS(Memory, RAM_usage, MB);
-	RTABMAP_STATS(Memory, RAM_estimated, MB);
 	RTABMAP_STATS(Memory, Triangulated_points, );
 
 	RTABMAP_STATS(Timing, Memory_update, ms);
@@ -173,7 +170,6 @@ class RTABMAP_EXP Statistics
 	RTABMAP_STATS(Timing, Joining_trash, ms);
 	RTABMAP_STATS(Timing, Emptying_trash, ms);
 	RTABMAP_STATS(Timing, Finalizing_statistics, ms);
-	RTABMAP_STATS(Timing, RAM_estimation, ms);
 
 	RTABMAP_STATS(TimingMem, Pre_update, ms);
 	RTABMAP_STATS(TimingMem, Signature_creation, ms);

corelib/include/rtabmap/core/VWDictionary.h

@@ -100,9 +100,8 @@ public:
 	int getLastIndexedWordId() const;
 	int getTotalActiveReferences() const {return _totalActiveReferences;}
 	unsigned int getIndexedWordsCount() const;
-	unsigned int getIndexMemoryUsed() const; // KB
-	unsigned long getMemoryUsed() const; //Bytes
-	bool setNNStrategy(NNStrategy strategy); // Return true if the search tree has been re-initialized
+	unsigned int getIndexMemoryUsed() const;
+	void setNNStrategy(NNStrategy strategy);
 	bool isIncremental() const {return _incrementalDictionary;}
 	bool isIncrementalFlann() const {return _incrementalFlann;}
 	void setIncrementalDictionary();
@@ -118,8 +117,8 @@ public:
 	void deleteUnusedWords();
 
 public:
-	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat = true);
-	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat = true);
+	static cv::Mat convertBinTo32F(const cv::Mat & descriptorsIn);
+	static cv::Mat convert32FToBin(const cv::Mat & descriptorsIn);
 
 protected:
 	int getNextId();
@@ -132,7 +131,6 @@ private:
 	bool _incrementalDictionary;
 	bool _incrementalFlann;
 	float _rebalancingFactor;
-	bool _byteToFloat;
 	float _nndrRatio;
 	std::string _dictionaryPath; // a pre-computed dictionary (.txt or .db)
 	std::string _newDictionaryPath; // a pre-computed dictionary (.txt or .db)

corelib/include/rtabmap/core/VisualWord.h

@@ -43,7 +43,6 @@ public:
 
 	void addRef(int signatureId);
 	int removeAllRef(int signatureId);
-	unsigned long getMemoryUsed() const;
 
 	int getTotalReferences() const {return _totalReferences;}
 	int id() const {return _id;}

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

@@ -35,8 +35,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "rtabmap/utilite/UTimer.h"
 
 #ifdef RTABMAP_K4A
-#include <k4a/k4atypes.h>
-#include <k4arecord/playback.h>
+ #include <k4a/k4atypes.h>
 #endif
 
 namespace rtabmap
@@ -73,14 +72,15 @@ private:
 private:
 
 #ifdef RTABMAP_K4A
-	k4a_device_t deviceHandle_;
+	k4a_device_t device_;
 	k4a_device_configuration_t config_;
 	k4a_calibration_t calibration_;
-	k4a_transformation_t transformationHandle_;
-	k4a_capture_t captureHandle_;
-	k4a_playback_t playbackHandle_;
+	k4a_transformation_t transformation_;
+	k4a_capture_t capture_;
 	std::string serial_number_;
 
+	void* playbackHandle_;
+	void* transformationHandle_;
 	CameraModel model_;
 	int deviceId_;
 	std::string fileName_;

corelib/src/BayesFilter.cpp

@@ -417,20 +417,6 @@ cv::Mat BayesFilter::generatePrediction(const Memory * memory, const std::vector
 	return prediction;
 }
 
-unsigned long BayesFilter::getMemoryUsed() const
-{
-	long memoryUsage = sizeof(BayesFilter);
-	memoryUsage += _posterior.size() * (sizeof(float)+sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, float>);
-	memoryUsage += _prediction.total() * _prediction.elemSize();
-	memoryUsage += _predictionLC.size() * sizeof(double);
-	memoryUsage += _neighborsIndex.size() * (sizeof(int)+sizeof(std::map<int, int>)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::map<int, int> >);
-	for(std::map<int, std::map<int, int> >::const_iterator iter=_neighborsIndex.begin(); iter!=_neighborsIndex.end(); ++iter)
-	{
-		memoryUsage += iter->second.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
-	}
-	return memoryUsage;
-}
-
 void BayesFilter::normalize(cv::Mat & prediction, unsigned int index, float addedProbabilitiesSum, bool virtualPlaceUsed) const
 {
 	UASSERT(index < (unsigned int)prediction.rows && index < (unsigned int)prediction.cols);

corelib/src/CameraModel.cpp

@@ -765,16 +765,4 @@ bool CameraModel::inFrame(int u, int v) const
 	return uIsInBounds(u, 0, imageWidth()) && uIsInBounds(v, 0, imageHeight());
 }
 
-std::ostream& operator<<(std::ostream& os, const CameraModel& model)
-{
-	os << "Name: " << model.name() << std::endl
-	   << "Size: " << model.imageWidth() << "x" << model.imageHeight() << std::endl
-	   << "K= " << model.K_raw() << std::endl
-	   << "D= " << model.D_raw() << std::endl
-	   << "R= " << model.R() << std::endl
-	   << "P= " << model.P() << std::endl
-	   << "LocalTransform= " << model.localTransform();
-	return os;
-}
-
 } /* namespace rtabmap */

corelib/src/DBDriver.cpp

@@ -107,9 +107,9 @@ bool DBDriver::isConnected() const
 }
 
 // In bytes
-unsigned long DBDriver::getMemoryUsed() const
+long DBDriver::getMemoryUsed() const
 {
-	unsigned long bytes;
+	long bytes;
 	_dbSafeAccessMutex.lock();
 	bytes = getMemoryUsedQuery();
 	_dbSafeAccessMutex.unlock();

corelib/src/DBDriverSqlite3.cpp

@@ -486,7 +486,7 @@ void DBDriverSqlite3::executeNoResultQuery(const std::string & sql) const
 	}
 }
 
-unsigned long DBDriverSqlite3::getMemoryUsedQuery() const
+long DBDriverSqlite3::getMemoryUsedQuery() const
 {
 	if(_dbInMemory)
 	{
@@ -2377,7 +2377,6 @@ void DBDriverSqlite3::getAllNodeIdsQuery(std::set<int> & ids, bool ignoreChildre
 			query << "INNER JOIN Link ";
 			query << "ON id = to_id "; // use to_id to ignore all children (which don't have link pointing on them)
 			query << "WHERE from_id != to_id "; // ignore self referring links
-			query << "AND weight>-9 "; //ignore invalid nodes
 		}
 
 		if(ignoreBadSignatures)
@@ -3079,10 +3078,9 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			const void * descriptor = 0;
 			int dRealSize = 0;
 			cv::KeyPoint kpt;
-			std::multimap<int, int> visualWords;
-			std::vector<cv::KeyPoint> visualWordsKpts;
-			std::vector<cv::Point3f> visualWords3;
-			cv::Mat descriptors;
+			std::multimap<int, cv::KeyPoint> visualWords;
+			std::multimap<int, cv::Point3f> visualWords3;
+			std::multimap<int, cv::Mat> descriptors;
 			bool allWords3NaN = true;
 			cv::Point3f depth(0,0,0);
 
@@ -3132,9 +3130,8 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 					depth.z = sqlite3_column_double(ppStmt, index++);
 				}
 
-				visualWordsKpts.push_back(kpt);
-				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, visualWordsKpts.size()-1));
-				visualWords3.push_back(depth);
+				visualWords.insert(visualWords.end(), std::make_pair(visualWordId, kpt));
+				visualWords3.insert(visualWords3.end(), std::make_pair(visualWordId, depth));
 
 				if(allWords3NaN && util3d::isFinite(depth))
 				{
@@ -3167,7 +3164,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 
 						memcpy(d.data, descriptor, dRealSize);
 
-						descriptors.push_back(d);
+						descriptors.insert(descriptors.end(), std::make_pair(visualWordId, d));
 					}
 				}
 
@@ -3181,12 +3178,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
 			}
 			else
 			{
-				if(allWords3NaN)
+				(*iter)->setWords(visualWords);
+				if(!allWords3NaN)
 				{
-					visualWords3.clear();
+					(*iter)->setWords3(visualWords3);
 				}
-				(*iter)->setWords(visualWords, visualWordsKpts, visualWords3, descriptors);
-				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.rows, (*iter)->id());
+				(*iter)->setWordsDescriptors(descriptors);
+				ULOGGER_DEBUG("Add %d keypoints, %d 3d points and %d descriptors to node %d", (int)visualWords.size(), allWords3NaN?0:(int)visualWords3.size(), (int)descriptors.size(), (*iter)->id());
 			}
 
 			//reset
@@ -3624,7 +3622,6 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		int descriptorSize;
 		const void * descriptor;
 		int dRealSize;
-		unsigned long dRealSizeTotal = 0;
 		for(std::set<int>::const_iterator iter=wordIds.begin(); iter!=wordIds.end(); ++iter)
 		{
 			// bind id
@@ -3657,7 +3654,6 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 				}
 
 				memcpy(d.data, descriptor, dRealSize);
-				dRealSizeTotal+=dRealSize;
 				VisualWord * vw = new VisualWord(*iter, d);
 				if(vw)
 				{
@@ -3679,7 +3675,7 @@ void DBDriverSqlite3::loadWordsQuery(const std::set<int> & wordIds, std::list<Vi
 		rc = sqlite3_finalize(ppStmt);
 		UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
 
-		UDEBUG("Time=%fs (%d words, %lu MB)", timer.ticks(), (int)vws.size(), dRealSizeTotal/1000000);
+		ULOGGER_DEBUG("Time=%fs", timer.ticks());
 
 		if(wordIds.size() != loaded.size())
 		{
@@ -4276,25 +4272,30 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
 		float nanFloat = std::numeric_limits<float>::quiet_NaN ();
 		for(std::list<Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 		{
-			UASSERT((*i)->getWords().size() == (*i)->getWordsKpts().size());
 			UASSERT((*i)->getWords3().empty() || (*i)->getWords().size() == (*i)->getWords3().size());
-			UASSERT((*i)->getWordsDescriptors().empty() || (int)(*i)->getWords().size() == (*i)->getWordsDescriptors().rows);
+			UASSERT((*i)->getWordsDescriptors().empty() || (*i)->getWords().size() == (*i)->getWordsDescriptors().size());
 
-			for(std::multimap<int, int>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
+			std::multimap<int, cv::Point3f>::const_iterator p=(*i)->getWords3().begin();
+			std::multimap<int, cv::Mat>::const_iterator d=(*i)->getWordsDescriptors().begin();
+			for(std::multimap<int, cv::KeyPoint>::const_iterator w=(*i)->getWords().begin(); w!=(*i)->getWords().end(); ++w)
 			{
 				cv::Point3f pt(nanFloat,nanFloat,nanFloat);
-				if(!(*i)->getWords3().empty())
+				if(p!=(*i)->getWords3().end())
 				{
-					pt = (*i)->getWords3()[w->second];
+					UASSERT(w->first == p->first); // must be same id!
+					pt = p->second;
+					++p;
 				}
 
 				cv::Mat descriptor;
-				if(!(*i)->getWordsDescriptors().empty())
+				if(d!=(*i)->getWordsDescriptors().end())
 				{
-					descriptor = (*i)->getWordsDescriptors().row(w->second);
+					UASSERT(w->first == d->first); // must be same id!
+					descriptor = d->second;
+					++d;
 				}
 
-				stepKeypoint(ppStmt, (*i)->id(), w->first, (*i)->getWordsKpts()[w->second], pt, descriptor);
+				stepKeypoint(ppStmt, (*i)->id(), w->first, w->second, pt, descriptor);
 			}
 		}
 		// Finalize (delete) the statement

corelib/src/DBReader.cpp

@@ -510,9 +510,23 @@ SensorData DBReader::getNextData(CameraInfo * info)
 					data.gps().stamp()!=0.0?1:0,
 					gravityTransform.isNull()?0:1);
 
-			cv::Mat descriptors = s->getWordsDescriptors().clone();
-			const std::vector<cv::KeyPoint> & keypoints = s->getWordsKpts();
-			const std::vector<cv::Point3f> & keypoints3D = s->getWords3();
+			cv::Mat descriptors;
+			if(!s->getWordsDescriptors().empty())
+			{
+				descriptors = cv::Mat(
+					s->getWordsDescriptors().size(),
+					s->getWordsDescriptors().begin()->second.cols,
+					s->getWordsDescriptors().begin()->second.type());
+				int i=0;
+				for(std::multimap<int, cv::Mat>::const_iterator iter=s->getWordsDescriptors().begin();
+					iter!=s->getWordsDescriptors().end();
+					++iter, ++i)
+				{
+					iter->second.copyTo(descriptors.row(i));
+				}
+			}
+			std::vector<cv::KeyPoint> keypoints = uValues(s->getWords());
+			std::vector<cv::Point3f> keypoints3D = uValues(s->getWords3());
 			if(!keypoints.empty() &&
 			   (keypoints3D.empty() || keypoints.size() == keypoints3D.size()) &&
 			   (descriptors.empty() || (int)keypoints.size() == descriptors.rows))

corelib/src/EpipolarGeometry.cpp

@@ -70,21 +70,15 @@ bool EpipolarGeometry::check(const Signature * ssA, const Signature * ssB)
 	}
 	ULOGGER_DEBUG("id(%d,%d)", ssA->id(), ssB->id());
 
-	std::list<std::pair<int, std::pair<int, int> > > pairsId;
+	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 
-	findPairsUnique(ssA->getWords(), ssB->getWords(), pairsId);
+	findPairsUnique(ssA->getWords(), ssB->getWords(), pairs);
 
-	if((int)pairsId.size()<_matchCountMinAccepted)
+	if((int)pairs.size()<_matchCountMinAccepted)
 	{
 		return false;
 	}
 
-	std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
-	for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter = pairsId.begin(); iter!=pairsId.end(); ++iter)
-	{
-		pairs.push_back(std::make_pair(iter->first, std::make_pair(ssA->getWordsKpts()[iter->second.first], ssB->getWordsKpts()[iter->second.second])));
-	}
-
 	std::vector<uchar> status;
 	cv::Mat f = findFFromWords(pairs, status, _ransacParam1, _ransacParam2);
 
@@ -412,6 +406,136 @@ cv::Mat EpipolarGeometry::findFFromCalibratedStereoCameras(double fx, double fy,
 	return K.inv().t()*E*K.inv();
 }
 
+/**
+ * if a=[1 2 3 4 6], b=[1 2 4 5 6], results= [(1,1) (2,2) (4,4) (6,6)]
+ * realPairsCount = 4
+ */
+int EpipolarGeometry::findPairs(
+		const std::map<int, cv::KeyPoint> & wordsA,
+		const std::map<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
+{
+	int realPairsCount = 0;
+	pairs.clear();
+	for(std::map<int, cv::KeyPoint>::const_iterator i=wordsA.begin(); i!=wordsA.end(); ++i)
+	{
+		if(!ignoreInvalidIds || (ignoreInvalidIds && i->first>=0))
+		{
+			std::map<int, cv::KeyPoint>::const_iterator ptB = wordsB.find(i->first);
+			if(ptB != wordsB.end())
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(i->first, std::pair<cv::KeyPoint, cv::KeyPoint>(i->second, ptB->second)));
+				++realPairsCount;
+			}
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (2,2) (4,4) (6a,6a) (6b,6b)]
+ * realPairsCount = 5
+ */
+int EpipolarGeometry::findPairs(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	std::multimap<int, cv::KeyPoint>::const_iterator iterA;
+	std::multimap<int, cv::KeyPoint>::const_iterator iterB;
+	pairs.clear();
+	int realPairsCount = 0;
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i >= 0))
+		{
+			iterA = wordsA.find(*i);
+			iterB = wordsB.find(*i);
+			while(iterA != wordsA.end() && iterB != wordsB.end() && (*iterA).first == (*iterB).first && (*iterA).first == *i)
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>((*iterA).second, (*iterB).second)));
+				++iterA;
+				++iterB;
+				++realPairsCount;
+			}
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(2,2) (4,4)]
+ * realPairsCount = 5
+ */
+int EpipolarGeometry::findPairsUnique(
+		const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
+{
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	int realPairsCount = 0;
+	pairs.clear();
+	for(std::list<int>::const_iterator i=ids.begin(); i!=ids.end(); ++i)
+	{
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *i>=0))
+		{
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *i);
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *i);
+			if(ptsA.size() == 1 && ptsB.size() == 1)
+			{
+				pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*i, std::pair<cv::KeyPoint, cv::KeyPoint>(ptsA.front(), ptsB.front())));
+				++realPairsCount;
+			}
+			else if(ptsA.size()>1 && ptsB.size()>1)
+			{
+				// just update the count
+				realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+			}
+		}
+	}
+	return realPairsCount;
+}
+
+/**
+ * if a=[1 2 3 4 6 6], b=[1 1 2 4 5 6 6], results= [(1,1a) (1,1b) (2,2) (4,4) (6a,6a) (6a,6b) (6b,6a) (6b,6b)]
+ * realPairsCount = 5
+ */
+int EpipolarGeometry::findPairsAll(const std::multimap<int, cv::KeyPoint> & wordsA,
+		const std::multimap<int, cv::KeyPoint> & wordsB,
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > & pairs,
+		bool ignoreInvalidIds)
+{
+	UTimer timer;
+	timer.start();
+	const std::list<int> & ids = uUniqueKeys(wordsA);
+	pairs.clear();
+	int realPairsCount = 0;;
+	for(std::list<int>::const_iterator iter=ids.begin(); iter!=ids.end(); ++iter)
+	{
+		if(!ignoreInvalidIds || (ignoreInvalidIds && *iter>=0))
+		{
+			std::list<cv::KeyPoint> ptsA = uValues(wordsA, *iter);
+			std::list<cv::KeyPoint> ptsB = uValues(wordsB, *iter);
+
+			realPairsCount += ptsA.size() > ptsB.size() ? ptsB.size() : ptsA.size();
+
+			for(std::list<cv::KeyPoint>::iterator jter=ptsA.begin(); jter!=ptsA.end(); ++jter)
+			{
+				for(std::list<cv::KeyPoint>::iterator kter=ptsB.begin(); kter!=ptsB.end(); ++kter)
+				{
+					pairs.push_back(std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> >(*iter, std::pair<cv::KeyPoint, cv::KeyPoint>(*jter, *kter)));
+				}
+			}
+		}
+	}
+	ULOGGER_DEBUG("time = %f", timer.ticks());
+	return realPairsCount;
+}
+
+
 
 /**
  source = SfM toy library: https://github.com/royshil/SfM-Toy-Library

corelib/src/Features2d.cpp

@@ -511,7 +511,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 
 #if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
 #ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift || type == Feature2D::kFeatureSurfFreak)
+	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift)
 	{
 #if CV_MAJOR_VERSION < 3
 		UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. GFTT/ORB is used instead.");
@@ -524,8 +524,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 	if(type == Feature2D::kFeatureFastBrief ||
 	   type == Feature2D::kFeatureFastFreak ||
 	   type == Feature2D::kFeatureGfttBrief ||
-	   type == Feature2D::kFeatureGfttFreak ||
-	   type == Feature2D::kFeatureSurfFreak)
+	   type == Feature2D::kFeatureGfttFreak)
 	{
 		UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. GFTT/ORB is used instead.");
 		type = Feature2D::kFeatureGfttOrb;
@@ -536,7 +535,7 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 #else // >= 4.4.0 >= 3.4.11
 
 #ifndef RTABMAP_NONFREE
-	if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSurfFreak)
+	if(type == Feature2D::kFeatureSurf)
 	{
 		UWARN("SURF features cannot be used because OpenCV was not built with nonfree module. SIFT is used instead.");
 		type = Feature2D::kFeatureSift;
@@ -615,9 +614,6 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame
 		feature2D = new SuperPointTorch(parameters);
 		break;
 #endif
-	case Feature2D::kFeatureSurfFreak:
-		feature2D = new SURF_FREAK(parameters);
-		break;
 #ifdef RTABMAP_NONFREE
 	default:
 		feature2D = new SURF(parameters);
@@ -1728,59 +1724,6 @@ cv::Mat GFTT_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<c
 	return descriptors;
 }
 
-//////////////////////////
-//SURF-FREAK
-//////////////////////////
-SURF_FREAK::SURF_FREAK(const ParametersMap & parameters) :
-	SURF(parameters),
-	orientationNormalized_(Parameters::defaultFREAKOrientationNormalized()),
-	scaleNormalized_(Parameters::defaultFREAKScaleNormalized()),
-	patternScale_(Parameters::defaultFREAKPatternScale()),
-	nOctaves_(Parameters::defaultFREAKNOctaves())
-{
-	parseParameters(parameters);
-}
-
-SURF_FREAK::~SURF_FREAK()
-{
-}
-
-void SURF_FREAK::parseParameters(const ParametersMap & parameters)
-{
-	SURF::parseParameters(parameters);
-
-	Parameters::parse(parameters, Parameters::kFREAKOrientationNormalized(), orientationNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKScaleNormalized(), scaleNormalized_);
-	Parameters::parse(parameters, Parameters::kFREAKPatternScale(), patternScale_);
-	Parameters::parse(parameters, Parameters::kFREAKNOctaves(), nOctaves_);
-
-#if CV_MAJOR_VERSION < 3
-	_freak = cv::Ptr<CV_FREAK>(new CV_FREAK(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_));
-#else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak = CV_FREAK::create(orientationNormalized_, scaleNormalized_, patternScale_, nOctaves_);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
-#endif
-}
-
-cv::Mat SURF_FREAK::generateDescriptorsImpl(const cv::Mat & image, std::vector<cv::KeyPoint> & keypoints) const
-{
-	UASSERT(!image.empty() && image.channels() == 1 && image.depth() == CV_8U);
-	cv::Mat descriptors;
-#if CV_MAJOR_VERSION < 3
-	_freak->compute(image, keypoints, descriptors);
-#else
-#ifdef HAVE_OPENCV_XFEATURES2D
-	_freak->compute(image, keypoints, descriptors);
-#else
-	UWARN("RTAB-Map is not built with OpenCV xfeatures2d module so Freak cannot be used!");
-#endif
-#endif
-	return descriptors;
-}
-
 //////////////////////////
 //GFTT-ORB
 //////////////////////////

corelib/src/FlannIndex.cpp

@@ -107,36 +107,32 @@ unsigned int FlannIndex::indexedFeatures() const
 	}
 }
 
-// return Bytes
-unsigned long FlannIndex::memoryUsed() const
+// return KB
+unsigned int FlannIndex::memoryUsed() const
 {
 	if(!index_)
 	{
 		return 0;
 	}
-	unsigned long memoryUsage = sizeof(FlannIndex);
-	memoryUsage += addedDescriptors_.size() * (sizeof(int) + sizeof(cv::Mat) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, cv::Mat>);
-	memoryUsage += sizeof(std::list<int>) + removedIndexes_.size() * sizeof(int);
 	if(featuresType_ == CV_8UC1)
 	{
-		memoryUsage += ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory();
+		return ((const rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->usedMemory()/1000;
 	}
 	else
 	{
 		if(useDistanceL1_)
 		{
-			memoryUsage += ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory();
+			return ((const rtflann::Index<rtflann::L1<float> >*)index_)->usedMemory()/1000;
 		}
 		else if(featuresDim_ <= 3)
 		{
-			memoryUsage += ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory();
+			return ((const rtflann::Index<rtflann::L2_Simple<float> >*)index_)->usedMemory()/1000;
 		}
 		else
 		{
-			memoryUsage += ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory();
+			return ((const rtflann::Index<rtflann::L2<float> >*)index_)->usedMemory()/1000;
 		}
 	}
-	return memoryUsage;
 }
 
 void FlannIndex::buildLinearIndex(
@@ -181,21 +177,10 @@ void FlannIndex::buildLinearIndex(
 		}
 	}
 
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
 	UDEBUG("");
 }
 
@@ -242,21 +227,10 @@ void FlannIndex::buildKDTreeIndex(
 		}
 	}
 
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
 	UDEBUG("");
 }
 
@@ -304,21 +278,10 @@ void FlannIndex::buildKDTreeSingleIndex(
 		}
 	}
 
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
 	UDEBUG("");
 }
 
@@ -342,21 +305,10 @@ void FlannIndex::buildLSHIndex(
 	index_ = new rtflann::Index<rtflann::Hamming<unsigned char> >(dataset, rtflann::LshIndexParams(12, 20, 2));
 	((rtflann::Index<rtflann::Hamming<unsigned char> >*)index_)->buildIndex();
 
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	if(rebalancingFactor_ > 1.0f)
-	{
-		for(int i=0; i<features.rows; ++i)
-		{
-			addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-		}
-	}
-	else
-	{
-		// tree won't ever be rebalanced, so just keep only one header for the data
-		addedDescriptors_.insert(std::make_pair(nextIndex_, features));
-		nextIndex_ += features.rows;
-	}
+	// incremental FLANN
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
+
+	nextIndex_ = features.rows;
 	UDEBUG("");
 }
 
@@ -365,12 +317,12 @@ bool FlannIndex::isBuilt()
 	return index_!=0;
 }
 
-std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
+unsigned int FlannIndex::addPoints(const cv::Mat & features)
 {
 	if(!index_)
 	{
 		UERROR("Flann index not yet created!");
-		return std::vector<unsigned int>();
+		return 0;
 	}
 	UASSERT(features.type() == featuresType_);
 	UASSERT(features.cols == featuresDim_);
@@ -449,16 +401,11 @@ std::vector<unsigned int> FlannIndex::addPoints(const cv::Mat & features)
 		removedIndexes_.clear();
 	}
 
-	// incremental FLANN: we should add all headers separately in case we remove
-	// some indexes (to keep underlying matrix data allocated)
-	std::vector<unsigned int> indexes;
-	for(int i=0; i<features.rows; ++i)
-	{
-		indexes.push_back(nextIndex_);
-		addedDescriptors_.insert(std::make_pair(nextIndex_++, features.row(i)));
-	}
+	addedDescriptors_.insert(std::make_pair(nextIndex_, features));
 
-	return indexes;
+	int r = nextIndex_;
+	nextIndex_ += features.rows;
+	return r;
 }
 
 void FlannIndex::removePoint(unsigned int index)

corelib/src/Graph.cpp

@@ -1129,22 +1129,19 @@ std::multimap<int, Link> filterDuplicateLinks(
 
 std::multimap<int, Link> filterLinks(
 		const std::multimap<int, Link> & links,
-		Link::Type filteredType,
-		bool inverted)
+		Link::Type filteredType)
 {
 	std::multimap<int, Link> output;
 	for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if((!inverted && iter->second.from() != iter->second.to())||
-			   (inverted && iter->second.from() == iter->second.to()))
+			if(iter->second.from() != iter->second.to())
 			{
 				output.insert(*iter);
 			}
 		}
-		else if((!inverted && iter->second.type() != filteredType)||
-				(inverted && iter->second.type() == filteredType))
+		else if(iter->second.type() != filteredType)
 		{
 			output.insert(*iter);
 		}
@@ -1154,22 +1151,19 @@ std::multimap<int, Link> filterLinks(
 
 std::map<int, Link> filterLinks(
 		const std::map<int, Link> & links,
-		Link::Type filteredType,
-		bool inverted)
+		Link::Type filteredType)
 {
 	std::map<int, Link> output;
 	for(std::map<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 	{
 		if(filteredType == Link::kSelfRefLink)
 		{
-			if((!inverted && iter->second.from() != iter->second.to())||
-			   (inverted && iter->second.from() == iter->second.to()))
+			if(iter->second.from() != iter->second.to())
 			{
 				output.insert(*iter);
 			}
 		}
-		else if((!inverted && iter->second.type() != filteredType)||
-				(inverted && iter->second.type() == filteredType))
+		else if(iter->second.type() != filteredType)
 		{
 			output.insert(*iter);
 		}
@@ -2060,28 +2054,23 @@ std::list<std::pair<int, Transform> > computePath(
 
 int findNearestNode(
 		const std::map<int, rtabmap::Transform> & nodes,
-		const rtabmap::Transform & targetPose,
-		float * distance)
+		const rtabmap::Transform & targetPose)
 {
 	int id = 0;
-	std::map<int, float> nearestNodes = findNearestNodes(nodes, targetPose, 1);
-	if(!nearestNodes.empty())
+	std::vector<int> nearestNodes = findNearestNodes(nodes, targetPose, 1);
+	if(nearestNodes.size())
 	{
-		id = nearestNodes.begin()->first;
-		if(distance)
-		{
-			*distance = nearestNodes.begin()->second;
-		}
+		id = nearestNodes[0];
 	}
 	return id;
 }
 
-std::map<int, float> findNearestNodes(
+std::vector<int> findNearestNodes(
 		const std::map<int, rtabmap::Transform> & nodes,
 		const rtabmap::Transform & targetPose,
 		int k)
 {
-	std::map<int, float> nearestIds;
+	std::vector<int> nearestIds;
 	if(nodes.size() && !targetPose.isNull())
 	{
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -2101,9 +2090,10 @@ std::map<int, float> findNearestNodes(
 		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
 		kdTree->nearestKSearch(pt, k, ind, dist);
 
+		nearestIds.resize(ind.size());
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
-			nearestIds.insert(std::make_pair(ids[ind[i]], dist[i]));
+			nearestIds[i] = ids[ind[i]];
 		}
 	}
 	return nearestIds;
@@ -2116,21 +2106,8 @@ std::map<int, float> getNodesInRadius(
 		float radius)
 {
 	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
-	nodesMinusTarget.erase(nodeId);
-	return getNodesInRadius(targetPose, nodesMinusTarget, radius);
-}
-
-// return <id, sqrd distance>, excluding query
-std::map<int, float> getNodesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius)
-{
 	std::map<int, float> foundNodes;
-	if(nodes.empty())
+	if(nodes.size() <= 1)
 	{
 		return foundNodes;
 	}
@@ -2141,21 +2118,26 @@ std::map<int, float> getNodesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-		ids[oi] = iter->first;
-		++oi;
+		if(iter->first != nodeId)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+			ids[oi] = iter->first;
+			++oi;
+		}
 	}
 	cloud->resize(oi);
 	ids.resize(oi);
 
+	Transform fromT = nodes.at(nodeId);
+
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{
@@ -2177,21 +2159,8 @@ std::map<int, Transform> getPosesInRadius(
 		float angle)
 {
 	UASSERT(uContains(nodes, nodeId));
-
-	std::map<int, Transform> nodesMinusTarget = nodes;
-	Transform targetPose = nodes.at(nodeId);
-	nodesMinusTarget.erase(nodeId);
-	return getPosesInRadius(targetPose, nodesMinusTarget, radius, angle);
-}
-// return <id, Transform>, excluding query
-std::map<int, Transform> getPosesInRadius(
-		const Transform & targetPose,
-		const std::map<int, Transform> & nodes,
-		float radius,
-		float angle)
-{
 	std::map<int, Transform> foundNodes;
-	if(nodes.empty())
+	if(nodes.size() <= 1)
 	{
 		return foundNodes;
 	}
@@ -2202,24 +2171,29 @@ std::map<int, Transform> getPosesInRadius(
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
-		(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-		UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
-		ids[oi] = iter->first;
-		++oi;
+		if(iter->first != nodeId)
+		{
+			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+			ids[oi] = iter->first;
+			++oi;
+		}
 	}
 	cloud->resize(oi);
 	ids.resize(oi);
 
+	Transform fromT = nodes.at(nodeId);
+
 	if(cloud->size())
 	{
 		pcl::search::KdTree<pcl::PointXYZ>::Ptr kdTree(new pcl::search::KdTree<pcl::PointXYZ>);
 		kdTree->setInputCloud(cloud);
 		std::vector<int> ind;
 		std::vector<float> sqrdDist;
-		pcl::PointXYZ pt(targetPose.x(), targetPose.y(), targetPose.z());
+		pcl::PointXYZ pt(fromT.x(), fromT.y(), fromT.z());
 		kdTree->radiusSearch(pt, radius, ind, sqrdDist, 0);
 
-		Eigen::Vector3f vA = targetPose.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
+		Eigen::Vector3f vA = fromT.toEigen3f().linear()*Eigen::Vector3f(1,0,0);
 
 		for(unsigned int i=0; i<ind.size(); ++i)
 		{

corelib/src/Memory.cpp

@@ -362,7 +362,7 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			const std::map<int, Signature *> & signatures = this->getSignatures();
 			for(std::map<int, Signature *>::const_iterator i=signatures.begin(); i!=signatures.end(); ++i)
 			{
-				const std::multimap<int, int> & words = i->second->getWords();
+				const std::multimap<int, cv::KeyPoint> & words = i->second->getWords();
 				std::list<int> keys = uUniqueKeys(words);
 				for(std::list<int>::iterator iter=keys.begin(); iter!=keys.end(); ++iter)
 				{
@@ -413,11 +413,11 @@ void Memory::loadDataFromDb(bool postInitClosingEvents)
 			Signature * s = this->_getSignature(i->first);
 			UASSERT(s != 0);
 
-			const std::multimap<int, int> & words = s->getWords();
+			const std::multimap<int, cv::KeyPoint> & words = s->getWords();
 			if(words.size())
 			{
 				UDEBUG("node=%d, word references=%d", s->id(), words.size());
-				for(std::multimap<int, int>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, cv::KeyPoint>::const_iterator iter = words.begin(); iter!=words.end(); ++iter)
 				{
 					if(iter->first > 0)
 					{
@@ -1131,7 +1131,7 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 					}
 				}
 
-				this->moveToTrash(s, false);
+				this->moveToTrash(s, _notLinkedNodesKeptInDb);
 				s = 0;
 			}
 		}
@@ -2368,7 +2368,7 @@ void Memory::moveToTrash(Signature * s, bool keepLinkedToGraph, std::list<int> *
 			}
 			s->removeLinks(true); // remove all links, but keep self referring link
 			s->removeLandmarks(); // remove all landmarks
-			s->setWeight(-9); // invalid
+			s->setWeight(0);
 			s->setLabel(""); // reset label
 		}
 		else
@@ -2753,16 +2753,20 @@ Transform Memory::computeTransform(
 		if(_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())
 		{
 			UDEBUG("");
-			tmpFrom.removeAllWords();
+			tmpFrom.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpFrom.setWords3(std::multimap<int, cv::Point3f>());
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			tmpTo.removeAllWords();
+			tmpTo.setWords(std::multimap<int, cv::KeyPoint>());
+			tmpTo.setWords3(std::multimap<int, cv::Point3f>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			tmpTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 		else if(useKnownCorrespondencesIfPossible)
 		{
 			// This will make RegistrationVis bypassing the correspondences computation
-			tmpFrom.setWordsDescriptors(cv::Mat());
-			tmpTo.setWordsDescriptors(cv::Mat());
+			tmpFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+			tmpTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 		}
 
 		bool isNeighborRefining = fromS.getLinks().find(toS.id()) != fromS.getLinks().end() && fromS.getLinks().find(toS.id())->second.type() == Link::kNeighbor;
@@ -2791,28 +2795,23 @@ Transform Memory::computeTransform(
 			   !tmpTo.getWords().empty() &&
 			   !tmpFrom.getWordsDescriptors().empty() &&
 			   !tmpFrom.getWords().empty() &&
-			   !tmpFrom.getWords3().empty() &&
-			   fromS.hasLink(0, Link::kNeighbor)) // If doesn't have neighbors, skip bundle
+			   !tmpFrom.getWords3().empty())
 		{
-			std::multimap<int, int> words;
-			std::vector<cv::Point3f> words3DMap;
-			std::vector<cv::KeyPoint> wordsMap;
-			cv::Mat wordsDescriptorsMap;
+			std::multimap<int, cv::Point3f> words3DMap;
+			std::multimap<int, cv::KeyPoint> wordsMap;
+			std::multimap<int, cv::Mat> wordsDescriptorsMap;
 
 			const std::multimap<int, Link> & links = fromS.getLinks();
-			if(!fromS.getWords3().empty())
 			{
-				const std::map<int, int> & wordsFrom = uMultimapToMapUnique(fromS.getWords());
-				UDEBUG("fromS.getWords()=%d  uniques=%d", (int)fromS.getWords().size(), (int)wordsFrom.size());
-				for(std::map<int, int>::const_iterator jter=wordsFrom.begin(); jter!=wordsFrom.end(); ++jter)
+				const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(fromS.getWords3());
+				UDEBUG("fromS.getWords3()=%d  uniques=%d", (int)fromS.getWords3().size(), (int)words3.size());
+				for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
 				{
-					const cv::Point3f & pt = fromS.getWords3()[jter->second];
-					if(util3d::isFinite(pt))
+					if(util3d::isFinite(jter->second))
 					{
-						words.insert(std::make_pair(jter->first, words.size()));
-						words3DMap.push_back(pt);
-						wordsMap.push_back(fromS.getWordsKpts()[jter->second]);
-						wordsDescriptorsMap.push_back(fromS.getWordsDescriptors().row(jter->second));
+						words3DMap.insert(*jter);
+						wordsMap.insert(*fromS.getWords().find(jter->first));
+						wordsDescriptorsMap.insert(*fromS.getWordsDescriptors().find(jter->first));
 					}
 				}
 			}
@@ -2821,23 +2820,21 @@ Transform Memory::computeTransform(
 			for(std::multimap<int, Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
 			{
 				int id = iter->first;
-				if(id != fromS.id() && iter->second.type() == Link::kNeighbor) // assemble only neighbors for the local feature map
+				if(id != fromS.id())
 				{
 					const Signature * s = this->getSignature(id);
-					if(s && !s->getWords3().empty())
+					if(s)
 					{
-						const std::map<int, int> & wordsTo = uMultimapToMapUnique(s->getWords());
-						for(std::map<int, int>::const_iterator jter=wordsTo.begin(); jter!=wordsTo.end(); ++jter)
+						const std::map<int, cv::Point3f> & words3 = uMultimapToMapUnique(s->getWords3());
+						for(std::map<int, cv::Point3f>::const_iterator jter=words3.begin(); jter!=words3.end(); ++jter)
 						{
-							const cv::Point3f & pt = s->getWords3()[jter->second];
 							if( jter->first > 0 &&
-								util3d::isFinite(pt) &&
-								words.find(jter->first) == words.end())
+								util3d::isFinite(jter->second) &&
+								words3DMap.find(jter->first) == words3DMap.end())
 							{
-								words.insert(words.end(), std::make_pair(jter->first, words.size()));
-								words3DMap.push_back(util3d::transformPoint(pt, iter->second.transform()));
-								wordsMap.push_back(s->getWordsKpts()[jter->second]);
-								wordsDescriptorsMap.push_back(s->getWordsDescriptors().row(jter->second));
+								words3DMap.insert(std::make_pair(jter->first, util3d::transformPoint(jter->second, iter->second.transform())));
+								wordsMap.insert(*s->getWords().find(jter->first));
+								wordsDescriptorsMap.insert(*s->getWordsDescriptors().find(jter->first));
 							}
 						}
 					}
@@ -2845,29 +2842,24 @@ Transform Memory::computeTransform(
 			}
 			UDEBUG("words3DMap=%d", (int)words3DMap.size());
 			Signature tmpFrom2(fromS.id());
-			tmpFrom2.setWords(words, wordsMap, words3DMap, wordsDescriptorsMap);
+			tmpFrom2.setWords3(words3DMap);
+			tmpFrom2.setWords(wordsMap);
+			tmpFrom2.setWordsDescriptors(wordsDescriptorsMap);
 
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom2, tmpTo, guess, info);
 
-			if(!transform.isNull() && info && !tmpFrom2.getWords3().empty())
+			if(!transform.isNull() && info)
 			{
-				std::map<int, cv::Point3f> points3DMap;
-				std::map<int, int> wordsMap = uMultimapToMapUnique(tmpFrom2.getWords());
-				for(std::map<int, int>::iterator iter=wordsMap.begin(); iter!=wordsMap.end(); ++iter)
-				{
-					points3DMap.insert(std::make_pair(iter->first, tmpFrom2.getWords3()[iter->second]));
-				}
+				std::map<int, cv::Point3f> points3DMap = uMultimapToMapUnique(tmpFrom2.getWords3());
 				std::map<int, Transform> bundlePoses;
 				std::multimap<int, Link> bundleLinks;
 				std::map<int, CameraModel> bundleModels;
 				std::map<int, std::map<int, FeatureBA> > wordReferences;
 
 				std::multimap<int, Link> links = fromS.getLinks();
-				links = graph::filterLinks(links, Link::kNeighbor, true); // assemble only neighbors for the local feature map
 				links.insert(std::make_pair(toS.id(), Link(fromS.id(), toS.id(), Link::kGlobalClosure, transform, info->covariance.inv())));
 				links.insert(std::make_pair(fromS.id(), Link()));
 
-				int totalWordReferences = 0;
 				for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
 				{
 					int id = iter->first;
@@ -2920,41 +2912,33 @@ Transform Memory::computeTransform(
 								bundlePoses.insert(std::make_pair(id, iter->second.transform()));
 							}
 
-							const std::map<int,int> & words = uMultimapToMapUnique(s->getWords());
-							for(std::map<int, int>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
+							const std::map<int,cv::KeyPoint> & words = uMultimapToMapUnique(s->getWords());
+							for(std::map<int, cv::KeyPoint>::const_iterator jter=words.begin(); jter!=words.end(); ++jter)
 							{
 								if(points3DMap.find(jter->first)!=points3DMap.end() &&
-										(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
+									(id == tmpTo.id() || jter->first > 0))
 								{
-									//get depth
-									float d = 0.0f;
-									if( !s->getWords3().empty() &&
-										util3d::isFinite(s->getWords3()[jter->second]))
-									{
-										//move back point in camera frame (to get depth along z)
-										d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
-									}
+									std::multimap<int, cv::Point3f>::const_iterator kter = s->getWords3().find(jter->first);
+									cv::Point3f pt3d = util3d::transformPoint(kter->second, invLocalTransform);
 									wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
-									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
-									++totalWordReferences;
+									wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(jter->second, pt3d.z)));
 								}
 							}
 						}
 					}
 				}
 
-
 				UDEBUG("sba...start");
 				// set root negative to fix all other poses
 				std::set<int> sbaOutliers;
 				UTimer bundleTimer;
-				OptimizerG2O sba(parameters_);
+				OptimizerG2O sba;
 				sba.setIterations(5);
 				UTimer bundleTime;
 				bundlePoses = sba.optimizeBA(-toS.id(), bundlePoses, bundleLinks, bundleModels, points3DMap, wordReferences, &sbaOutliers);
 				UDEBUG("sba...end");
 
-				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), totalWordReferences, (int)sbaOutliers.size());
+				UDEBUG("bundleTime=%fs (poses=%d wordRef=%d outliers=%d)", bundleTime.ticks(), (int)bundlePoses.size(), (int)wordReferences.size(), (int)sbaOutliers.size());
 
 				UDEBUG("Local Bundle Adjustment Before: %s", transform.prettyPrint().c_str());
 				if(!bundlePoses.rbegin()->second.isNull())
@@ -2996,6 +2980,36 @@ Transform Memory::computeTransform(
 		{
 			transform = _registrationPipeline->computeTransformationMod(tmpFrom, tmpTo, guess, info);
 		}
+
+		if(!transform.isNull() &&
+			fromS.sensorData().cameraModels().size()<=1 &&
+			toS.sensorData().cameraModels().size()<=1)
+		{
+			UDEBUG("");
+			// verify if it is a 180 degree transform, well verify > 90
+			float x,y,z, roll,pitch,yaw;
+			if(guess.isNull())
+			{
+				transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			}
+			else
+			{
+				Transform guessError = guess.inverse() * transform;
+				guessError.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw);
+			}
+			if(fabs(pitch) > CV_PI/2 ||
+			   fabs(yaw) > CV_PI/2)
+			{
+				transform.setNull();
+				std::string msg = uFormat("Too large rotation detected! (pitch=%f, yaw=%f) max is %f",
+						roll, pitch, yaw, CV_PI/2);
+				UINFO(msg.c_str());
+				if(info)
+				{
+					info->rejectedMsg = msg;
+				}
+			}
+		}
 	}
 	return transform;
 }
@@ -3422,25 +3436,21 @@ void Memory::dumpSignatures(const char * fileNameSign, bool words3D) const
 			{
 				if(words3D)
 				{
-					if(!ss->getWords3().empty())
+					const std::multimap<int, cv::Point3f> & ref = ss->getWords3();
+					for(std::multimap<int, cv::Point3f>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 					{
-						const std::multimap<int, int> & ref = ss->getWords();
-						for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+						//show only valid point according to current parameters
+						if(pcl::isFinite(jter->second) &&
+						   (jter->second.x != 0 || jter->second.y != 0 || jter->second.z != 0))
 						{
-							const cv::Point3f & pt = ss->getWords3()[jter->second];
-							//show only valid point according to current parameters
-							if(pcl::isFinite(pt) &&
-							   (pt.x != 0 || pt.y != 0 || pt.z != 0))
-							{
-								fprintf(foutSign, "%d ", (*jter).first);
-							}
+							fprintf(foutSign, "%d ", (*jter).first);
 						}
 					}
 				}
 				else
 				{
-					const std::multimap<int, int> & ref = ss->getWords();
-					for(std::multimap<int, int>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
+					const std::multimap<int, cv::KeyPoint> & ref = ss->getWords();
+					for(std::multimap<int, cv::KeyPoint>::const_iterator jter=ref.begin(); jter!=ref.end(); ++jter)
 					{
 						fprintf(foutSign, "%d ", (*jter).first);
 					}
@@ -3510,47 +3520,6 @@ void Memory::dumpMemoryTree(const char * fileNameTree) const
 
 }
 
-unsigned long Memory::getMemoryUsed() const
-{
-	unsigned long memoryUsage = sizeof(Memory);
-	memoryUsage += _signatures.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Signature *>);
-	for(std::map<int, Signature*>::const_iterator iter=_signatures.begin(); iter!=_signatures.end(); ++iter)
-	{
-		memoryUsage += iter->second->getMemoryUsed(true);
-	}
-	if(_vwd)
-	{
-		memoryUsage += _vwd->getMemoryUsed();
-	}
-	memoryUsage += _stMem.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	memoryUsage += _workingMem.size() * (sizeof(int)+sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, double>);
-	memoryUsage += _groundTruths.size() * (sizeof(int)+sizeof(Transform)+12*sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
-	memoryUsage += _labels.size() * (sizeof(int)+sizeof(std::string) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::string>);
-	for(std::map<int, std::string>::const_iterator iter=_labels.begin(); iter!=_labels.end(); ++iter)
-	{
-		memoryUsage+=iter->second.size();
-	}
-	memoryUsage += _landmarksIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
-	memoryUsage += _landmarksInvertedIndex.size() * (sizeof(int)+sizeof(std::set<int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::set<int> >);
-	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksIndex.begin(); iter!=_landmarksIndex.end(); ++iter)
-	{
-		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	}
-	for(std::map<int, std::set<int>>::const_iterator iter=_landmarksInvertedIndex.begin(); iter!=_landmarksInvertedIndex.end(); ++iter)
-	{
-		memoryUsage+=iter->second.size()*(sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	}
-	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
-	memoryUsage += sizeof(Feature2D) + _feature2D->getParameters().size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
-	memoryUsage += sizeof(Registration);
-	memoryUsage += sizeof(RegistrationIcp);
-	memoryUsage += _occupancy->getMemoryUsed();
-	memoryUsage += sizeof(MarkerDetector);
-	memoryUsage += sizeof(DBDriver);
-
-	return memoryUsage;
-}
-
 void Memory::rehearsal(Signature * signature, Statistics * stats)
 {
 	UTimer timer;
@@ -3710,7 +3679,6 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				{
 					_lastGlobalLoopClosureId = newS->id();
 				}
-				oldS->setWeight(-9);
 			}
 			else
 			{
@@ -3723,7 +3691,6 @@ bool Memory::rehearsalMerge(int oldId, int newId)
 				{
 					_lastSignature = oldS;
 				}
-				newS->setWeight(-9);
 			}
 
 			// remove location
@@ -3944,10 +3911,9 @@ SensorData Memory::getNodeData(int locationId, bool images, bool scan, bool user
 }
 
 void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
-		std::multimap<int, int> & words,
-		std::vector<cv::KeyPoint> & wordsKpts,
-		std::vector<cv::Point3f> & words3,
-		cv::Mat & wordsDescriptors,
+		std::multimap<int, cv::KeyPoint> & words,
+		std::multimap<int, cv::Point3f> & words3,
+		std::multimap<int, cv::Mat> & wordsDescriptors,
 		std::vector<GlobalDescriptor> & globalDescriptors) const
 {
 	//UDEBUG("nodeId=%d", nodeId);
@@ -3955,7 +3921,6 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 	if(s)
 	{
 		words = s->getWords();
-		wordsKpts = s->getWordsKpts();
 		words3 = s->getWords3();
 		wordsDescriptors = s->getWordsDescriptors();
 		globalDescriptors = s->sensorData().globalDescriptors();
@@ -3971,7 +3936,6 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
 		if(signatures.size())
 		{
 			words = signatures.front()->getWords();
-			wordsKpts = signatures.front()->getWordsKpts();
 			words3 = signatures.front()->getWords3();
 			wordsDescriptors = signatures.front()->getWordsDescriptors();
 			globalDescriptors = signatures.front()->sensorData().globalDescriptors();
@@ -4041,7 +4005,7 @@ void Memory::copyData(const Signature * from, Signature * to)
 	{
 		// words 2d
 		this->disableWordsRef(to->id());
-		to->setWords(from->getWords(), from->getWordsKpts(), from->getWords3(), from->getWordsDescriptors());
+		to->setWords(from->getWords());
 		std::list<int> id;
 		id.push_back(to->id());
 		this->enableWordsRef(id);
@@ -4058,6 +4022,8 @@ void Memory::copyData(const Signature * from, Signature * to)
 		to->sensorData().setId(to->id());
 
 		to->setPose(from->getPose());
+		to->setWords3(from->getWords3());
+		to->setWordsDescriptors(from->getWordsDescriptors());
 	}
 	else
 	{
@@ -4514,7 +4480,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
 					}
 				}
-				else if(useProvided3dPoints && keypoints.size() == data.keypoints3D().size())
+				else if(keypoints.size() == data.keypoints3D().size())
 				{
 					UDEBUG("Using provided 3d points (%d)", (int)data.keypoints3D().size());
 					keypoints3D = data.keypoints3D();
@@ -4713,10 +4679,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UDEBUG("id %d is a bad signature", id);
 	}
 
-	std::multimap<int, int> words;
-	std::vector<cv::KeyPoint> wordsKpts;
-	std::vector<cv::Point3f> words3D;
-	cv::Mat wordsDescriptors;
+	std::multimap<int, cv::KeyPoint> words;
+	std::multimap<int, cv::Point3f> words3D;
+	std::multimap<int, cv::Mat> wordsDescriptors;
 	int words3DValid = 0;
 	if(wordIds.size() > 0)
 	{
@@ -4736,12 +4701,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 				kpt.size *= decimationRatio;
 				kpt.octave += log2value;
 			}
-			words.insert(std::make_pair(*iter, words.size()));
-			wordsKpts.push_back(kpt);
+			words.insert(std::pair<int, cv::KeyPoint>(*iter, kpt));
 
 			if(keypoints3D.size())
 			{
-				words3D.push_back(keypoints3D.at(i));
+				words3D.insert(std::pair<int, cv::Point3f>(*iter, keypoints3D.at(i)));
 				if(util3d::isFinite(keypoints3D.at(i)))
 				{
 					++words3DValid;
@@ -4749,7 +4713,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			}
 			if(_rawDescriptorsKept)
 			{
-				wordsDescriptors.push_back(descriptors.row(i));
+				wordsDescriptors.insert(std::pair<int, cv::Mat>(*iter, descriptors.row(i).clone()));
 			}
 		}
 	}
@@ -4869,32 +4833,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 
 			Signature cpPrevious(2);
 			// IDs should be unique so that registration doesn't override them
-			std::map<int, int> uniqueWordsOld = uMultimapToMapUnique(previousS->getWords());
-			std::vector<cv::KeyPoint> uniqueWordsKpts;
-			cv::Mat uniqueWordsDescriptors;
-			std::multimap<int, int> uniqueWords;
-			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
-			{
-				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
-				uniqueWordsKpts.push_back(previousS->getWordsKpts()[iter->second]);
-				uniqueWordsDescriptors.push_back(previousS->getWordsDescriptors().row(iter->second));
-			}
+			std::map<int, cv::KeyPoint> uniqueWords = uMultimapToMapUnique(previousS->getWords());
+			std::map<int, cv::Mat> uniqueWordsDescriptors = uMultimapToMapUnique(previousS->getWordsDescriptors());
 			cpPrevious.sensorData().setCameraModels(previousS->sensorData().cameraModels());
-			cpPrevious.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
+			cpPrevious.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpPrevious.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 			Signature cpCurrent(1);
-			uniqueWordsOld = uMultimapToMapUnique(words);
-			uniqueWordsKpts.clear();
-			uniqueWordsDescriptors = cv::Mat();
-			uniqueWords.clear();
-			for(std::map<int, int>::iterator iter=uniqueWordsOld.begin(); iter!=uniqueWordsOld.end(); ++iter)
-			{
-				uniqueWords.insert(std::make_pair(iter->first, uniqueWords.size()));
-				uniqueWordsKpts.push_back(wordsKpts[iter->second]);
-				uniqueWordsDescriptors.push_back(wordsDescriptors.row(iter->second));
-			}
+			uniqueWords = uMultimapToMapUnique(words);
+			uniqueWordsDescriptors = uMultimapToMapUnique(wordsDescriptors);
 			cpCurrent.sensorData().setCameraModels(cameraModels);
 			// This will force comparing descriptors between both images directly
-			cpCurrent.setWords(uniqueWords, uniqueWordsKpts, std::vector<cv::Point3f>(), uniqueWordsDescriptors);
+			cpCurrent.setWords(std::multimap<int, cv::KeyPoint>(uniqueWords.begin(), uniqueWords.end()));
+			cpCurrent.setWordsDescriptors(std::multimap<int, cv::Mat>(uniqueWordsDescriptors.begin(), uniqueWordsDescriptors.end()));
 
 			// The following is used only to re-estimate the correspondences, the returned transform is ignored
 			Transform tmpt;
@@ -4912,21 +4862,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UDEBUG("t=%s", tmpt.prettyPrint().c_str());
 
 			// compute 3D words by epipolar geometry with the previous signature using odometry motion
-			std::map<int, int> currentUniqueWords = uMultimapToMapUnique(cpCurrent.getWords());
-			std::map<int, int> previousUniqueWords = uMultimapToMapUnique(cpPrevious.getWords());
-			std::map<int, cv::KeyPoint> currentWords;
-			std::map<int, cv::KeyPoint> previousWords;
-			for(std::map<int, int>::iterator iter=currentUniqueWords.begin(); iter!=currentUniqueWords.end(); ++iter)
-			{
-				currentWords.insert(std::make_pair(iter->first, cpCurrent.getWordsKpts()[iter->second]));
-			}
-			for(std::map<int, int>::iterator iter=previousUniqueWords.begin(); iter!=previousUniqueWords.end(); ++iter)
-			{
-				previousWords.insert(std::make_pair(iter->first, cpPrevious.getWordsKpts()[iter->second]));
-			}
 			std::map<int, cv::Point3f> inliers = util3d::generateWords3DMono(
-					currentWords,
-					previousWords,
+					uMultimapToMapUnique(cpCurrent.getWords()),
+					uMultimapToMapUnique(cpPrevious.getWords()),
 					cameraModels[0],
 					cameraTransform);
 
@@ -4937,26 +4875,32 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 			UASSERT(words3D.size() == 0 || words.size() == words3D.size());
 			bool words3DWasEmpty = words3D.empty();
 			int added3DPointsWithoutDepth = 0;
-			for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 			{
 				std::map<int, cv::Point3f>::iterator jter=inliers.find(iter->first);
-				if(words3DWasEmpty)
+				std::multimap<int, cv::Point3f>::iterator iter3D = words3D.find(iter->first);
+				if(iter3D == words3D.end())
 				{
 					if(jter != inliers.end())
 					{
-						words3D.push_back(jter->second);
+						words3D.insert(std::make_pair(iter->first, jter->second));
 						++added3DPointsWithoutDepth;
 					}
 					else
 					{
-						words3D.push_back(cv::Point3f(bad_point,bad_point,bad_point));
+						words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
 					}
 				}
-				else if(!util3d::isFinite(words3D[iter->second]) && jter != inliers.end())
+				else if(!util3d::isFinite(iter3D->second) && jter != inliers.end())
 				{
-					words3D[iter->second] = jter->second;
+					iter3D->second = jter->second;
 					++added3DPointsWithoutDepth;
 				}
+				else if(words3DWasEmpty && jter == inliers.end())
+				{
+					// duplicate
+					words3D.insert(std::make_pair(iter->first, cv::Point3f(bad_point,bad_point,bad_point)));
+				}
 			}
 			UDEBUG("added3DPointsWithoutDepth=%d", added3DPointsWithoutDepth);
 			if(stats) stats->addStatistic(Statistics::kMemoryTriangulated_points(), (float)added3DPointsWithoutDepth);
@@ -5209,7 +5153,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 						compressedUserData));
 	}
 
-	s->setWords(words, wordsKpts, words3D, wordsDescriptors);
+	s->setWords(words);
+	s->setWords3(words3D);
+	s->setWordsDescriptors(wordsDescriptors);
 
 	// set raw data
 	if(!cameraModels.empty())
@@ -5276,22 +5222,16 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	}
 	else if(data.gps().stamp() > 0.0)
 	{
-		if(uIsFinite(data.gps().altitude()) &&
-		   uIsFinite(data.gps().latitude()) &&
-		   uIsFinite(data.gps().longitude()) &&
-		   uIsFinite(data.gps().bearing()) &&
-		   uIsFinite(data.gps().error()) &&
-		   data.gps().error() > 0.0)
+		if(_gpsOrigin.stamp() <= 0.0)
+		{
+			_gpsOrigin =  data.gps();
+			UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
+		}
+		cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
+		Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
+		cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
+		if(data.gps().error() > 0.0)
 		{
-			if(_gpsOrigin.stamp() <= 0.0)
-			{
-				_gpsOrigin =  data.gps();
-				UINFO("Added GPS origin: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
-			}
-			cv::Point3f pt = data.gps().toGeodeticCoords().toENU_WGS84(_gpsOrigin.toGeodeticCoords());
-			Transform gpsPose(pt.x, pt.y, pose.z(), 0, 0, -(data.gps().bearing()-90.0)*180.0/M_PI);
-			cv::Mat gpsInfMatrix = cv::Mat::eye(6,6,CV_64FC1)/9999.0; // variance not used >= 9999
-
 			UDEBUG("Added GPS prior: x=%f y=%f z=%f yaw=%f", gpsPose.x(), gpsPose.y(), gpsPose.z(), gpsPose.theta());
 			// only set x, y as we don't know variance for other degrees of freedom.
 			gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 1.0/data.gps().error();
@@ -5300,7 +5240,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		}
 		else
 		{
-			UERROR("Invalid GPS value: long=%f lat=%f alt=%f bearing=%f error=%f", data.gps().longitude(), data.gps().latitude(), data.gps().altitude(), data.gps().bearing(), data.gps().error());
+			UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
 		}
 	}
 
@@ -5374,7 +5314,7 @@ void Memory::disableWordsRef(int signatureId)
 	Signature * ss = this->_getSignature(signatureId);
 	if(ss && ss->isEnabled())
 	{
-		const std::multimap<int, int> & words = ss->getWords();
+		const std::multimap<int, cv::KeyPoint> & words = ss->getWords();
 		const std::list<int> & keys = uUniqueKeys(words);
 		int count = _vwd->getTotalActiveReferences();
 		// First remove all references

corelib/src/OccupancyGrid.cpp

@@ -1532,36 +1532,4 @@ bool OccupancyGrid::update(const std::map<int, Transform> & posesIn)
 	return updated;
 }
 
-unsigned long OccupancyGrid::getMemoryUsed() const
-{
-	unsigned long memoryUsage = sizeof(OccupancyGrid);
-	memoryUsage += parameters_.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
-
-	memoryUsage += cache_.size()*(sizeof(int) + sizeof(std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >);
-	for(std::map<int, std::pair<std::pair<cv::Mat, cv::Mat>, cv::Mat> >::const_iterator iter=cache_.begin(); iter!=cache_.end(); ++iter)
-	{
-		memoryUsage += iter->second.first.first.total() * iter->second.first.first.elemSize();
-		memoryUsage += iter->second.first.second.total() * iter->second.first.second.elemSize();
-		memoryUsage += iter->second.second.total() * iter->second.second.elemSize();
-	}
-	memoryUsage += map_.total() * map_.elemSize();
-	memoryUsage += mapInfo_.total() * mapInfo_.elemSize();
-	memoryUsage += cellCount_.size()*(sizeof(int)*3 + sizeof(std::pair<int, int>) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, std::pair<int, int> >);
-	memoryUsage += addedNodes_.size()*(sizeof(int) + sizeof(Transform)+ sizeof(float)*12 + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
-
-	if(assembledGround_.get())
-	{
-		memoryUsage += assembledGround_->points.size() * sizeof(pcl::PointXYZRGB);
-	}
-	if(assembledObstacles_.get())
-	{
-		memoryUsage += assembledObstacles_->points.size() * sizeof(pcl::PointXYZRGB);
-	}
-	if(assembledEmptyCells_.get())
-	{
-		memoryUsage += assembledEmptyCells_->points.size() * sizeof(pcl::PointXYZRGB);
-	}
-	return memoryUsage;
-}
-
 }

corelib/src/Optimizer.cpp

@@ -617,8 +617,8 @@ void Optimizer::computeBACorrespondences(
 
 					if(!rematchFeatures)
 					{
-						sFrom.setWordsDescriptors(cv::Mat());
-						sTo.setWordsDescriptors(cv::Mat());
+						sFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
+						sTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 					}
 
 					RegistrationInfo info;
@@ -633,13 +633,13 @@ void Optimizer::computeBACorrespondences(
 							// set descriptors for the output
 							if(sFrom.getWords().size() &&
 							   sFrom.getWordsDescriptors().empty() &&
-							   (int)sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().rows)
+							   sFrom.getWords().size() == signatures.at(link.from()).getWordsDescriptors().size())
 							{
 								sFrom.setWordsDescriptors(signatures.at(link.from()).getWordsDescriptors());
 							}
 							if(sTo.getWords().size() &&
 							   sTo.getWordsDescriptors().empty() &&
-							   (int)sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().rows)
+							   sTo.getWords().size() == signatures.at(link.to()).getWordsDescriptors().size())
 							{
 								sTo.setWordsDescriptors(signatures.at(link.to()).getWordsDescriptors());
 							}
@@ -649,13 +649,11 @@ void Optimizer::computeBACorrespondences(
 						UASSERT(!pose.isNull());
 						for(unsigned int i=0; i<info.inliersIDs.size(); ++i)
 						{
-							int indexFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
-							cv::Point3f p = sFrom.getWords3()[indexFrom];
+							cv::Point3f p = sFrom.getWords3().lower_bound(info.inliersIDs[i])->second;
 							if(p.x > 0.0f) // make sure the point is valid
 							{
-								cv::KeyPoint ptFrom = sFrom.getWordsKpts()[indexFrom];
-								int indexTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
-								cv::KeyPoint ptTo = sTo.getWordsKpts()[indexTo];
+								cv::KeyPoint ptFrom = sFrom.getWords().lower_bound(info.inliersIDs[i])->second;
+								cv::KeyPoint ptTo = sTo.getWords().lower_bound(info.inliersIDs[i])->second;
 
 								int wordId = -1;
 
@@ -694,10 +692,10 @@ void Optimizer::computeBACorrespondences(
 								if(!fromAlreadyAdded)
 								{
 									cv::Mat descriptorFrom;
-									if(!sFrom.getWordsDescriptors().empty())
+									if(sFrom.getWordsDescriptors().size())
 									{
-										UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
-										descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
+										UASSERT(sFrom.getWordsDescriptors().find(info.inliersIDs[i]) != sFrom.getWordsDescriptors().end());
+										descriptorFrom = sFrom.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
 									frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
@@ -707,20 +705,17 @@ void Optimizer::computeBACorrespondences(
 								if(!toAlreadyAdded)
 								{
 									cv::Mat descriptorTo;
-									if(!sTo.getWordsDescriptors().empty())
+									if(sTo.getWordsDescriptors().size())
 									{
-										UASSERT(indexTo < sTo.getWordsDescriptors().rows);
-										descriptorTo = sTo.getWordsDescriptors().row(indexTo);
+										UASSERT(sTo.getWordsDescriptors().find(info.inliersIDs[i]) != sTo.getWordsDescriptors().end());
+										descriptorTo = sTo.getWordsDescriptors().lower_bound(info.inliersIDs[i])->second;
 									}
 									float depth = 0.0f;
-									if(!sTo.getWords3().empty())
+									std::multimap<int, cv::Point3f>::const_iterator iterTo = sTo.getWords3().lower_bound(info.inliersIDs[i]);
+									if( iterTo!=sTo.getWords3().end() &&
+										iterTo->second.x > 0)
 									{
-										UASSERT(indexTo < (int)sTo.getWords3().size());
-										const cv::Point3f & pt = sTo.getWords3()[indexTo];
-										if( pt.x > 0)
-										{
-											depth = pt.x;
-										}
+										depth = iterTo->second.x;
 									}
 									wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
 									frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));

corelib/src/Parameters.cpp

@@ -706,12 +706,6 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
 #else
 				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
-#endif
-				str = "With K4A:";
-#ifdef RTABMAP_K4A
-				std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
-#else
-				std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
 #endif
 				str = "With DC1394:";
 #ifdef RTABMAP_DC1394

corelib/src/Recovery.cpp

@@ -133,18 +133,6 @@ bool databaseRecovery(
 			*errorMsg = uFormat("Failed renaming database file from \"%s\" to \"%s\". Is it opened by another app?", UFile::getName(databasePath).c_str(), UFile::getName(backupPath).c_str());
 		return false;
 	}
-
-	bool incrementalMemory = true;
-	Parameters::parse(parameters, Parameters::kMemIncrementalMemory(), incrementalMemory);
-	if(!incrementalMemory)
-	{
-		if(progressState)
-		{
-			progressState->callback("Database is in localization mode, setting it to mapping mode to recover...");
-		}
-		uInsert(parameters, ParametersPair(Parameters::kMemIncrementalMemory(), "true"));
-	}
-
 	Rtabmap rtabmap;
 	rtabmap.init(parameters, databasePath);
 

corelib/src/RegistrationVis.cpp

@@ -213,10 +213,6 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), parameters.at(Parameters::kVisCorNNDR())));
 	}
-	if(uContains(parameters, Parameters::kKpByteToFloat()))
-	{
-		uInsert(_featureParameters, ParametersPair(Parameters::kKpByteToFloat(), parameters.at(Parameters::kKpByteToFloat())));
-	}
 	if(uContains(parameters, Parameters::kVisFeatureType()))
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), parameters.at(Parameters::kVisFeatureType())));
@@ -303,7 +299,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			fromSignature.id(),
 			(int)fromSignature.getWords().size(),
 			(int)fromSignature.getWords3().size(),
-			(int)fromSignature.getWordsDescriptors().rows,
+			(int)fromSignature.getWordsDescriptors().size(),
 			(int)fromSignature.sensorData().keypoints().size(),
 			(int)fromSignature.sensorData().keypoints3D().size(),
 			fromSignature.sensorData().descriptors().rows,
@@ -316,7 +312,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			toSignature.id(),
 			(int)toSignature.getWords().size(),
 			(int)toSignature.getWords3().size(),
-			(int)toSignature.getWordsDescriptors().rows,
+			(int)toSignature.getWordsDescriptors().size(),
 			(int)toSignature.sensorData().keypoints().size(),
 			(int)toSignature.sensorData().keypoints3D().size(),
 			toSignature.sensorData().descriptors().rows,
@@ -349,16 +345,16 @@ Transform RegistrationVis::computeTransformationImpl(
 				fromSignature.getWords3().empty() ||
 				(fromSignature.getWords().size() == fromSignature.getWords3().size()));
 		UASSERT((int)fromSignature.sensorData().keypoints().size() == fromSignature.sensorData().descriptors().rows ||
-				(int)fromSignature.getWords().size() == fromSignature.getWordsDescriptors().rows ||
-				fromSignature.sensorData().descriptors().empty() ||
-				fromSignature.getWordsDescriptors().empty() == 0);
+				fromSignature.getWords().size() == fromSignature.getWordsDescriptors().size() ||
+				fromSignature.sensorData().descriptors().rows == 0 ||
+				fromSignature.getWordsDescriptors().size() == 0);
 		UASSERT((toSignature.getWords().empty() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() && toSignature.getWords3().empty())||
 				(toSignature.getWords().size() == toSignature.getWords3().size()));
 		UASSERT((int)toSignature.sensorData().keypoints().size() == toSignature.sensorData().descriptors().rows ||
-				(int)toSignature.getWords().size() == toSignature.getWordsDescriptors().rows ||
-				toSignature.sensorData().descriptors().empty() ||
-				toSignature.getWordsDescriptors().empty());
+				toSignature.getWords().size() == toSignature.getWordsDescriptors().size() ||
+				toSignature.sensorData().descriptors().rows == 0 ||
+				toSignature.getWordsDescriptors().size() == 0);
 		UASSERT(fromSignature.sensorData().imageRaw().empty() ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC1 ||
 				fromSignature.sensorData().imageRaw().type() == CV_8UC3);
@@ -371,7 +367,6 @@ Transform RegistrationVis::computeTransformationImpl(
 		cv::Mat imageTo = toSignature.sensorData().imageRaw();
 
 		std::vector<int> orignalWordsFromIds;
-		int kptsFromSource = 0;
 		if(fromSignature.getWords().empty())
 		{
 			if(fromSignature.sensorData().keypoints().empty())
@@ -404,26 +399,22 @@ Transform RegistrationVis::computeTransformationImpl(
 			else
 			{
 				kptsFrom = fromSignature.sensorData().keypoints();
-				kptsFromSource = 1;
 			}
 		}
 		else
 		{
-			kptsFromSource = 2;
+			kptsFrom.resize(fromSignature.getWords().size());
 			orignalWordsFromIds.resize(fromSignature.getWords().size());
 			int i=0;
 			bool allUniques = true;
-			int previousIdAdded = 0;
-			kptsFrom = fromSignature.getWordsKpts();
-			for(std::multimap<int, int>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
+			for(std::multimap<int, cv::KeyPoint>::const_iterator iter=fromSignature.getWords().begin(); iter!=fromSignature.getWords().end(); ++iter)
 			{
-				UASSERT(iter->second>=0 && iter->second<(int)orignalWordsFromIds.size());
-				orignalWordsFromIds[iter->second] = iter->first;
-				if(i>0 && iter->first==previousIdAdded)
+				kptsFrom[i] = iter->second;
+				orignalWordsFromIds[i] = iter->first;
+				if(i>0 && iter->first==orignalWordsFromIds[i-1])
 				{
 					allUniques = false;
 				}
-				previousIdAdded = iter->first;
 				++i;
 			}
 			if(!allUniques)
@@ -433,14 +424,12 @@ Transform RegistrationVis::computeTransformationImpl(
 			}
 		}
 
-		std::multimap<int, int> wordsFrom;
-		std::multimap<int, int> wordsTo;
-		std::vector<cv::KeyPoint> wordsKptsFrom;
-		std::vector<cv::KeyPoint> wordsKptsTo;
-		std::vector<cv::Point3f> words3From;
-		std::vector<cv::Point3f> words3To;
-		cv::Mat wordsDescFrom;
-		cv::Mat wordsDescTo;
+		std::multimap<int, cv::KeyPoint> wordsFrom;
+		std::multimap<int, cv::KeyPoint> wordsTo;
+		std::multimap<int, cv::Point3f> words3From;
+		std::multimap<int, cv::Point3f> words3To;
+		std::multimap<int, cv::Mat> wordsDescFrom;
+		std::multimap<int, cv::Mat> wordsDescTo;
 		if(_correspondencesApproach == 1) //Optical Flow
 		{
 			UDEBUG("");
@@ -461,7 +450,7 @@ Transform RegistrationVis::computeTransformationImpl(
 			std::vector<cv::Point3f> kptsFrom3D;
 			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = fromSignature.getWords3();
+				kptsFrom3D = uValues(fromSignature.getWords3());
 			}
 			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
@@ -551,16 +540,13 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsTo3D.size() == 0 || kptsTo.size() == kptsTo3D.size());
 				for(unsigned int i=0; i< kptsFrom3DKept.size(); ++i)
 				{
-					int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-					wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-					wordsKptsFrom.push_back(kptsFrom[i]);
-					words3From.push_back(kptsFrom3DKept[i]);
-
-					wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
-					wordsKptsTo.push_back(kptsTo[i]);
-					if(!kptsTo3D.empty())
+					int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+					wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+					words3From.insert(std::make_pair(id, kptsFrom3DKept[i]));
+					wordsTo.insert(std::make_pair(id, kptsTo[i]));
+					if(kptsTo3D.size())
 					{
-						words3To.push_back(kptsTo3D[i]);
+						words3To.insert(std::make_pair(id, kptsTo3D[i]));
 					}
 				}
 				toSignature.sensorData().setFeatures(kptsTo, kptsTo3D, cv::Mat());
@@ -576,10 +562,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					if(util3d::isFinite(kptsFrom3D[i]))
 					{
-						int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-						wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-						wordsKptsFrom.push_back(kptsFrom[i]);
-						words3From.push_back(kptsFrom3D[i]);
+						int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+						wordsFrom.insert(std::make_pair(id, kptsFrom[i]));
+						words3From.insert(std::make_pair(id, kptsFrom3D[i]));
 					}
 				}
 				toSignature.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
@@ -591,7 +576,6 @@ Transform RegistrationVis::computeTransformationImpl(
 		{
 			UDEBUG("");
 			std::vector<cv::KeyPoint> kptsTo;
-			int kptsToSource = 0;
 			if(toSignature.getWords().empty())
 			{
 				if(toSignature.sensorData().keypoints().empty() &&
@@ -622,28 +606,33 @@ Transform RegistrationVis::computeTransformationImpl(
 				else
 				{
 					kptsTo = toSignature.sensorData().keypoints();
-					kptsToSource = 1;
 				}
 			}
 			else
 			{
-				kptsTo = toSignature.getWordsKpts();
-				kptsToSource = 2;
+				kptsTo = uValues(toSignature.getWords());
 			}
 
 			// extract descriptors
-			UDEBUG("kptsFrom=%d kptsFromSource=%d", (int)kptsFrom.size(), kptsFromSource);
-			UDEBUG("kptsTo=%d kptsToSource=%d", (int)kptsTo.size(), kptsToSource);
+			UDEBUG("kptsFrom=%d", (int)kptsFrom.size());
+			UDEBUG("kptsTo=%d", (int)kptsTo.size());
 			cv::Mat descriptorsFrom;
-			if(kptsFromSource == 2 &&
-				fromSignature.getWordsDescriptors().rows &&
-				((kptsFrom.empty() && fromSignature.getWordsDescriptors().rows) ||
-				 fromSignature.getWordsDescriptors().rows == (int)kptsFrom.size()))
+			if(fromSignature.getWordsDescriptors().size() &&
+					((kptsFrom.empty() && fromSignature.getWordsDescriptors().size()) ||
+					 fromSignature.getWordsDescriptors().size() == kptsFrom.size()))
 			{
-				descriptorsFrom = fromSignature.getWordsDescriptors();
+				descriptorsFrom = cv::Mat(fromSignature.getWordsDescriptors().size(),
+						fromSignature.getWordsDescriptors().begin()->second.cols,
+						fromSignature.getWordsDescriptors().begin()->second.type());
+				int i=0;
+				for(std::multimap<int, cv::Mat>::const_iterator iter=fromSignature.getWordsDescriptors().begin();
+					iter!=fromSignature.getWordsDescriptors().end();
+					++iter, ++i)
+				{
+					iter->second.copyTo(descriptorsFrom.row(i));
+				}
 			}
-			else if(kptsFromSource == 1 &&
-					fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
+			else if(fromSignature.sensorData().descriptors().rows == (int)kptsFrom.size())
 			{
 				descriptorsFrom = fromSignature.sensorData().descriptors();
 			}
@@ -663,13 +652,20 @@ Transform RegistrationVis::computeTransformationImpl(
 			cv::Mat descriptorsTo;
 			if(kptsTo.size())
 			{
-				if(kptsToSource == 2 &&
-					toSignature.getWordsDescriptors().rows == (int)kptsTo.size())
+				if(toSignature.getWordsDescriptors().size() == kptsTo.size())
 				{
-					descriptorsTo = toSignature.getWordsDescriptors();
+					descriptorsTo = cv::Mat(toSignature.getWordsDescriptors().size(),
+							toSignature.getWordsDescriptors().begin()->second.cols,
+							toSignature.getWordsDescriptors().begin()->second.type());
+					int i=0;
+					for(std::multimap<int, cv::Mat>::const_iterator iter=toSignature.getWordsDescriptors().begin();
+						iter!=toSignature.getWordsDescriptors().end();
+						++iter, ++i)
+					{
+						iter->second.copyTo(descriptorsTo.row(i));
+					}
 				}
-				else if(kptsToSource == 1 &&
-						toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
+				else if(toSignature.sensorData().descriptors().rows == (int)kptsTo.size())
 				{
 					descriptorsTo = toSignature.sensorData().descriptors();
 				}
@@ -689,13 +685,11 @@ Transform RegistrationVis::computeTransformationImpl(
 			// create 3D keypoints
 			std::vector<cv::Point3f> kptsFrom3D;
 			std::vector<cv::Point3f> kptsTo3D;
-			if(kptsFromSource == 2 &&
-				kptsFrom.size() == fromSignature.getWords3().size())
+			if(kptsFrom.size() == fromSignature.getWords3().size())
 			{
-				kptsFrom3D = fromSignature.getWords3();
+				kptsFrom3D = uValues(fromSignature.getWords3());
 			}
-			else if(kptsFromSource == 1 &&
-					kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
+			else if(kptsFrom.size() == fromSignature.sensorData().keypoints3D().size())
 			{
 				kptsFrom3D = fromSignature.sensorData().keypoints3D();
 			}
@@ -726,12 +720,11 @@ Transform RegistrationVis::computeTransformationImpl(
 				_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
 			}
 
-			if(kptsToSource == 2 && kptsTo.size() == toSignature.getWords3().size())
+			if(kptsTo.size() == toSignature.getWords3().size())
 			{
-				kptsTo3D = toSignature.getWords3();
+				kptsTo3D = uValues(toSignature.getWords3());
 			}
-			else if(kptsToSource == 1 &&
-					kptsTo.size() == toSignature.sensorData().keypoints3D().size())
+			else if(kptsTo.size() == toSignature.sensorData().keypoints3D().size())
 			{
 				kptsTo3D = toSignature.sensorData().keypoints3D();
 			}
@@ -861,7 +854,7 @@ Transform RegistrationVis::computeTransformationImpl(
 							UDEBUG("radius search done for guess");
 
 							// Process results (Nearest Neighbor Distance Ratio)
-							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
+							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
 							std::map<int,int> addedWordsFrom; //<id, index>
 							std::map<int, int> duplicates; //<fromId, toId>
 							int newWords = 0;
@@ -915,7 +908,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								if(matchedIndex >= 0)
 								{
 									matchedIndex = projectedIndexToDescIndex[matchedIndex];
-									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndex]:matchedIndex;
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndex]:matchedIndex;
 
 									if(addedWordsFrom.find(matchedIndex) != addedWordsFrom.end())
 									{
@@ -926,32 +919,29 @@ Transform RegistrationVis::computeTransformationImpl(
 									{
 										addedWordsFrom.insert(std::make_pair(matchedIndex, id));
 
-										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-										if(!kptsFrom.empty())
+										if(kptsFrom.size())
 										{
-											wordsKptsFrom.push_back(kptsFrom[matchedIndex]);
+											wordsFrom.insert(std::make_pair(id, kptsFrom[matchedIndex]));
 										}
-										words3From.push_back(kptsFrom3D[matchedIndex]);
-										wordsDescFrom.push_back(descriptorsFrom.row(matchedIndex));
+										words3From.insert(std::make_pair(id, kptsFrom3D[matchedIndex]));
+										wordsDescFrom.insert(std::make_pair(id, descriptorsFrom.row(matchedIndex)));
 									}
 
-									wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
-									wordsKptsTo.push_back(kptsTo[i]);
-									wordsDescTo.push_back(descriptorsTo.row(i));
-									if(!kptsTo3D.empty())
+									wordsTo.insert(std::make_pair(id, kptsTo[i]));
+									wordsDescTo.insert(std::make_pair(id, descriptorsTo.row(i)));
+									if(kptsTo3D.size())
 									{
-										words3To.push_back(kptsTo3D[i]);
+										words3To.insert(std::make_pair(id, kptsTo3D[i]));
 									}
 								}
 								else
 								{
 									// gen fake ids
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
-									wordsKptsTo.push_back(kptsTo[i]);
-									wordsDescTo.push_back(descriptorsTo.row(i));
-									if(!kptsTo3D.empty())
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
+									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
+									if(kptsTo3D.size())
 									{
-										words3To.push_back(kptsTo3D[i]);
+										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
 									}
 
 									++newToId;
@@ -968,11 +958,10 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-									wordsKptsFrom.push_back(kptsFrom[i]);
-									wordsDescFrom.push_back(descriptorsFrom.row(i));
-									words3From.push_back(kptsFrom3D[i]);
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
+									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
+									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
 
 									++addWordsFromNotMatched;
 								}
@@ -1014,7 +1003,7 @@ Transform RegistrationVis::computeTransformationImpl(
 
 								if(indices[i].size())
 								{
-									info.projectedIDs.push_back(!orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
+									info.projectedIDs.push_back(orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom);
 								}
 
 								if(util3d::isFinite(kptsFrom3D[matchedIndexFrom]))
@@ -1065,28 +1054,26 @@ Transform RegistrationVis::computeTransformationImpl(
 										matchedIndexTo = indices[i].at(0);
 									}
 
-									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[matchedIndexFrom]:matchedIndexFrom;
 									addedWordsFrom.insert(addedWordsFrom.end(), matchedIndexFrom);
 
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-									if(!kptsFrom.empty())
+									if(kptsFrom.size())
 									{
-										wordsKptsFrom.push_back(kptsFrom[matchedIndexFrom]);
+										wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[matchedIndexFrom]));
 									}
-									words3From.push_back(kptsFrom3D[matchedIndexFrom]);
-									wordsDescFrom.push_back(descriptorsFrom.row(matchedIndexFrom));
+									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[matchedIndexFrom]));
+									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(matchedIndexFrom)));
 
 									if(	matchedIndexTo >= 0 &&
 										addedWordsTo.find(matchedIndexTo) == addedWordsTo.end())
 									{
 										addedWordsTo.insert(matchedIndexTo);
 
-										wordsTo.insert(wordsTo.end(), std::make_pair(id, wordsTo.size()));
-										wordsKptsTo.push_back(kptsTo[matchedIndexTo]);
-										wordsDescTo.push_back(descriptorsTo.row(matchedIndexTo));
-										if(!kptsTo3D.empty())
+										wordsTo.insert(wordsTo.end(), std::make_pair(id, kptsTo[matchedIndexTo]));
+										wordsDescTo.insert(wordsDescTo.end(), std::make_pair(id, descriptorsTo.row(matchedIndexTo)));
+										if(kptsTo3D.size())
 										{
-											words3To.push_back(kptsTo3D[matchedIndexTo]);
+											words3To.insert(words3To.end(), std::make_pair(id, kptsTo3D[matchedIndexTo]));
 										}
 									}
 								}
@@ -1098,25 +1085,23 @@ Transform RegistrationVis::computeTransformationImpl(
 							{
 								if(util3d::isFinite(kptsFrom3D[i]) && addedWordsFrom.find(i) == addedWordsFrom.end())
 								{
-									int id = !orignalWordsFromIds.empty()?orignalWordsFromIds[i]:i;
-									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, wordsFrom.size()));
-									wordsKptsFrom.push_back(kptsFrom[i]);
-									wordsDescFrom.push_back(descriptorsFrom.row(i));
-									words3From.push_back(kptsFrom3D[i]);
+									int id = orignalWordsFromIds.size()?orignalWordsFromIds[i]:i;
+									wordsFrom.insert(wordsFrom.end(), std::make_pair(id, kptsFrom[i]));
+									wordsDescFrom.insert(wordsDescFrom.end(), std::make_pair(id, descriptorsFrom.row(i)));
+									words3From.insert(words3From.end(), std::make_pair(id, kptsFrom3D[i]));
 								}
 							}
 
-							int newToId = !orignalWordsFromIds.empty()?fromSignature.getWords().rbegin()->first+1:descriptorsFrom.rows;
+							int newToId = orignalWordsFromIds.size()?orignalWordsFromIds.back():descriptorsFrom.rows;
 							for(unsigned int i = 0; i < kptsTo.size(); ++i)
 							{
 								if(addedWordsTo.find(i) == addedWordsTo.end())
 								{
-									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, wordsTo.size()));
-									wordsKptsTo.push_back(kptsTo[i]);
-									wordsDescTo.push_back(descriptorsTo.row(i));
-									if(!kptsTo3D.empty())
+									wordsTo.insert(wordsTo.end(), std::make_pair(newToId, kptsTo[i]));
+									wordsDescTo.insert(wordsDescTo.end(), std::make_pair(newToId, descriptorsTo.row(i)));
+									if(kptsTo3D.size())
 									{
-										words3To.push_back(kptsTo3D[i]);
+										words3To.insert(words3To.end(), std::make_pair(newToId, kptsTo3D[i]));
 									}
 									++newToId;
 								}
@@ -1282,16 +1267,15 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(fromWordIdsSet.count(*iter) == 1)
 						{
-							wordsFrom.insert(wordsFrom.end(), std::make_pair(*iter, wordsFrom.size()));
-							if (!kptsFrom.empty())
+							if (kptsFrom.size())
 							{
-								wordsKptsFrom.push_back(kptsFrom[i]);
+								wordsFrom.insert(std::make_pair(*iter, kptsFrom[i]));
 							}
-							if(!kptsFrom3D.empty())
+							if(kptsFrom3D.size())
 							{
-								words3From.push_back(kptsFrom3D[i]);
+								words3From.insert(std::make_pair(*iter, kptsFrom3D[i]));
 							}
-							wordsDescFrom.push_back(descriptorsFrom.row(i));
+							wordsDescFrom.insert(std::make_pair(*iter, descriptorsFrom.row(i)));
 						}
 						++i;
 					}
@@ -1303,12 +1287,11 @@ Transform RegistrationVis::computeTransformationImpl(
 					{
 						if(toWordIdsSet.count(*iter) == 1)
 						{
-							wordsTo.insert(wordsTo.end(), std::make_pair(*iter, wordsTo.size()));
-							wordsKptsTo.push_back(kptsTo[i]);
-							wordsDescTo.push_back(descriptorsTo.row(i));
-							if(!kptsTo3D.empty())
+							wordsTo.insert(std::make_pair(*iter, kptsTo[i]));
+							wordsDescTo.insert(std::make_pair(*iter, descriptorsTo.row(i)));
+							if(kptsTo3D.size())
 							{
-								words3To.push_back(kptsTo3D[i]);
+								words3To.insert(std::make_pair(*iter, kptsTo3D[i]));
 							}
 						}
 						++i;
@@ -1321,19 +1304,21 @@ Transform RegistrationVis::computeTransformationImpl(
 				UASSERT(kptsFrom3D.empty() || int(kptsFrom3D.size()) == descriptorsFrom.rows);
 				for(int i=0; i<descriptorsFrom.rows; ++i)
 				{
-					wordsFrom.insert(wordsFrom.end(), std::make_pair(i, wordsFrom.size()));
-					wordsKptsFrom.push_back(kptsFrom[i]);
-					wordsDescFrom.push_back(descriptorsFrom.row(i));
-					if(!kptsFrom3D.empty())
+					wordsFrom.insert(std::make_pair(i, kptsFrom[i]));
+					wordsDescFrom.insert(std::make_pair(i, descriptorsFrom.row(i)));
+					if(kptsFrom3D.size())
 					{
-						words3From.push_back(kptsFrom3D[i]);
+						words3From.insert(std::make_pair(i, kptsFrom3D[i]));
 					}
 				}
 			}
 		}
-
-		fromSignature.setWords(wordsFrom, wordsKptsFrom, words3From, wordsDescFrom);
-		toSignature.setWords(wordsTo, wordsKptsTo, words3To, wordsDescTo);
+		fromSignature.setWords(wordsFrom);
+		fromSignature.setWords3(words3From);
+		fromSignature.setWordsDescriptors(wordsDescFrom);
+		toSignature.setWords(wordsTo);
+		toSignature.setWords3(words3To);
+		toSignature.setWordsDescriptors(wordsDescTo);
 	}
 
 	/////////////////////
@@ -1387,31 +1372,14 @@ Transform RegistrationVis::computeTransformationImpl(
 					Transform cameraTransform;
 					double variance = 1.0f;
 					std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
-					std::map<int, cv::KeyPoint> wordsA;
-					std::map<int, cv::Point3f> words3A;
-					std::map<int, cv::KeyPoint> wordsB;
-					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
-					{
-						wordsA.insert(std::make_pair(iter->first, signatureA->getWordsKpts()[iter->second]));
-						if(!signatureA->getWords3().empty())
-						{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
-						}
-					}
-					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-					{
-						wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
-					}
 					std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-							wordsA,
-							wordsB,
+							uMultimapToMapUnique(signatureA->getWords()),
+							uMultimapToMapUnique(signatureB->getWords()),
 							cameraModel,
 							cameraTransform,
 							_PnPReprojError,
 							0.99f,
-							words3A, // for scale estimation
+							uMultimapToMapUnique(signatureA->getWords3()), // for scale estimation
 							&variance,
 							&matchesV);
 					covariances[dir] *= variance;
@@ -1487,26 +1455,9 @@ Transform RegistrationVis::computeTransformationImpl(
 
 						std::vector<int> inliersV;
 						std::vector<int> matchesV;
-						std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-						std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
-						std::map<int, cv::Point3f> words3A;
-						std::map<int, cv::Point3f> words3B;
-						std::map<int, cv::KeyPoint> wordsB;
-						for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
-						{
-							words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
-						}
-						for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-						{
-							wordsB.insert(std::make_pair(iter->first, signatureB->getWordsKpts()[iter->second]));
-							if(!signatureB->getWords3().empty())
-							{
-								words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
-							}
-						}
 						transforms[dir] = util3d::estimateMotion3DTo2D(
-								words3A,
-								wordsB,
+								uMultimapToMapUnique(signatureA->getWords3()),
+								uMultimapToMapUnique(signatureB->getWords()),
 								cameraModel,
 								_minInliers,
 								_iterations,
@@ -1514,7 +1465,7 @@ Transform RegistrationVis::computeTransformationImpl(
 								_PnPFlags,
 								_PnPRefineIterations,
 								dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
-								words3B,
+								uMultimapToMapUnique(signatureB->getWords3()),
 								&covariances[dir],
 								&matchesV,
 								&inliersV);
@@ -1550,21 +1501,9 @@ Transform RegistrationVis::computeTransformationImpl(
 				{
 					std::vector<int> inliersV;
 					std::vector<int> matchesV;
-					std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
-					std::map<int, int> uniqueWordsB = uMultimapToMapUnique(signatureB->getWords());
-					std::map<int, cv::Point3f> words3A;
-					std::map<int, cv::Point3f> words3B;
-					for(std::map<int, int>::iterator iter=uniqueWordsA.begin(); iter!=uniqueWordsA.end(); ++iter)
-					{
-						words3A.insert(std::make_pair(iter->first, signatureA->getWords3()[iter->second]));
-					}
-					for(std::map<int, int>::iterator iter=uniqueWordsB.begin(); iter!=uniqueWordsB.end(); ++iter)
-					{
-						words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
-					}
 					transforms[dir] = util3d::estimateMotion3DTo3D(
-							words3A,
-							words3B,
+							uMultimapToMapUnique(signatureA->getWords3()),
+							uMultimapToMapUnique(signatureB->getWords3()),
 							_minInliers,
 							_inlierDistance,
 							_iterations,
@@ -1732,37 +1671,27 @@ Transform RegistrationVis::computeTransformationImpl(
 			models.insert(std::make_pair(2, cameraModelTo));
 
 			std::map<int, std::map<int, FeatureBA> > wordReferences;
-			std::set<int> sbaOutliers;
 			for(unsigned int i=0; i<allInliers.size(); ++i)
 			{
 				int wordId = allInliers[i];
-				int indexFrom = fromSignature.getWords().find(wordId)->second;
-				const cv::Point3f & pt3D = fromSignature.getWords3()[indexFrom];
-				if(!util3d::isFinite(pt3D))
-				{
-					UASSERT_MSG(!_forwardEstimateOnly, uFormat("3D point %d is not finite!?", wordId).c_str());
-					sbaOutliers.insert(wordId);
-					continue;
-				}
-
+				const cv::Point3f & pt3D = fromSignature.getWords3().find(wordId)->second;
 				points3DMap.insert(std::make_pair(wordId, pt3D));
 
 				std::map<int, FeatureBA> ptMap;
-				if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
+				if(fromSignature.getWords().size() && cameraModelFrom.isValidForProjection())
 				{
 					float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
+					const cv::KeyPoint & kpt = fromSignature.getWords().find(wordId)->second;
 					ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
 				}
-				if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
+				if(toSignature.getWords().size() && cameraModelTo.isValidForProjection())
 				{
-					int indexTo = toSignature.getWords().find(wordId)->second;
 					float depthTo = 0.0f;
-					if(!toSignature.getWords3().empty())
+					if(toSignature.getWords3().find(wordId) != toSignature.getWords3().end())
 					{
-						depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
+						depthTo = util3d::transformPoint(toSignature.getWords3().find(wordId)->second, invLocalTransformTo).z;
 					}
-					const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
+					const cv::KeyPoint & kpt = toSignature.getWords().find(wordId)->second;
 					ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
 				}
 
@@ -1775,6 +1704,7 @@ Transform RegistrationVis::computeTransformationImpl(
 				//}
 			}
 
+			std::set<int> sbaOutliers;
 			optimizedPoses = sba->optimizeBA(1, poses, links, models, points3DMap, wordReferences, &sbaOutliers);
 			delete sba;
 
@@ -1900,25 +1830,24 @@ Transform RegistrationVis::computeTransformationImpl(
 			{
 				if(_maxInliersMeanDistance>0.0f)
 				{
-					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					if(wordsIter != fromSignature.getWords().end() && !fromSignature.getWords3().empty())
+					std::multimap<int, cv::Point3f>::const_iterator words3Iter = fromSignature.getWords3().find(allInliers[i]);
+					if(words3Iter != fromSignature.getWords3().end())
 					{
-						const cv::Point3f & pt = fromSignature.getWords3()[wordsIter->second];
-						if(uIsFinite(pt.x))
+						if(uIsFinite(words3Iter->second.x))
 						{
-							distances.push_back(util3d::transformPoint(pt, transformInv).x);
+							cv::Point3f pt = util3d::transformPoint(words3Iter->second, transformInv);
+							distances.push_back(pt.x);
 						}
 					}
 				}
 
 				if(!pcaData.empty())
 				{
-					std::multimap<int, int>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
-					UASSERT(wordsIter != fromSignature.getWords().end() && !fromSignature.getWordsKpts().empty());
+					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = fromSignature.getWords().find(allInliers[i]);
+					UASSERT(wordsIter != fromSignature.getWords().end());
 					float * ptr = pcaData.ptr<float>(i, 0);
-					const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[wordsIter->second];
-					ptr[0] = (kpt.pt.x-cx) / w;
-					ptr[1] = (kpt.pt.y-cy) / h;
+					ptr[0] = (wordsIter->second.pt.x-cx) / w;
+					ptr[1] = (wordsIter->second.pt.y-cy) / h;
 				}
 			}
 
@@ -1959,7 +1888,6 @@ Transform RegistrationVis::computeTransformationImpl(
 	}
 
 	info.inliers = inliersCount;
-	info.inliersRatio = !toSignature.getWords().empty()?float(inliersCount)/float(toSignature.getWords().size()):0;
 	info.matches = matchesCount;
 	info.rejectedMsg = msg;
 	info.covariance = covariance;

corelib/src/Rtabmap.cpp

@@ -134,7 +134,6 @@ Rtabmap::Rtabmap() :
 	_lastProcessTime(0.0),
 	_someNodesHaveBeenTransferred(false),
 	_distanceTravelled(0.0f),
-	_distanceTravelledSinceLastLocalization(0.0f),
 	_optimizeFromGraphEndChanged(false),
 	_epipolarGeometry(0),
 	_bayesFilter(0),
@@ -351,17 +350,6 @@ void Rtabmap::init(const ParametersMap & parameters, const std::string & databas
 		std::map<int, Transform> tmp;
 		// Get just the links
 		_memory->getMetricConstraints(uKeysSet(_optimizedPoses), tmp, _constraints, false, true);
-
-		// Initialize Bayes' prediction matrix
-		UTimer time;
-		std::map<int, float> likelihood;
-		likelihood.insert(std::make_pair(Memory::kIdVirtual, 1));
-		for(std::map<int, Transform>::iterator iter=_optimizedPoses.begin(); iter!=_optimizedPoses.end(); ++iter)
-		{
-			likelihood.insert(std::make_pair(iter->first, 0));
-		}
-		_bayesFilter->computePosterior(_memory, likelihood);
-		UINFO("Time initializing Bayes' prediction with %ld nodes: %fs", _optimizedPoses.size(), time.ticks());
 	}
 	else
 	{
@@ -405,7 +393,6 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	_odomCacheConstraints.clear();
 	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
-	_distanceTravelledSinceLastLocalization = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
 	_gpsGeocentricCache.clear();
@@ -432,16 +419,6 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
 	{
 		if(databaseSaved)
 		{
-			if(_memory->isGraphReduced() && _memory->isIncremental())
-			{
-				// Force reducing graph, then remove filtered nodes from the optimized poses
-				std::map<int, int> reducedIds;
-				_memory->incrementMapId(&reducedIds);
-				for(std::map<int, int>::iterator iter=reducedIds.begin(); iter!=reducedIds.end(); ++iter)
-				{
-					_optimizedPoses.erase(iter->first);
-				}
-			}
 			_memory->saveOptimizedPoses(_optimizedPoses, _lastLocalizationPose);
 		}
 		_memory->close(databaseSaved, true, ouputDatabasePath);
@@ -678,6 +655,19 @@ int Rtabmap::getTotalMemSize() const
 	return 0;
 }
 
+std::multimap<int, cv::KeyPoint> Rtabmap::getWords(int locationId) const
+{
+	if(_memory)
+	{
+		const Signature * s = _memory->getSignature(locationId);
+		if(s)
+		{
+			return s->getWords();
+		}
+	}
+	return std::multimap<int, cv::KeyPoint>();
+}
+
 bool Rtabmap::isInSTM(int locationId) const
 {
 	if(_memory)
@@ -700,7 +690,25 @@ const Statistics & Rtabmap::getStatistics() const
 {
 	return statistics_;
 }
-
+/*
+bool Rtabmap::getMetricData(int locationId, cv::Mat & rgb, cv::Mat & depth, float & depthConstant, Transform & pose, Transform & localTransform) const
+{
+	if(_memory)
+	{
+		const Signature * s = _memory->getSignature(locationId);
+		if(s && _optimizedPoses.find(s->id()) != _optimizedPoses.end())
+		{
+			rgb = s->getImage();
+			depth = s->getDepth();
+			depthConstant = s->getDepthConstant();
+			pose = _optimizedPoses.at(s->id());
+			localTransform = s->getLocalTransform();
+			return true;
+		}
+	}
+	return false;
+}
+*/
 Transform Rtabmap::getPose(int locationId) const
 {
 	return uValue(_optimizedPoses, locationId, Transform());
@@ -743,8 +751,6 @@ int Rtabmap::triggerNewMap()
 		_odomCachePoses.clear();
 		_odomCacheConstraints.clear();
 		_odomCorrectionAcc = std::vector<float>(6,0);
-		_distanceTravelled = 0.0f;
-		_distanceTravelledSinceLastLocalization = 0.0f;
 
 		if(!_memory->isIncremental())
 		{
@@ -911,7 +917,6 @@ void Rtabmap::resetMemory()
 	_odomCacheConstraints.clear();
 	_odomCorrectionAcc = std::vector<float>(6,0);
 	_distanceTravelled = 0.0f;
-	_distanceTravelledSinceLastLocalization = 0.0f;
 	_optimizeFromGraphEndChanged = false;
 	this->clearPath(0);
 
@@ -1375,9 +1380,6 @@ bool Rtabmap::process(
 				_constraints.insert(std::make_pair(iter->first, iter->second.inverse()));
 			}
 		}
-
-		float distanceTravelledOld = _distanceTravelled;
-
 		// only in mapping mode we add a neighbor link
 		if(signature->getLinks().size() &&
 		   signature->getLinks().begin()->second.type() == Link::kNeighbor)
@@ -1450,7 +1452,6 @@ bool Rtabmap::process(
 				}
 			}
 		}
-		_distanceTravelledSinceLastLocalization += _distanceTravelled - distanceTravelledOld;
 
 		//============================================================
 		// Reduced graph
@@ -2183,7 +2184,6 @@ bool Rtabmap::process(
 	// Landmark
 	//============================================================
 	int landmarkDetected = 0;
-	bool rejectedLandmark = false;
 	std::set<int> landmarkDetectedNodesRef;
 	if(!signature->getLandmarks().empty())
 	{
@@ -2205,7 +2205,6 @@ bool Rtabmap::process(
 	//============================================================
 	std::list<std::pair<int, int> > loopClosureLinksAdded;
 	int loopClosureVisualInliers = 0; // for statistics
-	float loopClosureVisualInliersRatio = 0.0f;
 	int loopClosureVisualMatches = 0;
 	float loopClosureLinearVariance = 0.0f;
 	float loopClosureAngularVariance = 0.0f;
@@ -2357,7 +2356,6 @@ bool Rtabmap::process(
 									lastProximitySpaceClosureId = nearestId;
 
 									loopClosureVisualInliers = info.inliers;
-									loopClosureVisualInliersRatio = info.inliersRatio;
 									loopClosureVisualMatches = info.matches;
 
 									loopClosureLinearVariance = 1.0/information.at<double>(0,0);
@@ -2561,7 +2559,6 @@ bool Rtabmap::process(
 			loopClosureVisualInliersDistribution = info.inliersDistribution;
 
 			loopClosureVisualInliers = info.inliers;
-			loopClosureVisualInliersRatio = info.inliersRatio;
 			loopClosureVisualMatches = info.matches;
 			rejectedGlobalLoopClosure = transform.isNull();
 			if(rejectedGlobalLoopClosure)
@@ -2914,7 +2911,6 @@ bool Rtabmap::process(
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
 				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
 			}
 		}
 		else
@@ -2950,7 +2946,6 @@ bool Rtabmap::process(
 				_loopClosureHypothesis.first = 0;
 				lastProximitySpaceClosureId = 0;
 				rejectedGlobalLoopClosure = true;
-				rejectedLandmark = true;
 			}
 			else if(_memory->isIncremental() &&
 			  _optimizationMaxError > 0.0f &&
@@ -3036,7 +3031,6 @@ bool Rtabmap::process(
 					_loopClosureHypothesis.first = 0;
 					lastProximitySpaceClosureId = 0;
 					rejectedGlobalLoopClosure = true;
-					rejectedLandmark = true;
 				}
 			}
 
@@ -3143,7 +3137,6 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kLoopReactivate_id(), retrievalId);
 			statistics_.addStatistic(Statistics::kLoopHypothesis_ratio(), hypothesisRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_inliers(), loopClosureVisualInliers);
-			statistics_.addStatistic(Statistics::kLoopVisual_inliers_ratio(), loopClosureVisualInliersRatio);
 			statistics_.addStatistic(Statistics::kLoopVisual_matches(), loopClosureVisualMatches);
 			statistics_.addStatistic(Statistics::kLoopLinear_variance(), loopClosureLinearVariance);
 			statistics_.addStatistic(Statistics::kLoopAngular_variance(), loopClosureAngularVariance);
@@ -3167,9 +3160,7 @@ bool Rtabmap::process(
 			statistics_.setProximityDetectionId(lastProximitySpaceClosureId);
 			statistics_.setProximityDetectionMapId(_memory->getMapId(lastProximitySpaceClosureId));
 
-			int loopId = _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId;
-			statistics_.addStatistic(Statistics::kLoopId(), loopId);
-			statistics_.addStatistic(Statistics::kLoopMap_id(), (loopId>0 && sLoop)?sLoop->mapId():-1);
+			statistics_.addStatistic(Statistics::kLoopId(), _loopClosureHypothesis.first>0?_loopClosureHypothesis.first:lastProximitySpaceClosureId);
 
 			float x,y,z,roll,pitch,yaw;
 			if(_loopClosureHypothesis.first || lastProximitySpaceClosureId)
@@ -3181,9 +3172,8 @@ bool Rtabmap::process(
 				UINFO("Set loop closure transform = %s", loopIter->second.transform().prettyPrint().c_str());
 				statistics_.setLoopClosureTransform(loopIter->second.transform());
 
+				statistics_.addStatistic(Statistics::kLoopMap_id(), sLoop->mapId());
 				statistics_.addStatistic(Statistics::kLoopVisual_words(), sLoop->getWords().size());
-				statistics_.addStatistic(Statistics::kLoopDistance_since_last_loc(), _distanceTravelledSinceLastLocalization);
-				_distanceTravelledSinceLastLocalization = 0.0f;
 
 				// if ground truth exists, compute localization error
 				if(!sLoop->getGroundTruthPose().isNull() && !signature->getGroundTruthPose().isNull())
@@ -3291,16 +3281,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kMemoryFast_movement(), tooFastMovement?1.0f:0);
 			if(_publishRAMUsage)
 			{
-				UTimer ramTimer;
 				statistics_.addStatistic(Statistics::kMemoryRAM_usage(), UProcessInfo::getMemoryUsage()/(1024*1024));
-				long estimatedMemoryUsage = sizeof(Rtabmap);
-				estimatedMemoryUsage += _optimizedPoses.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Transform>);
-				estimatedMemoryUsage += _constraints.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double) + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
-				estimatedMemoryUsage += _memory->getMemoryUsed();
-				estimatedMemoryUsage += _bayesFilter->getMemoryUsed();
-				estimatedMemoryUsage += _parameters.size()*(sizeof(std::string)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(ParametersMap);
-				statistics_.addStatistic(Statistics::kMemoryRAM_estimated(), (float)(estimatedMemoryUsage/(1024*1024)));//MB
-				statistics_.addStatistic(Statistics::kTimingRAM_estimation(), ramTimer.ticks()*1000);
 			}
 
 			if(_publishLikelihood || _publishPdf)
@@ -3365,7 +3346,7 @@ bool Rtabmap::process(
 		if(_startNewMapOnLoopClosure &&
 			_memory->isIncremental() &&              // only in mapping mode
 			graph::filterLinks(signature->getLinks(), Link::kSelfRefLink).size() == 0 &&      // alone in the current map
-			(landmarkDetected == 0 || rejectedLandmark) &&      // if we re not seeing a landmark from a previous map
+			(landmarkDetected == 0 || rejectedGlobalLoopClosure) &&      // if we re not seeing a landmark from a previous map
 			_memory->getWorkingMem().size()>=2)       // The working memory should not be empty (beside virtual signature)
 		{
 			UWARN("Ignoring location %d because a global loop closure is required before starting a new map!",
@@ -4449,30 +4430,18 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
 				groundTruth,
 				data);
 
-		std::multimap<int, Link> links = _memory->getLinks(id, true, true);
-		for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
-		{
-			if(iter->second.type() == Link::kLandmark)
-			{
-				s.addLandmark(iter->second);
-			}
-			else
-			{
-				s.addLink(iter->second);
-			}
-		}
-
 		if(withWords || withGlobalDescriptors)
 		{
-			std::multimap<int, int> words;
-			std::vector<cv::KeyPoint> wordsKpts;
-			std::vector<cv::Point3f> words3;
-			cv::Mat wordsDescriptors;
+			std::multimap<int, cv::KeyPoint> words;
+			std::multimap<int, cv::Point3f> words3;
+			std::multimap<int, cv::Mat> wordsDescriptors;
 			std::vector<rtabmap::GlobalDescriptor> globalDescriptors;
-			_memory->getNodeWordsAndGlobalDescriptors(id, words, wordsKpts, words3, wordsDescriptors, globalDescriptors);
+			_memory->getNodeWordsAndGlobalDescriptors(id, words, words3, wordsDescriptors, globalDescriptors);
 			if(withWords)
 			{
-				s.setWords(words, wordsKpts, words3, wordsDescriptors);
+				s.setWords(words);
+				s.setWords3(words3);
+				s.setWordsDescriptors(wordsDescriptors);
 			}
 			if(withGlobalDescriptors)
 			{
@@ -4566,26 +4535,6 @@ void Rtabmap::getGraph(
 	}
 }
 
-std::map<int, Transform> Rtabmap::getNodesInRadius(const Transform & pose, float radius)
-{
-	return graph::getPosesInRadius(pose, _optimizedPoses, radius<=0?_localRadius:radius);
-}
-
-std::map<int, Transform> Rtabmap::getNodesInRadius(int nodeId, float radius)
-{
-	UDEBUG("nodeId=%d, radius=%f", nodeId, radius);
-	std::map<int, Transform> nearNodes;
-	if(nodeId==0 && !_optimizedPoses.empty())
-	{
-		nodeId = _optimizedPoses.rbegin()->first;
-	}
-	if(_optimizedPoses.find(nodeId) != _optimizedPoses.end())
-	{
-		nearNodes = graph::getPosesInRadius(nodeId, _optimizedPoses, radius<=0?_localRadius:radius);
-	}
-	return nearNodes;
-}
-
 int Rtabmap::detectMoreLoopClosures(
 		float clusterRadius,
 		float clusterAngle,
@@ -4618,7 +4567,7 @@ int Rtabmap::detectMoreLoopClosures(
 	std::map<int, Transform> posesToCheckLoopClosures;
 	std::map<int, Transform> poses;
 	std::multimap<int, Link> links;
-	std::map<int, Signature> signatures; // some signatures may be in LTM, get them all
+	std::map<int, Signature> signatures;
 	this->getGraph(poses, links, true, true, &signatures);
 
 	std::map<int, int> mapIds;
@@ -4656,7 +4605,7 @@ int Rtabmap::detectMoreLoopClosures(
 
 			int from = iter->first;
 			int to = iter->second;
-			if(from > to)
+			if(iter->first < iter->second)
 			{
 				from = iter->second;
 				to = iter->first;
@@ -4692,15 +4641,9 @@ int Rtabmap::detectMoreLoopClosures(
 						UASSERT(signatures.find(from) != signatures.end());
 						UASSERT(signatures.find(to) != signatures.end());
 
-						Transform guess;
-						if(_proximityOdomGuess && uContains(poses, from) && uContains(poses, to))
-						{
-							guess = poses.at(from).inverse() * poses.at(to);
-						}
-
 						RegistrationInfo info;
 						// use signatures instead of IDs because some signatures may not be in WM
-						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), guess, &info);
+						Transform t = _memory->computeTransform(signatures.at(from), signatures.at(to), Transform(), &info);
 
 						if(!t.isNull())
 						{

corelib/src/SensorData.cpp

@@ -763,10 +763,9 @@ void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const
 	_descriptors = descriptors;
 }
 
-unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
+long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 {
-	return sizeof(SensorData) +
-			_imageCompressed.total()*_imageCompressed.elemSize() +
+	return _imageCompressed.total()*_imageCompressed.elemSize() +
 			_imageRaw.total()*_imageRaw.elemSize() +
 			_depthOrRightCompressed.total()*_depthOrRightCompressed.elemSize() +
 			_depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
@@ -780,8 +779,8 @@ unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
 			_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
 			_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
 			_emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
-			_keypoints.size() * sizeof(cv::KeyPoint) +
-			_keypoints3D.size() * sizeof(cv::Point3f) +
+			_keypoints.size() * sizeof(float) * 7 +
+			_keypoints3D.size() * sizeof(float)*3 +
 			_descriptors.total()*_descriptors.elemSize();
 }
 

corelib/src/Signature.cpp

@@ -132,24 +132,11 @@ bool Signature::hasLink(int idTo, Link::Type type) const
 	{
 		return _links.find(idTo) != _links.end();
 	}
-	if(idTo==0)
+	for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
 	{
-		for(std::multimap<int, Link>::const_iterator iter=_links.begin(); iter!=_links.end(); ++iter)
+		if(type == iter->second.type())
 		{
-			if(type == iter->second.type())
-			{
-				return true;
-			}
-		}
-	}
-	else
-	{
-		for(std::multimap<int, Link>::const_iterator iter=_links.find(idTo); iter!=_links.end() && iter->first == idTo; ++iter)
-		{
-			if(type == iter->second.type())
-			{
-				return true;
-			}
+			return true;
 		}
 	}
 	return false;
@@ -222,11 +209,11 @@ void Signature::removeVirtualLinks()
 float Signature::compareTo(const Signature & s) const
 {
 	float similarity = 0.0f;
-	const std::multimap<int, int> & words = s.getWords();
+	const std::multimap<int, cv::KeyPoint> & words = s.getWords();
 
 	if(!s.isBadSignature() && !this->isBadSignature())
 	{
-		std::list<std::pair<int, std::pair<int, int> > > pairs;
+		std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 		int totalWords = ((int)_words.size()-_invalidWordsCount)>((int)words.size()-s.getInvalidWordsCount())?((int)_words.size()-_invalidWordsCount):((int)words.size()-s.getInvalidWordsCount());
 		UASSERT(totalWords > 0);
 		EpipolarGeometry::findPairs(words, _words, pairs);
@@ -238,9 +225,11 @@ float Signature::compareTo(const Signature & s) const
 
 void Signature::changeWordsRef(int oldWordId, int activeWordId)
 {
-	std::list<int> words = uValues(_words, oldWordId);
-	if(words.size())
+	std::list<cv::KeyPoint> kps = uValues(_words, oldWordId);
+	if(kps.size())
 	{
+		std::list<cv::Point3f> pts = uValues(_words3, oldWordId);
+		std::list<cv::Mat> descriptors = uValues(_wordsDescriptors, oldWordId);
 		if(oldWordId<=0)
 		{
 			_invalidWordsCount-=(int)_words.erase(oldWordId);
@@ -250,41 +239,37 @@ void Signature::changeWordsRef(int oldWordId, int activeWordId)
 		{
 			_words.erase(oldWordId);
 		}
-
+		_words3.erase(oldWordId);
+		_wordsDescriptors.erase(oldWordId);
 		_wordsChanged.insert(std::make_pair(oldWordId, activeWordId));
-		for(std::list<int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+		for(std::list<cv::KeyPoint>::const_iterator iter=kps.begin(); iter!=kps.end(); ++iter)
 		{
-			_words.insert(std::pair<int, int>(activeWordId, (*iter)));
+			_words.insert(std::pair<int, cv::KeyPoint>(activeWordId, (*iter)));
+		}
+		for(std::list<cv::Point3f>::const_iterator iter=pts.begin(); iter!=pts.end(); ++iter)
+		{
+			_words3.insert(std::pair<int, cv::Point3f>(activeWordId, (*iter)));
+		}
+		for(std::list<cv::Mat>::const_iterator iter=descriptors.begin(); iter!=descriptors.end(); ++iter)
+		{
+			_wordsDescriptors.insert(std::pair<int, cv::Mat>(activeWordId, (*iter)));
 		}
 	}
 }
 
-void Signature::setWords(const std::multimap<int, int> & words,
-		const std::vector<cv::KeyPoint> & keypoints,
-		const std::vector<cv::Point3f> & points,
-		const cv::Mat & descriptors)
+void Signature::setWords(const std::multimap<int, cv::KeyPoint> & words)
 {
-	UASSERT_MSG(descriptors.empty() || descriptors.rows == (int)words.size(), uFormat("words=%d, descriptors=%d", (int)words.size(), descriptors.rows).c_str());
-	UASSERT_MSG(points.empty() || points.size() == words.size(),  uFormat("words=%d, points=%d", (int)words.size(), (int)points.size()).c_str());
-	UASSERT_MSG(keypoints.empty() || keypoints.size() == words.size(),  uFormat("words=%d, descriptors=%d", (int)words.size(), (int)keypoints.size()).c_str());
-	UASSERT(words.empty() || !keypoints.empty() || !points.empty() || !descriptors.empty());
-
-	_invalidWordsCount = 0;
-	for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
-	{
-		if(iter->first<=0)
-		{
-			++_invalidWordsCount;
-		}
-		// make sure indexes are all valid!
-		UASSERT_MSG(iter->second >=0 && iter->second < (int)words.size(), uFormat("iter->second=%d words.size()=%d", iter->second, (int)words.size()).c_str());
-	}
-
 	_enabled = false;
 	_words = words;
-	_wordsKpts = keypoints;
-	_words3 = points;
-	_wordsDescriptors = descriptors.clone();
+	_invalidWordsCount = 0;
+	for(std::multimap<int, cv::KeyPoint>::iterator iter=_words.begin(); iter!=_words.end(); ++iter)
+	{
+		if(iter->first>0)
+		{
+			break;
+		}
+		++_invalidWordsCount;
+	}
 }
 
 bool Signature::isBadSignature() const
@@ -295,30 +280,24 @@ bool Signature::isBadSignature() const
 void Signature::removeAllWords()
 {
 	_words.clear();
-	_wordsKpts.clear();
 	_words3.clear();
-	_wordsDescriptors = cv::Mat();
+	_wordsDescriptors.clear();
 	_invalidWordsCount = 0;
 }
 
-void Signature::setWordsDescriptors(const cv::Mat & descriptors)
+void Signature::removeWord(int wordId)
 {
-	if(descriptors.empty())
+	if(wordId<=0)
 	{
-		if(_wordsKpts.empty() && _words3.empty())
-		{
-			removeAllWords();
-		}
-		else
-		{
-			_wordsDescriptors = cv::Mat();
-		}
+		_invalidWordsCount-=(int)_words.erase(wordId);
+		UASSERT(_invalidWordsCount>=0);
 	}
 	else
 	{
-		UASSERT(descriptors.rows == (int)_words.size());
-		_wordsDescriptors = descriptors.clone();
+		_words.erase(wordId);
 	}
+	_words3.erase(wordId);
+	_wordsDescriptors.clear();
 }
 
 cv::Mat Signature::getPoseCovariance() const
@@ -342,23 +321,19 @@ cv::Mat Signature::getPoseCovariance() const
 	return covariance;
 }
 
-unsigned long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
+long Signature::getMemoryUsed(bool withSensorData) const // Return memory usage in Bytes
 {
-	unsigned long total = sizeof(Signature);
-	total += _words.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, cv::KeyPoint>);
-	total += _wordsKpts.size() * sizeof(cv::KeyPoint) + sizeof(std::vector<cv::KeyPoint>);
-	total += _words3.size() * sizeof(cv::Point3f) + sizeof(std::vector<cv::Point3f>);
-	total += _wordsDescriptors.total() * _wordsDescriptors.elemSize() + sizeof(cv::Mat);
-	total += _wordsChanged.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, int>);
+	long total =  _words.size() * sizeof(float) * 8 +
+				  _words3.size() * sizeof(float) * 4;
+	if(!_wordsDescriptors.empty())
+	{
+		total += _wordsDescriptors.size() * sizeof(int);
+		total += _wordsDescriptors.size() * _wordsDescriptors.begin()->second.total() * _wordsDescriptors.begin()->second.elemSize();
+	}
 	if(withSensorData)
 	{
 		total+=_sensorData.getMemoryUsed();
 	}
-	total += _pose.size() * (sizeof(Transform) + sizeof(float)*12);
-	total += _groundTruthPose.size() * (sizeof(Transform) + sizeof(float)*12);
-	total += _velocity.size() * sizeof(float);
-	total += _links.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::multimap<int, Link>);
-	total += _landmarks.size() * (sizeof(int) + sizeof(Transform) + 12 * sizeof(float) + sizeof(cv::Mat) + 36 * sizeof(double)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, Link>);
 	return total;
 }
 

corelib/src/Transform.cpp

@@ -311,9 +311,14 @@ bool Transform::operator!=(const Transform & t) const
 
 std::ostream& operator<<(std::ostream& os, const Transform& s)
 {
-	os << "[" << s.data()[0] << ", " << s.data()[1] << ", " << s.data()[2] << ", " << s.data()[3] << ";" << std::endl
-	   << " " << s.data()[4] << ", " << s.data()[5] << ", " << s.data()[6] << ", " << s.data()[7] << ";" << std::endl
-	   << " " << s.data()[8] << ", " << s.data()[9] << ", " << s.data()[10]<< ", " << s.data()[11] << "]";
+	for(int i = 0; i < 3; ++i)
+	{
+		for(int j = 0; j < 4; ++j)
+		{
+			os << std::left << std::setw(12) << s.data()[i*4 + j] << " ";
+		}
+		os << std::endl;
+	}
 	return os;
 }
 

corelib/src/VWDictionary.cpp

@@ -65,7 +65,6 @@ VWDictionary::VWDictionary(const ParametersMap & parameters) :
 	_incrementalDictionary(Parameters::defaultKpIncrementalDictionary()),
 	_incrementalFlann(Parameters::defaultKpIncrementalFlann()),
 	_rebalancingFactor(Parameters::defaultKpFlannRebalancingFactor()),
-	_byteToFloat(Parameters::defaultKpByteToFloat()),
 	_nndrRatio(Parameters::defaultKpNndrRatio()),
 	_newDictionaryPath(Parameters::defaultKpDictionaryPath()),
 	_newWordsComparedTogether(Parameters::defaultKpNewWordsComparedTogether()),
@@ -91,8 +90,6 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kKpNewWordsComparedTogether(), _newWordsComparedTogether);
 	Parameters::parse(parameters, Parameters::kKpIncrementalFlann(), _incrementalFlann);
 	Parameters::parse(parameters, Parameters::kKpFlannRebalancingFactor(), _rebalancingFactor);
-	bool byteToFloat = _byteToFloat;
-	Parameters::parse(parameters, Parameters::kKpByteToFloat(), _byteToFloat);
 
 	UASSERT_MSG(_nndrRatio > 0.0f, uFormat("String=%s value=%f", uContains(parameters, Parameters::kKpNndrRatio())?parameters.at(Parameters::kKpNndrRatio()).c_str():"", _nndrRatio).c_str());
 
@@ -107,19 +104,10 @@ void VWDictionary::parseParameters(const ParametersMap & parameters)
 	}
 
 	// Verifying hypotheses strategy
-	bool treeUpdated = false;
 	if((iter=parameters.find(Parameters::kKpNNStrategy())) != parameters.end())
 	{
 		NNStrategy nnStrategy = (NNStrategy)std::atoi((*iter).second.c_str());
-		treeUpdated = this->setNNStrategy(nnStrategy);
-	}
-	if(!treeUpdated && byteToFloat!=_byteToFloat && _strategy == kNNFlannKdTree)
-	{
-		UINFO("KDTree: Binary to Float conversion approach has changed, re-initialize kd-tree.");
-		_dataTree = cv::Mat();
-		_notIndexedWords = uKeysSet(_visualWords);
-		_removedIndexedWords.clear();
-		this->update();
+		this->setNNStrategy(nnStrategy);
 	}
 
 	if(incrementalDictionary)
@@ -289,7 +277,7 @@ void VWDictionary::setFixedDictionary(const std::string & dictionaryPath)
 	_newDictionaryPath = dictionaryPath;
 }
 
-bool VWDictionary::setNNStrategy(NNStrategy strategy)
+void VWDictionary::setNNStrategy(NNStrategy strategy)
 {
 #if CV_MAJOR_VERSION < 3
 #ifdef HAVE_OPENCV_GPU
@@ -331,17 +319,11 @@ bool VWDictionary::setNNStrategy(NNStrategy strategy)
 	_strategy = strategy;
 	if(update)
 	{
-		if(_notIndexedWords.size() != _visualWords.size() || !_dataTree.empty())
-		{
-			UINFO("Nearest neighbor strategy has changed, re-initialize search tree.");
-		}
 		_dataTree = cv::Mat();
 		_notIndexedWords = uKeysSet(_visualWords);
 		_removedIndexedWords.clear();
 		this->update();
-		return true;
 	}
-	return false;
 }
 
 int VWDictionary::getLastIndexedWordId() const
@@ -366,103 +348,59 @@ unsigned int VWDictionary::getIndexMemoryUsed() const
 	return _flannIndex->memoryUsed();
 }
 
-unsigned long VWDictionary::getMemoryUsed() const
+cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn)
 {
-	long memoryUsage = sizeof(VWDictionary);
-	memoryUsage += getIndexMemoryUsed();
-	memoryUsage += _dataTree.total()*_dataTree.elemSize();
-	if(!_visualWords.empty())
+	// Old approach
+	//cv::Mat descriptorsOut;
+	//descriptorsIn.convertTo(descriptorsOut, CV_32F);
+	//return descriptorsOut;
+
+	// New approach
+	UASSERT(descriptorsIn.type() == CV_8UC1);
+	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
+	for(int i=0; i<descriptorsIn.rows; ++i)
 	{
-		memoryUsage += _visualWords.size()*(sizeof(int) + _visualWords.rbegin()->second->getMemoryUsed() + sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
-		if(_dataTree.empty() &&
-			_visualWords.begin()->second->getDescriptor().type() == CV_8U &&
-			_strategy == kNNFlannKdTree)
+		const unsigned char * ptrIn = descriptorsIn.ptr(i);
+		float * ptrOut = descriptorsOut.ptr<float>(i);
+		for(int j=0; j<descriptorsIn.cols; ++j)
 		{
-			// Binary descriptors were converted to float, and not included in _dataTree
-			memoryUsage += _visualWords.size() * _visualWords.begin()->second->getDescriptor().total() * sizeof(float) * (_byteToFloat?1:8);
+			int jo = j*8;
+			ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
+			ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
+			ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
+			ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
+			ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
+			ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
+			ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
+			ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
 		}
 	}
-	if(!_unusedWords.empty())
-	{
-		// they are the same words than in _visualWords, so just add the pointer size
-		memoryUsage += _unusedWords.size()*(sizeof(int) + sizeof(VisualWord *)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int, VisualWord *>);
-	}
-	memoryUsage += _mapIndexId.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _mapIdIndex.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _notIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	memoryUsage += _removedIndexedWords.size() * (sizeof(int)+sizeof(std::_Rb_tree_node_base)) + sizeof(std::set<int>);
-	return memoryUsage;
+	return descriptorsOut;
 }
 
-cv::Mat VWDictionary::convertBinTo32F(const cv::Mat & descriptorsIn, bool byteToFloat)
+cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn)
 {
-	if(byteToFloat)
+	UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
+	cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
+	for(int i=0; i<descriptorsIn.rows; ++i)
 	{
-		// Old approach
-		cv::Mat descriptorsOut;
-		descriptorsIn.convertTo(descriptorsOut, CV_32F);
-		return descriptorsOut;
-	}
-	else
-	{
-		// New approach
-		UASSERT(descriptorsIn.type() == CV_8UC1);
-		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols*8, CV_32FC1);
-		for(int i=0; i<descriptorsIn.rows; ++i)
+		const float * ptrIn = descriptorsIn.ptr<float>(i);
+		unsigned char * ptrOut = descriptorsOut.ptr(i);
+		for(int j=0; j<descriptorsOut.cols; ++j)
 		{
-			const unsigned char * ptrIn = descriptorsIn.ptr(i);
-			float * ptrOut = descriptorsOut.ptr<float>(i);
-			for(int j=0; j<descriptorsIn.cols; ++j)
-			{
-				int jo = j*8;
-				ptrOut[jo] = (ptrIn[j] & 1) == 1?1.0f:0.0f;
-				ptrOut[jo+1] = (ptrIn[j] & (1<<1)) != 0?1.0f:0.0f;
-				ptrOut[jo+2] = (ptrIn[j] & (1<<2)) != 0?1.0f:0.0f;
-				ptrOut[jo+3] = (ptrIn[j] & (1<<3)) != 0?1.0f:0.0f;
-				ptrOut[jo+4] = (ptrIn[j] & (1<<4)) != 0?1.0f:0.0f;
-				ptrOut[jo+5] = (ptrIn[j] & (1<<5)) != 0?1.0f:0.0f;
-				ptrOut[jo+6] = (ptrIn[j] & (1<<6)) != 0?1.0f:0.0f;
-				ptrOut[jo+7] = (ptrIn[j] & (1<<7)) != 0?1.0f:0.0f;
-			}
+			int jo = j*8;
+			ptrOut[j] =
+					(unsigned char)(ptrIn[jo] == 0?0:1) |
+					(ptrIn[jo+1] == 0?0:(1<<1)) |
+					(ptrIn[jo+2] == 0?0:(1<<2)) |
+					(ptrIn[jo+3] == 0?0:(1<<3)) |
+					(ptrIn[jo+4] == 0?0:(1<<4)) |
+					(ptrIn[jo+5] == 0?0:(1<<5)) |
+					(ptrIn[jo+6] == 0?0:(1<<6)) |
+					(ptrIn[jo+7] == 0?0:(1<<7));
 		}
-		return descriptorsOut;
-	}
-}
-
-cv::Mat VWDictionary::convert32FToBin(const cv::Mat & descriptorsIn, bool byteToFloat)
-{
-	if(byteToFloat)
-	{
-		// Old approach
-		cv::Mat descriptorsOut;
-		descriptorsIn.convertTo(descriptorsOut, CV_8UC1);
-		return descriptorsOut;
-	}
-	else
-	{
-		// New approach
-		UASSERT(descriptorsIn.type() == CV_32FC1 && descriptorsIn.cols % 8 == 0);
-		cv::Mat descriptorsOut(descriptorsIn.rows, descriptorsIn.cols/8, CV_8UC1);
-		for(int i=0; i<descriptorsIn.rows; ++i)
-		{
-			const float * ptrIn = descriptorsIn.ptr<float>(i);
-			unsigned char * ptrOut = descriptorsOut.ptr(i);
-			for(int j=0; j<descriptorsOut.cols; ++j)
-			{
-				int jo = j*8;
-				ptrOut[j] =
-						(unsigned char)(ptrIn[jo] == 0?0:1) |
-						(ptrIn[jo+1] == 0?0:(1<<1)) |
-						(ptrIn[jo+2] == 0?0:(1<<2)) |
-						(ptrIn[jo+3] == 0?0:(1<<3)) |
-						(ptrIn[jo+4] == 0?0:(1<<4)) |
-						(ptrIn[jo+5] == 0?0:(1<<5)) |
-						(ptrIn[jo+6] == 0?0:(1<<6)) |
-						(ptrIn[jo+7] == 0?0:(1<<7));
-			}
-		}
-		return descriptorsOut;
 	}
+	return descriptorsOut;
 }
 
 void VWDictionary::update()
@@ -513,7 +451,7 @@ void VWDictionary::update()
 						useDistanceL1_ = true;
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(w->getDescriptor(), _byteToFloat);
+							descriptor = convertBinTo32F(w->getDescriptor());
 						}
 						else
 						{
@@ -552,8 +490,7 @@ void VWDictionary::update()
 					{
 						UASSERT(descriptor.cols == _flannIndex->featuresDim());
 						UASSERT(descriptor.type() == _flannIndex->featuresType());
-						UASSERT(descriptor.rows == 1);
-						index = _flannIndex->addPoints(descriptor).front();
+						index = _flannIndex->addPoints(descriptor);
 					}
 					std::pair<std::map<int, int>::iterator, bool> inserted;
 					inserted = _mapIndexId.insert(std::pair<int, int>(index, w->id()));
@@ -606,10 +543,7 @@ void VWDictionary::update()
 					if(_strategy == kNNFlannKdTree)
 					{
 						type = CV_32F;
-						if(!_byteToFloat)
-						{
-							dim *= 8;
-						}
+						dim *= 8;
 					}
 					else
 					{
@@ -634,7 +568,7 @@ void VWDictionary::update()
 					{
 						if(_strategy == kNNFlannKdTree)
 						{
-							descriptor = convertBinTo32F(iter->second->getDescriptor(), _byteToFloat);
+							descriptor = convertBinTo32F(iter->second->getDescriptor());
 						}
 						else
 						{
@@ -660,15 +594,15 @@ void VWDictionary::update()
 				switch(_strategy)
 				{
 				case kNNFlannNaive:
-					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					_flannIndex->buildLinearIndex(_dataTree, useDistanceL1_, _rebalancingFactor);
 					break;
 				case kNNFlannKdTree:
 					UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!");
-					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					_flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_, _rebalancingFactor);
 					break;
 				case kNNFlannLSH:
 					UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!");
-					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _incrementalDictionary&&_incrementalFlann?_rebalancingFactor:1);
+					_flannIndex->buildLSHIndex(_dataTree, 12, 20, 2, _rebalancingFactor);
 					break;
 				default:
 					break;
@@ -802,7 +736,7 @@ std::list<int> VWDictionary::addNewWords(
 		useDistanceL1_ = true;
 		if(_strategy == kNNFlannKdTree)
 		{
-			descriptors = convertBinTo32F(descriptorsIn, _byteToFloat);
+			descriptors = convertBinTo32F(descriptorsIn);
 		}
 		else
 		{
@@ -1140,7 +1074,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 		{
 			if(_strategy == kNNFlannKdTree)
 			{
-				query = convertBinTo32F(queryIn, _byteToFloat);
+				query = convertBinTo32F(queryIn);
 			}
 			else
 			{
@@ -1268,7 +1202,7 @@ std::vector<int> VWDictionary::findNN(const cv::Mat & queryIn) const
 				{
 					if(_strategy == kNNFlannKdTree)
 					{
-						descriptor = convertBinTo32F(vw->getDescriptor(), _byteToFloat);
+						descriptor = convertBinTo32F(vw->getDescriptor());
 					}
 					else
 					{

corelib/src/VisualWord.cpp

@@ -69,13 +69,4 @@ int VisualWord::removeAllRef(int signatureId)
 	return removed;
 }
 
-unsigned long VisualWord::getMemoryUsed() const
-{
-	unsigned long memoryUsage = sizeof(VisualWord);
-	memoryUsage += _references.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _oldReferences.size() * (sizeof(int)*2+sizeof(std::_Rb_tree_node_base)) + sizeof(std::map<int ,int>);
-	memoryUsage += _descriptor.total() * _descriptor.elemSize();
-	return memoryUsage;
-}
-
 } // namespace rtabmap

corelib/src/camera/CameraImages.cpp

@@ -436,11 +436,6 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
 			UERROR("With Karlsruhe format, timestamps (%d) and poses (%d) should match!", (int)stamps.size(), (int)poses.size());
 			return false;
 		}
-		else if(!outputPoses.empty() && inOutStamps.empty() && stamps.empty())
-		{
-			UERROR("Timestamps are empty (poses=%d)! Forgot the set a timestamp file?", (int)outputPoses.size());
-			return false;
-		}
 	}
 	UASSERT_MSG(outputPoses.size() == inOutStamps.size(), uFormat("%d vs %d", (int)outputPoses.size(), (int)inOutStamps.size()).c_str());
 	return true;

corelib/src/camera/CameraK4A.cpp

@@ -55,11 +55,12 @@ CameraK4A::CameraK4A(
 			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
 			,
-			deviceHandle_(NULL),
+			device_(NULL),
 			config_(K4A_DEVICE_CONFIG_INIT_DISABLE_ALL),
-			transformationHandle_(NULL),
-			captureHandle_(NULL),
+			transformation_(NULL),
+			capture_(NULL),
 			playbackHandle_(NULL),
+			transformationHandle_(NULL),
 			deviceId_(deviceId),
 			rgb_resolution_(0),
 			framerate_(2),
@@ -77,10 +78,11 @@ CameraK4A::CameraK4A(
 			Camera(imageRate, localTransform)
 #ifdef RTABMAP_K4A
 			,
-			deviceHandle_(NULL),
-			transformationHandle_(NULL),
-			captureHandle_(NULL),
+			device_(NULL),
+			transformation_(NULL),
+			capture_(NULL),
 			playbackHandle_(NULL),
+			transformationHandle_(NULL),
 			deviceId_(-1),
 			fileName_(fileName),
 			rgb_resolution_(0),
@@ -100,25 +102,37 @@ CameraK4A::~CameraK4A()
 void CameraK4A::close()
 {
 #ifdef RTABMAP_K4A
-	if (playbackHandle_ != NULL)
+	if (!fileName_.empty())
 	{
-		k4a_playback_close((k4a_playback_t)playbackHandle_);
-		playbackHandle_ = NULL;
-	}
-	else if (deviceHandle_ != NULL)
-	{
-		k4a_device_stop_imu(deviceHandle_);
+		if (playbackHandle_ != NULL)
+		{
+			k4a_playback_close((k4a_playback_t)playbackHandle_);
+			playbackHandle_ = NULL;
+		}
 
-		k4a_device_stop_cameras(deviceHandle_);
-		k4a_device_close(deviceHandle_);
-		deviceHandle_ = NULL;
-		config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+		if (transformationHandle_ != NULL)
+		{
+			k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
+			transformationHandle_ = NULL;
+		}
 	}
-
-	if (transformationHandle_ != NULL)
+	else
 	{
-		k4a_transformation_destroy((k4a_transformation_t)transformationHandle_);
-		transformationHandle_ = NULL;
+		if (device_ != NULL)
+		{
+			k4a_device_stop_imu(device_);
+
+			if (transformation_ != NULL)
+			{
+				k4a_transformation_destroy(transformation_);
+				transformation_ = NULL;
+			}
+
+			k4a_device_stop_cameras(device_);
+			k4a_device_close(device_);
+			device_ = NULL;
+			config_ = K4A_DEVICE_CONFIG_INIT_DISABLE_ALL;
+		}
 	}
 #endif
 }
@@ -157,17 +171,50 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		uint64_t recording_length = k4a_playback_get_last_timestamp_usec((k4a_playback_t)playbackHandle_);
 		UINFO("Recording is %lld seconds long", recording_length / 1000000);
 
-		if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration_))
+		k4a_calibration_t calibration;
+		if (k4a_playback_get_calibration((k4a_playback_t)playbackHandle_, &calibration))
 		{
 			UERROR("Failed to get calibration");
 			close();
 			return false;
 		}
+		if (ir_)
+		{
+			model_ = CameraModel(
+					calibration.depth_camera_calibration.intrinsics.parameters.param.fx,
+					calibration.depth_camera_calibration.intrinsics.parameters.param.fy,
+					calibration.depth_camera_calibration.intrinsics.parameters.param.cx,
+					calibration.depth_camera_calibration.intrinsics.parameters.param.cy,
+					this->getLocalTransform(),
+					0,
+					cv::Size(calibration.depth_camera_calibration.resolution_width, calibration.depth_camera_calibration.resolution_height));
+		}
+		else
+		{
+			model_ = CameraModel(
+					calibration.color_camera_calibration.intrinsics.parameters.param.fx,
+					calibration.color_camera_calibration.intrinsics.parameters.param.fy,
+					calibration.color_camera_calibration.intrinsics.parameters.param.cx,
+					calibration.color_camera_calibration.intrinsics.parameters.param.cy,
+					this->getLocalTransform(),
+					0,
+					cv::Size(calibration.color_camera_calibration.resolution_width, calibration.color_camera_calibration.resolution_height));
 
+			transformationHandle_ = k4a_transformation_create(&calibration);
+		}
+
+		k4a_record_configuration_t config;
+
+		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
+		{
+			UERROR("Failed to getting recording configuration");
+			close();
+			return false;
+		}
 	}
 	else if (deviceId_ >= 0)
 	{
-		if(deviceHandle_!=NULL)
+		if(device_!=NULL)
 		{
 			this->close();
 		}
@@ -218,7 +265,7 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		UINFO("CameraK4A found %d k4a device(s) attached", device_count);
 
 		// Open the first plugged in Kinect device
-		if (K4A_FAILED(k4a_device_open(deviceId_, &deviceHandle_)))
+		if (K4A_FAILED(k4a_device_open(deviceId_, &device_)))
 		{
 			UERROR("Failed to open k4a device!");
 			return false;
@@ -226,18 +273,18 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 
 		// Get the size of the serial number
 		size_t serial_size = 0;
-		k4a_device_get_serialnum(deviceHandle_, NULL, &serial_size);
+		k4a_device_get_serialnum(device_, NULL, &serial_size);
 
 		// Allocate memory for the serial, then acquire it
 		char *serial = (char*)(malloc(serial_size));
-		k4a_device_get_serialnum(deviceHandle_, serial, &serial_size);
+		k4a_device_get_serialnum(device_, serial, &serial_size);
 		serial_number_.assign(serial, serial_size);
 		free(serial);
 
 		UINFO("Opened K4A device: %s", serial_number_.c_str());
 
 		// Start the camera with the given configuration
-		if (K4A_FAILED(k4a_device_start_cameras(deviceHandle_, &config_)))
+		if (K4A_FAILED(k4a_device_start_cameras(device_, &config_)))
 		{
 			UERROR("Failed to start cameras!");
 			close();
@@ -246,121 +293,59 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 
 		UINFO("K4A camera started successfully");
 
-		if (K4A_FAILED(k4a_device_get_calibration(deviceHandle_, config_.depth_mode, config_.color_resolution, &calibration_)))
+		if (K4A_FAILED(k4a_device_get_calibration(device_, config_.depth_mode, config_.color_resolution, &calibration_)))
 		{
 			UERROR("k4a_device_get_calibration() failed!");
 			close();
 			return false;
 		}
-	}
-	else
-	{
-		UERROR("k4a_device_get_calibration() no file and no valid device id!");
-		return false;
-	}
 
-	if (ir_)
-	{
-		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
-		K.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fx;
-		K.at<double>(1,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.fy;
-		K.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cx;
-		K.at<double>(1,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.cy;
-		cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
-		D.at<double>(0,0) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k1;
-		D.at<double>(0,1) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k2;
-		D.at<double>(0,2) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p1;
-		D.at<double>(0,3) = calibration_.depth_camera_calibration.intrinsics.parameters.param.p2;
-		D.at<double>(0,4) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k3;
-		D.at<double>(0,5) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k4;
-		D.at<double>(0,6) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k5;
-		D.at<double>(0,7) = calibration_.depth_camera_calibration.intrinsics.parameters.param.k6;
-		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
-		cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
-		P.at<double>(0,0) = K.at<double>(0,0);
-		P.at<double>(1,1) = K.at<double>(1,1);
-		P.at<double>(0,2) = K.at<double>(0,2);
-		P.at<double>(1,2) = K.at<double>(1,2);
-		model_ = CameraModel(
-				"k4a_ir",
-				cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height),
-				K,D,R,P,
-				this->getLocalTransform());
-		UASSERT(model_.isValidForRectification());
-		model_.initRectificationMap();
-	}
-	else
-	{
-		cv::Mat K = cv::Mat::eye(3, 3, CV_64FC1);
-		K.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.fx;
-		K.at<double>(1,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.fy;
-		K.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cx;
-		K.at<double>(1,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.cy;
-		cv::Mat D = cv::Mat::eye(1, 8, CV_64FC1);
-		D.at<double>(0,0) = calibration_.color_camera_calibration.intrinsics.parameters.param.k1;
-		D.at<double>(0,1) = calibration_.color_camera_calibration.intrinsics.parameters.param.k2;
-		D.at<double>(0,2) = calibration_.color_camera_calibration.intrinsics.parameters.param.p1;
-		D.at<double>(0,3) = calibration_.color_camera_calibration.intrinsics.parameters.param.p2;
-		D.at<double>(0,4) = calibration_.color_camera_calibration.intrinsics.parameters.param.k3;
-		D.at<double>(0,5) = calibration_.color_camera_calibration.intrinsics.parameters.param.k4;
-		D.at<double>(0,6) = calibration_.color_camera_calibration.intrinsics.parameters.param.k5;
-		D.at<double>(0,7) = calibration_.color_camera_calibration.intrinsics.parameters.param.k6;
-		cv::Mat R = cv::Mat::eye(3, 3, CV_64FC1);
-		cv::Mat P = cv::Mat::eye(3, 4, CV_64FC1);
-		P.at<double>(0,0) = K.at<double>(0,0);
-		P.at<double>(1,1) = K.at<double>(1,1);
-		P.at<double>(0,2) = K.at<double>(0,2);
-		P.at<double>(1,2) = K.at<double>(1,2);
-		model_ = CameraModel(
-				"k4a_color",
-				cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height),
-				K,D,R,P,
-				this->getLocalTransform());
-	}
-
-	if (ULogger::level() <= ULogger::kInfo)
-	{
-		UINFO("K4A calibration:");
-		std::cout << model_ << std::endl;
-	}
-
-	transformationHandle_ = k4a_transformation_create(&calibration_);
-
-	// Get imu transform
-	k4a_calibration_extrinsics_t* imu_extrinsics;
-	if(ir_)
-	{
-		imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
-	}
-	else
-	{
-		imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
-	}
-	imuLocalTransform_ = Transform(
-		imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
-		imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
-		imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
-
-	UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-	UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
-	imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
-	UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
-
-
-	// Start playback or camera
-	if (!fileName_.empty())
-	{
-		k4a_record_configuration_t config;
-		if (k4a_playback_get_record_configuration((k4a_playback_t)playbackHandle_, &config))
+		if (ir_)
 		{
-			UERROR("Failed to getting recording configuration");
-			close();
-			return false;
+			model_ = CameraModel(
+					calibration_.depth_camera_calibration.intrinsics.parameters.param.fx,
+					calibration_.depth_camera_calibration.intrinsics.parameters.param.fy,
+					calibration_.depth_camera_calibration.intrinsics.parameters.param.cx,
+					calibration_.depth_camera_calibration.intrinsics.parameters.param.cy,
+					this->getLocalTransform(),
+					0,
+					cv::Size(calibration_.depth_camera_calibration.resolution_width, calibration_.depth_camera_calibration.resolution_height));
 		}
-	}
-	else
-	{
-		if (K4A_FAILED(k4a_device_start_imu(deviceHandle_)))
+		else
+		{
+			model_ = CameraModel(
+					calibration_.color_camera_calibration.intrinsics.parameters.param.fx,
+					calibration_.color_camera_calibration.intrinsics.parameters.param.fy,
+					calibration_.color_camera_calibration.intrinsics.parameters.param.cx,
+					calibration_.color_camera_calibration.intrinsics.parameters.param.cy,
+					this->getLocalTransform(),
+					0,
+					cv::Size(calibration_.color_camera_calibration.resolution_width, calibration_.color_camera_calibration.resolution_height));
+		}
+
+		transformation_ = k4a_transformation_create(&calibration_);
+
+		// Get imu transform
+		k4a_calibration_extrinsics_t* imu_extrinsics;
+		if(ir_)
+		{
+			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_DEPTH];
+		}
+		else
+		{
+			imu_extrinsics = &calibration_.extrinsics[K4A_CALIBRATION_TYPE_ACCEL][K4A_CALIBRATION_TYPE_COLOR];
+		}
+		imuLocalTransform_ = Transform(
+			imu_extrinsics->rotation[0], imu_extrinsics->rotation[1], imu_extrinsics->rotation[2], imu_extrinsics->translation[0] / 1000.0f,
+			imu_extrinsics->rotation[3], imu_extrinsics->rotation[4], imu_extrinsics->rotation[5], imu_extrinsics->translation[1] / 1000.0f,
+			imu_extrinsics->rotation[6], imu_extrinsics->rotation[7], imu_extrinsics->rotation[8], imu_extrinsics->translation[2] / 1000.0f);
+
+		UINFO("camera to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+		UINFO("base to camera=%s", this->getLocalTransform().prettyPrint().c_str());
+		imuLocalTransform_ = this->getLocalTransform()*imuLocalTransform_;
+		UINFO("base to imu=%s", imuLocalTransform_.prettyPrint().c_str());
+
+		if (K4A_FAILED(k4a_device_start_imu(device_)))
 		{
 			UERROR("Failed to start K4A IMU");
 			close();
@@ -370,9 +355,9 @@ bool CameraK4A::init(const std::string & calibrationFolder, const std::string &
 		UINFO("K4a IMU started successfully");
 
 		// Get an initial capture to put the camera in the right state
-		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE))
+		if (K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE))
 		{
-			k4a_capture_release(captureHandle_);
+			k4a_capture_release(capture_);
 			return true;
 		}
 
@@ -410,23 +395,197 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 
 #ifdef RTABMAP_K4A
 
-	k4a_image_t ir_image_ = NULL;
-	k4a_image_t rgb_image_ = NULL;
-	k4a_imu_sample_t imu_sample_;
-
-	double t = UTimer::now();
-
-	bool captured = false;
-	if(playbackHandle_)
+	if (playbackHandle_ != NULL)
 	{
-		k4a_stream_result_t result = K4A_STREAM_RESULT_FAILED;
-		while((UTimer::now()-t < 0.1) &&
-				(K4A_STREAM_RESULT_SUCCEEDED != (result=k4a_playback_get_next_capture(playbackHandle_, &captureHandle_)) ||
-				((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
+		k4a_capture_t capture = NULL;
+		k4a_stream_result_t result = K4A_STREAM_RESULT_SUCCEEDED;
+
+		// wait to get all frames
+		UTimer time;
+		while (result == K4A_STREAM_RESULT_SUCCEEDED && time.elapsed() < 5.0)
 		{
-			k4a_capture_release(captureHandle_);
-			// the first frame may be null, just retry for 1 second
+			result = k4a_playback_get_next_capture((k4a_playback_t)playbackHandle_, &capture);
+
+			if (result == K4A_STREAM_RESULT_SUCCEEDED)
+			{
+				cv::Mat bgrCV;
+				cv::Mat depthCV;
+				double stamp = 0;
+
+				// Process capture here
+				if (ir_)
+				{
+					k4a_image_t ir = k4a_capture_get_ir_image(capture);
+					if (ir != NULL)
+					{
+						/*UDEBUG("ir res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(ir),
+							k4a_image_get_width_pixels(ir),
+							k4a_image_get_stride_bytes(ir),
+							k4a_image_get_format(ir),
+							double(k4a_image_get_timestamp_usec(ir)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(ir) == K4A_IMAGE_FORMAT_IR16);
+
+						cv::Mat bgrCV16(k4a_image_get_height_pixels(ir), k4a_image_get_width_pixels(ir), CV_16UC1, (void*)k4a_image_get_buffer(ir));
+						bgrCV16.convertTo(bgrCV, CV_8U);
+
+						// Release the image
+						k4a_image_release(ir);
+					}
+				}
+				else
+				{
+					k4a_image_t color = k4a_capture_get_color_image(capture);
+					if (color != NULL)
+					{
+						/*UDEBUG("Color res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(color),
+							k4a_image_get_width_pixels(color),
+							k4a_image_get_stride_bytes(color),
+							k4a_image_get_format(color),
+							double(k4a_image_get_timestamp_usec(color)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG || k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_BGRA32);
+
+						if (k4a_image_get_format(color) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+						{
+							bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(color), CV_8UC1, (void*)k4a_image_get_buffer(color)));
+							//UDEBUG("Uncompressed = %d %d %d", bgrCV.rows, bgrCV.cols, bgrCV.channels());
+						}
+						else
+						{
+							cv::Mat bgra(k4a_image_get_height_pixels(color), k4a_image_get_width_pixels(color), CV_8UC4, (void*)k4a_image_get_buffer(color));
+							cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+						}
+
+						// Release the image
+						k4a_image_release(color);
+					}
+				}
+
+				if (!bgrCV.empty())
+				{
+					k4a_image_t depth = k4a_capture_get_depth_image(capture);
+					if (depth != NULL)
+					{
+						/*UDEBUG("Depth16 res:%4dx%4d stride:%5d format:%d stamp=%f",
+							k4a_image_get_height_pixels(depth),
+							k4a_image_get_width_pixels(depth),
+							k4a_image_get_stride_bytes(depth),
+							k4a_image_get_format(depth),
+							double(k4a_image_get_timestamp_usec(depth)) / 1000000.0);*/
+
+						UASSERT(k4a_image_get_format(depth) == K4A_IMAGE_FORMAT_DEPTH16);
+
+						stamp = ((double)k4a_image_get_timestamp_usec(depth)) / 1000000;
+
+						if (ir_)
+						{
+							depthCV = cv::Mat(k4a_image_get_height_pixels(depth), k4a_image_get_width_pixels(depth), CV_16UC1, (void*)k4a_image_get_buffer(depth)).clone();
+						}
+						else
+						{
+							k4a_image_t transformedDepth;
+							if (k4a_image_create(k4a_image_get_format(depth), bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+							{
+								if (k4a_transformation_depth_image_to_color_camera((k4a_transformation_t)transformationHandle_, depth, transformedDepth) == K4A_RESULT_SUCCEEDED)
+								{
+									depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth), k4a_image_get_width_pixels(transformedDepth), CV_16UC1, (void*)k4a_image_get_buffer(transformedDepth)).clone();
+								}
+								else
+								{
+									UERROR("Failed registration!");
+								}
+								k4a_image_release(transformedDepth);
+							}
+							else
+							{
+								UERROR("Failed allocating depth registered! (%d %d %d)", bgrCV.cols, bgrCV.rows, bgrCV.cols * 2);
+							}
+							
+						}
+
+						// Release the image
+						k4a_image_release(depth);
+					}
+				}
+
+				k4a_capture_release(capture);
+
+				IMU imu;
+				// FIXME: local imu transform missing
+				/*k4a_imu_sample_t imuSample;
+				if (k4a_playback_get_next_imu_sample((k4a_playback_t)playbackHandle_, &imuSample) == K4A_STREAM_RESULT_SUCCEEDED)
+				{
+					// K4A IMU Co-ordinates
+					//  x+ = "backwards"
+					//  y+ = "left"
+					//  z+ = "down"
+					//
+					// ROS Standard co-ordinates:
+					//  x+ = "forward"
+					//  y+ = "left"
+					//  z+ = "up"
+					//
+					// Remap K4A IMU to ROS co-ordinate system:
+					// ROS_X+ = K4A_X-
+					// ROS_Y+ = K4A_Y+
+					// ROS_Z+ = K4A_Z-
+
+					imu = IMU(
+						cv::Vec3d(-1*imuSample.gyro_sample.xyz.x, imuSample.gyro_sample.xyz.y, -1 * imuSample.gyro_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						cv::Vec3d(-1 * imuSample.acc_sample.xyz.x, imuSample.acc_sample.xyz.y, -1 * imuSample.acc_sample.xyz.z),
+						cv::Mat::eye(3, 3, CV_64FC1),
+						Transform::getIdentity());
+				}*/
+
+				if (!bgrCV.empty() && !depthCV.empty())
+				{
+					data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
+					data.setIMU(imu);
+
+					// Frame rate
+					if (this->getImageRate() < 0.0f)
+					{
+						if (stamp == 0)
+						{
+							UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
+						}
+						else if (previousStamp_ > 0)
+						{
+							float ratio = -this->getImageRate();
+							int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
+							if (sleepTime > 10000)
+							{
+								UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
+										sleepTime / 1000, previousStamp_, stamp);
+								sleepTime = 10000;
+							}
+							if (sleepTime > 2)
+							{
+								uSleep(sleepTime - 2);
+							}
+
+							// Add precision at the cost of a small overhead
+							while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
+							{
+								//
+							}
+
+							double slept = timer_.getElapsedTime();
+							timer_.start();
+							UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
+						}
+						previousStamp_ = stamp;
+					}
+
+					break;
+				}
+			}
 		}
+
 		if (result == K4A_STREAM_RESULT_EOF)
 		{
 			// End of file reached
@@ -436,138 +595,112 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 		{
 			UERROR("Failed to read entire recording");
 		}
-		captured = result == K4A_STREAM_RESULT_SUCCEEDED;
 	}
-	else // device
+	else
 	{
+		k4a_image_t ir_image_ = NULL;
+		k4a_image_t rgb_image_ = NULL;
+		k4a_imu_sample_t imu_sample_;
+
+		double t = UTimer::now();
 		k4a_wait_result_t result = K4A_WAIT_RESULT_FAILED;
 		while((UTimer::now()-t < 5.0) &&
-				(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(deviceHandle_, &captureHandle_, K4A_WAIT_INFINITE)) ||
-				((ir_ && (ir_image_=k4a_capture_get_ir_image(captureHandle_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(captureHandle_)) == NULL))))
+				(K4A_WAIT_RESULT_SUCCEEDED != (result=k4a_device_get_capture(device_, &capture_, K4A_WAIT_INFINITE)) ||
+				((ir_ && (ir_image_=k4a_capture_get_ir_image(capture_)) == NULL) || (!ir_ && (rgb_image_=k4a_capture_get_color_image(capture_)) == NULL))))
 		{
-			k4a_capture_release(captureHandle_);
+			k4a_capture_release(capture_);
 			// the first frame may be null, just retry for 5 seconds
 		}
-		captured = result == K4A_WAIT_RESULT_SUCCEEDED;
-	}
 
-	if (captured && (rgb_image_!=NULL || ir_image_!=NULL))
-	{
-		cv::Mat bgrCV;
-		cv::Mat depthCV;
-		IMU imu;
-
-		if (ir_image_ != NULL)
+		if (result == K4A_WAIT_RESULT_SUCCEEDED && (rgb_image_!=NULL || ir_image_!=NULL))
 		{
-			// Convert IR image
-			cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
-					k4a_image_get_width_pixels(ir_image_),
-					CV_16UC1,
-					(void*)k4a_image_get_buffer(ir_image_));
+			cv::Mat bgrCV;
+			cv::Mat depthCV;
+			IMU imu;
 
-			bgrCV16.convertTo(bgrCV, CV_8U);
-
-			bgrCV = model_.rectifyImage(bgrCV);
-
-			// Release the image
-			k4a_image_release(ir_image_);
-		}
-		else
-		{
-			// Convert RGB image
-			if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
+			if (ir_image_ != NULL)
 			{
-				bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
-						CV_8UC1,
-						(void*)k4a_image_get_buffer(rgb_image_)));
+				// Convert IR image
+				cv::Mat bgrCV16(k4a_image_get_height_pixels(ir_image_),
+						k4a_image_get_width_pixels(ir_image_),
+						CV_16UC1,
+						(void*)k4a_image_get_buffer(ir_image_));
+
+				bgrCV16.convertTo(bgrCV, CV_8U);
+
+				// Release the image
+				k4a_image_release(ir_image_);
 			}
 			else
 			{
-				cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
-						k4a_image_get_width_pixels(rgb_image_),
-						CV_8UC4,
-						(void*)k4a_image_get_buffer(rgb_image_));
-
-				cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
-			}
-
-			// Release the image
-			k4a_image_release(rgb_image_);
-		}
-
-		double stamp = UTimer::now();
-		if(!bgrCV.empty())
-		{
-			// Retrieve depth image from capture
-			k4a_image_t depth_image_ = k4a_capture_get_depth_image(captureHandle_);
-
-			if (depth_image_ != NULL)
-			{
-				stamp = ((double)k4a_image_get_timestamp_usec(depth_image_)) / 1000000;
-
-				if (ir_)
+				// Convert RGB image
+				if (k4a_image_get_format(rgb_image_) == K4A_IMAGE_FORMAT_COLOR_MJPG)
 				{
-					depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
-							k4a_image_get_width_pixels(depth_image_),
-							CV_16UC1,
-							(void*)k4a_image_get_buffer(depth_image_));
-
-					depthCV = model_.rectifyDepth(depthCV);
+					bgrCV = uncompressImage(cv::Mat(1, (int)k4a_image_get_size(rgb_image_),
+							CV_8UC1,
+							(void*)k4a_image_get_buffer(rgb_image_)));
 				}
 				else
 				{
-					k4a_image_t transformedDepth = NULL;
-					if (k4a_image_create(k4a_image_get_format(depth_image_),
-							bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
-					{
-						if(k4a_transformation_depth_image_to_color_camera(transformationHandle_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
-						{
-							depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
-									k4a_image_get_width_pixels(transformedDepth),
-									CV_16UC1,
-									(void*)k4a_image_get_buffer(transformedDepth)).clone();
-						}
-						else
-						{
-							UERROR("K4A failed to register depth image");
-						}
+					cv::Mat bgra(k4a_image_get_height_pixels(rgb_image_),
+							k4a_image_get_width_pixels(rgb_image_),
+							CV_8UC4,
+							(void*)k4a_image_get_buffer(rgb_image_));
 
-						k4a_image_release(transformedDepth);
+					cv::cvtColor(bgra, bgrCV, CV_BGRA2BGR);
+				}
+
+				// Release the image
+				k4a_image_release(rgb_image_);
+			}
+
+			if(!bgrCV.empty())
+			{
+				// Retrieve depth image from capture
+				k4a_image_t depth_image_ = k4a_capture_get_depth_image(capture_);
+
+				if (depth_image_ != NULL)
+				{
+					if (ir_)
+					{
+						depthCV = cv::Mat(k4a_image_get_height_pixels(depth_image_),
+								k4a_image_get_width_pixels(depth_image_),
+								CV_16UC1,
+								(void*)k4a_image_get_buffer(depth_image_)).clone();
 					}
 					else
 					{
-						UERROR("K4A failed to allocate registered depth image");
+						k4a_image_t transformedDepth = NULL;
+						if (k4a_image_create(k4a_image_get_format(depth_image_),
+								bgrCV.cols, bgrCV.rows, bgrCV.cols * 2, &transformedDepth) == K4A_RESULT_SUCCEEDED)
+						{
+							if(k4a_transformation_depth_image_to_color_camera(transformation_, depth_image_, transformedDepth) == K4A_RESULT_SUCCEEDED)
+							{
+								depthCV = cv::Mat(k4a_image_get_height_pixels(transformedDepth),
+										k4a_image_get_width_pixels(transformedDepth),
+										CV_16UC1,
+										(void*)k4a_image_get_buffer(transformedDepth)).clone();
+							}
+							else
+							{
+								UERROR("K4A failed to register depth image");
+							}
+
+							k4a_image_release(transformedDepth);
+						}
+						else
+						{
+							UERROR("K4A failed to allocate registered depth image");
+						}
 					}
+					k4a_image_release(depth_image_);
 				}
-				k4a_image_release(depth_image_);
 			}
-		}
 
-		k4a_capture_release(captureHandle_);
+			k4a_capture_release(capture_);
 
-		if(playbackHandle_)
-		{
 			// Get IMU sample, clear buffer
-			// FIXME: not tested, uncomment when tested.
-			k4a_playback_seek_timestamp(playbackHandle_, stamp* 1000000+1, K4A_PLAYBACK_SEEK_BEGIN);
-			if(K4A_STREAM_RESULT_SUCCEEDED == k4a_playback_get_previous_imu_sample(playbackHandle_, &imu_sample_))
-			{
-				double stmp = ((double)imu_sample_.acc_timestamp_usec) / 1000000;
-				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						cv::Vec3d(imu_sample_.acc_sample.xyz.x, imu_sample_.acc_sample.xyz.y, imu_sample_.acc_sample.xyz.z),
-						cv::Mat::eye(3, 3, CV_64FC1),
-						imuLocalTransform_);
-			}
-			else
-			{
-				UWARN("IMU data NULL");
-			}
-		}
-		else
-		{
-			// Get IMU sample, clear buffer
-			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(deviceHandle_, &imu_sample_, 60))
+			if(K4A_WAIT_RESULT_SUCCEEDED == k4a_device_get_imu_sample(device_, &imu_sample_, 60))
 			{
 				imu = IMU(cv::Vec3d(imu_sample_.gyro_sample.xyz.x, imu_sample_.gyro_sample.xyz.y, imu_sample_.gyro_sample.xyz.z),
 						cv::Mat::eye(3, 3, CV_64FC1),
@@ -579,51 +712,13 @@ SensorData CameraK4A::captureImage(CameraInfo * info)
 			{
 				UERROR("IMU data NULL");
 			}
-		}
 
-		// Relay the data to rtabmap
-		if (!bgrCV.empty() && !depthCV.empty())
-		{
-			data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), stamp);
-			if(!imu.empty())
+			// Relay the data to rtabmap
+			if (!bgrCV.empty() && !depthCV.empty())
 			{
+				data = SensorData(bgrCV, depthCV, model_, this->getNextSeqID(), UTimer::now());
 				data.setIMU(imu);
 			}
-
-			// Frame rate
-			if (playbackHandle_ && this->getImageRate() < 0.0f)
-			{
-				if (stamp == 0)
-				{
-					UWARN("The option to use mkv stamps is set (framerate<0), but there are no stamps saved in the file! Aborting...");
-				}
-				else if (previousStamp_ > 0)
-				{
-					float ratio = -this->getImageRate();
-					int sleepTime = 1000.0*(stamp - previousStamp_) / ratio - 1000.0*timer_.getElapsedTime();
-					if (sleepTime > 10000)
-					{
-						UWARN("Detected long delay (%d sec, stamps = %f vs %f). Waiting a maximum of 10 seconds.",
-								sleepTime / 1000, previousStamp_, stamp);
-						sleepTime = 10000;
-					}
-					if (sleepTime > 2)
-					{
-						uSleep(sleepTime - 2);
-					}
-
-					// Add precision at the cost of a small overhead
-					while (timer_.getElapsedTime() < (stamp - previousStamp_) / ratio - 0.000001)
-					{
-						//
-					}
-
-					double slept = timer_.getElapsedTime();
-					timer_.start();
-					UDEBUG("slept=%fs vs target=%fs (ratio=%f)", slept, (stamp - previousStamp_) / ratio, ratio);
-				}
-				previousStamp_ = stamp;
-			}
 		}
 	}
 #else

corelib/src/camera/CameraRealSense2.cpp

@@ -633,14 +633,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UINFO("%s %d %d %d %d %s type=%d", rs2_format_to_string(
+				UINFO("%s %d %d %d %d", rs2_format_to_string(
 						video_profile.format()),
 						video_profile.width(),
 						video_profile.height(),
 						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+						video_profile.stream_index());
 			}
 		}
 		int pi = 0;
@@ -657,7 +655,7 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 					auto intrinsic = video_profile.get_intrinsics();
 
 					// rgb or ir left
-					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8 && video_profile.stream_type() == RS2_STREAM_COLOR) ||
+					if((!ir_ && video_profile.format() == RS2_FORMAT_RGB8) ||
 					  (ir_ && video_profile.format() == RS2_FORMAT_Y8 && video_profile.stream_index() == 1))
 					{
 						if(!profilesPerSensor[i].empty())
@@ -698,35 +696,15 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 				else if(video_profile.format() == RS2_FORMAT_MOTION_XYZ32F || video_profile.format() == RS2_FORMAT_6DOF)
 				{
 					//D435i:
-					//MOTION_XYZ32F 0 0 200 (gyro)
-					//MOTION_XYZ32F 0 0 400 (gyro)
-					//MOTION_XYZ32F 0 0 63 6 (accel)
-					//MOTION_XYZ32F 0 0 250 6 (accel)
+					//MOTION_XYZ32F 0 0 200
+					//MOTION_XYZ32F 0 0 400
+					//MOTION_XYZ32F 0 0 63
+					//MOTION_XYZ32F 0 0 250
 					// or dualMode_ T265:
-					//MOTION_XYZ32F 0 0 200 5 (gyro)
-					//MOTION_XYZ32F 0 0 62 6 (accel)
-					//6DOF 0 0 200 4 (pose)
-					bool modified = false;
-					for (size_t j= 0; j < profilesPerSensor[i].size(); ++j)
-					{
-						if (profilesPerSensor[i][j].stream_type() == profile.stream_type())
-						{
-							if (profile.stream_type() == RS2_STREAM_ACCEL)
-							{
-								if(profile.fps() > profilesPerSensor[i][j].fps())
-									profilesPerSensor[i][j] = profile;
-								modified = true;
-							}
-							else if (profile.stream_type() == RS2_STREAM_GYRO)
-							{
-								if(profile.fps() < profilesPerSensor[i][j].fps())
-									profilesPerSensor[i][j] = profile;
-								modified = true;
-							}
-						}
-					}
-					if(!modified)
-						profilesPerSensor[i].push_back(profile);
+					//MOTION_XYZ32F 0 0 200
+					//MOTION_XYZ32F 0 0 62
+					//6DOF 0 0 200
+					profilesPerSensor[i].push_back(profile);
 					added = true;
 				}
 			}
@@ -777,14 +755,12 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 			for (auto& profile : profiles)
 			{
 				auto video_profile = profile.as<rs2::video_stream_profile>();
-				UERROR("%s %d %d %d %d %s type=%d", rs2_format_to_string(
+				UERROR("%s %d %d %d %d", rs2_format_to_string(
 						video_profile.format()),
 						video_profile.width(),
 						video_profile.height(),
 						video_profile.fps(),
-						video_profile.stream_index(),
-						video_profile.stream_name().c_str(),
-						video_profile.stream_type());
+						video_profile.stream_index());
 			}
 			return false;
 		}
@@ -960,18 +936,6 @@ bool CameraRealSense2::init(const std::string & calibrationFolder, const std::st
 		 if(profilesPerSensor[i].size())
 		 {
 			 UINFO("Starting sensor %d with %d profiles", (int)i, (int)profilesPerSensor[i].size());
-			 for (size_t j = 0; j < profilesPerSensor[i].size(); ++j)
-			 {
-				 auto video_profile = profilesPerSensor[i][j].as<rs2::video_stream_profile>();
-				 UINFO("Opening: %s %d %d %d %d %s type=%d", rs2_format_to_string(
-					 video_profile.format()),
-					 video_profile.width(),
-					 video_profile.height(),
-					 video_profile.fps(),
-					 video_profile.stream_index(),
-					 video_profile.stream_name().c_str(),
-					 video_profile.stream_type());
-			 }
 			 sensors[i].open(profilesPerSensor[i]);
 			 if(sensors[i].is<rs2::depth_sensor>())
 			 {
@@ -1248,13 +1212,13 @@ SensorData CameraRealSense2::captureImage(CameraInfo * info)
 			IMU imu;
 			unsigned int confidence = 0;
 			double imuStamp = stamp*1000.0;
-			Transform pose;
-			getPoseAndIMU(imuStamp, pose, confidence, imu);
+			UASSERT(info!=0);
+			getPoseAndIMU(imuStamp, info->odomPose, confidence, imu);
 
-			if(info && odometryProvided_ && !pose.isNull())
+			if(odometryProvided_ && !info->odomPose.isNull())
 			{
 				// Transform in base frame (local transform should contain base to pose transform)
-				info->odomPose = this->getLocalTransform() * pose * this->getLocalTransform().inverse();
+				info->odomPose = this->getLocalTransform() * info->odomPose * this->getLocalTransform().inverse();
 
 				info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * 0.0001;
 				info->odomCovariance.rowRange(0,3) *= pow(10, 3-(int)confidence);

corelib/src/odometry/OdometryF2F.cpp

@@ -137,7 +137,9 @@ Transform OdometryF2F::computeTransform(
 		{
 			tmpRefFrame = refFrame_;
 			// reset matches, but keep already extracted features in newFrame.sensorData()
-			newFrame.removeAllWords();
+			newFrame.setWords(std::multimap<int, cv::KeyPoint>());
+			newFrame.setWords3(std::multimap<int, cv::Point3f>());
+			newFrame.setWordsDescriptors(std::multimap<int, cv::Mat>());
 			UWARN("Failed to find a transformation with the provided guess (%s), trying again without a guess.", guess.prettyPrint().c_str());
 			// If optical flow is used, switch temporary to feature matching
 			int visCorTypeBackup = Parameters::defaultVisCorType();
@@ -174,18 +176,18 @@ Transform OdometryF2F::computeTransform(
 
 		if(info && this->isInfoDataFilled())
 		{
-			std::list<std::pair<int, std::pair<int, int> > > pairs;
+			std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > > pairs;
 			EpipolarGeometry::findPairsUnique(tmpRefFrame.getWords(), newFrame.getWords(), pairs);
 			info->refCorners.resize(pairs.size());
 			info->newCorners.resize(pairs.size());
 			std::map<int, int> idToIndex;
 			int i=0;
-			for(std::list<std::pair<int, std::pair<int, int> > >::iterator iter=pairs.begin();
+			for(std::list<std::pair<int, std::pair<cv::KeyPoint, cv::KeyPoint> > >::iterator iter=pairs.begin();
 				iter!=pairs.end();
 				++iter)
 			{
-				info->refCorners[i] = tmpRefFrame.getWordsKpts()[iter->second.first].pt;
-				info->newCorners[i] = newFrame.getWordsKpts()[iter->second.second].pt;
+				info->refCorners[i] = iter->second.first.pt;
+				info->newCorners[i] = iter->second.second.pt;
 				idToIndex.insert(std::make_pair(iter->first, i));
 				++i;
 			}
@@ -197,21 +199,12 @@ Transform OdometryF2F::computeTransform(
 			}
 
 			Transform t = this->getPose()*motionSinceLastKeyFrame.inverse();
-			if(!tmpRefFrame.getWords3().empty())
+			for(std::multimap<int, cv::Point3f>::const_iterator iter=tmpRefFrame.getWords3().begin(); iter!=tmpRefFrame.getWords3().end(); ++iter)
 			{
-				for(std::multimap<int, int>::const_iterator iter=tmpRefFrame.getWords().begin(); iter!=tmpRefFrame.getWords().end(); ++iter)
-				{
-					info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(tmpRefFrame.getWords3()[iter->second], t)));
-				}
+				info->localMap.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, t)));
 			}
 			info->localMapSize = tmpRefFrame.getWords3().size();
-			if(!newFrame.getWordsKpts().empty())
-			{
-				for(std::multimap<int, int>::const_iterator iter=newFrame.getWords().begin(); iter!=newFrame.getWords().end(); ++iter)
-				{
-					info->words.insert(std::make_pair(iter->first, newFrame.getWordsKpts()[iter->second]));
-				}
-			}
+			info->words = newFrame.getWords();
 
 			info->localScanMapSize = tmpRefFrame.sensorData().laserScanRaw().size();
 
@@ -239,7 +232,7 @@ Transform OdometryF2F::computeTransform(
 			(registrationPipeline_->isScanRequired() && (scanKeyFrameThr_ == 0.0f || regInfo.icpInliersRatio <= scanKeyFrameThr_)))
 		{
 			UDEBUG("Update key frame");
-			int features = newFrame.getWordsDescriptors().rows;
+			int features = newFrame.getWordsDescriptors().size();
 			if(registrationPipeline_->isImageRequired() && features == 0)
 			{
 				newFrame = Signature(data);
@@ -258,7 +251,9 @@ Transform OdometryF2F::computeTransform(
 			{
 				refFrame_ = newFrame;
 
-				refFrame_.removeAllWords();
+				refFrame_.setWords(std::multimap<int, cv::KeyPoint>());
+				refFrame_.setWords3(std::multimap<int, cv::Point3f>());
+				refFrame_.setWordsDescriptors(std::multimap<int, cv::Mat>());
 
 				//reset motion
 				lastKeyFramePose_.setNull();

corelib/src/odometry/OdometryF2M.cpp

@@ -133,27 +133,6 @@ OdometryF2M::OdometryF2M(const ParametersMap & parameters) :
 	}
 	uInsert(bundleParameters, ParametersPair(Parameters::kVisCorType(), uNumber2Str(corType)));
 
-	int estType = Parameters::defaultVisEstimationType();
-	Parameters::parse(parameters, Parameters::kVisEstimationType(), estType);
-	if(estType > 1)
-	{
-		UWARN("%s=%d is not supported by OdometryF2M, using 2D->3D approach instead (type=1).",
-				Parameters::kVisEstimationType().c_str(),
-				estType);
-		estType = 1;
-	}
-	uInsert(bundleParameters, ParametersPair(Parameters::kVisEstimationType(), uNumber2Str(estType)));
-
-	bool forwardEst = Parameters::defaultVisForwardEstOnly();
-	Parameters::parse(parameters, Parameters::kVisForwardEstOnly(), forwardEst);
-	if(!forwardEst)
-	{
-		UWARN("%s=false is not supported by OdometryF2M, setting to true.",
-				Parameters::kVisForwardEstOnly().c_str());
-		forwardEst = true;
-	}
-	uInsert(bundleParameters, ParametersPair(Parameters::kVisForwardEstOnly(), uBool2Str(forwardEst)));
-
 	regPipeline_ = Registration::create(bundleParameters);
 	if(bundleAdjustment_>0 && regPipeline_->isScanRequired())
 	{
@@ -293,7 +272,9 @@ Transform OdometryF2M::computeTransform(
 			{
 				tmpMap = *map_;
 				// reset matches, but keep already extracted features in lastFrame_->sensorData()
-				lastFrame_->removeAllWords();
+				lastFrame_->setWords(std::multimap<int, cv::KeyPoint>());
+				lastFrame_->setWords3(std::multimap<int, cv::Point3f>());
+				lastFrame_->setWordsDescriptors(std::multimap<int, cv::Mat>());
 
 				points3DMap.clear();
 				bundlePoses.clear();
@@ -412,9 +393,11 @@ Transform OdometryF2M::computeTransform(
 								int wordId =regInfo.inliersIDs[i];
 
 								// 3D point
-								std::multimap<int, int>::const_iterator iter3D = tmpMap.getWords().find(wordId);
-								UASSERT(iter3D!=tmpMap.getWords().end() && !tmpMap.getWords3().empty());
-								points3DMap.insert(std::make_pair(wordId, tmpMap.getWords3()[iter3D->second]));
+								std::multimap<int, cv::Point3f>::const_iterator iter3D = tmpMap.getWords3().find(wordId);
+								UASSERT(iter3D!=tmpMap.getWords3().end());
+								points3DMap.insert(*iter3D);
+
+								std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
 
 								// all other references
 								std::map<int, std::map<int, FeatureBA> >::iterator refIter = bundleWordReferences_.find(wordId);
@@ -444,19 +427,12 @@ Transform OdometryF2M::computeTransform(
 									}
 								}
 
-								std::multimap<int, int>::const_iterator iter2D = lastFrame_->getWords().find(wordId);
 								if(iter2D!=lastFrame_->getWords().end())
 								{
-									UASSERT(!lastFrame_->getWordsKpts().empty());
-									//get depth
-									float d = 0.0f;
-									if( !lastFrame_->getWords3().empty() &&
-										util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
-									{
-										//move back point in camera frame (to get depth along z)
-										d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
-									}
-									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
+									UASSERT(lastFrame_->getWords3().find(wordId) != lastFrame_->getWords3().end());
+									//move back point in camera frame (to get depth along z)
+									cv::Point3f pt3d = util3d::transformPoint(lastFrame_->getWords3().find(wordId)->second, invLocalTransform);
+									references.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, pt3d.z)));
 								}
 								wordReferences.insert(std::make_pair(wordId, references));
 
@@ -581,10 +557,9 @@ Transform OdometryF2M::computeTransform(
 
 				// fields to update
 				LaserScan mapScan = tmpMap.sensorData().laserScanRaw();
-				std::multimap<int, int> mapWords = tmpMap.getWords();
-				std::vector<cv::KeyPoint> mapWordsKpts = tmpMap.getWordsKpts();
-				std::vector<cv::Point3f> mapPoints = tmpMap.getWords3();
-				cv::Mat mapDescriptors = tmpMap.getWordsDescriptors();
+				std::multimap<int, cv::KeyPoint> mapWords = tmpMap.getWords();
+				std::multimap<int, cv::Point3f> mapPoints = tmpMap.getWords3();
+				std::multimap<int, cv::Mat> mapDescriptors = tmpMap.getWordsDescriptors();
 
 				bool addVisualKeyFrame = regPipeline_->isImageRequired() &&
 						 (keyFrameThr_ == 0.0f ||
@@ -615,9 +590,8 @@ Transform OdometryF2M::computeTransform(
 
 					// update local map
 					UASSERT(mapWords.size() == mapPoints.size());
-					UASSERT(mapWords.size() == mapWordsKpts.size());
-					UASSERT((int)mapPoints.size() == mapDescriptors.rows);
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
+					UASSERT(mapPoints.size() == mapDescriptors.size());
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
 
 					std::map<int, int>::iterator iterBundlePosesRef = bundlePoseReferences_.end();
 					if(bundleAdjustment_>0)
@@ -639,15 +613,17 @@ Transform OdometryF2M::computeTransform(
 						// update local map 3D points (if bundle adjustment was done)
 						for(std::map<int, cv::Point3f>::iterator iter=points3DMap.begin(); iter!=points3DMap.end(); ++iter)
 						{
-							UASSERT(mapWords.count(iter->first) == 1);
-							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints[mapWords.find(iter->first)->second].x, mapPoints[mapWords.find(iter->first)->second].y, mapPoints[mapWords.find(iter->first)->second].z, iter->second.x, iter->second.y, iter->second.z);
-							mapPoints[mapWords.find(iter->first)->second] = iter->second;
+							UASSERT(mapPoints.count(iter->first) == 1);
+							//UDEBUG("Updated %d (%f,%f,%f) -> (%f,%f,%f)", iter->first, mapPoints.find(origin)->second.x, mapPoints.find(origin)->second.y, mapPoints.find(origin)->second.z, iter->second.x, iter->second.y, iter->second.z);
+							mapPoints.find(iter->first)->second = iter->second;
 						}
 					}
 
 					// sort by feature response
 					std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
+					std::multimap<int, cv::KeyPoint>::const_iterator iter2D = lastFrame_->getWords().begin();
+					std::multimap<int, cv::Mat>::const_iterator iterDesc = lastFrame_->getWordsDescriptors().begin();
 					UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
 					std::set<int> seenStatusUpdated;
 					Transform invLocalTransform;
@@ -672,11 +648,11 @@ Transform OdometryF2M::computeTransform(
 					if(!visDepthAsMask && validDepthRatio_ < 1.0f)
 					{
 						int ptsWithDepth = 0;
-						for (std::vector<cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+						for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
 							iter != lastFrame_->getWords3().end();
 							++iter)
 						{
-							if(util3d::isFinite(*iter))
+							if(util3d::isFinite(iter->second))
 							{
 								++ptsWithDepth;
 							}
@@ -690,29 +666,27 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
+					for(std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin(); iter!=lastFrame_->getWords3().end(); ++iter, ++iter2D, ++iterDesc)
 					{
-						const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
-						const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
-						if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
+						if(mapPoints.find(iter->first) == mapPoints.end()) // Point not in map
 						{
-							if(util3d::isFinite(pt) || addPointsWithoutDepth)
+							if(util3d::isFinite(iter->second) || addPointsWithoutDepth)
 							{
 								newIds.insert(
-										std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
+										std::make_pair(iter2D->second.response>0?1.0f/iter2D->second.response:0.0f,
 												std::make_pair(iter->first,
-														std::make_pair(kpt,
-																std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
+														std::make_pair(iter2D->second,
+																std::make_pair(iter->second, iterDesc->second)))));
 							}
 						}
 						else if(bundleAdjustment_>0)
 						{
 							if(lastFrame_->getWords().count(iter->first) == 1)
 							{
-								std::multimap<int, int>::iterator iterKpts = mapWords.find(iter->first);
-								if(iterKpts!=mapWords.end() && !mapWordsKpts.empty())
+								std::multimap<int, cv::KeyPoint>::iterator iterKpts = mapWords.find(iter->first);
+								if(iterKpts!=mapWords.end())
 								{
-									mapWordsKpts[iterKpts->second].octave = kpt.octave;
+									iterKpts->second.octave = iter2D->second.octave;
 								}
 
 								UASSERT(iterBundlePosesRef!=bundlePoseReferences_.end());
@@ -720,19 +694,19 @@ Transform OdometryF2M::computeTransform(
 
 								//move back point in camera frame (to get depth along z)
 								float depth = 0.0f;
-								if(util3d::isFinite(pt))
+								if(util3d::isFinite(iter->second))
 								{
-									depth = util3d::transformPoint(pt, invLocalTransform).z;
+									depth = util3d::transformPoint(iter->second, invLocalTransform).z;
 								}
 								if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
 								{
 									std::map<int, FeatureBA> framePt;
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
+									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
 									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 								}
 								else
 								{
-									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
+									bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter2D->second, depth)));
 								}
 							}
 						}
@@ -773,8 +747,7 @@ Transform OdometryF2M::computeTransform(
 								}
 							}
 
-							mapWords.insert(mapWords.end(), std::make_pair(iter->second.first, mapWords.size()));
-							mapWordsKpts.push_back(iter->second.second.first);
+							mapWords.insert(std::make_pair(iter->second.first, iter->second.second.first));
 							cv::Point3f pt = iter->second.second.second.first;
 							if(!util3d::isFinite(pt))
 							{
@@ -810,8 +783,8 @@ Transform OdometryF2M::computeTransform(
 								float scaleInf = (0.05 * model.fx()) / 0.01;
 								pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
 							}
-							mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
-							mapDescriptors.push_back(iter->second.second.second.second);
+							mapPoints.insert(std::make_pair(iter->second.first, util3d::transformPoint(pt, newFramePose)));
+							mapDescriptors.insert(std::make_pair(iter->second.first, iter->second.second.second.second));
 							if(lastFrameOldestNewId_ > iter->second.first)
 							{
 								lastFrameOldestNewId_ = iter->second.first;
@@ -821,7 +794,7 @@ Transform OdometryF2M::computeTransform(
 					}
 
 					// remove words in map if max size is reached
-					if((int)mapWords.size() > maximumMapSize_)
+					if((int)mapPoints.size() > maximumMapSize_)
 					{
 						// remove oldest outliers first
 						std::set<int> inliers(regInfo.inliersIDs.begin(), regInfo.inliersIDs.end());
@@ -840,7 +813,7 @@ Transform OdometryF2M::computeTransform(
 							ids.resize(regInfo.matchesIDs.size()+oi);
 							UDEBUG("projected added=%d/%d minLastFrameId=%d", oi, (int)regInfo.projectedIDs.size(), lastFrameOldestNewId);
 						}
-						for(unsigned int i=0; i<ids.size() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size(); ++i)
+						for(unsigned int i=0; i<ids.size() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size(); ++i)
 						{
 							int id = ids.at(i);
 							if(inliers.find(id) == inliers.end())
@@ -858,14 +831,18 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}
 
+								mapPoints.erase(id);
+								mapDescriptors.erase(id);
 								mapWords.erase(id);
 								++removed;
 							}
 						}
 
 						// remove oldest first
-						for(std::multimap<int, int>::iterator iter = mapWords.begin();
-							iter!=mapWords.end() && (int)mapWords.size() > maximumMapSize_ && mapWords.size() >= newIds.size();)
+						std::multimap<int, cv::Mat>::iterator iterMapDescriptors = mapDescriptors.begin();
+						std::multimap<int, cv::KeyPoint>::iterator iterMapWords = mapWords.begin();
+						for(std::multimap<int, cv::Point3f>::iterator iter = mapPoints.begin();
+							iter!=mapPoints.end() && (int)mapPoints.size() > maximumMapSize_ && mapPoints.size() >= newIds.size();)
 						{
 							if(inliers.find(iter->first) == inliers.end())
 							{
@@ -882,36 +859,19 @@ Transform OdometryF2M::computeTransform(
 									bundleWordReferences_.erase(iterRef);
 								}
 
-								mapWords.erase(iter++);
+								mapPoints.erase(iter++);
+								mapDescriptors.erase(iterMapDescriptors++);
+								mapWords.erase(iterMapWords++);
 								++removed;
 							}
 							else
 							{
 								++iter;
+								++iterMapDescriptors;
+								++iterMapWords;
 							}
 						}
 
-						if(mapWords.size() != mapPoints.size())
-						{
-							UDEBUG("Remove points");
-							std::vector<cv::KeyPoint> mapWordsKptsClean(mapWords.size());
-							std::vector<cv::Point3f> mapPointsClean(mapWords.size());
-							cv::Mat mapDescriptorsClean(mapWords.size(), mapDescriptors.cols, mapDescriptors.type());
-							int index = 0;
-							for(std::multimap<int, int>::iterator iter = mapWords.begin(); iter!=mapWords.end(); ++iter, ++index)
-							{
-								mapWordsKptsClean[index] = mapWordsKpts[iter->second];
-								mapPointsClean[index] = mapPoints[iter->second];
-								mapDescriptors.row(iter->second).copyTo(mapDescriptorsClean.row(index));
-								iter->second = index;
-							}
-							mapWordsKpts = mapWordsKptsClean;
-							mapWordsKptsClean.clear();
-							mapPoints = mapPointsClean;
-							mapPointsClean.clear();
-							mapDescriptors = mapDescriptorsClean;
-						}
-
 						Link * previousLink = 0;
 						for(std::map<int, int>::iterator iter=bundlePoseReferences_.begin(); iter!=bundlePoseReferences_.end();)
 						{
@@ -1139,7 +1099,9 @@ Transform OdometryF2M::computeTransform(
 										newFramePose.translation()));
 					}
 
-					map_->setWords(mapWords, mapWordsKpts, mapPoints, mapDescriptors);
+					map_->setWords(mapWords);
+					map_->setWords3(mapPoints);
+				 	map_->setWordsDescriptors(mapDescriptors);
 				}
 			}
 
@@ -1150,14 +1112,7 @@ Transform OdometryF2M::computeTransform(
 				info->localScanMapSize = tmpMap.sensorData().laserScanRaw().size();
 				if(this->isInfoDataFilled())
 				{
-					info->localMap.clear();
-					if(!tmpMap.getWords3().empty())
-					{
-						for(std::multimap<int, int>::const_iterator iter=tmpMap.getWords().begin(); iter!=tmpMap.getWords().end(); ++iter)
-						{
-							info->localMap.insert(std::make_pair(iter->first, tmpMap.getWords3()[iter->second]));
-						}
-					}
+					info->localMap = uMultimapToMap(tmpMap.getWords3());
 					info->localScanMap = tmpMap.sensorData().laserScanRaw();
 				}
 			}
@@ -1184,12 +1139,11 @@ Transform OdometryF2M::computeTransform(
 			if(regPipeline_->isImageRequired())
 			{
 				int ptsWithDepth = 0;
-				for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
-					iter != lastFrame_->getWords().end();
+				for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+					iter != lastFrame_->getWords3().end();
 					++iter)
 				{
-					if(!lastFrame_->getWords3().empty() && 
-					   util3d::isFinite(lastFrame_->getWords3()[iter->second]))
+					if(util3d::isFinite(iter->second))
 					{
 						++ptsWithDepth;
 					}
@@ -1199,29 +1153,26 @@ Transform OdometryF2M::computeTransform(
 				{
 					frameValid = true;
 					// update local map
-					UASSERT_MSG(lastFrame_->getWordsDescriptors().rows == (int)lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().rows, (int)lastFrame_->getWords3().size()).c_str());
+					UASSERT_MSG(lastFrame_->getWordsDescriptors().size() == lastFrame_->getWords3().size(), uFormat("%d vs %d", lastFrame_->getWordsDescriptors().size(), lastFrame_->getWords3().size()).c_str());
 					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
 
-					std::multimap<int, int> words;
-					std::vector<cv::KeyPoint> wordsKpts;
-					std::vector<cv::Point3f> transformedPoints;
+					std::multimap<int, cv::KeyPoint> words;
+					std::multimap<int, cv::Point3f> transformedPoints;
 					std::multimap<int, int> mapPointWeights;
-					cv::Mat descriptors;
-					if(!lastFrame_->getWords3().empty())
+					std::multimap<int, cv::Mat> descriptors;
+					UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWordsDescriptors().size());
+					std::multimap<int, cv::KeyPoint>::const_iterator wordsIter = lastFrame_->getWords().begin();
+					std::multimap<int, cv::Mat>::const_iterator descIter = lastFrame_->getWordsDescriptors().begin();
+					for (std::multimap<int, cv::Point3f>::const_iterator iter = lastFrame_->getWords3().begin();
+						iter != lastFrame_->getWords3().end();
+						++iter, ++descIter, ++wordsIter)
 					{
-						for (std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin();
-							iter != lastFrame_->getWords().end();
-							++iter)
+						if (util3d::isFinite(iter->second))
 						{
-							const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
-							if (util3d::isFinite(pt))
-							{
-								words.insert(words.end(), std::make_pair(iter->first, words.size()));
-								wordsKpts.push_back(lastFrame_->getWordsKpts()[iter->second]);
-								transformedPoints.push_back(util3d::transformPoint(pt, newFramePose));
-								mapPointWeights.insert(std::make_pair(iter->first, 0));
-								descriptors.push_back(lastFrame_->getWordsDescriptors().row(iter->second));
-							}
+							words.insert(*wordsIter);
+							transformedPoints.insert(std::make_pair(iter->first, util3d::transformPoint(iter->second, newFramePose)));
+							mapPointWeights.insert(std::make_pair(iter->first, 0));
+							descriptors.insert(*descIter);
 						}
 					}
 
@@ -1242,29 +1193,25 @@ Transform OdometryF2M::computeTransform(
 						}
 
 						// update bundleWordReferences_: used for bundle adjustment
-						if(!wordsKpts.empty())
+						for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 						{
-							for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+							if(words.count(iter->first) == 1)
 							{
-								if(words.count(iter->first) == 1)
+								UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
+								std::map<int, FeatureBA> framePt;
+
+								//get depth
+								float d = 0.0f;
+								if(lastFrame_->getWords3().count(iter->first) == 1 &&
+									util3d::isFinite(lastFrame_->getWords3().find(iter->first)->second))
 								{
-									UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
-									std::map<int, FeatureBA> framePt;
-
-									//get depth
-									float d = 0.0f;
-									if(lastFrame_->getWords().count(iter->first) == 1 &&
-									  !lastFrame_->getWords3().empty() &&
-										util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
-									{
-										//move back point in camera frame (to get depth along z)
-										d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
-									}
-
-
-									framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
-									bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
+									//move back point in camera frame (to get depth along z)
+									d = util3d::transformPoint(lastFrame_->getWords3().find(iter->first)->second, invLocalTransform).z;
 								}
+
+
+								framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second, d)));
+								bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
 							}
 						}
 
@@ -1299,7 +1246,9 @@ Transform OdometryF2M::computeTransform(
 						}
 					}
 
-					map_->setWords(words, wordsKpts, transformedPoints, descriptors);
+					map_->setWords(words);
+					map_->setWords3(transformedPoints);
+					map_->setWordsDescriptors(descriptors);
 					addKeyFrame = true;
 				}
 				else
@@ -1322,17 +1271,9 @@ Transform OdometryF2M::computeTransform(
 						Parameters::parse(parameters_, Parameters::kIcpPointToPlaneMinComplexity(), minComplexity);
 						if(p2n && minComplexity>0.0f)
 						{
-							if(lastFrame_->sensorData().laserScanRaw().hasNormals())
+							complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
+							if(complexity > minComplexity)
 							{
-								complexity = util3d::computeNormalsComplexity(*mapCloudNormals, Transform::getIdentity(), lastFrame_->sensorData().laserScanRaw().is2d());
-								if(complexity > minComplexity)
-								{
-									frameValid = true;
-								}
-							}
-							else
-							{
-								UWARN("Input raw scan doesn't have normals, complexity check on first frame is not done.");
 								frameValid = true;
 							}
 						}
@@ -1398,14 +1339,7 @@ Transform OdometryF2M::computeTransform(
 
 				if(this->isInfoDataFilled())
 				{
-					info->localMap.clear();
-					if(!map_->getWords3().empty())
-					{
-						for(std::multimap<int, int>::const_iterator iter=map_->getWords().begin(); iter!=map_->getWords().end(); ++iter)
-						{
-							info->localMap.insert(std::make_pair(iter->first, map_->getWords3()[iter->second]));
-						}
-					}
+					info->localMap = uMultimapToMap(map_->getWords3());
 					info->localScanMap = map_->sensorData().laserScanRaw();
 				}
 			}
@@ -1418,14 +1352,7 @@ Transform OdometryF2M::computeTransform(
 		{
 			if(regPipeline_->isImageRequired())
 			{
-				info->words.clear();
-				if(!lastFrame_->getWordsKpts().empty())
-				{
-					for(std::multimap<int, int>::const_iterator iter=lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
-					{
-						info->words.insert(std::make_pair(iter->first, lastFrame_->getWordsKpts()[iter->second]));
-					}
-				}
+				info->words = lastFrame_->getWords();
 			}
 		}
 	}

corelib/src/odometry/OdometryMono.cpp

@@ -283,15 +283,15 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						newCorners[oi] = imagePoints[i];
 						if(localMap_.count(ids[i]) == 1)
 						{
-							if(prevS->getWords().count(ids[i]) == 1 && !prevS->getWordsKpts().empty())
+							if(prevS->getWords().count(ids[i]) == 1)
 							{
 								// set guess if unique
-								refCorners[oi] = prevS->getWordsKpts()[prevS->getWords().find(ids[i])->second].pt;
+								refCorners[oi] = prevS->getWords().find(ids[i])->second.pt;
 							}
-							if(newS->getWords().count(ids[i]) == 1 && !newS->getWordsKpts().empty())
+							if(newS->getWords().count(ids[i]) == 1)
 							{
 								// set guess if unique
-								newCorners[oi] = newS->getWordsKpts()[newS->getWords().find(ids[i])->second].pt;
+								newCorners[oi] = newS->getWords().find(ids[i])->second.pt;
 							}
 						}
 						objectPointsTmp[oi] = objectPoints[i];
@@ -338,9 +338,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 						if(this->isInfoDataFilled() && info)
 						{
 							cv::KeyPoint kpt;
-							if(newS->getWords().count(matches[i]) == 1 && !newS->getWordsKpts().empty())
+							if(newS->getWords().count(matches[i]) == 1)
 							{
-								kpt = newS->getWordsKpts()[newS->getWords().find(matches[i])->second];
+								kpt = newS->getWords().find(matches[i])->second;
 							}
 							kpt.pt = newCorners[i];
 							info->words.insert(std::make_pair(matches[i], kpt));
@@ -437,9 +437,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
 							{
 								const Signature * s = memory_->getSignature(*iter);
-								for(std::multimap<int, int>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
+								for(std::multimap<int, cv::KeyPoint>::const_iterator jter=s->getWords().begin(); jter!=s->getWords().end(); ++jter)
 								{
-									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end() && !s->getWordsKpts().empty())
+									if(s->getWords().count(jter->first) == 1 && localMap_.find(jter->first)!=localMap_.end())
 									{
 										if(wordReferences.find(jter->first)==wordReferences.end())
 										{
@@ -451,8 +451,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 										{
 											depth = keyFrameWords3D_.at(s->id()).at(jter->first).x;
 										}
-										const cv::KeyPoint & kpts = s->getWordsKpts()[jter->second];
-										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(kpts, depth, cv::Mat())));
+										wordReferences.at(jter->first).insert(std::make_pair(s->id(), FeatureBA(jter->second, depth, cv::Mat())));
 									}
 								}
 							}
@@ -503,21 +502,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 							}
 							else if(float(inliers)/float(imagePoints.size()) < keyFrameThr_)
 							{
-								std::map<int, int> uniqueWordsPrevious = uMultimapToMapUnique(previousS->getWords());
-								std::map<int, int> uniqueWordsNew = uMultimapToMapUnique(newS->getWords());
-								std::map<int, cv::KeyPoint> wordsPrevious;
-								std::map<int, cv::KeyPoint> wordsNew;
-								for(std::map<int, int>::iterator iter=uniqueWordsPrevious.begin(); iter!=uniqueWordsPrevious.end(); ++iter)
-								{
-									wordsPrevious.insert(std::make_pair(iter->first, previousS->getWordsKpts()[iter->second]));
-								}
-								for(std::map<int, int>::iterator iter=uniqueWordsNew.begin(); iter!=uniqueWordsNew.end(); ++iter)
-								{
-									wordsNew.insert(std::make_pair(iter->first, newS->getWordsKpts()[iter->second]));
-								}
 								std::map<int, cv::Point3f> inliers3D = util3d::generateWords3DMono(
-										wordsPrevious,
-										wordsNew,
+										uMultimapToMapUnique(previousS->getWords()),
+										uMultimapToMapUnique(newS->getWords()),
 										cameraModel,
 										cameraTransform,
 										fundMatrixReprojError_,
@@ -639,9 +626,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 		int ii=0;
 		for(std::map<int, cv::Point2f>::iterator iter=firstFrameGuessCorners_.begin(); iter!=firstFrameGuessCorners_.end(); ++iter)
 		{
-			std::multimap<int, int>::const_iterator jter=refS->getWords().find(iter->first);
-			UASSERT(jter != refS->getWords().end() && !refS->getWordsKpts().empty());
-			refCorners[ii] = refS->getWordsKpts()[jter->second].pt;
+			std::multimap<int, cv::KeyPoint>::const_iterator jter=refS->getWords().find(iter->first);
+			UASSERT(jter != refS->getWords().end());
+			refCorners[ii] = jter->second.pt;
 			refCornersGuess[ii] = iter->second;
 			cornerIds[ii] = iter->first;
 			++ii;
@@ -813,15 +800,14 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
 			// generate kpts
 		if(memory_->update(SensorData(data)))
 		{
-			const Signature * s = memory_->getLastWorkingSignature();
-			const std::multimap<int, int> & words = s->getWords();
-			if((int)words.size() > minInliers_ && !s->getWordsKpts().empty())
+			const std::multimap<int, cv::KeyPoint> & words = memory_->getLastWorkingSignature()->getWords();
+			if((int)words.size() > minInliers_)
 			{
-				for(std::multimap<int, int>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
+				for(std::multimap<int, cv::KeyPoint>::const_iterator iter=words.begin(); iter!=words.end(); ++iter)
 				{
 					if(words.count(iter->first) == 1)
 					{
-						firstFrameGuessCorners_.insert(std::make_pair(iter->first, s->getWordsKpts()[iter->second].pt));
+						firstFrameGuessCorners_.insert(std::make_pair(iter->first, iter->second.pt));
 					}
 				}
 			}

corelib/src/odometry/OdometryVINS.cpp

@@ -185,7 +185,7 @@ public:
 	        mBuf.unlock();
 	        TicToc processTime;
 	        processMeasurements();
-	        UDEBUG("VINS process time: %f", processTime.toc());
+	        printf("process time: %f\n", processTime.toc());
 	    }
 
 	}

corelib/src/optimizer/OptimizerG2O.cpp

@@ -1496,11 +1496,6 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 			if(points3DMap.find(id) != points3DMap.end())
 			{
 				cv::Point3f pt3d = points3DMap.at(id);
-				if(!util3d::isFinite(pt3d))
-				{
-					UWARN("Ignoring 3D point %d because it has nan value(s)!", id);
-					continue;
-				}
 				g2o::VertexSBAPointXYZ* vpt3d = new g2o::VertexSBAPointXYZ();
 
 				vpt3d->setEstimate(Eigen::Vector3d(pt3d.x, pt3d.y, pt3d.z));
@@ -1527,7 +1522,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 						const FeatureBA & pt = jter->second;
 						double depth = pt.depth;
 
-						//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
+						//UDEBUG("Added observation pt=%d to cam=%d (%f,%f) depth=%f", vpt3d->id()-stepVertexId, camId, pt.x, pt.y, depth);
 
 						g2o::OptimizableGraph::Edge * e;
 						double baseline = 0.0;
@@ -1573,9 +1568,9 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
 							if(baseline > 0.0)
 							{
 								UDEBUG("Stereo camera model detected but current "
-										"observation (pt=%d to cam=%d, kpt=[%d,%d]) has null depth (%f m), adding "
+										"observation (pt=%d to cam=%d) has null depth (%f m), adding "
 										"mono observation instead.",
-										vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
+										vpt3d->id()-stepVertexId, camId, depth);
 							}
 							// mono edge
 #ifdef RTABMAP_ORB_SLAM2

corelib/src/optimizer/OptimizerTORO.cpp

@@ -146,14 +146,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
-				else if(id1 == id2)
-				{
-					UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
-				}
-				else if(id1 < 0 || id2 < 0)
-				{
-					UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
-				}
+				//else // not supporting pose prior and landmarks
 			}
 		}
 		else
@@ -185,14 +178,7 @@ std::map<int, Transform> OptimizerTORO::optimize(
 						UERROR("Map: Edge already exits between nodes %d and %d, skipping", id1, id2);
 					}
 				}
-				else if(id1 == id2)
-				{
-					UWARN("TORO optimizer doesn't support prior or gravity links, use GTSAM or g2o optimizers (see parameter %s). Link %d ignored...", Parameters::kOptimizerStrategy().c_str(), id1);
-				}
-				else if(id1 < 0 || id2 < 0)
-				{
-					UWARN("TORO optimizer doesn't support landmark links, use GTSAM or g2o optimizers (see parameter %s). Link %d->%d ignored...", Parameters::kOptimizerStrategy().c_str(), id1, id2);
-				}
+				//else // not supporting pose prior and landmarks
 			}
 		}
 		UDEBUG("buildMST... root=%d", rootId);

corelib/src/pcl18/surface/impl/texture_mapping.hpp

@@ -41,7 +41,6 @@
 #include <pcl/common/distances.h>
 #include <pcl18/surface/texture_mapping.h>
 #include <pcl/search/octree.h>
-#include <pcl/common/common.h> // for getAngle3D
 
 ///////////////////////////////////////////////////////////////////////////////////////////////
 template<typename PointInT> std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >

docker/jfr2018/run_euroc_datasets.sh

@@ -29,7 +29,7 @@ then
     STRATEGY=$2
     DISP=$3
 else
-echo "Usage: run_euroc_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 6=okvis(rect) 66=okvis(raw) 8=msckf_vio(rect) 88=msckf_vio(raw) 9=vins_vio(rect) 99=vins_vio(raw) 10=vins_stereo(rect) 100=vins_stereo(raw)\" \"Disparity: 0 or 1\" [sequence]"
+echo "Usage: run_euroc_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 6=okvis(rect) 66=okvis(raw) 8=msckf_vio(rect) 88=msckf_vio(raw)\" \"Disparity: 0 or 1\" [sequence]"
 exit
 fi
 

docker/xenial/android/Dockerfile

@@ -1,4 +1,4 @@
-# Image: introlab3it/rtabmap:android-deps-api19
+# Image: introlab3it/rtabmap:android-deps
 
 FROM ubuntu:16.04
 

docker/xenial/android/tango/Dockerfile

@@ -1,6 +1,6 @@
-# Image: introlab3it/rtabmap:tango-api19
+# Image: introlab3it/rtabmap:tango
 
-FROM introlab3it/rtabmap:android-deps-api19
+FROM introlab3it/rtabmap:android-deps
 
 WORKDIR /root/
 

docker/xenial/android/tango/build.bash

@@ -31,24 +31,38 @@ rm -r lib_tango_support_api
 wget 'https://docs.google.com/uc?authuser=0&id=1s5iPJ7xiridj9Jj--gCy2XiQFniheVm6&export=download' -O TangoSDK_Ikariotikos_Java.jar
 mv TangoSDK_Ikariotikos_Java.jar rtabmap-tango/app/android/libs/.
 
-# Patch to remove not supported arcore and arengine stuff on API19
-git config --global user.email "you@example.com"
-git config --global user.name "Your Name"
-cd rtabmap-tango
-git pull origin tango-api19
+# ARCore
+wget 'https://docs.google.com/uc?authuser=0&id=1A4gMviyxHCnA19MTMbitOWoSOyoZcCef&export=download' -O arcore.zip
+unzip -qq arcore.zip
+rm arcore.zip
+cp arcore/*.jar rtabmap-tango/app/android/libs/.
+rm -r arcore
+
+# AREngine
+wget 'https://docs.google.com/uc?authuser=0&id=1rdaD2Z1QBv-SUeUy0oBmg3C2odfxTHgR&export=download' -O arengine.zip
+unzip -qq arengine.zip
+rm arengine.zip
+cp arengine/*.jar rtabmap-tango/app/android/libs/.
+rm -r arengine
 
 # resource tool
-cd build
+cd rtabmap-tango/build
 cmake -DANDROID_PREBUILD=ON ..
 make
 cd ../..
 
-# rtabmap (do only 32 bits for api19)
+# rtabmap
 mkdir rtabmap-tango/build/armeabi-v7a
 cd rtabmap-tango/build/armeabi-v7a
 cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=armeabi-v7a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/armeabi-v7a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/armeabi-v7a ../..
 make
 
+cd ../../..
+mkdir rtabmap-tango/build/arm64-v8a
+cd rtabmap-tango/build/arm64-v8a
+cmake -DCMAKE_TOOLCHAIN_FILE=../../cmake_modules/android.toolchain.cmake -DANDROID_ABI=arm64-v8a -DBUILD_SHARED_LIBS=OFF -DBUILD_EXAMPLES=OFF -DBUILD_TOOLS=OFF -DCMAKE_BUILD_TYPE=Release -DOpenCV_DIR=$prefix/arm64-v8a/sdk/native/jni -DCMAKE_INSTALL_PREFIX=$prefix/arm64-v8a ../..
+make
+
 # package with binaries of both architectures
 cp -r ../armeabi-v7a/app/android/libs/armeabi-v7a app/android/libs/.
 make

examples/BOWMapping/CMakeLists.txt

@@ -24,7 +24,7 @@ else()
 endif()
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

examples/NoEventsExample/CMakeLists.txt

@@ -32,7 +32,7 @@ else()
 endif()
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

examples/RGBDMapping/CMakeLists.txt

@@ -38,7 +38,7 @@ else()
 endif()
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

examples/WifiMapping/CMakeLists.txt

@@ -32,7 +32,7 @@ else()
 endif()
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

guilib/include/rtabmap/gui/DatabaseViewer.h

@@ -145,7 +145,6 @@ private Q_SLOTS:
 	void updateStereo();
 	void notifyParametersChanged(const QStringList &);
 	void setupMainLayout(bool vertical);
-	void updateConstraintButtons();
 
 private:
 	QString getIniFilePath() const;
@@ -177,6 +176,7 @@ private:
 			bool updateImageSliders = true,
 			const Signature & signatureFrom = Signature(0),
 			const Signature & signatureTo = Signature(0));
+	void updateConstraintButtons();
 	Link findActiveLink(int from, int to);
 	bool containsLink(
 			std::multimap<int, Link> & links,

guilib/include/rtabmap/gui/EditConstraintDialog.h

@@ -42,15 +42,10 @@ class RTABMAPGUI_EXP EditConstraintDialog : public QDialog
 	Q_OBJECT
 
 public:
-	EditConstraintDialog(const Transform & constraint, double linearSigma = 0, double angularSigma = 0, QWidget * parent = 0);
+	EditConstraintDialog(const Transform & constraint, QWidget * parent = 0);
 
 	virtual ~EditConstraintDialog();
 	Transform getTransform() const;
-	double getLinearVariance() const;
-	double getAngularVariance() const;
-
-private Q_SLOTS:
-	void switchUnits();
 
 private:
 	Ui_EditConstraintDialog * _ui;

guilib/include/rtabmap/utilite/UPlot.h

@@ -398,7 +398,6 @@ public:
 	bool isFlat() const {return _flat;}
 	void addItem(UPlotCurve * curve);
 	bool remove(const UPlotCurve * curve);
-	QString getAllCurveDataAsText() const;
 
 private Q_SLOTS:
 	void removeLegendItem(const UPlotCurve * curve);
@@ -511,7 +510,6 @@ public:
 	QStringList curveNames();
 	bool contains(const QString & curveName);
 	void removeCurves();
-	QString getAllCurveDataAsText() const;
 	/**
 	 * Add a threshold to the plot.
 	 */

guilib/src/AboutDialog.cpp

@@ -143,7 +143,6 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_zed->setText(CameraStereoZed::available()?"Yes":"No");
 	_ui->label_k4w2->setText(CameraK4W2::available() ? "Yes" : "No");
 	_ui->label_k4a->setText(CameraK4A::available() ? "Yes" : "No");
-	_ui->label_mynteye->setText(CameraMyntEye::available() ? "Yes" : "No");
 
 	_ui->label_toro->setText(Optimizer::isAvailable(Optimizer::kTypeTORO)?"Yes":"No");
 	_ui->label_toro_license->setEnabled(Optimizer::isAvailable(Optimizer::kTypeTORO)?true:false);

guilib/src/DatabaseViewer.cpp

@@ -360,7 +360,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_odomFrame, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintView()));
 	ui_->checkBox_showOptimized->setEnabled(false);
 	connect(ui_->toolButton_constraint, SIGNAL(clicked(bool)), this, SLOT(editConstraint()));
-	connect(ui_->checkBox_enableForAll, SIGNAL(stateChanged(int)), this, SLOT(updateConstraintButtons()));
 
 	ui_->horizontalSlider_iterations->setTracking(false);
 	ui_->horizontalSlider_iterations->setEnabled(false);
@@ -378,7 +377,6 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
 	connect(ui_->checkBox_ignoreLocalLoopSpace, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreLocalLoopTime, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_ignoreUserLoop, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
-	connect(ui_->checkBox_ignoreLandmarks, SIGNAL(stateChanged(int)), this, SLOT(updateGraphView()));
 	connect(ui_->doubleSpinBox_optimizationScale, SIGNAL(editingFinished()), this, SLOT(updateGraphView()));
 	connect(ui_->checkBox_octomap, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
 	connect(ui_->checkBox_grid_2d, SIGNAL(stateChanged(int)), this, SLOT(updateGrid()));
@@ -709,7 +707,6 @@ void DatabaseViewer::restoreDefaultSettings()
 	ui_->checkBox_ignoreLocalLoopSpace->setChecked(false);
 	ui_->checkBox_ignoreLocalLoopTime->setChecked(false);
 	ui_->checkBox_ignoreUserLoop->setChecked(false);
-	ui_->checkBox_ignoreLandmarks->setChecked(false);
 	ui_->doubleSpinBox_optimizationScale->setValue(1.0);
 	ui_->doubleSpinBox_gainCompensationRadius->setValue(0.0);
 	ui_->doubleSpinBox_voxelSize->setValue(0.0);
@@ -1071,9 +1068,6 @@ bool DatabaseViewer::closeDatabase()
 		ui_->label_constraint_opt->clear();
 		ui_->label_variance->clear();
 		ui_->lineEdit_covariance->clear();
-		ui_->label_type->clear();
-		ui_->label_type_name->clear();
-		ui_->checkBox_showOptimized->setEnabled(false);
 
 		ui_->horizontalSlider_A->setEnabled(false);
 		ui_->horizontalSlider_A->setMaximum(0);
@@ -2044,7 +2038,7 @@ void DatabaseViewer::updateInfo()
 			total+=mem;
 			ui_->textEdit_info->append(tr("Statistics size:\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
 			mem = dbSize - total;
-			ui_->textEdit_info->append(tr("Other (indexing, unused):\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
+			ui_->textEdit_info->append(tr("Other (indexing):\t%1 %2\t%3%").arg(mem>1000000?mem/1000000:mem>1000?mem/1000:mem).arg(mem>1000000?"MB":mem>1000?"KB":"Bytes").arg(dbSize>0?QString::number(double(mem)/double(dbSize)*100.0, 'f', 2 ):"0"));
 			ui_->textEdit_info->append("");
 			std::set<int> idsWithoutBad;
 			dbDriver_->getAllNodeIds(idsWithoutBad, false, true);
@@ -4130,6 +4124,7 @@ void DatabaseViewer::update(int value,
 	labelGravity->clear();
 	labelGps->clear();
 	labelSensors->clear();
+	QRectF rect;
 	if(value >= 0 && value < ids_.size())
 	{
 		view->clear();
@@ -4163,48 +4158,14 @@ void DatabaseViewer::update(int value,
 					imgDepth = depth;
 				}
 
-				QRectF rect;
-				if(!img.isNull())
-				{
-					view->setImage(img);
-					rect = img.rect();
-				}
-				else
-				{
-					ULOGGER_DEBUG("Image is empty");
-				}
-
-				if(!imgDepth.empty())
-				{
-					view->setImageDepth(imgDepth);
-					if(img.isNull())
-					{
-						rect.setWidth(imgDepth.cols);
-						rect.setHeight(imgDepth.rows);
-					}
-				}
-				else
-				{
-					ULOGGER_DEBUG("Image depth is empty");
-				}
-				if(rect.isValid())
-				{
-					view->setSceneRect(rect);
-				}
-
 				std::list<int> ids;
 				ids.push_back(id);
 				std::list<Signature*> signatures;
 				dbDriver_->loadSignatures(ids, signatures);
 
-				if(signatures.size() && signatures.front()!=0 && !signatures.front()->getWordsKpts().empty())
+				if(signatures.size() && signatures.front()!=0 && signatures.front()->getWords().size())
 				{
-					std::multimap<int, cv::KeyPoint> keypoints;
-					for(std::map<int, int>::const_iterator iter=signatures.front()->getWords().begin(); iter!=signatures.front()->getWords().end(); ++iter)
-					{
-						keypoints.insert(std::make_pair(iter->first, signatures.front()->getWordsKpts()[iter->second]));
-					}
-					view->setFeatures(keypoints, data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
+					view->setFeatures(signatures.front()->getWords(), data.depthOrRightRaw().type() == CV_8UC1?cv::Mat():data.depthOrRightRaw(), Qt::yellow);
 				}
 
 				Transform odomPose, g;
@@ -4561,19 +4522,16 @@ void DatabaseViewer::update(int value,
 					}
 
 					//words
-					if(ui_->checkBox_showWords->isChecked() &&
-						!signatures.empty() &&
-						!(*signatures.begin())->getWords3().empty())
+					if(ui_->checkBox_showWords->isChecked() && signatures.size())
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 						cloud->resize((*signatures.begin())->getWords3().size());
 						int i=0;
-						for(std::multimap<int, int>::const_iterator iter=(*signatures.begin())->getWords().begin();
-							iter!=(*signatures.begin())->getWords().end();
+						for(std::multimap<int, cv::Point3f>::const_iterator iter=(*signatures.begin())->getWords3().begin();
+							iter!=(*signatures.begin())->getWords3().end();
 							++iter)
 						{
-							const cv::Point3f & pt = (*signatures.begin())->getWords3()[iter->second];
-							cloud->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+							cloud->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
 						}
 
 						if(cloud->size())
@@ -4792,6 +4750,30 @@ void DatabaseViewer::update(int value,
 				}
 			}
 
+			if(!img.isNull())
+			{
+				view->setImage(img);
+				rect = img.rect();
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image is empty");
+			}
+
+			if(!imgDepth.empty())
+			{
+				view->setImageDepth(imgDepth);
+				if(img.isNull())
+				{
+					rect.setWidth(imgDepth.cols);
+					rect.setHeight(imgDepth.rows);
+				}
+			}
+			else
+			{
+				ULOGGER_DEBUG("Image depth is empty");
+			}
+
 			// loops
 			std::multimap<int, rtabmap::Link> links;
 			dbDriver_->loadLinks(id, links);
@@ -4890,11 +4872,9 @@ void DatabaseViewer::update(int value,
 			constraintsViewer_->removeAllClouds();
 
 			Link link = this->findActiveLink(from, to);
-			bool constraintViewUpdated = false;
 			if(link.isValid() && link.type() != Link::kGravity)
 			{
 				this->updateConstraintView(link, false);
-				constraintViewUpdated = true;
 			}
 			else if(graphes_.size())
 			{
@@ -4909,24 +4889,18 @@ void DatabaseViewer::update(int value,
 					{
 						Link link(from, to, Link::kUndef, fromIter->second.inverse() * toIter->second);
 						this->updateConstraintView(link, false);
-						constraintViewUpdated = true;
 					}
 				}
 			}
-			if(!constraintViewUpdated)
-			{
-				ui_->label_constraint->clear();
-				ui_->label_constraint_opt->clear();
-				ui_->label_variance->clear();
-				ui_->lineEdit_covariance->clear();
-				ui_->label_type->clear();
-				ui_->label_type_name->clear();
-				ui_->checkBox_showOptimized->setEnabled(false);
-			}
 			constraintsViewer_->update();
 
 		}
 	}
+
+	if(rect.isValid())
+	{
+		view->setSceneRect(rect);
+	}
 }
 
 void DatabaseViewer::updateLoggerLevel()
@@ -5277,23 +5251,12 @@ void DatabaseViewer::editConstraint()
 		Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
 		if(link.isValid())
 		{
-			cv::Mat covBefore = link.infMatrix().inv();
-			EditConstraintDialog dialog(link.transform(),
-					covBefore.at<double>(0,0)!=1.0?std::sqrt(covBefore.at<double>(0,0)):0,
-					covBefore.at<double>(5,5)!=1.0?std::sqrt(covBefore.at<double>(5,5)):0);
+			EditConstraintDialog dialog(link.transform());
 			if(dialog.exec() == QDialog::Accepted)
 			{
 				bool updated = false;
-				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
-				if(dialog.getLinearVariance()>0)
-				{
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
-				}
-				if(dialog.getAngularVariance()>0)
-				{
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
-				}
-				Link newLink(link.from(), link.to(), link.type(), dialog.getTransform(), covariance.inv());
+				Link newLink = link;
+				newLink.setTransform(dialog.getTransform());
 				std::multimap<int, Link>::iterator iter = linksRefined_.find(link.from());
 				while(iter != linksRefined_.end() && iter->first == link.from())
 				{
@@ -5314,41 +5277,9 @@ void DatabaseViewer::editConstraint()
 				if(updated)
 				{
 					updateConstraintView();
-					this->updateGraphView();
 				}
 			}
 		}
-		else
-		{
-			EditConstraintDialog dialog(Transform::getIdentity());
-			if(dialog.exec() == QDialog::Accepted)
-			{
-				cv::Mat covariance = cv::Mat::eye(6, 6, CV_64FC1);
-				if(dialog.getLinearVariance()>0)
-				{
-					covariance(cv::Range(0,3), cv::Range(0,3)) *= dialog.getLinearVariance()*dialog.getLinearVariance();
-				}
-				if(dialog.getAngularVariance()>0)
-				{
-					covariance(cv::Range(3,6), cv::Range(3,6)) *= dialog.getAngularVariance()*dialog.getAngularVariance();
-				}
-				int from = ids_.at(ui_->horizontalSlider_A->value());
-				int to = ids_.at(ui_->horizontalSlider_B->value());
-				Link newLink(
-						from,
-						to,
-						Link::kUserClosure,
-						dialog.getTransform(),
-						covariance.inv());
-				if(newLink.from() < newLink.to())
-				{
-					newLink = newLink.inverse();
-				}
-				linksAdded_.insert(std::make_pair(newLink.from(), newLink));
-				updateLoopClosuresSlider(from, to);
-				this->updateGraphView();
-			}
-		}
 	}
 }
 
@@ -5442,7 +5373,7 @@ void DatabaseViewer::updateConstraintView(
 				 link.type()==Link::kLocalSpaceClosure?"Space proximity link":
 				 link.type()==Link::kLocalTimeClosure?"Time proximity link":
 				 link.type()==Link::kUserClosure?"User link":
-				 link.type()==Link::kLandmark?"Landmark "+QString::number(-link.to()):
+				 link.type()==Link::kLandmark?"Landmark link":
 				 link.type()==Link::kVirtualClosure?"Virtual link":
 				 link.type()==Link::kGravity?"Gravity link":"Undefined"));
 	ui_->label_variance->setText(QString("%1, %2")
@@ -5468,7 +5399,7 @@ void DatabaseViewer::updateConstraintView(
 			Transform v2 = topt.rotation()*Transform(1,0,0,0,0,0);
 			float a = pcl::getAngle3D(Eigen::Vector4f(v1.x(), v1.y(), v1.z(), 0), Eigen::Vector4f(v2.x(), v2.y(), v2.z(), 0));
 			a = (a *180.0f) / CV_PI;
-			ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((t.getNorm()>0?diff/t.getNorm():0)*100.0f).arg(a));
+			ui_->label_constraint_opt->setText(QString("%1\n(error=%2% a=%3)").arg(QString(topt.prettyPrint().c_str()).replace(" ", "\n")).arg((diff/t.getNorm())*100.0f).arg(a));
 
 			if(ui_->checkBox_showOptimized->isChecked())
 			{
@@ -5550,7 +5481,7 @@ void DatabaseViewer::updateConstraintView(
 		{
 			dataTo = signatureTo.sensorData();
 		}
-		else if(link.to()>0)
+		else
 		{
 			dbDriver_->getNodeData(link.to(), dataTo);
 		}
@@ -5638,50 +5569,31 @@ void DatabaseViewer::updateConstraintView(
 		{
 			std::list<int> ids;
 			ids.push_back(link.from());
-			if(link.to()>0)
-			{
-				ids.push_back(link.to());
-			}
+			ids.push_back(link.to());
 			std::list<Signature*> signatures;
 			dbDriver_->loadSignatures(ids, signatures);
-			if(signatures.size() == 2 || (link.to()<0 && signatures.size()==1))
+			if(signatures.size() == 2)
 			{
 				const Signature * sFrom = signatureFrom.id()>0?&signatureFrom:signatures.front();
-				const Signature * sTo = 0;
-				if(signatures.size()==2)
-				{
-					sTo = signatureTo.id()>0?&signatureTo:signatures.back();
-					UASSERT(sTo);
-				}
-				UASSERT(sFrom);
+				const Signature * sTo = signatureTo.id()>0?&signatureTo:signatures.back();
+				UASSERT(sFrom && sTo);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudFrom(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTo(new pcl::PointCloud<pcl::PointXYZ>);
 				cloudFrom->resize(sFrom->getWords3().size());
-				if(sTo)
-				{
-					cloudTo->resize(sTo->getWords3().size());
-				}
+				cloudTo->resize(sTo->getWords3().size());
 				int i=0;
-				if(!sFrom->getWords3().empty())
+				for(std::multimap<int, cv::Point3f>::const_iterator iter=sFrom->getWords3().begin();
+					iter!=sFrom->getWords3().end();
+					++iter)
 				{
-					for(std::multimap<int, int>::const_iterator iter=sFrom->getWords().begin();
-						iter!=sFrom->getWords().end();
-						++iter)
-					{
-						const cv::Point3f & pt = sFrom->getWords3()[iter->second];
-						cloudFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
-					}
+					cloudFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
 				}
 				i=0;
-				if(sTo && !sTo->getWords3().empty())
+				for(std::multimap<int, cv::Point3f>::const_iterator iter=sTo->getWords3().begin();
+					iter!=sTo->getWords3().end();
+					++iter)
 				{
-					for(std::multimap<int, int>::const_iterator iter=sTo->getWords().begin();
-						iter!=sTo->getWords().end();
-						++iter)
-					{
-						const cv::Point3f & pt = sTo->getWords3()[iter->second];
-						cloudTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
-					}
+					cloudTo->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
 				}
 
 				if(cloudFrom->size())
@@ -5712,10 +5624,7 @@ void DatabaseViewer::updateConstraintView(
 				}
 				else
 				{
-					if(sTo)
-					{
-						UWARN("Empty 3D words for node %d", link.to());
-					}
+					UWARN("Empty 3D words for node %d", link.to());
 					constraintsViewer_->removeCloud("words1");
 				}
 			}
@@ -5946,12 +5855,7 @@ void DatabaseViewer::updateConstraintButtons()
 
 	int from = ids_.at(ui_->horizontalSlider_A->value());
 	int to = ids_.at(ui_->horizontalSlider_B->value());
-	if(from!=to && from && to &&
-	   odomPoses_.find(from) != odomPoses_.end() &&
-	   odomPoses_.find(to) != odomPoses_.end() &&
-	   (ui_->checkBox_enableForAll->isChecked() ||
-	   (weights_.find(from) != weights_.end() && weights_.at(from)>=0 &&
-	   weights_.find(to) != weights_.end() && weights_.at(to)>=0)))
+	if(from!=to && from && to && odomPoses_.find(from) != odomPoses_.end() && odomPoses_.find(to) != odomPoses_.end())
 	{
 		if((!containsLink(links_, from ,to) && !containsLink(linksAdded_, from ,to)) ||
 			containsLink(linksRemoved_, from ,to))
@@ -6571,11 +6475,6 @@ void DatabaseViewer::updateGraphView()
 			}
 			else if(iter->second.type() == Link::kLandmark)
 			{
-				if(ui_->checkBox_ignoreLandmarks->isChecked())
-				{
-					links.erase(iter++);
-					continue;
-				}
 				UASSERT(iter->second.from() > 0 && iter->second.to() < 0);
 				if(poses.find(iter->second.from()) != poses.end() && poses.find(iter->second.to()) == poses.end())
 				{
@@ -7158,9 +7057,13 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 
 		if(reextractVisualFeatures)
 		{
-			fromS->removeAllWords();
+			fromS->setWords(std::multimap<int, cv::KeyPoint>());
+			fromS->setWords3(std::multimap<int, cv::Point3f>());
+			fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 			fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-			toS->removeAllWords();
+			toS->setWords(std::multimap<int, cv::KeyPoint>());
+			toS->setWords3(std::multimap<int, cv::Point3f>());
+			toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 			toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 		}
 
@@ -7272,33 +7175,17 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 			if(toS && fromS->id() > 0 && toS->id() > 0)
 			{
 				updateLoopClosuresSlider(fromS->id(), toS->id());
-				std::multimap<int, cv::KeyPoint> keypointsFrom;
-				std::multimap<int, cv::KeyPoint> keypointsTo;
-				if(!fromS->getWordsKpts().empty())
-				{
-					for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
-					{
-						keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
-					}
-				}
-				if(!toS->getWordsKpts().empty())
-				{
-					for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
-					{
-						keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
-					}
-				}
 				if(newLink.type() != Link::kNeighbor && fromS->id() < toS->id())
 				{
 					this->updateConstraintView(newLink.inverse(), true, *toS, *fromS);
-					ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
-					ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
+					ui_->graphicsView_A->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
+					ui_->graphicsView_B->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
 				}
 				else
 				{
 					this->updateConstraintView(newLink, true, *fromS, *toS);
-					ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
-					ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
+					ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+					ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
 				}
 
 				updateWordsMatching(info.inliersIDs);
@@ -7314,32 +7201,8 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent)
 		if(toS && fromS->id() > 0 && toS->id() > 0)
 		{
 			// just update matches in the views
-			std::multimap<int, cv::KeyPoint> keypointsFrom;
-			std::multimap<int, cv::KeyPoint> keypointsTo;
-			if(!fromS->getWordsKpts().empty())
-			{
-				for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
-				{
-					keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
-				}
-			}
-			if(!toS->getWordsKpts().empty())
-			{
-				for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
-				{
-					keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
-				}
-			}
-			if(currentLink.type() != Link::kNeighbor && fromS->id() < toS->id())
-			{
-				ui_->graphicsView_A->setFeatures(keypointsTo, toS->sensorData().depthRaw());
-				ui_->graphicsView_B->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
-			}
-			else
-			{
-				ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
-				ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
-			}
+			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
 			updateWordsMatching(info.inliersIDs);
 		}
 
@@ -7426,9 +7289,13 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 			toS->sensorData().uncompressData();
 			if(reextractVisualFeatures)
 			{
-				fromS->removeAllWords();
+				fromS->setWords(std::multimap<int, cv::KeyPoint>());
+				fromS->setWords3(std::multimap<int, cv::Point3f>());
+				fromS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 				fromS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-				toS->removeAllWords();
+				toS->setWords(std::multimap<int, cv::KeyPoint>());
+				toS->setWords3(std::multimap<int, cv::Point3f>());
+				toS->setWordsDescriptors(std::multimap<int, cv::Mat>());
 				toS->sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 			}
 		}
@@ -7443,7 +7310,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 		}
 
 		Transform guess;
-		bool guessFromGraphRejected = false;
 		if(!reg->isImageRequired())
 		{
 			// make a fake guess using globally optimized poses
@@ -7473,10 +7339,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 							{
 								guess = fromIter->second.inverse() * toIter->second;
 							}
-							else
-							{
-								guessFromGraphRejected = true;
-							}
 						}
 						else
 						{
@@ -7485,7 +7347,7 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 					}
 				}
 			}
-			if(guess.isNull() && !silent && !guessFromGraphRejected)
+			if(guess.isNull() && !silent)
 			{
 				if(QMessageBox::question(this,
 						tr("Add constraint without guess"),
@@ -7501,10 +7363,6 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				{
 					guess.setIdentity();
 				}
-				else
-				{
-					guessFromGraphRejected = true;
-				}
 			}
 		}
 
@@ -7528,18 +7386,11 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 
 			newLink = Link(from, to, Link::kUserClosure, t, information);
 		}
-		else if(!silent && !guessFromGraphRejected)
+		else if(!silent)
 		{
-			QMessageBox::StandardButton button = QMessageBox::warning(this,
+			QMessageBox::warning(this,
 					tr("Add link"),
-					tr("Cannot find a transformation between nodes %1 and %2: %3\n\nDo you want to add it manually?").arg(from).arg(to).arg(info.rejectedMsg.c_str()),
-					QMessageBox::Yes | QMessageBox::No,
-					QMessageBox::No);
-			if(button == QMessageBox::Yes)
-			{
-				editConstraint();
-				silent = true;
-			}
+					tr("Cannot find a transformation between nodes %1 and %2: %3").arg(from).arg(to).arg(info.rejectedMsg.c_str()));
 		}
 	}
 	else if(containsLink(linksRemoved_, from, to))
@@ -7697,24 +7548,8 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
 				this->updateConstraintView(newLink, false, *fromS, *toS);
 			}
 
-			std::multimap<int, cv::KeyPoint> keypointsFrom;
-			std::multimap<int, cv::KeyPoint> keypointsTo;
-			if(!fromS->getWordsKpts().empty())
-			{
-				for(std::map<int, int>::const_iterator iter=fromS->getWords().begin(); iter!=fromS->getWords().end(); ++iter)
-				{
-					keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, fromS->getWordsKpts()[iter->second]));
-				}
-			}
-			if(!toS->getWordsKpts().empty())
-			{
-				for(std::map<int, int>::const_iterator iter=toS->getWords().begin(); iter!=toS->getWords().end(); ++iter)
-				{
-					keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, toS->getWordsKpts()[iter->second]));
-				}
-			}
-			ui_->graphicsView_A->setFeatures(keypointsFrom, fromS->sensorData().depthRaw());
-			ui_->graphicsView_B->setFeatures(keypointsTo, toS->sensorData().depthRaw());
+			ui_->graphicsView_A->setFeatures(fromS->getWords(), fromS->sensorData().depthRaw());
+			ui_->graphicsView_B->setFeatures(toS->getWords(), toS->sensorData().depthRaw());
 			updateWordsMatching(info.inliersIDs);
 		}
 		else if(updateConstraints)

guilib/src/EditConstraintDialog.cpp

@@ -30,7 +30,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double linearSigma, double angularSigma, QWidget * parent) :
+EditConstraintDialog::EditConstraintDialog(const Transform & constraint, QWidget * parent) :
 	QDialog(parent)
 {
 	_ui = new Ui_EditConstraintDialog();
@@ -44,11 +44,6 @@ EditConstraintDialog::EditConstraintDialog(const Transform & constraint, double
 	_ui->roll->setValue(roll);
 	_ui->pitch->setValue(pitch);
 	_ui->yaw->setValue(yaw);
-	_ui->linear_sigma->setValue(linearSigma);
-	_ui->angular_sigma->setValue(angularSigma);
-
-	connect(_ui->checkBox_radians, SIGNAL(stateChanged(int)), this, SLOT(switchUnits()));
-	_ui->checkBox_radians->setChecked(false);
 }
 
 EditConstraintDialog::~EditConstraintDialog()
@@ -56,67 +51,9 @@ EditConstraintDialog::~EditConstraintDialog()
 	delete _ui;
 }
 
-void EditConstraintDialog::switchUnits()
-{
-	double conversion = 180.0/M_PI;
-	if(_ui->checkBox_radians->isChecked())
-	{
-		conversion = M_PI/180.0;
-	}
-	QVector<QDoubleSpinBox*> boxes;
-	boxes.push_back(_ui->roll);
-	boxes.push_back(_ui->pitch);
-	boxes.push_back(_ui->yaw);
-	boxes.push_back(_ui->angular_sigma);
-	for(int i=0; i<boxes.size(); ++i)
-	{
-		double value = boxes[i]->value()*conversion;
-		if(_ui->checkBox_radians->isChecked())
-		{
-			if(boxes[i]!=_ui->angular_sigma)
-			{
-				boxes[i]->setMinimum(-M_PI);
-			}
-			boxes[i]->setMaximum(M_PI);
-			boxes[i]->setSuffix(" rad");
-			boxes[i]->setSingleStep(0.01);
-		}
-		else
-		{
-			if(boxes[i]!=_ui->angular_sigma)
-			{
-				boxes[i]->setMinimum(-180);
-			}
-			boxes[i]->setMaximum(180);
-			boxes[i]->setSuffix(" deg");
-			boxes[i]->setSingleStep(1);
-		}
-		boxes[i]->setValue(value);
-	}
-}
-
 Transform EditConstraintDialog::getTransform() const
 {
-	double conversion = 1.0f;
-	if(!_ui->checkBox_radians->isChecked())
-	{
-		conversion = M_PI/180.0;
-	}
-	return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value()*conversion, _ui->pitch->value()*conversion, _ui->yaw->value()*conversion);
-}
-
-double EditConstraintDialog::getLinearVariance() const
-{
-	return _ui->linear_sigma->value();
-}
-double EditConstraintDialog::getAngularVariance() const
-{
-	double conversion = 1.0f;
-	if(!_ui->checkBox_radians->isChecked())
-	{
-		conversion = M_PI/180.0;
-	}
-	return _ui->angular_sigma->value()*conversion;
+	return Transform(_ui->x->value(), _ui->y->value(), _ui->z->value(), _ui->roll->value(), _ui->pitch->value(), _ui->yaw->value());
 }
 
 }

guilib/src/GraphViewer.cpp

@@ -37,8 +37,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QtGui/QDesktopServices>
 #include <QtGui/QContextMenuEvent>
 #include <QColorDialog>
-#include <QPrinter>
-#include <QFileDialog>
 #ifdef QT_SVG_LIB
 #include <QtSvg/QSvgGenerator>
 #endif
@@ -56,11 +54,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/ULogger.h>
 #include <rtabmap/utilite/UTimer.h>
 
-#include <QtGlobal>
-#if QT_VERSION >= 0x050000
-	#include <QStandardPaths>
-#endif
-
 namespace rtabmap {
 
 class NodeItem: public QGraphicsEllipseItem
@@ -1547,7 +1540,11 @@ QIcon createIcon(const QColor & color)
 void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 {
 	QMenu menu;
-	QAction * aScreenShot = menu.addAction(tr("Take a screenshot..."));
+	QAction * aScreenShotPNG = menu.addAction(tr("Take a screenshot (PNG)"));
+	QAction * aScreenShotSVG = menu.addAction(tr("Take a screenshot (SVG)"));
+#ifndef QT_SVG_LIB
+	aScreenShotSVG->setEnabled(false);
+#endif
 	menu.addSeparator();
 
 	QAction * aChangeNodeColor = menu.addAction(createIcon(_nodeColor), tr("Set node color..."));
@@ -1713,103 +1710,85 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
 	QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
 
 	QAction * r = menu.exec(event->globalPos());
-	if(r == aScreenShot)
+	if(r == aScreenShotPNG || r == aScreenShotSVG)
 	{
 		if(_root)
 		{
-			QString filePath;
-#if QT_VERSION >= 0x050000
-			filePath = QStandardPaths::writableLocation(QStandardPaths::PicturesLocation);
-#endif
+			QString targetDir = _workingDirectory + "/ScreensCaptured";
 			QDir dir;
-			if(!dir.exists(filePath))
+			if(!dir.exists(targetDir))
 			{
-				filePath = QDir::homePath();
+				dir.mkdir(targetDir);
 			}
-			filePath += "/graph.png";
-
-#ifdef QT_SVG_LIB
-			filePath = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), filePath, "*.png *.xpm *.jpg *.pdf *.svg");
-#else
-			filePath = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), filePath, "*.png *.xpm *.jpg *.pdf");
-#endif
-			if(!filePath.isEmpty())
+			targetDir += "/";
+			targetDir += "Graph_view";
+			if(!dir.exists(targetDir))
 			{
-				if(QFileInfo(filePath).suffix() == "")
-				{
-					//use png by default
-					filePath += ".png";
-				}
+				dir.mkdir(targetDir);
+			}
+			targetDir += "/";
+			bool isPNG = r == aScreenShotPNG;
+			QString name = (QDateTime::currentDateTime().toString("yyMMddhhmmsszzz") + (isPNG?".png":".svg"));
 
-				if(_gridCellSize)
+			if(_gridCellSize)
+			{
+				_root->setScale(1.0f/(_gridCellSize*100.0f)); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
+			}
+			else
+			{
+				_root->setScale(this->transform().m11()); // current view
+			}
+
+			this->scene()->clearSelection();                                  // Selections would also render to the file
+			this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+			QSize sceneSize = this->scene()->sceneRect().size().toSize();
+
+			if(isPNG)
+			{
+				QImage image(sceneSize, QImage::Format_ARGB32);  // Create the image with the exact size of the shrunk scene
+				image.fill(Qt::transparent);                     // Start all pixels transparent
+				QPainter painter(&image);
+
+				this->scene()->render(&painter);
+				if(!image.isNull())
 				{
-					_root->setScale(1.0f/(_gridCellSize*100.0f)); // grid map precision (for 5cm grid cell, x20 to have 1pix/5cm)
+					image.save(targetDir + name);
 				}
 				else
 				{
-					_root->setScale(this->transform().m11()); // current view
+					QMessageBox::warning(this,
+							tr("Save PNG"),
+							tr("Could not export in PNG (the scene may be too large %1x%2), try saving in SVG.").arg(sceneSize.width()).arg(sceneSize.height()));
 				}
-
-				this->scene()->clearSelection();                                  // Selections would also render to the file
-				this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-				QSize sceneSize = this->scene()->sceneRect().size().toSize();
-
-				if(QFileInfo(filePath).suffix().compare("pdf") == 0)
-				{
-					QPrinter printer(QPrinter::HighResolution);
-					printer.setOrientation(QPrinter::Portrait);
-					printer.setOutputFileName( filePath );
-					QPainter p(&printer);
-					scene()->render(&p);
-					p.end();
-				}
-				else if(QFileInfo(filePath).suffix().compare("svg") == 0)
-				{
-#ifdef QT_SVG_LIB
-					QSvgGenerator svgGen;
-
-					svgGen.setFileName( filePath );
-					svgGen.setSize(sceneSize);
-					// add 1% border to make sure values are not cropped
-					int borderH = sceneSize.width()/100;
-					int borderV = sceneSize.height()/100;
-					svgGen.setViewBox(QRect(-borderH, -borderV, sceneSize.width()+borderH*2, sceneSize.height()+borderV*2));
-					svgGen.setTitle(tr("RTAB-Map graph"));
-					svgGen.setDescription(tr("RTAB-Map map and graph"));
-
-					QPainter painter( &svgGen );
-
-					this->scene()->render(&painter);
-#else
-					UERROR("RTAB-MAp is not built with Qt's SVG library, cannot save picture in svg format.");
-#endif
-				}
-				else
-				{
-					QImage image(sceneSize, QImage::Format_ARGB32);  // Create the image with the exact size of the shrunk scene
-					image.fill(Qt::transparent);                     // Start all pixels transparent
-					QPainter painter(&image);
-
-					this->scene()->render(&painter);
-					if(!image.isNull())
-					{
-						image.save(filePath);
-					}
-					else
-					{
-						QMessageBox::warning(this,
-								tr("Save PNG"),
-								tr("Could not export in PNG (the scene may be too large %1x%2), try saving in SVG.").arg(sceneSize.width()).arg(sceneSize.height()));
-					}
-				}
-
-				//reset scale
-				_root->setScale(1.0f);
-				this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
-
-
-				QDesktopServices::openUrl(QUrl::fromLocalFile(filePath));
 			}
+			else
+			{
+#ifdef QT_SVG_LIB
+				QSvgGenerator svgGen;
+
+				svgGen.setFileName( targetDir + name );
+				svgGen.setSize(sceneSize);
+				// add 1% border to make sure values are not cropped
+				int borderH = sceneSize.width()/100;
+				int borderV = sceneSize.height()/100;
+				svgGen.setViewBox(QRect(-borderH, -borderV, sceneSize.width()+borderH*2, sceneSize.height()+borderV*2));
+				svgGen.setTitle(tr("RTAB-Map graph"));
+				svgGen.setDescription(tr("RTAB-Map map and graph"));
+
+				QPainter painter( &svgGen );
+
+				this->scene()->render(&painter);
+#else
+				UERROR("RTAB-MAp is not built with Qt's SVG library, cannot save picture in svg format.");
+#endif
+			}
+
+			//reset scale
+			_root->setScale(1.0f);
+			this->scene()->setSceneRect(this->scene()->itemsBoundingRect());  // Re-shrink the scene to it's bounding contents
+
+
+			QDesktopServices::openUrl(QUrl::fromLocalFile(targetDir + name));
 		}
 		return; // without emitting configChanged
 	}

guilib/src/ImageView.cpp

@@ -37,17 +37,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <QInputDialog>
 #include <QVBoxLayout>
 #include <QColorDialog>
-#include <QPrinter>
 #include <QGraphicsRectItem>
 #include "rtabmap/utilite/ULogger.h"
 #include "rtabmap/gui/KeypointItem.h"
 #include "rtabmap/core/util2d.h"
 
-#include <QtGlobal>
-#if QT_VERSION >= 0x050000
-	#include <QStandardPaths>
-#endif
-
 namespace rtabmap {
 
 //LineItem
@@ -164,6 +158,7 @@ QIcon ImageView::createIcon(const QColor & color)
 
 ImageView::ImageView(QWidget * parent) :
 		QWidget(parent),
+		_savedFileName((QDir::homePath()+ "/") + "picture" + ".png"),
 		_alpha(100),
 		_featuresSize(0.0f),
 		_defaultBgColor(Qt::black),
@@ -173,19 +168,6 @@ ImageView::ImageView(QWidget * parent) :
 		_imageItem(0),
 		_imageDepthItem(0)
 {
-#if QT_VERSION >= 0x050000
-	_savedFileName = QStandardPaths::writableLocation(QStandardPaths::PicturesLocation);
-#endif
-	QDir dir;
-	if(!dir.exists(_savedFileName))
-	{
-		_savedFileName = QDir::homePath()+ "/picture.png";
-	}
-	else
-	{
-		_savedFileName += "/picture.png";
-	}
-
 	_graphicsView = new QGraphicsView(this);
 	_graphicsView->setTransformationAnchor(QGraphicsView::AnchorUnderMouse);
 	_graphicsView->setScene(new QGraphicsScene(this));
@@ -824,16 +806,11 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 		if(!_graphicsView->scene()->sceneRect().isNull())
 		{
 			QString text;
-			QString extensions = "*.png *.xpm *.jpg";
-			if(_graphicsView->isVisible())
-			{
-				extensions += " *.pdf";
-			}
 #ifdef QT_SVG_LIB
-			extensions += " *.svg";
+			text = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), _savedFileName, "*.png *.xpm *.jpg *.pdf *.svg");
+#else
+			text = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), _savedFileName, "*.png *.xpm *.jpg *.pdf");
 #endif
-			text = QFileDialog::getSaveFileName(this, tr("Save figure to ..."), _savedFileName, extensions);
-
 			if(!text.isEmpty())
 			{
 				if(QFileInfo(text).suffix() == "")
@@ -843,34 +820,17 @@ void ImageView::contextMenuEvent(QContextMenuEvent * e)
 				}
 
 				_savedFileName = text;
-				if(QFileInfo(text).suffix().compare("pdf") == 0)
+				QImage img(_graphicsView->sceneRect().width(), _graphicsView->sceneRect().height(), QImage::Format_ARGB32_Premultiplied);
+				QPainter p(&img);
+				if(_graphicsView->isVisible())
 				{
-					QPrinter printer(QPrinter::HighResolution);
-					printer.setOrientation(QPrinter::Portrait);
-					printer.setOutputFileName( text );
-					QPainter p(&printer);
-					p.begin(&printer);
-					double xscale = printer.pageRect().width()/double(_graphicsView->sceneRect().width());
-					double yscale = printer.pageRect().height()/double(_graphicsView->sceneRect().height());
-					double scale = qMin(xscale, yscale);
-					p.scale(scale, scale);
 					_graphicsView->scene()->render(&p, _graphicsView->sceneRect(), _graphicsView->sceneRect());
-					p.end();
 				}
 				else
 				{
-					QImage img(_graphicsView->sceneRect().width(), _graphicsView->sceneRect().height(), QImage::Format_ARGB32_Premultiplied);
-					QPainter p(&img);
-					if(_graphicsView->isVisible())
-					{
-						_graphicsView->scene()->render(&p, _graphicsView->sceneRect(), _graphicsView->sceneRect());
-					}
-					else
-					{
-						this->render(&p, QPoint(), _graphicsView->sceneRect().toRect());
-					}
-					img.save(text);
+					this->render(&p, QPoint(), _graphicsView->sceneRect().toRect());
 				}
+				img.save(text);
 			}
 		}
 	}

guilib/src/MainWindow.cpp

@@ -1715,14 +1715,11 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 					_cachedSignatures.insert(signature.id(), signature);
 					_cachedMemoryUsage += signature.sensorData().getMemoryUsed();
 					unsigned int count = 0;
-					if(!signature.getWords3().empty())
+					for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().upper_bound(-1); jter!=signature.getWords3().end(); ++jter)
 					{
-						for(std::multimap<int, int>::const_iterator jter=signature.getWords().upper_bound(-1); jter!=signature.getWords().end(); ++jter)
+						if(util3d::isFinite(jter->second))
 						{
-							if(util3d::isFinite(signature.getWords3()[jter->second]))
-							{
-								++count;
-							}
+							++count;
 						}
 					}
 					_cachedWordsCount.insert(std::make_pair(signature.id(), (float)count));
@@ -1955,17 +1952,7 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
 			UDEBUG("time= %d ms", time.restart());
 
 			// do it after scaling
-			std::multimap<int, cv::KeyPoint> wordsA;
-			std::multimap<int, cv::KeyPoint> wordsB;
-			for(std::map<int, int>::const_iterator iter=signature.getWords().begin(); iter!=signature.getWords().end(); ++iter)
-			{
-				wordsA.insert(wordsA.end(), std::make_pair(iter->first, signature.getWordsKpts()[iter->second]));
-			}
-			for(std::map<int, int>::const_iterator iter=loopSignature.getWords().begin(); iter!=loopSignature.getWords().end(); ++iter)
-			{
-				wordsB.insert(wordsB.end(), std::make_pair(iter->first, loopSignature.getWordsKpts()[iter->second]));
-			}
-			this->drawKeypoints(wordsA, wordsB);
+			this->drawKeypoints(signature.getWords(), loopSignature.getWords());
 
 			UDEBUG("time= %d ms", time.restart());
 
@@ -2331,12 +2318,12 @@ void MainWindow::updateMapCloud(
 	int maxNodes = uStr2Int(_preferencesDialog->getParameter(Parameters::kGridGlobalMaxNodes()));
 	if(maxNodes > 0 && poses.size()>1)
 	{
-		std::map<int, float> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
+		std::vector<int> nodes = graph::findNearestNodes(poses, poses.rbegin()->second, maxNodes);
 		std::map<int, Transform> nearestPoses;
 		nearestPoses.insert(*poses.rbegin());
-		for(std::map<int, float>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
+		for(std::vector<int>::iterator iter=nodes.begin(); iter!=nodes.end(); ++iter)
 		{
-			std::map<int, Transform>::iterator pter = poses.find(iter->first);
+			std::map<int, Transform>::iterator pter = poses.find(*iter);
 			if(pter != poses.end())
 			{
 				nearestPoses.insert(*pter);
@@ -3719,57 +3706,40 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
 		cloud->resize(iter->getWords3().size());
 		int oi=0;
 		UASSERT(iter->getWords().size() == iter->getWords3().size());
+		std::multimap<int, cv::KeyPoint>::const_iterator kter=iter->getWords().begin();
 		float maxDepth = _preferencesDialog->getCloudMaxDepth(0);
 		UDEBUG("rgb.channels()=%d");
-		if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
+		for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->getWords3().begin();
+				jter!=iter->getWords3().end(); ++jter, ++kter)
 		{
-			Transform invLocalTransform = Transform::getIdentity();
-			if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
+			if(util3d::isFinite(jter->second) && (maxDepth == 0.0f || jter->second.z < maxDepth))
 			{
-				invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
-			}
-			else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
-			{
-				invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().inverse();
-			}
-
-			for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
-			{
-				const cv::Point3f & pt = iter->getWords3()[jter->second];
-				if(util3d::isFinite(pt) &&
-					(maxDepth == 0.0f ||
-							//move back point in camera frame (to get depth along z), ignore for multi-camera
-							(iter.value().sensorData().cameraModels().size()<=1 &&
-							 util3d::transformPoint(pt, invLocalTransform).z < maxDepth)))
+				(*cloud)[oi].x = jter->second.x;
+				(*cloud)[oi].y = jter->second.y;
+				(*cloud)[oi].z = jter->second.z;
+				int u = kter->second.pt.x+0.5;
+				int v = kter->second.pt.y+0.5;
+				if(!rgb.empty() &&
+					uIsInBounds(u, 0, rgb.cols-1) &&
+					uIsInBounds(v, 0, rgb.rows-1))
 				{
-					(*cloud)[oi].x = pt.x;
-					(*cloud)[oi].y = pt.y;
-					(*cloud)[oi].z = pt.z;
-					const cv::KeyPoint & kpt = iter->getWordsKpts()[jter->second];
-					int u = kpt.pt.x+0.5;
-					int v = kpt.pt.y+0.5;
-					if(!rgb.empty() &&
-						uIsInBounds(u, 0, rgb.cols-1) &&
-						uIsInBounds(v, 0, rgb.rows-1))
+					if(rgb.channels() == 1)
 					{
-						if(rgb.channels() == 1)
-						{
-							(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
-						}
-						else
-						{
-							cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
-							(*cloud)[oi].b = bgr.val[0];
-							(*cloud)[oi].g = bgr.val[1];
-							(*cloud)[oi].r = bgr.val[2];
-						}
+						(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = rgb.at<unsigned char>(v, u);
 					}
 					else
 					{
-						(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
+						cv::Vec3b bgr = rgb.at<cv::Vec3b>(v, u);
+						(*cloud)[oi].b = bgr.val[0];
+						(*cloud)[oi].g = bgr.val[1];
+						(*cloud)[oi].r = bgr.val[2];
 					}
-					++oi;
 				}
+				else
+				{
+					(*cloud)[oi].r = (*cloud)[oi].g = (*cloud)[oi].b = 255;
+				}
+				++oi;
 			}
 		}
 		cloud->resize(oi);
@@ -4092,14 +4062,11 @@ void MainWindow::processRtabmapEvent3DMap(const rtabmap::RtabmapEvent3DMap & eve
 				_cachedSignatures.insert(iter->first, iter->second);
 				_cachedMemoryUsage += iter->second.sensorData().getMemoryUsed();
 				unsigned int count = 0;
-				if(!iter->second.getWords3().empty())
+				for(std::multimap<int, cv::Point3f>::const_iterator jter=iter->second.getWords3().upper_bound(-1); jter!=iter->second.getWords3().end(); ++jter)
 				{
-					for(std::multimap<int, int>::const_iterator jter=iter->second.getWords().upper_bound(-1); jter!=iter->second.getWords().end(); ++jter)
+					if(util3d::isFinite(jter->second))
 					{
-						if(util3d::isFinite(iter->second.getWords3()[jter->second]))
-						{
-							++count;
-						}
+						++count;
 					}
 				}
 				_cachedWordsCount.insert(std::make_pair(iter->first, (float)count));
@@ -5907,9 +5874,13 @@ void MainWindow::postProcessing()
 										}
 										else
 										{
-											signatureFrom.removeAllWords();
+											signatureFrom.setWords(std::multimap<int, cv::KeyPoint>());
+											signatureFrom.setWords3(std::multimap<int, cv::Point3f>());
+											signatureFrom.setWordsDescriptors(std::multimap<int, cv::Mat>());
 											signatureFrom.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
-											signatureTo.removeAllWords();
+											signatureTo.setWords(std::multimap<int, cv::KeyPoint>());
+											signatureTo.setWords3(std::multimap<int, cv::Point3f>());
+											signatureTo.setWordsDescriptors(std::multimap<int, cv::Mat>());
 											signatureTo.sensorData().setFeatures(std::vector<cv::KeyPoint>(), std::vector<cv::Point3f>(), cv::Mat());
 										}
 									}

guilib/src/PreferencesDialog.cpp

@@ -194,9 +194,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 //SURF
 #ifndef RTABMAP_NONFREE
 	_ui->comboBox_detector_strategy->setItemData(0, 0, Qt::UserRole - 1);
-	_ui->comboBox_detector_strategy->setItemData(12, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(0, 0, Qt::UserRole - 1);
-	_ui->vis_feature_detector->setItemData(12, 0, Qt::UserRole - 1);
 #endif
 
 // SIFT
@@ -212,12 +210,10 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_detector_strategy->setItemData(4, 0, Qt::UserRole - 1);
 	_ui->comboBox_detector_strategy->setItemData(5, 0, Qt::UserRole - 1);
 	_ui->comboBox_detector_strategy->setItemData(6, 0, Qt::UserRole - 1);
-	_ui->comboBox_detector_strategy->setItemData(12, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(3, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(4, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(5, 0, Qt::UserRole - 1);
 	_ui->vis_feature_detector->setItemData(6, 0, Qt::UserRole - 1);
-	_ui->vis_feature_detector->setItemData(12, 0, Qt::UserRole - 1);
 #endif
 
 #ifndef RTABMAP_ORB_OCTREE
@@ -754,8 +750,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	connect(_ui->toolButton_openni2OniPath, SIGNAL(clicked()), this, SLOT(selectSourceOni2Path()));
 	connect(_ui->comboBox_k4a_rgb_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->comboBox_k4a_framerate, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
-	connect(_ui->checkbox_k4a_irDepth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
+	connect(_ui->comboBox_k4a_depth_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
 	connect(_ui->toolButton_k4a_mkv, SIGNAL(clicked()), this, SLOT(selectSourceMKVPath()));
 	connect(_ui->toolButton_source_distortionModel, SIGNAL(clicked()), this, SLOT(selectSourceDistortionModel()));
 	connect(_ui->toolButton_distortionModel, SIGNAL(clicked()), this, SLOT(visualizeDistortionModel()));
@@ -882,7 +877,6 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
 	_ui->comboBox_dictionary_strategy->setObjectName(Parameters::kKpNNStrategy().c_str());
 	_ui->checkBox_dictionary_incremental->setObjectName(Parameters::kKpIncrementalDictionary().c_str());
 	_ui->checkBox_kp_incrementalFlann->setObjectName(Parameters::kKpIncrementalFlann().c_str());
-	_ui->checkBox_kp_byteToFloat->setObjectName(Parameters::kKpByteToFloat().c_str());
 	_ui->surf_doubleSpinBox_rebalancingFactor->setObjectName(Parameters::kKpFlannRebalancingFactor().c_str());
 	_ui->comboBox_detector_strategy->setObjectName(Parameters::kKpDetectorStrategy().c_str());
 	_ui->surf_doubleSpinBox_nndrRatio->setObjectName(Parameters::kKpNndrRatio().c_str());
@@ -2967,7 +2961,7 @@ bool PreferencesDialog::validateForm()
 #ifndef RTABMAP_NONFREE
 	// verify that SURF/SIFT cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
-	if(_ui->comboBox_detector_strategy->currentIndex() <= 1 || _ui->comboBox_detector_strategy->currentIndex() == 12)
+	if(_ui->comboBox_detector_strategy->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
@@ -2975,7 +2969,7 @@ bool PreferencesDialog::validateForm()
 		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureOrb);
 	}
 	// BOW Reextract features type
-	if(_ui->vis_feature_detector->currentIndex() <= 1 || _ui->vis_feature_detector->currentIndex() == 12)
+	if(_ui->vis_feature_detector->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Selected feature type (SURF/SIFT) is not available. RTAB-Map is not built "
@@ -2988,7 +2982,7 @@ bool PreferencesDialog::validateForm()
 #ifndef RTABMAP_NONFREE
 	// verify that SURF cannot be selected if not built with OpenCV nonfree module
 	// BOW dictionary type
-	if(_ui->comboBox_detector_strategy->currentIndex() < 1 || _ui->comboBox_detector_strategy->currentIndex() == 12)
+	if(_ui->comboBox_detector_strategy->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Selected feature type (SURF) is not available. RTAB-Map is not built "
@@ -2996,7 +2990,7 @@ bool PreferencesDialog::validateForm()
 		_ui->comboBox_detector_strategy->setCurrentIndex(Feature2D::kFeatureSift);
 	}
 	// BOW Reextract features type
-	if(_ui->vis_feature_detector->currentIndex() < 1 || _ui->vis_feature_detector->currentIndex() == 12)
+	if(_ui->vis_feature_detector->currentIndex() <= 1)
 	{
 		QMessageBox::warning(this, tr("Parameter warning"),
 				tr("Selected feature type (SURF) is not available. RTAB-Map is not built "
@@ -4179,7 +4173,7 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 			else
 			{
 #ifndef RTABMAP_NONFREE
-				if(valueInt == 0 &&
+				if(valueInt <= 1 &&
 						(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
 						 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
 				{
@@ -4189,18 +4183,6 @@ void PreferencesDialog::setParameter(const std::string & key, const std::string
 						  combo->currentText().toStdString().c_str());
 					ok = false;
 				}
-#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 4 && CV_MINOR_VERSION <= 3) || (CV_MAJOR_VERSION == 3 && (CV_MINOR_VERSION < 4 || (CV_MINOR_VERSION==4 && CV_SUBMINOR_VERSION<11)))
-				if(valueInt == 1 &&
-						(combo->objectName().toStdString().compare(Parameters::kKpDetectorStrategy()) == 0 ||
-						 combo->objectName().toStdString().compare(Parameters::kVisFeatureType()) == 0))
-				{
-					UWARN("Trying to set \"%s\" to SIFT but RTAB-Map isn't built "
-						  "with the nonfree module from OpenCV. Keeping default combo value: %s.",
-						  combo->objectName().toStdString().c_str(),
-						  combo->currentText().toStdString().c_str());
-					ok = false;
-				}
-#endif
 #endif
 #ifndef RTABMAP_ORB_SLAM2
 				if(!Optimizer::isAvailable(Optimizer::kTypeG2O))

guilib/src/ui/DatabaseViewer.ui

@@ -61,7 +61,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>414</width>
+           <width>296</width>
            <height>311</height>
           </rect>
          </property>
@@ -304,7 +304,7 @@
           <rect>
            <x>0</x>
            <y>0</y>
-           <width>413</width>
+           <width>296</width>
            <height>311</height>
           </rect>
          </property>
@@ -826,7 +826,7 @@
          </property>
          <property name="sizeHint" stdset="0">
           <size>
-           <width>0</width>
+           <width>40</width>
            <height>20</height>
           </size>
          </property>
@@ -1024,16 +1024,6 @@
          </property>
         </widget>
        </item>
-       <item>
-        <widget class="QCheckBox" name="checkBox_enableForAll">
-         <property name="toolTip">
-          <string>Enable &quot;Add&quot; for all nodes (even not in graph)</string>
-         </property>
-         <property name="text">
-          <string/>
-         </property>
-        </widget>
-       </item>
        <item>
         <widget class="QPushButton" name="pushButton_reject">
          <property name="text">
@@ -1048,7 +1038,7 @@
          </property>
          <property name="sizeHint" stdset="0">
           <size>
-           <width>0</width>
+           <width>40</width>
            <height>20</height>
           </size>
          </property>
@@ -1379,7 +1369,7 @@
      <item>
       <widget class="QToolBox" name="toolBox">
        <property name="currentIndex">
-        <number>0</number>
+        <number>1</number>
        </property>
        <widget class="QWidget" name="page_3">
         <property name="geometry">
@@ -1387,7 +1377,7 @@
           <x>0</x>
           <y>0</y>
           <width>318</width>
-          <height>226</height>
+          <height>165</height>
          </rect>
         </property>
         <attribute name="label">
@@ -1455,7 +1445,7 @@
            </property>
           </widget>
          </item>
-         <item row="7" column="0">
+         <item row="6" column="0">
           <spacer name="verticalSpacer_3">
            <property name="orientation">
             <enum>Qt::Vertical</enum>
@@ -1518,23 +1508,6 @@
            </property>
           </widget>
          </item>
-         <item row="6" column="2">
-          <widget class="QLabel" name="label_58">
-           <property name="text">
-            <string>Ignore landmarks</string>
-           </property>
-          </widget>
-         </item>
-         <item row="6" column="0">
-          <widget class="QCheckBox" name="checkBox_ignoreLandmarks">
-           <property name="text">
-            <string/>
-           </property>
-           <property name="checked">
-            <bool>false</bool>
-           </property>
-          </widget>
-         </item>
         </layout>
        </widget>
        <widget class="QWidget" name="page_4">
@@ -2096,8 +2069,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>312</width>
-          <height>226</height>
+          <width>226</width>
+          <height>160</height>
          </rect>
         </property>
         <attribute name="label">
@@ -2224,8 +2197,8 @@
          <rect>
           <x>0</x>
           <y>0</y>
-          <width>446</width>
-          <height>226</height>
+          <width>185</width>
+          <height>485</height>
          </rect>
         </property>
         <attribute name="label">

guilib/src/ui/aboutDialog.ui

@@ -162,9 +162,9 @@ p, li { white-space: pre-wrap; }
            <property name="geometry">
             <rect>
              <x>0</x>
-             <y>-225</y>
+             <y>0</y>
              <width>596</width>
-             <height>866</height>
+             <height>843</height>
             </rect>
            </property>
            <layout class="QVBoxLayout" name="verticalLayout_2">
@@ -195,7 +195,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="0">
+              <item row="29" column="0">
                <widget class="QLabel" name="label_25">
                 <property name="text">
                  <string>With FOVIS :</string>
@@ -205,7 +205,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="2">
+              <item row="29" column="2">
                <widget class="QLabel" name="label_fovis_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -251,7 +251,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="0">
+              <item row="33" column="0">
                <widget class="QLabel" name="label_35">
                 <property name="text">
                  <string>With OKVIS :</string>
@@ -261,7 +261,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="1">
+              <item row="31" column="1">
                <widget class="QLabel" name="label_dvo">
                 <property name="text">
                  <string/>
@@ -307,7 +307,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="1">
+              <item row="33" column="1">
                <widget class="QLabel" name="label_okvis">
                 <property name="text">
                  <string/>
@@ -320,7 +320,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="36" column="0">
+              <item row="35" column="0">
                <widget class="QLabel" name="label_34">
                 <property name="text">
                  <string>With MSCKF :</string>
@@ -330,7 +330,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="1">
+              <item row="25" column="1">
                <widget class="QLabel" name="label_octomap">
                 <property name="text">
                  <string/>
@@ -343,7 +343,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="36" column="1">
+              <item row="35" column="1">
                <widget class="QLabel" name="label_msckf">
                 <property name="text">
                  <string/>
@@ -399,7 +399,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="0">
+              <item row="26" column="0">
                <widget class="QLabel" name="label_24">
                 <property name="text">
                  <string>With CPU-TSDF :</string>
@@ -419,7 +419,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="0">
+              <item row="30" column="0">
                <widget class="QLabel" name="label_26">
                 <property name="text">
                  <string>With Viso2 :</string>
@@ -429,7 +429,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="0">
+              <item row="32" column="0">
                <widget class="QLabel" name="label_28">
                 <property name="text">
                  <string>With ORB SLAM 2 :</string>
@@ -449,7 +449,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="1">
+              <item row="32" column="1">
                <widget class="QLabel" name="label_orbslam2">
                 <property name="text">
                  <string/>
@@ -472,7 +472,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="2">
+              <item row="30" column="2">
                <widget class="QLabel" name="label_viso2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -492,7 +492,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="0">
+              <item row="25" column="0">
                <widget class="QLabel" name="label_20">
                 <property name="text">
                  <string>With Octomap :</string>
@@ -502,7 +502,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="0">
+              <item row="19" column="0">
                <widget class="QLabel" name="label_77">
                 <property name="text">
                  <string>With TORO :</string>
@@ -512,7 +512,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="30" column="1">
+              <item row="29" column="1">
                <widget class="QLabel" name="label_fovis">
                 <property name="text">
                  <string/>
@@ -525,7 +525,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="26" column="2">
+              <item row="25" column="2">
                <widget class="QLabel" name="label_octomap_license">
                 <property name="text">
                  <string>BSD</string>
@@ -578,7 +578,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="1">
+              <item row="26" column="1">
                <widget class="QLabel" name="label_cputsdf">
                 <property name="text">
                  <string/>
@@ -601,7 +601,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="0">
+              <item row="22" column="0">
                <widget class="QLabel" name="label_18">
                 <property name="text">
                  <string>With cvsba :</string>
@@ -611,7 +611,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="0">
+              <item row="20" column="0">
                <widget class="QLabel" name="label_14">
                 <property name="text">
                  <string>With g2o :</string>
@@ -621,7 +621,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="1">
+              <item row="22" column="1">
                <widget class="QLabel" name="label_cvsba">
                 <property name="text">
                  <string/>
@@ -644,7 +644,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="1">
+              <item row="21" column="1">
                <widget class="QLabel" name="label_gtsam">
                 <property name="text">
                  <string/>
@@ -670,7 +670,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="0">
+              <item row="21" column="0">
                <widget class="QLabel" name="label_19">
                 <property name="text">
                  <string>With GTSAM :</string>
@@ -736,7 +736,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="0">
+              <item row="24" column="0">
                <widget class="QLabel" name="label_29">
                 <property name="text">
                  <string>With libpointmatcher :</string>
@@ -746,7 +746,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="1">
+              <item row="20" column="1">
                <widget class="QLabel" name="label_g2o">
                 <property name="text">
                  <string/>
@@ -795,7 +795,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="36" column="2">
+              <item row="35" column="2">
                <widget class="QLabel" name="label_msckf_license">
                 <property name="text">
                  <string>Penn Software License</string>
@@ -818,7 +818,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="2">
+              <item row="31" column="2">
                <widget class="QLabel" name="label_dvo_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -828,7 +828,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="35" column="0">
+              <item row="34" column="0">
                <widget class="QLabel" name="label_36">
                 <property name="text">
                  <string>With loam_velodyne :</string>
@@ -861,7 +861,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="1">
+              <item row="27" column="1">
                <widget class="QLabel" name="label_openchisel">
                 <property name="text">
                  <string/>
@@ -874,7 +874,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="22" column="2">
+              <item row="21" column="2">
                <widget class="QLabel" name="label_gtsam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -884,7 +884,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="1">
+              <item row="24" column="1">
                <widget class="QLabel" name="label_libpointmatcher">
                 <property name="text">
                  <string/>
@@ -897,7 +897,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="35" column="1">
+              <item row="34" column="1">
                <widget class="QLabel" name="label_loam">
                 <property name="text">
                  <string/>
@@ -920,7 +920,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="32" column="0">
+              <item row="31" column="0">
                <widget class="QLabel" name="label_27">
                 <property name="text">
                  <string>With DVO :</string>
@@ -930,7 +930,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="31" column="1">
+              <item row="30" column="1">
                <widget class="QLabel" name="label_viso2">
                 <property name="text">
                  <string/>
@@ -943,7 +943,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="33" column="2">
+              <item row="32" column="2">
                <widget class="QLabel" name="label_orbslam2_license">
                 <property name="text">
                  <string>GPLv3</string>
@@ -953,7 +953,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="28" column="0">
+              <item row="27" column="0">
                <widget class="QLabel" name="label_30">
                 <property name="text">
                  <string>With OpenChisel :</string>
@@ -976,7 +976,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="1">
+              <item row="19" column="1">
                <widget class="QLabel" name="label_toro">
                 <property name="text">
                  <string/>
@@ -989,7 +989,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="21" column="2">
+              <item row="20" column="2">
                <widget class="QLabel" name="label_g2o_license">
                 <property name="text">
                  <string>BSD</string>
@@ -999,7 +999,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="34" column="2">
+              <item row="33" column="2">
                <widget class="QLabel" name="label_okvis_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1009,7 +1009,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="35" column="2">
+              <item row="34" column="2">
                <widget class="QLabel" name="label_loam_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1039,7 +1039,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="25" column="2">
+              <item row="24" column="2">
                <widget class="QLabel" name="label_libpointmatcher_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1049,7 +1049,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="27" column="2">
+              <item row="26" column="2">
                <widget class="QLabel" name="label_cputsdf_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1069,7 +1069,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="23" column="2">
+              <item row="22" column="2">
                <widget class="QLabel" name="label_cvsba_license">
                 <property name="text">
                  <string>GPLv2</string>
@@ -1079,7 +1079,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="20" column="2">
+              <item row="19" column="2">
                <widget class="QLabel" name="label_toro_license">
                 <property name="text">
                  <string>Creative Commons [Attribution-NonCommercial-ShareAlike]</string>
@@ -1191,7 +1191,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="0">
+              <item row="23" column="0">
                <widget class="QLabel" name="label_40">
                 <property name="text">
                  <string>With Ceres :</string>
@@ -1201,7 +1201,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="2">
+              <item row="23" column="2">
                <widget class="QLabel" name="label_ceres_license">
                 <property name="text">
                  <string>BSD</string>
@@ -1211,7 +1211,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="24" column="1">
+              <item row="23" column="1">
                <widget class="QLabel" name="label_ceres">
                 <property name="text">
                  <string/>
@@ -1224,7 +1224,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="0">
+              <item row="28" column="0">
                <widget class="QLabel" name="label_41">
                 <property name="text">
                  <string>With AliceVision :</string>
@@ -1234,7 +1234,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="1">
+              <item row="28" column="1">
                <widget class="QLabel" name="label_aliceVision">
                 <property name="text">
                  <string/>
@@ -1247,7 +1247,7 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="29" column="2">
+              <item row="28" column="2">
                <widget class="QLabel" name="label_aliceVision_license">
                 <property name="text">
                  <string>MPL2</string>
@@ -1323,29 +1323,6 @@ p, li { white-space: pre-wrap; }
                 </property>
                </widget>
               </item>
-              <item row="19" column="0">
-               <widget class="QLabel" name="label_69">
-                <property name="text">
-                 <string>With MYNTEYE S :</string>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
-              <item row="19" column="1">
-               <widget class="QLabel" name="label_mynteye">
-                <property name="text">
-                 <string/>
-                </property>
-                <property name="alignment">
-                 <set>Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter</set>
-                </property>
-                <property name="wordWrap">
-                 <bool>true</bool>
-                </property>
-               </widget>
-              </item>
              </layout>
             </item>
            </layout>

guilib/src/ui/editConstraintDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>430</width>
-    <height>231</height>
+    <width>347</width>
+    <height>147</height>
    </rect>
   </property>
   <property name="windowTitle">
@@ -15,7 +15,7 @@
   </property>
   <layout class="QVBoxLayout" name="verticalLayout">
    <item>
-    <layout class="QGridLayout" name="gridLayout" columnstretch="0,0,0,0">
+    <layout class="QGridLayout" name="gridLayout" columnstretch="0,1,0,1">
      <item row="0" column="0">
       <widget class="QLabel" name="label">
        <property name="text">
@@ -172,74 +172,6 @@
        </property>
       </widget>
      </item>
-     <item row="3" column="1">
-      <widget class="QDoubleSpinBox" name="linear_sigma">
-       <property name="suffix">
-        <string> m</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>0.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>9999.000000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-       <property name="value">
-        <double>0.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="3" column="3">
-      <widget class="QDoubleSpinBox" name="angular_sigma">
-       <property name="suffix">
-        <string> rad</string>
-       </property>
-       <property name="decimals">
-        <number>6</number>
-       </property>
-       <property name="minimum">
-        <double>0.000000000000000</double>
-       </property>
-       <property name="maximum">
-        <double>3.150000000000000</double>
-       </property>
-       <property name="singleStep">
-        <double>0.001000000000000</double>
-       </property>
-       <property name="value">
-        <double>0.000000000000000</double>
-       </property>
-      </widget>
-     </item>
-     <item row="3" column="0">
-      <widget class="QLabel" name="label_7">
-       <property name="text">
-        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Linear &amp;sigma; &lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-       </property>
-      </widget>
-     </item>
-     <item row="3" column="2">
-      <widget class="QLabel" name="label_8">
-       <property name="text">
-        <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Angular σ&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-       </property>
-      </widget>
-     </item>
-     <item row="4" column="3">
-      <widget class="QCheckBox" name="checkBox_radians">
-       <property name="text">
-        <string>Radians</string>
-       </property>
-       <property name="checked">
-        <bool>true</bool>
-       </property>
-      </widget>
-     </item>
     </layout>
    </item>
    <item>
@@ -255,13 +187,6 @@
      </property>
     </spacer>
    </item>
-   <item>
-    <widget class="QLabel" name="label_9">
-     <property name="text">
-      <string>&lt;html&gt;&lt;head/&gt;&lt;body&gt;&lt;p&gt;Setting &amp;sigma; to 0 will set identity covariance.&lt;/p&gt;&lt;/body&gt;&lt;/html&gt;</string>
-     </property>
-    </widget>
-   </item>
    <item>
     <widget class="QDialogButtonBox" name="buttonBox">
      <property name="orientation">

guilib/src/ui/preferencesDialog.ui

@@ -6,8 +6,8 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>982</width>
-    <height>820</height>
+    <width>976</width>
+    <height>896</height>
    </rect>
   </property>
   <property name="sizePolicy">
@@ -63,9 +63,9 @@
           <property name="geometry">
            <rect>
             <x>0</x>
-            <y>-1179</y>
-            <width>686</width>
-            <height>3236</height>
+            <y>-360</y>
+            <width>680</width>
+            <height>3270</height>
            </rect>
           </property>
           <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -95,7 +95,7 @@
                 <enum>QFrame::Raised</enum>
                </property>
                <property name="currentIndex">
-                <number>21</number>
+                <number>5</number>
                </property>
                <widget class="QWidget" name="page_22">
                 <layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -3124,7 +3124,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                           <property name="checked">
                            <bool>false</bool>
                           </property>
-                          <layout class="QVBoxLayout" name="verticalLayout_11" stretch="0,0,0,1">
+                          <layout class="QVBoxLayout" name="verticalLayout_11">
                            <item>
                             <widget class="QLabel" name="label_40">
                              <property name="text">
@@ -3235,7 +3235,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="title">
                                   <string>OpenNI</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_57" columnstretch="0,0,1">
+                                 <layout class="QGridLayout" name="gridLayout_57" columnstretch="0,0,0">
                                   <item row="0" column="1">
                                    <widget class="QLineEdit" name="lineEdit_openniOniPath">
                                     <property name="text">
@@ -3667,6 +3667,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
+                                  <item row="7" column="0">
+                                   <spacer name="verticalSpacer_32">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
                                   <item row="1" column="0">
                                    <widget class="QDoubleSpinBox" name="doubleSpinBox_freenect2MinDepth">
                                     <property name="maximum">
@@ -3755,6 +3768,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                   <string>RealSense</string>
                                  </property>
                                  <layout class="QGridLayout" name="gridLayout_73" columnstretch="0,1">
+                                  <item row="5" column="0">
+                                   <spacer name="verticalSpacer_51">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                  <item row="0" column="1">
+                                   <widget class="QLabel" name="label_317">
+                                    <property name="text">
+                                     <string>Preset for RGB stream.</string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
+                                    </property>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                                    </property>
+                                   </widget>
+                                  </item>
                                   <item row="0" column="0">
                                    <widget class="QComboBox" name="comboBox_realsensePresetRGB">
                                     <property name="sizeAdjustPolicy">
@@ -3790,19 +3829,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="0" column="1">
-                                   <widget class="QLabel" name="label_317">
-                                    <property name="text">
-                                     <string>Preset for RGB stream.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="1" column="0">
                                    <widget class="QComboBox" name="comboBox_realsensePresetDepth">
                                     <property name="sizeAdjustPolicy">
@@ -3906,19 +3932,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="0">
-                                   <spacer name="verticalSpacer_32">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
-                                     <size>
-                                      <width>20</width>
-                                      <height>40</height>
-                                     </size>
-                                    </property>
-                                   </spacer>
-                                  </item>
                                  </layout>
                                 </widget>
                                </item>
@@ -4160,7 +4173,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                               </layout>
                              </widget>
                              <widget class="QWidget" name="page_55">
-                              <layout class="QVBoxLayout" name="verticalLayout_103" stretch="0">
+                              <layout class="QVBoxLayout" name="verticalLayout_103" stretch="0,1">
                                <item>
                                 <widget class="QGroupBox" name="groupBox_realsense2">
                                  <property name="sizePolicy">
@@ -4172,7 +4185,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="title">
                                   <string>RealSense2</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_100" columnstretch="0,0,1">
+                                 <layout class="QGridLayout" name="gridLayout_100" columnstretch="0,1,0">
                                   <item row="8" column="1">
                                    <widget class="QLineEdit" name="lineEdit_rs2_jsonFile"/>
                                   </item>
@@ -4375,10 +4388,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  </layout>
                                 </widget>
                                </item>
+                               <item>
+                                <spacer name="verticalSpacer_72">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>0</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
                               </layout>
                              </widget>
                              <widget class="QWidget" name="page_84">
-                              <layout class="QVBoxLayout" name="verticalLayout_145" stretch="0">
+                              <layout class="QVBoxLayout" name="verticalLayout_145" stretch="0,1">
                                <item>
                                 <widget class="QGroupBox" name="groupBox_k4a">
                                  <property name="sizePolicy">
@@ -4390,8 +4416,8 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                  <property name="title">
                                   <string>Kinect for Azure</string>
                                  </property>
-                                 <layout class="QGridLayout" name="gridLayout_113" columnstretch="0,0,1">
-                                  <item row="3" column="1">
+                                 <layout class="QGridLayout" name="gridLayout_113">
+                                  <item row="2" column="0">
                                    <widget class="QCheckBox" name="checkbox_k4a_irDepth">
                                     <property name="text">
                                      <string/>
@@ -4401,33 +4427,13 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="1">
+                                  <item row="8" column="0">
                                    <widget class="QCheckBox" name="source_checkBox_useMKVStamps">
                                     <property name="text">
                                      <string/>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="6" column="2">
-                                   <widget class="QLabel" name="label_604">
-                                    <property name="text">
-                                     <string>Use MKV file stamps as input rate.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
-                                    </property>
-                                    <property name="textInteractionFlags">
-                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="4" column="2">
-                                   <widget class="QLabel" name="label_601">
-                                    <property name="text">
-                                     <string>Frames per second</string>
-                                    </property>
-                                   </widget>
-                                  </item>
                                   <item row="5" column="0">
                                    <widget class="QToolButton" name="toolButton_k4a_mkv">
                                     <property name="text">
@@ -4435,74 +4441,31 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="5" column="1">
-                                   <widget class="QLineEdit" name="lineEdit_k4a_mkv">
+                                  <item row="9" column="0">
+                                   <spacer name="verticalSpacer_83">
+                                    <property name="orientation">
+                                     <enum>Qt::Vertical</enum>
+                                    </property>
+                                    <property name="sizeHint" stdset="0">
+                                     <size>
+                                      <width>20</width>
+                                      <height>0</height>
+                                     </size>
+                                    </property>
+                                   </spacer>
+                                  </item>
+                                  <item row="2" column="1" colspan="2">
+                                   <widget class="QLabel" name="label_558">
                                     <property name="text">
-                                     <string/>
+                                     <string>Use IR for RGB image </string>
+                                    </property>
+                                    <property name="wordWrap">
+                                     <bool>true</bool>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="4" column="1">
-                                   <widget class="QComboBox" name="comboBox_k4a_framerate">
-                                    <property name="sizePolicy">
-                                     <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
-                                      <horstretch>0</horstretch>
-                                      <verstretch>0</verstretch>
-                                     </sizepolicy>
-                                    </property>
-                                    <item>
-                                     <property name="text">
-                                      <string>5</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>15</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>30</string>
-                                     </property>
-                                    </item>
-                                   </widget>
-                                  </item>
-                                  <item row="0" column="1">
-                                   <widget class="QComboBox" name="comboBox_k4a_rgb_resolution">
-                                    <item>
-                                     <property name="text">
-                                      <string>720p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>1080p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>1440p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>1536p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>2160p</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>3072p</string>
-                                     </property>
-                                    </item>
-                                   </widget>
-                                  </item>
-                                  <item row="5" column="2">
-                                   <widget class="QLabel" name="label_603">
+                                  <item row="5" column="3">
+                                   <widget class="QLabel" name="label_556">
                                     <property name="text">
                                      <string>Path to a *.MKV file.</string>
                                     </property>
@@ -4514,90 +4477,199 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="3" column="2">
-                                   <widget class="QLabel" name="label_558">
+                                  <item row="8" column="1">
+                                   <widget class="QLabel" name="label_557">
                                     <property name="text">
-                                     <string>Use IR for RGB image (odometry may be better)</string>
+                                     <string>Use MKV file stamps as input rate.</string>
                                     </property>
                                     <property name="wordWrap">
                                      <bool>true</bool>
                                     </property>
-                                   </widget>
-                                  </item>
-                                  <item row="1" column="1">
-                                   <widget class="QComboBox" name="comboBox_k4a_depth_resolution">
-                                    <item>
-                                     <property name="text">
-                                      <string>320x288 (NFOV 2x2BINNED)</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>640x576 (NFOV UNBINNED)</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>512x512 (WFOV 2x2BINNED)</string>
-                                     </property>
-                                    </item>
-                                    <item>
-                                     <property name="text">
-                                      <string>1024x1024 (WFOV UNBINNED)</string>
-                                     </property>
-                                    </item>
-                                   </widget>
-                                  </item>
-                                  <item row="1" column="2">
-                                   <widget class="QLabel" name="label_602">
-                                    <property name="text">
-                                     <string>Depth camera resolution. 2x2 binning mode extends the Z-range in comparison to the corresponding unbinned mode. Binning is done at the cost of lowering image resolution.</string>
-                                    </property>
-                                    <property name="wordWrap">
-                                     <bool>true</bool>
+                                    <property name="textInteractionFlags">
+                                     <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
                                     </property>
                                    </widget>
                                   </item>
-                                  <item row="0" column="2">
-                                   <widget class="QLabel" name="label_600">
-                                    <property name="text">
-                                     <string>RGB camera resolution</string>
-                                    </property>
-                                   </widget>
-                                  </item>
-                                  <item row="7" column="1">
-                                   <spacer name="verticalSpacer_42">
-                                    <property name="orientation">
-                                     <enum>Qt::Vertical</enum>
-                                    </property>
-                                    <property name="sizeHint" stdset="0">
+                                  <item row="0" column="0" colspan="3">
+                                   <widget class="QFrame" name="frame">
+                                    <property name="minimumSize">
                                      <size>
-                                      <width>20</width>
-                                      <height>40</height>
+                                      <width>200</width>
+                                      <height>90</height>
                                      </size>
                                     </property>
-                                   </spacer>
+                                    <property name="frameShape">
+                                     <enum>QFrame::NoFrame</enum>
+                                    </property>
+                                    <property name="frameShadow">
+                                     <enum>QFrame::Plain</enum>
+                                    </property>
+                                    <property name="lineWidth">
+                                     <number>0</number>
+                                    </property>
+                                    <widget class="QLabel" name="label_600">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>80</x>
+                                       <y>0</y>
+                                       <width>161</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>RGB camera resolution</string>
+                                     </property>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_rgb_resolution">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>0</y>
+                                       <width>71</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>720p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1080p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1440p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1536p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>2160p</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>3072p</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_framerate">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>60</y>
+                                       <width>51</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>5</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>15</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>30</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QLabel" name="label_601">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>60</x>
+                                       <y>60</y>
+                                       <width>131</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>Frames per second</string>
+                                     </property>
+                                    </widget>
+                                    <widget class="QComboBox" name="comboBox_k4a_depth_resolution">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>0</x>
+                                       <y>30</y>
+                                       <width>111</width>
+                                       <height>25</height>
+                                      </rect>
+                                     </property>
+                                     <item>
+                                      <property name="text">
+                                       <string>320x288</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>640x576</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>512x512</string>
+                                      </property>
+                                     </item>
+                                     <item>
+                                      <property name="text">
+                                       <string>1024x1024</string>
+                                      </property>
+                                     </item>
+                                    </widget>
+                                    <widget class="QLabel" name="label_602">
+                                     <property name="geometry">
+                                      <rect>
+                                       <x>120</x>
+                                       <y>30</y>
+                                       <width>171</width>
+                                       <height>21</height>
+                                      </rect>
+                                     </property>
+                                     <property name="text">
+                                      <string>Depth camera resolution</string>
+                                     </property>
+                                    </widget>
+                                   </widget>
+                                  </item>
+                                  <item row="5" column="1" colspan="2">
+                                   <widget class="QLineEdit" name="lineEdit_k4a_mkv">
+                                    <property name="text">
+                                     <string/>
+                                    </property>
+                                   </widget>
                                   </item>
                                  </layout>
                                 </widget>
                                </item>
+                               <item>
+                                <spacer name="verticalSpacer_82">
+                                 <property name="orientation">
+                                  <enum>Qt::Vertical</enum>
+                                 </property>
+                                 <property name="sizeHint" stdset="0">
+                                  <size>
+                                   <width>20</width>
+                                   <height>0</height>
+                                  </size>
+                                 </property>
+                                </spacer>
+                               </item>
                               </layout>
                              </widget>
                             </widget>
                            </item>
-                           <item>
-                            <spacer name="verticalSpacer_83">
-                             <property name="orientation">
-                              <enum>Qt::Vertical</enum>
-                             </property>
-                             <property name="sizeHint" stdset="0">
-                              <size>
-                               <width>20</width>
-                               <height>0</height>
-                              </size>
-                             </property>
-                            </spacer>
-                           </item>
                           </layout>
                          </widget>
                         </item>
@@ -6067,32 +6139,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                        </property>
                        <item>
                         <layout class="QGridLayout" name="gridLayout_68" columnstretch="0,0,1">
-                         <item row="7" column="2">
-                          <widget class="QLabel" name="label_289">
-                           <property name="text">
-                            <string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
-                         <item row="0" column="2">
-                          <widget class="QLabel" name="label_265">
-                           <property name="text">
-                            <string>Bayer mode. For convenience, if the images are bayered.</string>
-                           </property>
-                           <property name="wordWrap">
-                            <bool>true</bool>
-                           </property>
-                           <property name="textInteractionFlags">
-                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                           </property>
-                          </widget>
-                         </item>
                          <item row="6" column="0">
                           <widget class="QToolButton" name="toolButton_cameraImages_gt">
                            <property name="text">
@@ -6187,6 +6233,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="7" column="2">
+                          <widget class="QLabel" name="label_289">
+                           <property name="text">
+                            <string>Ground truth format. See tool tip for more details on formats. Note that formats without stamps should have the same number of values than the source images.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="1" column="1">
                           <widget class="QCheckBox" name="checkBox_cameraImages_timestamps">
                            <property name="text">
@@ -6345,6 +6404,19 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
                            </property>
                           </widget>
                          </item>
+                         <item row="0" column="2">
+                          <widget class="QLabel" name="label_265">
+                           <property name="text">
+                            <string>Bayer mode. For convenience, if the images are bayered.</string>
+                           </property>
+                           <property name="wordWrap">
+                            <bool>true</bool>
+                           </property>
+                           <property name="textInteractionFlags">
+                            <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
+                           </property>
+                          </widget>
+                         </item>
                          <item row="0" column="1">
                           <widget class="QComboBox" name="comboBox_cameraImages_bayerMode">
                            <property name="sizeAdjustPolicy">
@@ -8964,11 +9036,6 @@ generate the number of words requested.</string>
                              <string>SuperPoint Torch</string>
                             </property>
                            </item>
-                           <item>
-                            <property name="text">
-                             <string>SURF+FREAK</string>
-                            </property>
-                           </item>
                           </widget>
                          </item>
                          <item row="7" column="0">
@@ -9571,7 +9638,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="2">
+                       <item row="7" column="2">
                         <widget class="QLabel" name="label_260">
                          <property name="text">
                           <string>When using a FLANN-based nearest neighbor strategy, add/remove points to its index without always rebuilding the index (the index is built only when the dictionary increases of the factor below in size).</string>
@@ -9584,7 +9651,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="8" column="0">
+                       <item row="7" column="0">
                         <widget class="QCheckBox" name="checkBox_kp_incrementalFlann">
                          <property name="text">
                           <string/>
@@ -9594,7 +9661,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="2">
+                       <item row="8" column="2">
                         <widget class="QLabel" name="label_451">
                          <property name="text">
                           <string>Factor used when rebuilding the incremental FLANN index. Set 1 to disable.</string>
@@ -9607,7 +9674,7 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="9" column="0">
+                       <item row="8" column="0">
                         <widget class="QDoubleSpinBox" name="surf_doubleSpinBox_rebalancingFactor">
                          <property name="decimals">
                           <number>1</number>
@@ -9626,29 +9693,6 @@ When set to false, no new words are added to dictionary, so no more updates are
                          </property>
                         </widget>
                        </item>
-                       <item row="7" column="2">
-                        <widget class="QLabel" name="label_556">
-                         <property name="text">
-                          <string>For FLANN KdTree, binary descriptors are converted to float by converting each byte to float instead of converting each bit to float. When converting bytes instead of bits, less memory is used and search is faster at the cost of slightly less accurate matching.</string>
-                         </property>
-                         <property name="wordWrap">
-                          <bool>true</bool>
-                         </property>
-                         <property name="textInteractionFlags">
-                          <set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
-                         </property>
-                        </widget>
-                       </item>
-                       <item row="7" column="0">
-                        <widget class="QCheckBox" name="checkBox_kp_byteToFloat">
-                         <property name="text">
-                          <string/>
-                         </property>
-                         <property name="checked">
-                          <bool>true</bool>
-                         </property>
-                        </widget>
-                       </item>
                       </layout>
                      </widget>
                     </item>
@@ -18244,11 +18288,6 @@ Lower the ratio -&gt; higher the precision.</string>
                              <string>SuperPoint Torch</string>
                             </property>
                            </item>
-                           <item>
-                            <property name="text">
-                             <string>SURF+FREAK</string>
-                            </property>
-                           </item>
                           </widget>
                          </item>
                          <item row="3" column="0">

guilib/src/utilite/UPlot.cpp

@@ -1678,86 +1678,6 @@ void UPlotLegend::moveDown(UPlotLegendItem * item)
 	}
 }
 
-QString UPlotLegend::getAllCurveDataAsText() const
-{
-	QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
-	if(items.size())
-	{
-		// create common x-axis
-		QMap<qreal, qreal> xAxisMap;
-		for(int i=0; i<items.size(); ++i)
-		{
-			QMap<qreal, qreal> data;
-			items.at(i)->curve()->getData(data);
-			for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
-			{
-				xAxisMap.insert(iter.key(), iter.value());
-			}
-		}
-		QList<qreal> xAxis = xAxisMap.uniqueKeys();
-
-		QVector<QVector<qreal> > axes;
-		for(int i=0; i<items.size(); ++i)
-		{
-			QMap<qreal, qreal> data;
-			items.at(i)->curve()->getData(data);
-
-			QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
-			// just to make sure that we have the same number of data on each curve, set NAN for unknowns
-			int j=0;
-			for(QList<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
-			{
-				if(data.contains(*iter))
-				{
-					y[j] = data.value(*iter);
-				}
-				++j;
-			}
-			axes.push_back(y);
-		}
-		if(!xAxis.empty())
-		{
-			axes.push_front(xAxis.toVector());
-			QString text;
-			text.append('x');
-			text.append('\t');
-			for(int i=0; i<items.size(); ++i)
-			{
-				text.append(items.at(i)->curve()->name());
-				if(i+1<axes.size())
-				{
-					text.append('\t');
-				}
-			}
-			text.append('\n');
-			for(int i=0; i<axes[0].size(); ++i)
-			{
-				for(int j=0; j<axes.size(); ++j)
-				{
-					if(uIsNan(axes[j][i]))
-					{
-						text.append("NaN"); // NaN is interpreted by default as NaN in MatLab/Octave
-					}
-					else
-					{
-						text.append(QString::number(axes[j][i]));
-					}
-					if(j+1<axes.size())
-					{
-						text.append('\t');
-					}
-				}
-				if(i+1<axes[0].size())
-				{
-					text.append("\n");
-				}
-			}
-			return text;
-		}
-	}
-	return "";
-}
-
 void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 {
 	QAction * action = _menu->exec(event->globalPos());
@@ -1767,11 +1687,81 @@ void UPlotLegend::contextMenuEvent(QContextMenuEvent * event)
 	}
 	else if(action == _aCopyAllCurvesToClipboard)
 	{
-		QString data = getAllCurveDataAsText();
-		if(!data.isEmpty())
+		QList<UPlotLegendItem *> items = this->findChildren<UPlotLegendItem*>();
+		if(items.size())
 		{
-			QClipboard * clipboard = QApplication::clipboard();
-			clipboard->setText(data);
+			// create common x-axis
+			QMap<qreal, qreal> xAxisMap;
+			for(int i=0; i<items.size(); ++i)
+			{
+				QMap<qreal, qreal> data;
+				items.at(i)->curve()->getData(data);
+				for(QMap<qreal, qreal>::iterator iter=data.begin(); iter!=data.end(); ++iter)
+				{
+					xAxisMap.insert(iter.key(), iter.value());
+				}
+			}
+			QList<qreal> xAxis = xAxisMap.uniqueKeys();
+
+			QVector<QVector<qreal> > axes;
+			for(int i=0; i<items.size(); ++i)
+			{
+				QMap<qreal, qreal> data;
+				items.at(i)->curve()->getData(data);
+
+				QVector<qreal> y(xAxis.size(), std::numeric_limits<qreal>::quiet_NaN());
+				// just to make sure that we have the same number of data on each curve, set NAN for unknowns
+				int j=0;
+				for(QList<qreal>::iterator iter=xAxis.begin(); iter!=xAxis.end(); ++iter)
+				{
+					if(data.contains(*iter))
+					{
+						y[j] = data.value(*iter);
+					}
+					++j;
+				}
+				axes.push_back(y);
+			}
+			if(!xAxis.empty())
+			{
+				axes.push_front(xAxis.toVector());
+				QString text;
+				text.append('x');
+				text.append('\t');
+				for(int i=0; i<items.size(); ++i)
+				{
+					text.append(items.at(i)->curve()->name());
+					if(i+1<axes.size())
+					{
+						text.append('\t');
+					}
+				}
+				text.append('\n');
+				for(int i=0; i<axes[0].size(); ++i)
+				{
+					for(int j=0; j<axes.size(); ++j)
+					{
+						if(uIsNan(axes[j][i]))
+						{
+							text.append("NA");
+						}
+						else
+						{
+							text.append(QString::number(axes[j][i]));
+						}
+						if(j+1<axes.size())
+						{
+							text.append('\t');
+						}
+					}
+					if(i+1<axes[0].size())
+					{
+						text.append("\n");
+					}
+				}
+				QClipboard * clipboard = QApplication::clipboard();
+				clipboard->setText(text);
+			}
 		}
 	}
 	else if(action == _aShowAllStdDevMeanMax)
@@ -3272,12 +3262,3 @@ void UPlot::moveCurve(const UPlotCurve * curve, int index)
 		this->update();
 	}
 }
-
-QString UPlot::getAllCurveDataAsText() const
-{
-	if(_legend)
-	{
-		return _legend->getAllCurveDataAsText();
-	}
-	return "";
-}

package.xml

@@ -12,7 +12,6 @@
 
   <buildtool_depend>cmake</buildtool_depend>
   
-  <build_depend>libvtk-qt</build_depend>
   <build_depend>qt_gui_cpp</build_depend> <!-- libqt4-dev or libqt5-dev -->
   <build_depend>libpcl-all-dev</build_depend>
   <build_depend>libsqlite3-dev</build_depend>
@@ -26,7 +25,6 @@
   <build_depend>octomap</build_depend>
   <build_depend>libg2o</build_depend>
   
-  <run_depend>libvtk-qt</run_depend>
   <run_depend>qt_gui_cpp</run_depend>
   <run_depend>libpcl-all-dev</run_depend>
   <run_depend>libsqlite3-dev</run_depend>

tools/DetectMoreLoopClosures/CMakeLists.txt

@@ -9,7 +9,7 @@ SET(RTABMap_LIBRARIES
 )  
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/EurocDataset/CMakeLists.txt

@@ -27,7 +27,7 @@ SET(RTABMap_LIBRARIES
 )  
 	
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Export/main.cpp

@@ -66,7 +66,6 @@ void showUsage()
 			"    --decimation    #     Depth image decimation before creating the clouds (default 4).\n"
 			"    --voxel         #     Voxel size of the created clouds (default 0.01 m).\n"
 			"    --color_radius  #     Radius used to colorize polygons (default 0.05 m, set 0 for nearest color).\n"
-			"    --save_in_db          Save resulting assembled point cloud or mesh in the database.\n"
 			"\n%s", Parameters::showUsage());
 	;
 	exit(1);
@@ -94,11 +93,10 @@ int main(int argc, char * argv[])
 	float maxRange = 4.0f;
 	float voxelSize = 0.01f;
 	int textureSize = 4096;
-	int textureCount = 1;
+	int textureCount = 8;
 	int textureRange = 0;
 	bool multiband = false;
 	float colorRadius = 0.05;
-	bool saveInDb = false;
 	for(int i=1; i<argc-1; ++i)
 	{
 		if(std::strcmp(argv[i], "--help") == 0)
@@ -246,26 +244,7 @@ int main(int argc, char * argv[])
 				showUsage();
 			}
 		}
-		else if(std::strcmp(argv[i], "--save_in_db") == 0)
-		{
-			saveInDb = true;
-		}
 	}
-
-	if(saveInDb)
-	{
-		if(multiband)
-		{
-			printf("Option --multiband is not supported with --save_in_db option, disabling multiband...\n");
-			multiband = false;
-		}
-		if(textureCount>1)
-		{
-			printf("Option --texture_count > 1 is not supported with --save_in_db option, setting texture_count to 1...\n");
-			textureCount = 1;
-		}
-	}
-
 	ParametersMap params = Parameters::parseArguments(argc, argv, false);
 
 	std::string dbPath = argv[argc-1];
@@ -284,7 +263,6 @@ int main(int argc, char * argv[])
 		return -1;
 	}
 	delete driver;
-	driver = 0;
 
 	for(ParametersMap::iterator iter=params.begin(); iter!=params.end(); ++iter)
 	{
@@ -385,35 +363,15 @@ int main(int argc, char * argv[])
 
 	if(mergedClouds->size())
 	{
-		if(saveInDb)
-		{
-			driver = DBDriver::create();
-			UASSERT(driver->openConnection(dbPath, false));
-			Transform lastlocalizationPose;
-			driver->loadOptimizedPoses(&lastlocalizationPose);
-			//optimized poses have changed, reset 2d map
-			driver->save2DMap(cv::Mat(), 0, 0, 0);
-			driver->saveOptimizedPoses(optimizedPoses, lastlocalizationPose);
-		}
-
 		if(!(mesh || texture))
 		{
 			printf("Voxel grid filtering of the assembled cloud (voxel=%f, %d points)\n", 0.01f, (int)mergedClouds->size());
 			mergedClouds = util3d::voxelize(mergedClouds, voxelSize);
 
-			if(saveInDb)
-			{
-				printf("Saving in db... (%d points)\n", (int)mergedClouds->size());
-				driver->saveOptimizedMesh(util3d::laserScanFromPointCloud(*mergedClouds, Transform(), false));
-				printf("Saving in db... done!\n");
-			}
-			else
-			{
-				std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply";
-				printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size());
-				pcl::io::savePLYFile(outputPath, *mergedClouds);
-				printf("Saving %s... done!\n", outputPath.c_str());
-			}
+			std::string outputPath=outputDirectory+"/"+baseName+"_cloud.ply";
+			printf("Saving %s... (%d points)\n", outputPath.c_str(), (int)mergedClouds->size());
+			pcl::io::savePLYFile(outputPath, *mergedClouds);
+			printf("Saving %s... done!\n", outputPath.c_str());
 		}
 		else
 		{
@@ -457,23 +415,10 @@ int main(int argc, char * argv[])
 
 				if(!texture)
 				{
-					if(saveInDb)
-					{
-						printf("Saving mesh in db...\n");
-						std::vector<std::vector<std::vector<unsigned int> > > polygons;
-						polygons.push_back(util3d::convertPolygonsFromPCL(mesh->polygons));
-						driver->saveOptimizedMesh(
-								util3d::laserScanFromPointCloud(mesh->cloud, false).data(),
-								polygons);
-						printf("Saving mesh in db... done!\n");
-					}
-					else
-					{
-						std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
-						printf("Saving %s...\n", outputPath.c_str());
-						pcl::io::savePLYFile(outputPath, *mesh);
-						printf("Saving %s... done!\n", outputPath.c_str());
-					}
+					std::string outputPath=outputDirectory+"/"+baseName+"_mesh.ply";
+					printf("Saving %s...\n", outputPath.c_str());
+					pcl::io::savePLYFile(outputPath, *mesh);
+					printf("Saving %s... done!\n", outputPath.c_str());
 				}
 				else
 				{
@@ -534,51 +479,38 @@ int main(int argc, char * argv[])
 								&contrastValues);
 						printf("Merging to %d texture(s)... done (%fs).\n", (int)textureMesh->tex_materials.size(), timer.ticks());
 
-						if(saveInDb)
+						// TextureMesh OBJ
+						bool success = false;
+						UASSERT(!textures.empty());
+						for(size_t i=0; i<textureMesh->tex_materials.size(); ++i)
 						{
-							printf("Saving texture mesh in db...\n");
-							driver->saveOptimizedMesh(
-									util3d::laserScanFromPointCloud(textureMesh->cloud, false).data(),
-									util3d::convertPolygonsFromPCL(textureMesh->tex_polygons),
-									textureMesh->tex_coordinates,
-									textures);
-							printf("Saving texture mesh in db... done!\n");
-						}
-						else
-						{
-							// TextureMesh OBJ
-							bool success = false;
-							UASSERT(!textures.empty());
-							for(size_t i=0; i<textureMesh->tex_materials.size(); ++i)
+							textureMesh->tex_materials[i].tex_file += ".jpg";
+							printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
+							UASSERT(textures.cols % textures.rows == 0);
+							success = cv::imwrite(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1))));
+							if(!success)
 							{
-								textureMesh->tex_materials[i].tex_file += ".jpg";
-								printf("Saving texture to %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
-								UASSERT(textures.cols % textures.rows == 0);
-								success = cv::imwrite(outputDirectory+"/"+textureMesh->tex_materials[i].tex_file, cv::Mat(textures, cv::Range::all(), cv::Range(textures.rows*i, textures.rows*(i+1))));
-								if(!success)
-								{
-									UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());
-								}
-								else
-								{
-									printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
-								}
+								UERROR("Failed saving %s!", textureMesh->tex_materials[i].tex_file.c_str());
 							}
+							else
+							{
+								printf("Saved %s.\n", textureMesh->tex_materials[i].tex_file.c_str());
+							}
+						}
+						if(success)
+						{
+
+							std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
+							printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
+							success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
+
 							if(success)
 							{
-
-								std::string outputPath=outputDirectory+"/"+baseName+"_mesh.obj";
-								printf("Saving obj (%d vertices) to %s.\n", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, outputPath.c_str());
-								success = pcl::io::saveOBJFile(outputPath, *textureMesh) == 0;
-
-								if(success)
-								{
-									printf("Saved obj to %s!\n", outputPath.c_str());
-								}
-								else
-								{
-									UERROR("Failed saving obj to %s!", outputPath.c_str());
-								}
+								printf("Saved obj to %s!\n", outputPath.c_str());
+							}
+							else
+							{
+								UERROR("Failed saving obj to %s!", outputPath.c_str());
 							}
 						}
 
@@ -619,12 +551,5 @@ int main(int argc, char * argv[])
 		printf("Export failed! The cloud is empty.\n");
 	}
 
-	if(driver)
-	{
-		driver->closeConnection();
-		delete driver;
-		driver = 0;
-	}
-
 	return 0;
 }

tools/Info/CMakeLists.txt

@@ -9,7 +9,7 @@ SET(RTABMap_LIBRARIES
 )  
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Info/main.cpp

@@ -33,7 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include "rtabmap/utilite/UFile.h"
-#include "rtabmap/utilite/UStl.h"
 
 using namespace rtabmap;
 
@@ -230,7 +229,7 @@ int main(int argc, char * argv[])
 		driver->getAllNodeIds(ids);
 		Transform lastLocalization;
 		std::map<int, Transform> optimizedPoses = driver->loadOptimizedPoses(&lastLocalization);
-		std::multimap<int, int> mapIdsLinkedToLastGraph;
+		std::set<int> mapsLinkedToLastGraph;
 		int lastMapId=0;
 		double previousStamp = 0.0f;
 		Transform previousPose;
@@ -266,7 +265,7 @@ int main(int argc, char * argv[])
 			}
 			if(optimizedPoses.find(id) != optimizedPoses.end())
 			{
-				mapIdsLinkedToLastGraph.insert(std::make_pair(mapId, id));
+				mapsLinkedToLastGraph.insert(mapId);
 			}
 			if(iter!=ids.begin())
 			{
@@ -319,14 +318,13 @@ int main(int argc, char * argv[])
 		}
 
 		std::stringstream sessionsInOptGraphStr;
-		std::list<int> mapsLinkedToLastGraph = uUniqueKeys(mapIdsLinkedToLastGraph);
-		for(std::list<int>::iterator iter=mapsLinkedToLastGraph.begin(); iter!=mapsLinkedToLastGraph.end(); ++iter)
+		for(std::set<int>::iterator iter=mapsLinkedToLastGraph.begin(); iter!=mapsLinkedToLastGraph.end(); ++iter)
 		{
 			if(iter!=mapsLinkedToLastGraph.begin())
 			{
 				sessionsInOptGraphStr << ", ";
 			}
-			sessionsInOptGraphStr << *iter << "(" << mapIdsLinkedToLastGraph.count(*iter) << ")";
+			sessionsInOptGraphStr << *iter;
 		}
 
 		std::cout << (uFormat("%s%fs\n", pad("Total time:").c_str(), infoTotalTime));
@@ -381,7 +379,7 @@ int main(int argc, char * argv[])
 		total+=mem;
 		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Statistics size:").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		mem = dbSize - total;
-		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Other (indexing, unused):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
+		std::cout << (uFormat("%s%d %s\t(%.2f%%)\n", pad("Other (indexing):").c_str(), mem>1000000?mem/1000000:mem>1000?mem/1000:mem, mem>1000000?"MB":mem>1000?"KB":"Bytes", dbSize>0?double(mem)/double(dbSize)*100.0:0.0));
 		std::cout << ("\n");
 	}
 

tools/KittiDataset/CMakeLists.txt

@@ -11,7 +11,7 @@ SET(RTABMap_LIBRARIES
 )  
 	
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Matcher/main.cpp

@@ -275,7 +275,7 @@ int main(int argc, char * argv[])
 
 		if(reg.getNNType()==6 &&
 		   !dataFrom.getWordsDescriptors().empty() &&
-		   dataFrom.getWordsDescriptors().type()!=CV_32F)
+		   dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)
 		{
 			UWARN("PyMatcher is selected for matching but binary features "
 				  "are not compatible. BruteForce with CrossCheck (%s=5) "
@@ -298,7 +298,7 @@ int main(int argc, char * argv[])
 				.arg(Parameters::kVisCorNNType().c_str())
 				.arg(reg.getNNType())
 				.arg(reg.getNNType()<VWDictionary::kNNUndef?VWDictionary::nnStrategyName((VWDictionary::NNStrategy)reg.getNNType()).c_str():
-						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().type()!=CV_32F)?"BFCrossCheck":
+						reg.getNNType()==5||(reg.getNNType()==6&&!dataFrom.getWordsDescriptors().empty()&& dataFrom.getWordsDescriptors().begin()->second.type()!=CV_32F)?"BFCrossCheck":
 						reg.getNNType()==6?QString(uSplit(UFile::getName(pyMatcherPath), '.').front().c_str()).replace("rtabmap_", ""):
 						reg.getNNType()==7?"GMS":"?")
 				.arg(reg.getNNType()<5?QString(" %1=%2").arg(Parameters::kVisCorNNDR().c_str()).arg(reg.getNNDR()):"")
@@ -322,21 +322,9 @@ int main(int argc, char * argv[])
 			if(reg.getEstimationType() == 2)
 			{
 				// triangulate 3D words based on the transform computed
-				std::map<int, int> wordsFrom = uMultimapToMapUnique(dataFrom.getWords());
-				std::map<int, int> wordsTo = uMultimapToMapUnique(dataTo.getWords());
-				std::map<int, cv::KeyPoint> kptsFrom;
-				std::map<int, cv::KeyPoint> kptsTo;
-				for(std::map<int, int>::iterator iter=wordsFrom.begin(); iter!=wordsFrom.end(); ++iter)
-				{
-					kptsFrom.insert(std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
-				}
-				for(std::map<int, int>::iterator iter=wordsTo.begin(); iter!=wordsTo.end(); ++iter)
-				{
-					kptsTo.insert(std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
-				}
 				std::map<int, cv::Point3f> points3d = util3d::generateWords3DMono(
-						kptsFrom,
-						kptsTo,
+						uMultimapToMapUnique(dataFrom.getWords()),
+						uMultimapToMapUnique(dataTo.getWords()),
 						model.isValidForProjection()?model:stereoModel.left(),
 						t);
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
@@ -365,12 +353,11 @@ int main(int argc, char * argv[])
 					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
 					cloudWordsFrom->resize(dataFrom.getWords3().size());
 					int i=0;
-					for(std::multimap<int, int>::const_iterator iter=dataFrom.getWords().begin();
-						iter!=dataFrom.getWords().end();
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataFrom.getWords3().begin();
+						iter!=dataFrom.getWords3().end();
 						++iter)
 					{
-						const cv::Point3f & pt = dataFrom.getWords3()[iter->second];
-						cloudWordsFrom->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
 					}
 					if(cloudWordsFrom->size())
 					{
@@ -384,23 +371,22 @@ int main(int argc, char * argv[])
 				}
 				if(!dataTo.getWords3().empty())
 				{
-					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsTo(new pcl::PointCloud<pcl::PointXYZ>);
-					cloudWordsTo->resize(dataTo.getWords3().size());
+					pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWordsFrom(new pcl::PointCloud<pcl::PointXYZ>);
+					cloudWordsFrom->resize(dataTo.getWords3().size());
 					int i=0;
-					for(std::multimap<int, int>::const_iterator iter=dataTo.getWords().begin();
-						iter!=dataTo.getWords().end();
+					for(std::multimap<int, cv::Point3f>::const_iterator iter=dataTo.getWords3().begin();
+						iter!=dataTo.getWords3().end();
 						++iter)
 					{
-						const cv::Point3f & pt = dataTo.getWords3()[iter->second];
-						cloudWordsTo->at(i++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
+						cloudWordsFrom->at(i++) = pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z);
 					}
-					if(cloudWordsTo->size())
+					if(cloudWordsFrom->size())
 					{
-						cloudWordsTo = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsTo);
+						cloudWordsFrom = rtabmap::util3d::removeNaNFromPointCloud(cloudWordsFrom);
 					}
-					if(cloudWordsTo->size())
+					if(cloudWordsFrom->size())
 					{
-						viewer->addCloud("wordsTo", cloudWordsTo, t, Qt::cyan);
+						viewer->addCloud("wordsTo", cloudWordsFrom, t, Qt::cyan);
 						viewer->setCloudPointSize("wordsTo", 5);
 					}
 				}
@@ -457,24 +443,8 @@ int main(int argc, char * argv[])
 			viewB->setImageDepth(uCvMat2QImage(toDepth, false, uCvQtDepthRedToBlue));
 			viewB->setImageDepthShown(true);
 		}
-		std::multimap<int, cv::KeyPoint> keypointsFrom;
-		std::multimap<int, cv::KeyPoint> keypointsTo;
-		if(!dataFrom.getWordsKpts().empty())
-		{
-			for(std::map<int, int>::const_iterator iter=dataFrom.getWords().begin(); iter!=dataFrom.getWords().end(); ++iter)
-			{
-				keypointsFrom.insert(keypointsFrom.end(), std::make_pair(iter->first, dataFrom.getWordsKpts()[iter->second]));
-			}
-		}
-		if(!dataTo.getWordsKpts().empty())
-		{
-			for(std::map<int, int>::const_iterator iter=dataTo.getWords().begin(); iter!=dataTo.getWords().end(); ++iter)
-			{
-				keypointsTo.insert(keypointsTo.end(), std::make_pair(iter->first, dataTo.getWordsKpts()[iter->second]));
-			}
-		}
-		viewA->setFeatures(keypointsFrom);
-		viewB->setFeatures(keypointsTo);
+		viewA->setFeatures(dataFrom.getWords());
+		viewB->setFeatures(dataTo.getWords());
 		std::set<int> inliersSet(info.inliersIDs.begin(), info.inliersIDs.end());
 
 		const QMultiMap<int, KeypointItem*> & wordsA = viewA->getFeatures();

tools/Recovery/CMakeLists.txt

@@ -9,7 +9,7 @@ SET(RTABMap_LIBRARIES
 )  
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Report/CMakeLists.txt

@@ -12,7 +12,7 @@ SET(RTABMap_LIBRARIES
 )  
 	
 if(POLICY CMP0020)
-    cmake_policy(SET CMP0020 NEW)
+    cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Report/main.cpp

@@ -40,7 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifdef WITH_QT
 #include <rtabmap/utilite/UPlot.h>
 #include <QApplication>
-#include <QFile>
 #endif
 
 using namespace rtabmap;
@@ -70,13 +69,10 @@ void showUsage()
 			"                       database, the inverse will be done. \n"
 			"    --ids              Use IDs for x axis instead of time in the figures. \n"
 			"    --start #          Start from this node ID for the figures.\n"
-			"    --export           Export figures' data to txt files.\n"
-			"    --export_prefix    Prefix to filenames of exported figures' data (default is \"Stat\").\n"
 #endif
 			"    --report           Export all evaluation statistics values in report.txt \n"
-			"    --loc #            Show localization statistics for each \"Statistic/Id\" per\n"
-			"                       session for 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%%\n"
-			"    --loc_delay #      Delay to split sessions for localization statistics (default 60 seconds)\n"
+			"    --loc #            Show localization statistics for each \"Statistic/Id\" per "
+			"                       session for 1=min,2=max,4=mean,8=stddev,16=total,32=nonnull%% "
 			"                       (it is a mask, we can combine those numbers, e.g., 63 for all) \n"
 			"    --help             Show usage\n\n");
 	exit(1);
@@ -137,10 +133,7 @@ int main(int argc, char * argv[])
 	bool invertFigures = false;
 	bool useIds = false;
 	int startId = 0;
-	bool exportFigures = false;
-	std::string exportPrefix = "Stat";
 	int showLoc = 0;
-	float locDelay = 60;
 	std::vector<std::string> statsToShow;
 #ifdef WITH_QT
 	std::map<std::string, UPlot*> figures;
@@ -191,24 +184,6 @@ int main(int argc, char * argv[])
 		{
 			useIds = true;
 		}
-		else if(strcmp(argv[i],"--export") == 0)
-		{
-			exportFigures = true;
-		}
-		else if(strcmp(argv[i],"--export_prefix") == 0)
-		{
-			++i;
-			if(i<argc-1)
-			{
-				exportPrefix = argv[i];
-				printf("Export prefix=%s (--export_prefix)\n", exportPrefix.c_str());
-			}
-			else
-			{
-				printf("Missing value for \"--export_prefix\" option.\n");
-				showUsage();
-			}
-		}
 		else if(strcmp(argv[i],"--loc") == 0)
 		{
 			++i;
@@ -219,21 +194,7 @@ int main(int argc, char * argv[])
 			}
 			else
 			{
-				printf("Missing type for \"--loc\" option.\n");
-				showUsage();
-			}
-		}
-		else if(strcmp(argv[i],"--loc_delay") == 0)
-		{
-			++i;
-			if(i<argc-1)
-			{
-				locDelay = atof(argv[i]);
-				printf("Localization delay=%fs (--loc_delay)\n", locDelay);
-			}
-			else
-			{
-				printf("Missing value for \"--loc_delay\" option.\n");
+				printf("Missing type for \"--showLoc\" option.\n");
 				showUsage();
 			}
 		}
@@ -265,7 +226,7 @@ int main(int argc, char * argv[])
 	{
 		invertFigures = !invertFigures;
 	}
-	std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > > localizationMultiStats; //<statsName, <Database<Session>> >
+	std::map<std::string, std::map<std::string, std::vector<LocStats> > > localizationMultiStats; //<statsName, <Database<Session>> >
 	for(size_t i=0; i<statsToShow.size(); ++i)
 	{
 		std::string figureTitle = statsToShow[i];
@@ -289,7 +250,7 @@ int main(int argc, char * argv[])
 		}
 		if(showLoc & 0b111111)
 		{
-			localizationMultiStats.insert(std::make_pair(figureTitle, std::vector<std::pair<std::string, std::vector<LocStats> > >()));
+			localizationMultiStats.insert(std::make_pair(figureTitle, std::map<std::string, std::vector<LocStats> >()));
 		}
 	}
 	if(!invertFigures)
@@ -315,6 +276,7 @@ int main(int argc, char * argv[])
 			if(UFile::getExtension(currentPath).compare("db") == 0)
 			{
 				currentPathIsDatabase=true;
+				localizationMultiStats.clear();
 				printf("Database: %s\n", currentPath.c_str());
 			}
 			else
@@ -350,14 +312,12 @@ int main(int argc, char * argv[])
 		// For all databases in currentDir
 		while(currentPathIsDatabase || !(fileName = currentDir.getNextFileName()).empty())
 		{
-			int startIdPerDb = startId;
 			if(currentPathIsDatabase || UFile::getExtension(fileName).compare("db") == 0)
 			{
 				std::string filePath;
 				if(currentPathIsDatabase)
 				{
 					filePath = currentPath;
-					fileName = UFile::getName(currentPath);
 				}
 				else
 				{
@@ -423,7 +383,7 @@ int main(int argc, char * argv[])
 						{
 							curves.insert(std::make_pair(iter->first, iter->second->addCurve(filePath.c_str())));
 							if(!localizationMultiStats.empty())
-								localizationMultiStats.at(iter->first).push_back(std::make_pair(fileName, std::vector<LocStats>()));
+								localizationMultiStats.at(iter->first).insert(std::make_pair(fileName, std::vector<LocStats>()));
 						}
 					}
 					else
@@ -449,7 +409,7 @@ int main(int argc, char * argv[])
 							{
 								curves.insert(std::make_pair(statsToShow[i], fig->addCurve(statsToShow[i].c_str())));
 								if(!localizationMultiStats.empty())
-									localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
+									localizationMultiStats.at(statsToShow[i]).insert(std::make_pair(fileName, std::vector<LocStats>()));
 							}
 						}
 					}
@@ -457,36 +417,17 @@ int main(int argc, char * argv[])
 					for(size_t i=0; i<statsToShow.size(); ++i)
 					{
 						if(!localizationMultiStats.empty())
-							localizationMultiStats.at(statsToShow[i]).push_back(std::make_pair(fileName, std::vector<LocStats>()));
+							localizationMultiStats.at(statsToShow[i]).insert(std::make_pair(fileName, std::vector<LocStats>()));
 					}
 #endif
 
 					// Find localization sessions and adjust startId
-					std::set<int> mappingSessionIds;
-					if(!localizationMultiStats.empty())
+					if(!localizationMultiStats.empty() && startId ==0)
 					{
 						std::map<int, Transform> poses = driver->loadOptimizedPoses();
 						if(!poses.empty())
 						{
-							for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-							{
-								Transform p, gt;
-								GPS gps;
-								int m=-1, w=-1;
-								std::string l;
-								double s;
-								std::vector<float> v;
-								EnvSensors sensors;
-								if(driver->getNodeInfo(iter->first, p, m, w, l, s, gt, v, gps, sensors))
-								{
-									mappingSessionIds.insert(m);
-								}
-							}
-
-							if(startIdPerDb ==0)
-							{
-								startIdPerDb = poses.rbegin()->first+1;
-							}
+							startId = poses.rbegin()->first+1;
 						}
 					}
 
@@ -509,13 +450,7 @@ int main(int argc, char * argv[])
 							{
 								gtPoses.insert(std::make_pair(*iter, gt));
 							}
-
-							if(!localizationMultiStats.empty() && mappingSessionIds.find(m) != mappingSessionIds.end())
-							{
-								continue;
-							}
-
-							if(*iter >= startIdPerDb && uContains(stats, *iter))
+							if(uContains(stats, *iter))
 							{
 								const std::map<std::string, float> & stat = stats.at(*iter).first;
 								if(uContains(stat, Statistics::kGtTranslational_rmse()))
@@ -541,14 +476,14 @@ int main(int argc, char * argv[])
 
 								if(uContains(stat, std::string("RtabmapROS/TotalTime/ms")))
 								{
-									if(w!=-1)
+									if(w>=0)
 									{
 										slamTime.push_back(stat.at("RtabmapROS/TotalTime/ms"));
 									}
 								}
 								else if(uContains(stat, Statistics::kTimingTotal()))
 								{
-									if(w!=-1)
+									if(w>=0)
 									{
 										slamTime.push_back(stat.at(Statistics::kTimingTotal()));
 									}
@@ -575,43 +510,41 @@ int main(int argc, char * argv[])
 								for(std::map<std::string, UPlotCurve*>::iterator jter=curves.begin(); jter!=curves.end(); ++jter)
 								{
 #else
-								for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
+								for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator jter=localizationMultiStats.begin();
 									jter!=localizationMultiStats.end();
 									++jter)
 								{
 #endif
-									if(uContains(stat, jter->first))
+									if(*iter >= startId)
 									{
-										double y = stat.at(jter->first);
-#ifdef WITH_QT
-										double x = s;
-										if(useIds)
+										if(uContains(stat, jter->first))
 										{
-											x = *iter;
-										}
-										jter->second->addValue(x,y);
+											double y = stat.at(jter->first);
+#ifdef WITH_QT
+											double x = s;
+											if(useIds)
+											{
+												x = *iter;
+											}
+											jter->second->addValue(x,y);
 #endif
 
-										if(!localizationMultiStats.empty())
-										{
-											if(previousStamp > 0 && fabs(s - previousStamp) > locDelay && uContains(localizationSessionStats, jter->first))
+											if(!localizationMultiStats.empty())
 											{
-												// changed session
-												for(std::map<std::string, std::vector<float> >::iterator kter=localizationSessionStats.begin(); kter!=localizationSessionStats.end(); ++kter)
+												if(previousStamp > 0 && s - previousStamp > 10 && uContains(localizationSessionStats, jter->first))
 												{
-													LocStats values = LocStats::from(localizationSessionStats.at(kter->first));
-													localizationMultiStats.at(kter->first).rbegin()->second.push_back(values);
-													localizationSessionStats.at(kter->first).clear();
+													// changed session
+													LocStats values = LocStats::from(localizationSessionStats.at(jter->first));
+													localizationMultiStats.at(jter->first).rbegin()->second.push_back(values);
+													localizationSessionStats.at(jter->first).clear();
 												}
 
-												previousStamp = s;
+												if(!uContains(localizationSessionStats, jter->first))
+												{
+													localizationSessionStats.insert(std::make_pair(jter->first, std::vector<float>()));
+												}
+												localizationSessionStats.at(jter->first).push_back(y);
 											}
-
-											if(!uContains(localizationSessionStats, jter->first))
-											{
-												localizationSessionStats.insert(std::make_pair(jter->first, std::vector<float>()));
-											}
-											localizationSessionStats.at(jter->first).push_back(y);
 										}
 									}
 								}
@@ -620,7 +553,7 @@ int main(int argc, char * argv[])
 						}
 					}
 
-					for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator jter=localizationMultiStats.begin();
+					for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator jter=localizationMultiStats.begin();
 						jter!=localizationMultiStats.end();
 						++jter)
 					{
@@ -959,7 +892,7 @@ int main(int argc, char * argv[])
 							}
 						}
 					}
-					printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s%s, %s: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
+					printf("   %s (%d, s=%.3f):\terror lin=%.3fm (max=%.3fm, odom=%.3fm) ang=%.1fdeg%s%s, slam: avg=%dms (max=%dms) loops=%d%s, odom: avg=%dms (max=%dms), camera: avg=%dms, %smap=%dMB\n",
 							fileName.c_str(),
 							(int)ids.size(),
 							bestScale,
@@ -969,7 +902,6 @@ int main(int argc, char * argv[])
 							bestRMSEAng,
 							!outputKittiError?"":uFormat(", KITTI: t_err=%.2f%% r_err=%.2f deg/100m", kitti_t_err, kitti_r_err*100).c_str(),
 							!outputRelativeError?"":uFormat(", Relative: t_err=%.3fm r_err=%.2f deg", relative_t_err, relative_r_err).c_str(),
-							!localizationMultiStats.empty()?"loc":"slam",
 							(int)uMean(slamTime), (int)uMax(slamTime),
 							(int)loopClosureLinks.size(),
 							!outputLoopAccuracy?"":uFormat(" (t_err=%.3fm r_err=%.2f deg)", loop_t_err, loop_r_err).c_str(),
@@ -1009,29 +941,7 @@ int main(int argc, char * argv[])
 			currentPathIsDatabase = false;
 		}
 
-		if(!localizationMultiStats.empty())
-		{
-			printf("---Localization results---\n");
-			std::string prefix = "header={";
-			printf("%s", prefix.c_str());
-			for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator iter=localizationMultiStats.begin()->second.begin();
-						iter!=localizationMultiStats.begin()->second.end();)
-			{
-				if(iter!=localizationMultiStats.begin()->second.begin())
-				{
-					printf("%s",  std::string(prefix.size(), ' ').c_str());
-				}
-				printf("%s", iter->first.c_str());
-				++iter;
-				if(iter!=localizationMultiStats.begin()->second.end())
-				{
-					printf(";\n");
-				}
-			}
-			printf("}\n");
-		}
-
-		for(std::map<std::string, std::vector<std::pair<std::string, std::vector<LocStats> > > >::iterator iter=localizationMultiStats.begin();
+		for(std::map<std::string, std::map<std::string, std::vector<LocStats> > >::iterator iter=localizationMultiStats.begin();
 			iter!=localizationMultiStats.end();
 			++iter)
 		{
@@ -1040,25 +950,21 @@ int main(int argc, char * argv[])
 			{
 				if(showLoc & (0x1 << k))
 				{
-					std::string prefix = uFormat("  %s=[",
+					printf("  %s:\n",
 							k==0?"min":
 							k==1?"max":
 							k==2?"mean":
 							k==3?"stddev":
 							k==4?"total":
 							"nonnull%");
-					printf("%s", prefix.c_str());
-					for(std::vector<std::pair<std::string, std::vector<LocStats> > >::iterator jter=iter->second.begin(); jter!=iter->second.end();)
+					for(std::map<std::string, std::vector<LocStats> >::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
 					{
-						if(jter!=iter->second.begin())
-						{
-							printf("%s", std::string(prefix.size(), ' ').c_str());
-						}
+						printf("    %s ", jter->first.c_str());
 						for(size_t j=0; j<jter->second.size(); ++j)
 						{
 							if(k<4)
 							{
-								printf("%f",
+								printf("%f ",
 										k==0?jter->second[j].min:
 										k==1?jter->second[j].max:
 										k==2?jter->second[j].mean:
@@ -1066,24 +972,15 @@ int main(int argc, char * argv[])
 							}
 							else if(k==4)
 							{
-								printf("%d",jter->second[j].total);
+								printf("%d ",jter->second[j].total);
 							}
 							else if(k==5)
 							{
-								printf("%.2f", (jter->second[j].nonNull*100));
-							}
-							if(j+1 < jter->second.size())
-							{
-								printf(" ");
+								printf("%.2f ", (jter->second[j].nonNull*100));
 							}
 						}
-						++jter;
-						if(jter!=iter->second.end())
-						{
-							printf(";\n");
-						}
+						printf("\n");
 					}
-					printf("]\n");
 				}
 			}
 			iter->second.clear();
@@ -1194,34 +1091,9 @@ int main(int argc, char * argv[])
 			{
 				iter->second->frameData();
 			}
-			if(exportFigures)
-			{
-				QString data = iter->second->getAllCurveDataAsText();
-				if(!data.isEmpty())
-				{
-					QString filePath = QString(exportPrefix.c_str()) + (exportPrefix.empty()?"":"-") + iter->second->windowTitle().replace('/', "-") + ".txt";
-					QFile file(filePath);
-					if(file.open(QIODevice::Text | QIODevice::WriteOnly))
-					{
-						file.write(data.toUtf8());
-						file.close();
-						printf("Exported \"%s\".\n", filePath.toStdString().c_str());
-					}
-					else
-					{
-						printf("ERROR: could not open file \"%s\" for writing!\n", filePath.toStdString().c_str());
-					}
-				}
-			}
-			else
-			{
-				iter->second->show();
-			}
-		}
-		if(!exportFigures)
-		{
-			return app.exec();
+			iter->second->show();
 		}
+		return app.exec();
 	}
 #endif
 	return 0;

tools/Reprocess/CMakeLists.txt

@@ -9,7 +9,7 @@ SET(RTABMap_LIBRARIES
 )  
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS

tools/Reprocess/main.cpp

@@ -33,8 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #endif
 #include <rtabmap/core/OccupancyGrid.h>
 #include <rtabmap/core/Graph.h>
-#include <rtabmap/core/Memory.h>
-#include <rtabmap/core/CameraThread.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
 #include <rtabmap/utilite/UTimer.h>
@@ -58,7 +56,6 @@ void showUsage()
 			"   To see warnings when loop closures are rejected, add \"--uwarn\" argument.\n"
 			"  Options:\n"
 			"     -r                Use database stamps as input rate.\n"
-			"     -skip #           Skip # frames after each processed frame (default 0=don't skip any frames).\n"
 			"     -c \"path.ini\"   Configuration file, overwriting parameters read \n"
 			"                       from the database. If custom parameters are also set as \n"
 			"                       arguments, they overwrite those in config file and the database.\n"
@@ -69,14 +66,6 @@ void showUsage()
 			"     -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"
-			"     -scan_from_depth    Generate scans from depth images (overwrite previous\n"
-			"                         scans if they exist).\n"
-			"     -scan_downsample #  Downsample input scans.\n"
-			"     -scan_range_min #.#   Filter input scans with minimum range (m).\n"
-			"     -scan_range_max #.#   Filter input scans with maximum range (m).\n"
-			"     -scan_voxel_size #.#  Voxel filter input scans (m).\n"
-			"     -scan_normal_k #         Compute input scan normals (k-neighbors approach).\n"
-			"     -scan_normal_radius #.#  Compute input scan normals (radius(m)-neighbors approach).\n\n"
 			"%s\n"
 			"\n", Parameters::showUsage());
 	exit(1);
@@ -214,14 +203,6 @@ int main(int argc, char * argv[])
 	bool useDatabaseRate = false;
 	int startId = 0;
 	int stopId = 0;
-	int framesToSkip = 0;
-	bool scanFromDepth = false;
-	int scanDecimation = 1;
-	float scanRangeMin = 0.0f;
-	float scanRangeMax = 0.0f;
-	float scanVoxelSize = 0;
-	int scanNormalK = 0;
-	float scanNormalRadius = 0.0f;
 	ParametersMap configParameters;
 	for(int i=1; i<argc-2; ++i)
 	{
@@ -241,12 +222,10 @@ int main(int argc, char * argv[])
 			else if(i < argc - 2)
 			{
 				printf("Config file \"%s\" is not valid or doesn't exist!\n", argv[i]);
-				showUsage();
 			}
 			else
 			{
 				printf("Config file is not set!\n");
-				showUsage();
 			}
 		}
 		else if (strcmp(argv[i], "-start") == 0 || strcmp(argv[i], "--start") == 0)
@@ -257,11 +236,6 @@ int main(int argc, char * argv[])
 				startId = atoi(argv[i]);
 				printf("Start at node ID = %d.\n", startId);
 			}
-			else
-			{
-				printf("-start option requires a value\n");
-				showUsage();
-			}
 		}
 		else if (strcmp(argv[i], "-stop") == 0 || strcmp(argv[i], "--stop") == 0)
 		{
@@ -271,25 +245,6 @@ int main(int argc, char * argv[])
 				stopId = atoi(argv[i]);
 				printf("Stop at node ID = %d.\n", stopId);
 			}
-			else
-			{
-				printf("-stop option requires a value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-skip") == 0 || strcmp(argv[i], "--skip") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				framesToSkip = atoi(argv[i]);
-				printf("Will skip %d frames.\n", framesToSkip);
-			}
-			else
-			{
-				printf("-skip option requires a value\n");
-				showUsage();
-			}
 		}
 		else if(strcmp(argv[i], "-p") == 0 || strcmp(argv[i], "--p") == 0)
 		{
@@ -324,95 +279,6 @@ int main(int argc, char * argv[])
 			printf("RTAB-Map is not built with OctoMap support, cannot set -o3 option!\n");
 #endif
 		}
-		else if (strcmp(argv[i], "-scan_from_depth") == 0 || strcmp(argv[i], "--scan_from_depth") == 0)
-		{
-			scanFromDepth = true;
-		}
-		else if (strcmp(argv[i], "-scan_downsample") == 0 || strcmp(argv[i], "--scan_downsample") == 0 ||
-				strcmp(argv[i], "-scan_decimation") == 0 || strcmp(argv[i], "--scan_decimation") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanDecimation = atoi(argv[i]);
-				printf("Scan from depth decimation = %d.\n", scanDecimation);
-			}
-			else
-			{
-				printf("-scan_downsample option requires 1 value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-scan_range_min") == 0 || strcmp(argv[i], "--scan_range_min") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanRangeMin = atof(argv[i]);
-				printf("Scan range min = %f m.\n", scanRangeMin);
-			}
-			else
-			{
-				printf("-scan_range_min option requires 1 value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-scan_range_max") == 0 || strcmp(argv[i], "--scan_range_max") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanRangeMax = atof(argv[i]);
-				printf("Scan range max = %f m.\n", scanRangeMax);
-			}
-			else
-			{
-				printf("-scan_range_max option requires 1 value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-scan_voxel_size") == 0 || strcmp(argv[i], "--scan_voxel_size") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanVoxelSize = atof(argv[i]);
-				printf("Scan voxel size = %f m.\n", scanVoxelSize);
-			}
-			else
-			{
-				printf("-scan_voxel_size option requires 1 value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-scan_normal_k") == 0 || strcmp(argv[i], "--scan_normal_k") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanNormalK = atoi(argv[i]);
-				printf("Scan normal k = %d.\n", scanNormalK);
-			}
-			else
-			{
-				printf("-scan_normal_k option requires 1 value\n");
-				showUsage();
-			}
-		}
-		else if (strcmp(argv[i], "-scan_normal_radius") == 0 || strcmp(argv[i], "--scan_normal_radius") == 0)
-		{
-			++i;
-			if(i < argc - 2)
-			{
-				scanNormalRadius = atof(argv[i]);
-				printf("Scan normal radius = %f m.\n", scanNormalRadius);
-			}
-			else
-			{
-				printf("-scan_normal_radius option requires 1 value\n");
-				showUsage();
-			}
-		}
 	}
 
 	std::string inputDatabasePath = uReplaceChar(argv[argc-2], '~', UDirectory::homeDir());
@@ -532,11 +398,6 @@ int main(int argc, char * argv[])
 	delete dbDriver;
 	dbDriver = 0;
 
-	if(framesToSkip)
-	{
-		totalIds/=framesToSkip+1;
-	}
-
 	std::string workingDirectory = UDirectory::getDir(outputDatabasePath);
 	printf("Set working directory to \"%s\".\n", workingDirectory.c_str());
 	uInsert(parameters, ParametersPair(Parameters::kRtabmapWorkingDirectory(), workingDirectory));
@@ -556,8 +417,8 @@ int main(int argc, char * argv[])
 	bool rgbdEnabled = Parameters::defaultRGBDEnabled();
 	Parameters::parse(parameters, Parameters::kRGBDEnabled(), rgbdEnabled);
 	bool odometryIgnored = !rgbdEnabled;
-	DBReader * dbReader = new DBReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored, false, false, startId, -1, stopId);
-	dbReader->init();
+	DBReader dbReader(inputDatabasePath, useDatabaseRate?-1:0, odometryIgnored, false, false, startId, -1, stopId);
+	dbReader.init();
 
 	OccupancyGrid grid(parameters);
 	grid.setCloudAssembling(assemble3dMap);
@@ -574,14 +435,7 @@ int main(int argc, char * argv[])
 	std::map<std::string, float> globalMapStats;
 	int processed = 0;
 	CameraInfo info;
-	SensorData data = dbReader->takeImage(&info);
-	CameraThread camThread(dbReader, parameters); // take ownership of dbReader
-	camThread.setScanParameters(scanFromDepth, scanDecimation, scanRangeMin, scanRangeMax, scanVoxelSize, scanNormalK, scanNormalRadius);
-	if(scanFromDepth)
-	{
-		data.setLaserScan(LaserScan());
-	}
-	camThread.postUpdate(&data, &info);
+	SensorData data = dbReader.takeImage(&info);
 	Transform lastLocalizationOdomPose = info.odomPose;
 	bool inMotion = true;
 	while(data.isValid() && g_loopForever)
@@ -763,32 +617,7 @@ int main(int argc, char * argv[])
 		}
 
 		Transform odomPose = info.odomPose;
-
-		if(framesToSkip>0)
-		{
-			int skippedFrames = framesToSkip;
-			while(skippedFrames-- > 0)
-			{
-				data = dbReader->takeImage(&info);
-				if(!odometryIgnored && !info.odomCovariance.empty() && info.odomCovariance.at<double>(0,0)>=9999)
-				{
-					printf("High variance detected, triggering a new map...\n");
-					if(!incrementalMemory && processed>0)
-					{
-						showLocalizationStats(outputDatabasePath);
-						lastLocalizationOdomPose = info.odomPose;
-					}
-					rtabmap.triggerNewMap();
-				}
-			}
-		}
-
-		data = dbReader->takeImage(&info);
-		if(scanFromDepth)
-		{
-			data.setLaserScan(LaserScan());
-		}
-		camThread.postUpdate(&data, &info);
+		data = dbReader.takeImage(&info);
 
 		inMotion = true;
 		if(!incrementalMemory &&
@@ -805,7 +634,6 @@ int main(int argc, char * argv[])
 		}
 	}
 
-	int databasesMerged = 0;
 	if(!incrementalMemory)
 	{
 		showLocalizationStats(outputDatabasePath);
@@ -813,23 +641,6 @@ int main(int argc, char * argv[])
 	else
 	{
 		printf("Total loop closures = %d (Loop=%d, Prox=%d, In Motion=%d/%d)\n", loopCount+proxCount, loopCount, proxCount, loopCountMotion, totalFramesMotion);
-
-		if(databases.size()>1)
-		{
-			std::map<int, Transform> poses;
-			std::multimap<int, Link> constraints;
-			rtabmap.getGraph(poses, constraints, 0, 1, 0, false, false, false, false, false, false);
-			std::set<int> mapIds;
-			for(std::map<int, Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
-			{
-				int id;
-				if((id=rtabmap.getMemory()->getMapId(iter->first, true))>=0)
-				{
-					mapIds.insert(id);
-				}
-			}
-			databasesMerged = mapIds.size();
-		}
 	}
 
 	printf("Closing database \"%s\"...\n", outputDatabasePath.c_str());
@@ -1001,5 +812,5 @@ int main(int argc, char * argv[])
 	}
 #endif
 
-	return databasesMerged;
+	return 0;
 }

tools/RgbdDataset/CMakeLists.txt

@@ -11,7 +11,7 @@ SET(RTABMap_LIBRARIES
 )  
 
 if(POLICY CMP0020)
-	cmake_policy(SET CMP0020 NEW)
+	cmake_policy(SET CMP0020 OLD)
 endif()
 
 SET(INCLUDE_DIRS