merged master->rolling

ORB_SLAM3: fixed error if left image is color. SensorCaptureThread: stop thread if data cannot be captured (e.g., end of dataset) instead of skipping indefinitly with error log.
arcore_java: fixed high speed filtering jumping (#1281 )
2026-09-11 05:50:21 +08:00 · 2024-05-27 11:42:12 -07:00 · 2024-05-26 18:04:07 -07:00 · 2024-05-25 15:54:57 -07:00 · 2024-05-25 14:42:54 -07:00 · 2024-05-25 13:50:01 -07:00
186 changed files with 13888 additions and 8418 deletions
@@ -20,6 +20,7 @@ jobs:
          docker_platforms: |
            linux/amd64
            linux/arm64
+            linux/arm/v7
          docker_path: 'focal/deps'
        - docker_tag: jammy-deps
          docker_tags: |
@@ -94,6 +95,7 @@ jobs:
          docker_platforms: |
            linux/amd64
            linux/arm64
+            linux/arm/v7
          docker_path: 'focal'
        - docker_tag: jammy
          docker_tags: |
@@ -0,0 +1,16 @@
+name: RTAB-Map Scheduled Stats Extraction From GitHub
+
+on:
+    workflow_dispatch:
+    schedule:
+        - cron: '0 5 * * *'
+jobs:
+    get_stats:
+        runs-on: ubuntu-latest
+        steps:
+            - name: Update Stats
+              uses: introlab/github-stats-action@v1
+              with:
+                  github-stats-token: ${{ secrets.STATS_TOKEN }}
+                  google-application-credentials: ${{ secrets.GOOGLE_APPLICATION_CREDENTIALS }}
+                  spreadsheet-id: ${{ secrets.SPREADSHEET_ID }}
@@ -1,5 +1,5 @@
 # Top-Level CmakeLists.txt
-cmake_minimum_required(VERSION 3.10)
+cmake_minimum_required(VERSION 3.14)
 PROJECT( RTABMap )
 SET(PROJECT_PREFIX rtabmap)

@@ -20,7 +20,7 @@ SET(CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake_modules")
 #######################
 SET(RTABMAP_MAJOR_VERSION 0)
 SET(RTABMAP_MINOR_VERSION 21)
-SET(RTABMAP_PATCH_VERSION 4)
+SET(RTABMAP_PATCH_VERSION 5)
 SET(RTABMAP_VERSION
  ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
  
@@ -252,10 +252,10 @@ endif()

 FIND_PACKAGE(ZLIB REQUIRED QUIET)

-FIND_PACKAGE(Sqlite3 QUIET)
-IF(Sqlite3_FOUND)
-   MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
-ENDIF(Sqlite3_FOUND)
+FIND_PACKAGE(SQLite3 QUIET)
+IF(SQLite3_FOUND)
+   MESSAGE(STATUS "Found SQLite3: ${SQLite3_INCLUDE_DIRS} ${SQLite3_LIBRARIES}")
+ENDIF(SQLite3_FOUND)

 if(NOT "${PCL_LIBRARIES}" STREQUAL "")
   # fix libproj.so not found on Xenial
@@ -539,6 +539,11 @@ IF(WITH_POINTMATCHER)
            message(STATUS "libnabo found, version ${libnabo_VERSION} (Config mode)")
            SET(libpointmatcher_LIBRARIES "${libpointmatcher_LIBRARIES};libnabo::nabo")
        ENDIF(value EQUAL -1)
+        string(FIND "${libpointmatcher_LIBRARIES}" "yaml-cpp::yaml-cpp" value)
+        IF(NOT value EQUAL -1)
+            # Find yaml-cpp (Issue #1268):
+            find_package(yaml-cpp QUIET)
+        ENDIF(NOT value EQUAL -1)
    ENDIF(libpointmatcher_FOUND)
 ENDIF(WITH_POINTMATCHER)

@@ -651,7 +656,7 @@ IF(WITH_MYNTEYE)
 ENDIF(WITH_MYNTEYE)

 IF(WITH_DEPTHAI)
-    FIND_PACKAGE(depthai 2 QUIET)
+    FIND_PACKAGE(depthai 2.24 QUIET)
    IF(depthai_FOUND)
       MESSAGE(STATUS "Found depthai-core (targets)")
    ENDIF(depthai_FOUND)
@@ -1332,10 +1337,10 @@ ELSE()
 MESSAGE(STATUS "  With Qt                   = NO (Qt not found)")
 ENDIF()

-IF(Sqlite3_FOUND)
+IF(SQLite3_FOUND)
 MESSAGE(STATUS "  With external SQLite3     = YES (License: Public Domain)")
 ELSE()
-MESSAGE(STATUS "  With external SQLite3     = NO (sqlite3 not found, internal version is used for convenience)")
+MESSAGE(STATUS "  With external SQLite3     = NO (SQLite3 not found, internal version is used for convenience)")
 ENDIF()

 IF(WITH_ORB_OCTREE)
@@ -334,11 +334,12 @@ void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayR
 	}
 }

-SensorData CameraARCore::captureImage(CameraInfo * info)
+SensorData CameraARCore::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");

+	pose.setNull();
 	SensorData data;
 	if(!arSession_)
 	{
@@ -370,7 +371,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	if (geometry_changed != 0 || !uvs_initialized_) {
 		ArFrame_transformCoordinates2d(
 				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
-				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVerticesDevice, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
 				transformed_uvs_);
 		UASSERT(transformed_uvs_);
 		uvs_initialized_ = true;
@@ -393,7 +394,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);

-	Transform pose;
 	CameraModel model;
 	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
 	{
@@ -401,24 +401,13 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		float pose_raw[7];
 		ArCamera_getPose(arSession_, ar_camera, arPose_);
 		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+		Transform poseArCore = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+		poseArCore = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world;

-		Transform poseArCore = pose;
-		if(pose.isNull())
+		if(poseArCore.isNull())
 		{
 			LOGE("CameraARCore: Pose is null");
 		}
-		else
-		{
-			this->poseReceived(pose);
-			// adjust origin
-			if(!getOriginOffset().isNull())
-			{
-				pose = getOriginOffset() * pose;
-			}
-			info->odomPose = pose;
-		}

 		// Get calibration parameters
 		float fx,fy, cx, cy;
@@ -551,6 +540,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)

 						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
+
+						if(!poseArCore.isNull())
+						{
+							pose = poseArCore;
+							this->poseReceived(pose, stamp);
+							// adjust origin
+							if(!getOriginOffset().isNull())
+							{
+								pose = getOriginOffset() * pose;
+							}
+						}
 					}
 				}
 				else
@@ -571,134 +571,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 	ArCamera_release(ar_camera);

 	return data;
-
-}
-
-void CameraARCore::capturePoseOnly()
-{
-	UScopeMutex lock(arSessionMutex_);
-	//LOGI("Capturing image...");
-
-	if(!arSession_)
-	{
-		return;
-	}
-
-	if(textureId_ != 0)
-	{
-		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
-		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
-		ArSession_setCameraTextureName(arSession_, textureId_);
-	}
-
-	// Update session to get current frame and render camera background.
-	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
-		return;
-	}
-
-	// If display rotation changed (also includes view size change), we need to
-	// re-query the uv coordinates for the on-screen portion of the camera image.
-	int32_t geometry_changed = 0;
-	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
-	if (geometry_changed != 0 || !uvs_initialized_) {
-		ArFrame_transformCoordinates2d(
-				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
-				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
-				transformed_uvs_);
-		UASSERT(transformed_uvs_);
-		uvs_initialized_ = true;
-	}
-
-	ArCamera* ar_camera;
-	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
-
-	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
-	ArCamera_getProjectionMatrix(arSession_, ar_camera,
-							   /*near=*/0.1f, /*far=*/100.f,
-							   glm::value_ptr(projectionMatrix_));
-
-	// adjust origin
-	if(!getOriginOffset().isNull())
-	{
-		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-	}
-
-	ArTrackingState camera_tracking_state;
-	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
-
-	Transform pose;
-	CameraModel model;
-	if(camera_tracking_state == AR_TRACKING_STATE_TRACKING)
-	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		ArCamera_getPose(arSession_, ar_camera, arPose_);
-		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		if(!pose.isNull())
-		{
-			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-			this->poseReceived(pose);
-
-			if(!getOriginOffset().isNull())
-			{
-				pose = getOriginOffset() * pose;
-			}
-		}
-
-		int32_t is_depth_supported = 0;
-		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
-
-		if(is_depth_supported)
-		{
-			LOGD("Acquire depth image!");
-			ArImage * depthImage = nullptr;
-			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
-
-			ArImageFormat format;
-			ArImage_getFormat(arSession_, depthImage, &format);
-			if(format == AR_IMAGE_FORMAT_DEPTH16)
-			{
-				LOGD("Depth format detected!");
-				int planeCount;
-				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
-				LOGD("planeCount=%d", planeCount);
-				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-				const uint8_t *data = nullptr;
-				int len = 0;
-				int stride;
-				int width;
-				int height;
-				ArImage_getWidth(arSession_, depthImage, &width);
-				ArImage_getHeight(arSession_, depthImage, &height);
-				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
-				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
-
-				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
-
-				cv::Mat occlusionImage = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
-
-				float fx,fy, cx, cy;
-				int32_t rgb_width, rgb_height;
-				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
-				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
-				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
-				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
-
-				float scaleX = (float)width / (float)rgb_width;
-				float scaleY = (float)height / (float)rgb_height;
-				CameraModel occlusionModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
-				this->setOcclusionImage(occlusionImage, occlusionModel);
-			}
-			ArImage_release(depthImage);
-		}
-	}
-
-	ArCamera_release(ar_camera);
 }

 } /* namespace rtabmap */
@@ -63,23 +63,14 @@ public:
 	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();

-	bool uvsInitialized() const {return uvs_initialized_;}
-	const float* uvsTransformed() const {return transformed_uvs_;}
-	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}
-
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	void setupGL();
-	virtual void close(); // close Tango connection
+	virtual void close(); // close ARCore connection
 	virtual std::string getSerial() const;
-	GLuint getTextureId() const {return textureId_;}
-
-	void imageCallback(AImageReader *reader);

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
-	virtual void capturePoseOnly();
+	virtual SensorData updateDataOnRender(Transform & pose); // should be called in opengl thread

 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -117,9 +117,6 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri

 	deviceTColorCamera_ = opticalRotation;

-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	HwArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (HwArSession_resume(arSession_) != HWAR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -169,38 +166,87 @@ void CameraAREngine::close()
 	CameraMobile::close();
 }

-SensorData CameraAREngine::captureImage(CameraInfo * info)
+void CameraAREngine::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		HwArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
+SensorData CameraAREngine::updateDataOnRender(Transform & pose)
 {
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");

+	pose.setNull();
 	SensorData data;
 	if(!arSession_)
 	{
 		return data;
 	}

+	if(textureId_ == 0)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	if(textureId_!=0)
+		HwArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
 		LOGE("CameraAREngine::captureImage() ArSession_update error");
 		return data;
 	}

+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	HwArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		HwArFrame_transformDisplayUvCoords(
+				arSession_, arFrame_,
+				BackgroundRenderer::kNumVertices*2, BackgroundRenderer_kVerticesView,
+				transformed_uvs_);
+		UERROR("uv: (%f,%f) (%f,%f) (%f,%f) (%f,%f)", 
+			transformed_uvs_[0], transformed_uvs_[1],
+			transformed_uvs_[2], transformed_uvs_[3],
+			transformed_uvs_[4], transformed_uvs_[5],
+			transformed_uvs_[6], transformed_uvs_[7]);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	HwArCamera* ar_camera;
 	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);

+	HwArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	HwArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
+	// adjust origin
+	if(!getOriginOffset().isNull())
+	{
+		viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+	}
+
 	HwArTrackingState camera_tracking_state;
 	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);

-	Transform pose;
 	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
 	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		HwArCamera_getPose(arSession_, ar_camera, arPose_);
-		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-
 		// Get calibration parameters
 		// FIXME: Hard-coded as getting intrinsics with the api fails
 		float fx=492.689667,fy=492.606201, cx=323.594849, cy=234.659744;
@@ -274,6 +320,26 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 					double stamp = double(timestamp_ns)/10e8;
 					CameraModel model = CameraModel(fx, fy, cx, cy, deviceTColorCamera_, 0, cv::Size(camWidth, camHeight));
 					data = SensorData(outputRGB, outputDepth, model, 0, stamp);
+
+					// pose in OpenGL coordinates
+					float pose_raw[7];
+					HwArCamera_getPose(arSession_, ar_camera, arPose_);
+					HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
+					pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
+					if(pose.isNull())
+					{
+						LOGE("CameraAREngine: Pose is null");
+					}
+					else
+					{
+						pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+						this->poseReceived(pose, stamp);
+						// adjust origin
+						if(!getOriginOffset().isNull())
+						{
+							pose = getOriginOffset() * pose;
+						}
+					}
 				}
 			}
 			else
@@ -291,66 +357,8 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
 	}

 	HwArCamera_release(ar_camera);
-
-	if(pose.isNull())
-	{
-		LOGE("CameraAREngine: Pose is null");
-	}
-	else
-	{
-		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-		this->poseReceived(pose);
-        // adjust origin
-        if(!getOriginOffset().isNull())
-        {
-            pose = getOriginOffset() * pose;
-        }
-		info->odomPose = pose;
-	}
 	return data;

 }

-void CameraAREngine::capturePoseOnly()
-{
-	UScopeMutex lock(arSessionMutex_);
-	//LOGI("Capturing image...");
-
-	SensorData data;
-	if(!arSession_)
-	{
-		return;
-	}
-
-	// Update session to get current frame and render camera background.
-	if (HwArSession_update(arSession_, arFrame_) != HWAR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
-		return;
-	}
-
-	HwArCamera* ar_camera;
-	HwArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
-
-	HwArTrackingState camera_tracking_state;
-	HwArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
-
-	Transform pose;
-	CameraModel model;
-	if(camera_tracking_state == HWAR_TRACKING_STATE_TRACKING)
-	{
-		// pose in OpenGL coordinates
-		float pose_raw[7];
-		HwArCamera_getPose(arSession_, ar_camera, arPose_);
-		HwArPose_getPoseRaw(arSession_, arPose_, pose_raw);
-		pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]);
-		if(!pose.isNull())
-		{
-			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-			this->poseReceived(pose);
-		}
-	}
-
-	HwArCamera_release(ar_camera);
-}
-
 } /* namespace rtabmap */
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>

 #include <huawei_arengine_interface.h>

@@ -48,13 +49,14 @@ public:
 	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
 	virtual ~CameraAREngine();

+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual void close(); // close Tango connection
+	virtual void close(); // close AREngine connection
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void capturePoseOnly();
+	virtual SensorData updateDataOnRender(Transform & pose);

 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -69,7 +71,6 @@ private:
 	HwArCameraIntrinsics *arCameraIntrinsics_ = nullptr;
 	HwArPose * arPose_ = nullptr;
 	bool arInstallRequested_;
-	GLuint textureId_;
 	UMutex arSessionMutex_;

 };
@@ -55,10 +55,8 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 CameraMobile::CameraMobile(bool smoothing) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
-		spinOncePreviousStamp_(0.0),
 		textureId_(0),
 		uvs_initialized_(false),
-		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
@@ -79,13 +77,12 @@ bool CameraMobile::init(const std::string &, const std::string &)

 void CameraMobile::close()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
 	originOffset_ = Transform();
 	originUpdate_ = false;
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();

    if(textureId_ != 0)
@@ -97,35 +94,107 @@ void CameraMobile::close()

 void CameraMobile::resetOrigin()
 {
-	previousPose_.setNull();
-	previousStamp_ = 0.0;
+	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
-	pose_ = Transform();
+	dataPose_ = Transform();
 	data_ = SensorData();
 	originUpdate_ = true;
 }

-void CameraMobile::poseReceived(const Transform & pose)
+bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+{
+	pose.setNull();
+
+	int maxWaitTimeMs = maxWaitTime * 1000;
+
+	// Interpolate pose
+	if(!poseBuffer_.empty())
+	{
+		poseMutex_.lock();
+		int waitTry = 0;
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		{
+			poseMutex_.unlock();
+			++waitTry;
+			uSleep(1);
+			poseMutex_.lock();
+		}
+		if(poseBuffer_.rbegin()->first < stamp)
+		{
+			if(maxWaitTimeMs > 0)
+			{
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", stamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
+			}
+			else
+			{
+				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
+			}
+		}
+		else
+		{
+			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
+			std::map<double, Transform>::const_iterator iterA = iterB;
+			if(iterA != poseBuffer_.begin())
+			{
+				iterA = --iterA;
+			}
+			if(iterB == poseBuffer_.end())
+			{
+				iterB = --iterB;
+			}
+			if(iterA == iterB && stamp == iterA->first)
+			{
+				pose = iterA->second;
+			}
+			else if(stamp >= iterA->first && stamp <= iterB->first)
+			{
+				pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+			}
+			else // stamp < iterA->first
+			{
+				UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", stamp, iterA->first);
+			}
+		}
+		poseMutex_.unlock();
+	}
+	return !pose.isNull();
+}
+
+void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 {
 	if(!pose.isNull())
 	{
-		// send pose of the camera (without optical rotation)
-		Transform p = pose*deviceTColorCamera_;
+		Transform p = pose;
 		if(originUpdate_)
 		{
 			originOffset_ = p.translation().inverse();
 			originUpdate_ = false;
 		}
+		
+		if(stampEpochOffset_ == 0.0)
+		{
+			stampEpochOffset_ = UTimer::now() - deviceStamp;
+		}
+        
+		double epochStamp = stampEpochOffset_ + deviceStamp;

 		if(!originOffset_.isNull())
 		{
-			this->post(new PoseEvent(originOffset_*p));
+			p = originOffset_*p;
 		}
-		else
+
 		{
-			this->post(new PoseEvent(p));
+			UScopeMutex lock(poseMutex_);
+			poseBuffer_.insert(poseBuffer_.end(), std::make_pair(epochStamp, p));
+			if(poseBuffer_.size() > 1000)
+			{
+				poseBuffer_.erase(poseBuffer_.begin());
+			}
 		}
+
+		// send pose of the camera (with optical rotation)
+		this->post(new PoseEvent(p * deviceTColorCamera_));
 	}
 }

@@ -139,11 +208,20 @@ void CameraMobile::setGPS(const GPS & gps)
 	lastKnownGPS_ = gps;
 }

-void CameraMobile::setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+void CameraMobile::addEnvSensor(int type, float value)
 {
-	LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
+}
+
+void CameraMobile::update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord)
+{
+	UScopeMutex lock(dataMutex_);
+
+	bool notify = !data_.isValid();
+
+	LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
 	data_ = data;
-    pose_ = pose;
+    dataPose_ = pose;

    viewMatrix_ = viewMatrix;
    projectionMatrix_ = projectionMatrix;
@@ -151,7 +229,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
    // adjust origin
    if(!originOffset_.isNull())
    {
-        pose_ = originOffset_ * pose_;
+        dataPose_ = originOffset_ * dataPose_;
        viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
    }
    
@@ -166,7 +244,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
        uvs_initialized_ = true;
    }
    
-    LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
+    LOGD("CameraMobile::update textureId_=%d", (int)textureId_);

    if(textureId_ != 0 && texCoord != 0)
    {
@@ -193,78 +271,63 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
            return;
        }
    }
-}

-void CameraMobile::addEnvSensor(int type, float value)
-{
-	lastEnvSensors_.insert(std::make_pair((EnvSensor::Type)type, EnvSensor((EnvSensor::Type)type, value)));
-}
-
-void CameraMobile::spinOnce()
-{
-	if(!this->isRunning())
+	postUpdate();
+	
+	if(notify)
 	{
-		bool ignoreFrame = false;
-		//float rate = 10.0f; // maximum 10 FPS for image data
-		double now = UTimer::now();
-		/*if(rate>0.0f)
-		{
-			if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) ||
-				((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate))
-			{
-				ignoreFrame = true;
-			}
-		}*/
+		dataReady_.release();
+	}
+}

-		if(!ignoreFrame)
+void CameraMobile::updateOnRender()
+{
+	UScopeMutex lock(dataMutex_);
+	bool notify = !data_.isValid();
+
+	data_ = updateDataOnRender(dataPose_);
+
+	if(data_.isValid())
+	{
+		postUpdate();
+
+		if(notify)
 		{
-			spinOnceFrameRateTimer_.start();
-			spinOncePreviousStamp_ = now;
-			mainLoop();
-		}
-		else
-		{
-			// just send pose
-			capturePoseOnly();
+			dataReady_.release();
 		}
 	}
 }

-void CameraMobile::mainLoopBegin()
+SensorData CameraMobile::updateDataOnRender(Transform & pose)
 {
-	double t = cameraStartedTime_.elapsed();
-	if(t < 5.0)
-	{
-		uSleep((5.0-t)*1000); // just to make sure that the camera is started
-	}
+	LOGE("To use CameraMobile::updateOnRender(), CameraMobile::updateDataOnRender() "
+	     "should be overridden by inherited classes. Returning empty data!\n");
+	return SensorData();
 }

-void CameraMobile::mainLoop()
+void CameraMobile::postUpdate()
 {
-	CameraInfo info;
-	SensorData data = this->captureImage(&info);
-
-	if(data.isValid() && !info.odomPose.isNull())
+	if(data_.isValid())
 	{
-		if(lastKnownGPS_.stamp() > 0.0 && data.stamp()-lastKnownGPS_.stamp()<1.0)
+		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
 		{
-			data.setGPS(lastKnownGPS_);
+			data_.setGPS(lastKnownGPS_);
 		}
 		else if(lastKnownGPS_.stamp()>0.0)
 		{
-			LOGD("GPS too old (current time=%f, gps time = %f)", data.stamp(), lastKnownGPS_.stamp());
+			LOGD("GPS too old (current time=%f, gps time = %f)", data_.stamp(), lastKnownGPS_.stamp());
 		}

 		if(lastEnvSensors_.size())
 		{
-			data.setEnvSensors(lastEnvSensors_);
+			data_.setEnvSensors(lastEnvSensors_);
 			lastEnvSensors_.clear();
 		}

-		if(smoothing_ && !data.depthRaw().empty())
+		if(smoothing_ && !data_.depthRaw().empty())
 		{
 			//UTimer t;
-			data.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
+			data_.setDepthOrRightRaw(rtabmap::util2d::fastBilateralFiltering(data_.depthRaw(), bilateralFilteringSigmaS, bilateralFilteringSigmaR));
 			//LOGD("Bilateral filtering, time=%fs", t.ticks());
 		}

@@ -273,15 +336,15 @@ void CameraMobile::mainLoop()
 		{
 			UDEBUG("ROTATION_90");
 			cv::Mat rgb, depth;
-			cv::Mat rgbt(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
-			cv::flip(data.imageRaw(),rgb,1);
+			cv::Mat rgbt(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::flip(data_.imageRaw(),rgb,1);
 			cv::transpose(rgb,rgbt);
 			rgb = rgbt;
-			cv::Mat deptht(data.depthRaw().cols, data.depthRaw().rows, data.depthRaw().type());
-			cv::flip(data.depthRaw(),depth,1);
+			cv::Mat deptht(data_.depthRaw().cols, data_.depthRaw().rows, data_.depthRaw().type());
+			cv::flip(data_.depthRaw(),depth,1);
 			cv::transpose(depth,deptht);
 			depth = deptht;
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -290,25 +353,25 @@ void CameraMobile::mainLoop()
 					model.cx()>0?model.imageWidth()-model.cx():0,
 					model.localTransform()*rtabmap::Transform(0,-1,0,0, 1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);

-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = data.keypoints()[i].pt.y;
-				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.x;
+				keypoints[i].pt.x = data_.keypoints()[i].pt.y;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.x;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_180)
 		{
 			UDEBUG("ROTATION_180");
 			cv::Mat rgb, depth;
-			cv::flip(data.imageRaw(),rgb,1);
+			cv::flip(data_.imageRaw(),rgb,1);
 			cv::flip(rgb,rgb,0);
-			cv::flip(data.depthOrRightRaw(),depth,1);
+			cv::flip(data_.depthOrRightRaw(),depth,1);
 			cv::flip(depth,depth,0);
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageWidth(), model.imageHeight());
 			model = CameraModel(
 					model.fx(),
@@ -317,26 +380,26 @@ void CameraMobile::mainLoop()
 					model.cy()>0?model.imageHeight()-model.cy():0,
 					model.localTransform()*rtabmap::Transform(0,0,0,0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);

-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.x;
-				keypoints[i].pt.y = rgb.rows - data.keypoints()[i].pt.y;
+				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.x;
+				keypoints[i].pt.y = rgb.rows - data_.keypoints()[i].pt.y;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
 		else if(colorCameraToDisplayRotation_ == ROTATION_270)
 		{
 			UDEBUG("ROTATION_270");
-			cv::Mat rgb(data.imageRaw().cols, data.imageRaw().rows, data.imageRaw().type());
-			cv::transpose(data.imageRaw(),rgb);
+			cv::Mat rgb(data_.imageRaw().cols, data_.imageRaw().rows, data_.imageRaw().type());
+			cv::transpose(data_.imageRaw(),rgb);
 			cv::flip(rgb,rgb,1);
-			cv::Mat depth(data.depthOrRightRaw().cols, data.depthOrRightRaw().rows, data.depthOrRightRaw().type());
-			cv::transpose(data.depthOrRightRaw(),depth);
+			cv::Mat depth(data_.depthOrRightRaw().cols, data_.depthOrRightRaw().rows, data_.depthOrRightRaw().type());
+			cv::transpose(data_.depthOrRightRaw(),depth);
 			cv::flip(depth,depth,1);
-			CameraModel model = data.cameraModels()[0];
+			CameraModel model = data_.cameraModels()[0];
 			cv::Size sizet(model.imageHeight(), model.imageWidth());
 			model = CameraModel(
 					model.fy(),
@@ -345,61 +408,54 @@ void CameraMobile::mainLoop()
 					model.cx(),
 					model.localTransform()*rtabmap::Transform(0,1,0,0, -1,0,0,0, 0,0,1,0));
 			model.setImageSize(sizet);
-			data.setRGBDImage(rgb, depth, model);
+			data_.setRGBDImage(rgb, depth, model);

-			std::vector<cv::KeyPoint> keypoints = data.keypoints();
+			std::vector<cv::KeyPoint> keypoints = data_.keypoints();
 			for(size_t i=0; i<keypoints.size(); ++i)
 			{
-				keypoints[i].pt.x = rgb.cols - data.keypoints()[i].pt.y;
-				keypoints[i].pt.y = data.keypoints()[i].pt.x;
+				keypoints[i].pt.x = rgb.cols - data_.keypoints()[i].pt.y;
+				keypoints[i].pt.y = data_.keypoints()[i].pt.x;
 			}
-			data.setFeatures(keypoints, data.keypoints3D(), cv::Mat());
+			data_.setFeatures(keypoints, data_.keypoints3D(), cv::Mat());
 		}
-
-		rtabmap::Transform pose = info.odomPose;
-		data.setGroundTruth(Transform());
-
-		// convert stamp to epoch
-		bool firstFrame = previousPose_.isNull();
-		if(firstFrame)
-		{
-			stampEpochOffset_ = UTimer::now()-data.stamp();
-		}
-		data.setStamp(stampEpochOffset_ + data.stamp());
-		OdometryInfo info;
-		if(!firstFrame)
-		{
-			info.interval = data.stamp()-previousStamp_;
-			info.transform = previousPose_.inverse() * pose;
-		}
-		// linear cov = 0.0001
-		info.reg.covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
-		if(!firstFrame)
-		{
-			// angular cov = 0.000001
-			info.reg.covariance.at<double>(3,3) *= 0.01;
-			info.reg.covariance.at<double>(4,4) *= 0.01;
-			info.reg.covariance.at<double>(5,5) *= 0.01;
-		}
-		LOGI("Publish odometry message (variance=%f)", firstFrame?9999:0.0001);
-		this->post(new OdometryEvent(data, pose, info));
-		previousPose_ = pose;
-		previousStamp_ = data.stamp();
-	}
-	else if(!this->isKilled() && info.odomPose.isNull())
-	{
-		LOGW("Odometry lost");
-		this->post(new OdometryEvent());
 	}
 }

-SensorData CameraMobile::captureImage(CameraInfo * info)
+SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 {
-	if(info)
+	SensorData data;
+	if(dataReady_.acquire(1, 5000))
 	{
-		info->odomPose = pose_;
+		UScopeMutex lock(dataMutex_);
+		data = data_;
+		data_ = SensorData();
 	}
-	return data_;
+	if(data.isValid())
+	{
+		data.setGroundTruth(Transform());
+		data.setStamp(stampEpochOffset_ + data.stamp());
+
+		if(info)
+		{
+			// linear cov = 0.0001
+			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
+			if(!firstFrame_)
+			{
+				// angular cov = 0.000001
+				info->odomCovariance.at<double>(3,3) *= 0.01;
+				info->odomCovariance.at<double>(4,4) *= 0.01;
+				info->odomCovariance.at<double>(5,5) *= 0.01;
+			}
+			info->odomPose = dataPose_;
+		}
+
+		firstFrame_ = false;
+	}
+	else
+	{
+		UWARN("CameraMobile::captureImage() invalid data!");
+	}
+	return data;
 }

 LaserScan CameraMobile::scanFromPointCloudData(
@@ -68,7 +68,7 @@ private:
 	Transform pose_;
 };

-class CameraMobile : public Camera, public UThread, public UEventsSender {
+class CameraMobile : public Camera, public UEventsSender {
 public:
 	static const float bilateralFilteringSigmaS;
 	static const float bilateralFilteringSigmaR;
@@ -93,14 +93,20 @@ public:

 	// abstract functions
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
-	virtual void close(); // inherited classes should call its parent in their close().
+	virtual void close(); // inherited classes should call its parent at the end of their close().
 	virtual std::string getSerial() const {return "CameraMobile";}

+	void update(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);
+	void updateOnRender();
+
 	const Transform & getOriginOffset() const {return originOffset_;} // in rtabmap frame
 	void resetOrigin();
 	virtual bool isCalibrated() const;

-	void poseReceived(const Transform & pose); // in rtabmap frame
+	virtual bool odomProvided() const { return true; }
+	virtual bool getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06); // Return pose of device in rtabmap frame (with origin offset), stamp should be epoch time
+	void poseReceived(const Transform & pose, double deviceStamp); // original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
+	double getStampEpochOffset() const {return stampEpochOffset_;}

 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
@@ -108,10 +114,7 @@ public:
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
-	void setData(const SensorData & data, const Transform & pose, const glm::mat4 & viewMatrix, const glm::mat4 & projectionMatrix, const float * texCoord);

-	void spinOnce(); // Should only be called if not thread is not running, otherwise it does nothing
-    
    GLuint getTextureId() {return textureId_;}
    bool uvsInitialized() const {return uvs_initialized_;}
    const float* uvsTransformed() const {return transformed_uvs_;}
@@ -122,17 +125,15 @@ public:
    const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
-	virtual void capturePoseOnly() {}
+	virtual SensorData updateDataOnRender(Transform & pose);

-	virtual void mainLoopBegin();
-	virtual void mainLoop();
+private:
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
+	void postUpdate(); // Should be called while being protected by dataMutex_

 protected:
 	CameraModel model_; // local transform is the device to camera optical rotation in rtabmap frame
 	Transform deviceTColorCamera_; // device to camera optical rotation in rtabmap frame
-	UTimer spinOnceFrameRateTimer_;
-	double spinOncePreviousStamp_;
    
    GLuint textureId_;
    glm::mat4 viewMatrix_;
@@ -141,9 +142,7 @@ protected:
    bool uvs_initialized_ = false;

 private:
-	Transform previousPose_;
-	double previousStamp_;
-	UTimer cameraStartedTime_;
+	bool firstFrame_;
 	double stampEpochOffset_;
 	bool smoothing_;
 	ScreenRotation colorCameraToDisplayRotation_;
@@ -152,8 +151,13 @@ private:
 	Transform originOffset_;
 	bool originUpdate_;

+	USemaphore dataReady_;
+	UMutex dataMutex_;
 	SensorData data_;
-	Transform pose_;
+	Transform dataPose_;
+
+	UMutex poseMutex_;
+	std::map<double, Transform> poseBuffer_; // <stamp, Pose>
    
    cv::Mat occlusionImage_;
    CameraModel occlusionModel_;
@@ -101,7 +101,7 @@ void onPoseAvailableRouter(void* context, const TangoPoseData* pose)
 	if(pose->status_code == TANGO_POSE_VALID)
 	{
 		CameraTango* app = static_cast<CameraTango*>(context);
-		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world);
+		app->poseReceived(rtabmap_world_T_tango_world * app->tangoPoseToTransform(pose) * tango_device_T_rtabmap_world, pose->timestamp);
 	}
 }

@@ -444,7 +444,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 		//LOGD("Depth received! %fs (%d points)", timestamp, cloud.cols);

 		UASSERT(cloud.type() == CV_32FC4);
-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);

 		// From post: http://stackoverflow.com/questions/29236110/timing-issues-with-tango-image-frames
 		// "In the current version of Project Tango Tablet RGB IR camera
@@ -463,7 +463,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)

 			if(dt >= 0.0 && dt < 0.5)
 			{
-				bool notify = !data_.isValid();
+				bool notify = !tangoData_.isValid();

 				cv::Mat tangoImage = tangoColor_;
 				cv::Mat rgb;
@@ -495,7 +495,7 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 				else
 				{
 					LOGE("Not supported color format : %d.", tangoColorType);
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}

@@ -678,24 +678,24 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)

 					if(rawScanPublished_)
 					{
-						data_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(LaserScan::backwardCompatibility(scan, cloud.total()/scanDownsampling, 0, scanLocalTransform), rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
 					else
 					{
-						data_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
+						tangoData_ = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp);
 					}
-					data_.setGroundTruth(odom);
+					tangoData_.setGroundTruth(odom);
 				}
 				else
 				{
 					LOGE("Could not get depth and rgb images!?!");
-					data_ = SensorData();
+					tangoData_ = SensorData();
 					return;
 				}

 				if(notify)
 				{
-					dataReady_.release();
+					tangoDataReady_.release();
 				}
 				LOGD("process cloud received %fs", timer.ticks());
 			}
@@ -709,7 +709,7 @@ void CameraTango::rgbReceived(const cv::Mat & tangoImage, int type, double times
 	{
 		//LOGD("RGB received! %fs", timestamp);

-		boost::mutex::scoped_lock  lock(dataMutex_);
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);

 		tangoColor_ = tangoImage.clone();
 		tangoColorStamp_ = timestamp;
@@ -775,10 +775,11 @@ rtabmap::Transform CameraTango::getPoseAtTimestamp(double timestamp)
 	return pose;
 }

-SensorData CameraTango::captureImage(CameraInfo * info)
+SensorData CameraTango::updateDataOnRender(Transform & pose)
 {
 	//LOGI("Capturing image...");

+	pose.setNull();
 	if(textureId_ == 0)
 	{
 		glGenTextures(1, &textureId_);
@@ -797,10 +798,7 @@ SensorData CameraTango::captureImage(CameraInfo * info)

 		if (status == TANGO_SUCCESS)
 		{
-			if(info)
-			{
-				info->odomPose = getPoseAtTimestamp(video_overlay_timestamp);
-			}
+			pose = getPoseAtTimestamp(video_overlay_timestamp);

 			int rotation = static_cast<int>(getScreenRotation()) + 1; // remove 90deg camera rotation
 			  if (rotation > 3) {
@@ -876,16 +874,13 @@ SensorData CameraTango::captureImage(CameraInfo * info)
 	}

 	SensorData data;
-	if(dataReady_.acquireTry(1))
+	if(tangoDataReady_.acquireTry(1))
 	{
-		boost::mutex::scoped_lock  lock(dataMutex_);
-		data = data_;
-		data_ = SensorData();
-		if(info)
-		{
-			info->odomPose = data.groundTruth();
-			data.setGroundTruth(Transform());
-		}
+		boost::mutex::scoped_lock  lock(tangoDataMutex_);
+		data = tangoData_;
+		tangoData_ = SensorData();
+		pose = data.groundTruth();
+		data.setGroundTruth(Transform());
 	}
 	return data;

@@ -52,7 +52,6 @@ public:
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
 	rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
-	void setColorCamera(bool enabled) {if(!this->isRunning()) colorCamera_ = enabled;}
 	void setDecimation(int value) {decimation_ = value;}
 	void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;}

@@ -61,7 +60,7 @@ public:
 	void tangoEventReceived(int type, const char * key, const char * value);

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData updateDataOnRender(Transform & pose);

 private:
 	rtabmap::Transform getPoseAtTimestamp(double timestamp);
@@ -71,12 +70,12 @@ private:
 	bool colorCamera_;
 	int decimation_;
 	bool rawScanPublished_;
-	SensorData data_;
+	SensorData tangoData_;
 	cv::Mat tangoColor_;
 	int tangoColorType_;
 	double tangoColorStamp_;
-	boost::mutex dataMutex_;
-	USemaphore dataReady_;
+	boost::mutex tangoDataMutex_;
+	USemaphore tangoDataReady_;
 	cv::Mat fisheyeRectifyMapX_;
 	cv::Mat fisheyeRectifyMapY_;
 };
@@ -65,6 +65,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/GainCompensator.h>
 #include <rtabmap/core/DBDriver.h>
 #include <rtabmap/core/Recovery.h>
+#include <rtabmap/core/lidar/LidarVLP16.h>
 #include <pcl/common/common.h>
 #include <pcl/filters/extract_indices.h>
 #include <pcl/io/ply_io.h>
@@ -202,6 +203,7 @@ RTABMapApp::RTABMapApp() :
 #endif
 		cameraDriver_(0),
 		camera_(0),
+		sensorCaptureThread_(0),
 		rtabmapThread_(0),
 		rtabmap_(0),
 		logHandler_(0),
@@ -216,6 +218,7 @@ RTABMapApp::RTABMapApp() :
 		cameraColor_(true),
 		fullResolution_(false),
 		appendMode_(true),
+		useExternalLidar_(false),
 		maxCloudDepth_(2.5),
 		minCloudDepth_(0.0),
 		cloudDensityLevel_(1),
@@ -537,7 +540,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 							// Voxelize and filter depending on the previous cloud?
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if(!data.imageRaw().empty() && !data.depthRaw().empty())
+							if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 							{
                                int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
                                
@@ -885,7 +888,7 @@ bool RTABMapApp::startCamera()
 #endif
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
-    
+
 	if(cameraDriver_ == 0) // Tango
 	{
 #ifdef RTABMAP_TANGO
@@ -937,6 +940,19 @@ bool RTABMapApp::startCamera()

 		LOGI("Start camera thread");
 		cameraJustInitialized_ = true;
+		if(useExternalLidar_)
+		{
+			rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
+			lidar->init();
+			camera_->setImageRate(0); // if lidar, to get close camera synchronization
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
+            sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
+		}
+		else
+		{
+			sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
+		}
+		sensorCaptureThread_->start();
 		return true;
 	}
 	UERROR("Failed camera initialization!");
@@ -948,13 +964,12 @@ void RTABMapApp::stopCamera()
 	LOGI("stopCamera()");
 	{
 		boost::mutex::scoped_lock  lock(cameraMutex_);
-		if(camera_!=0)
+		if(sensorCaptureThread_!=0)
 		{
-			camera_->join(true);
-			camera_->close();
-			delete camera_;
+			sensorCaptureThread_->join(true);
+			delete sensorCaptureThread_; // camera_ is closed and deleted inside
+			sensorCaptureThread_ = 0;
 			camera_ = 0;
-			poseBuffer_.clear();
 		}
 	}
    {
@@ -1241,7 +1256,7 @@ int RTABMapApp::Render()
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
 	try
 	{
-        if(camera_ == 0)
+        if(sensorCaptureThread_ == 0)
        {
            // We are not doing continous drawing, just measure single draw
            fpsTime_.restart();
@@ -1272,49 +1287,45 @@ int RTABMapApp::Render()
 			{
 				if(cameraDriver_ <= 2)
 				{
-					camera_->spinOnce();
+					camera_->updateOnRender();
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Camera spinOnce %fs", time.ticks());
+				LOGW("Camera updateOnRender %fs", time.ticks());
 #endif
-
-				if(cameraDriver_ != 2)
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
 				{
-					if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
+					main_scene_.background_renderer_ = new BackgroundRenderer();
+					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
+				}
+				if(camera_->uvsInitialized())
+				{
+					uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
+					((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+					if(graphOptimization_ && !mapToOdom_.isIdentity())
 					{
-						main_scene_.background_renderer_ = new BackgroundRenderer();
-						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ == 0 || cameraDriver_ == 1);
+						rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
+						arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
 					}
-					if(camera_->uvsInitialized())
-					{
-						uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
-						((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
-						if(graphOptimization_ && !mapToOdom_.isIdentity())
-						{
-							rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
-							arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
-						}
-					}
-					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
-					{
-						rtabmap::CameraModel occlusionModel;
-						cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
+				}
+				if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+				{
+					rtabmap::CameraModel occlusionModel;
+					cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);

-						if(occlusionModel.isValidForProjection())
-						{
-							pcl::IndicesPtr indices(new std::vector<int>);
-							int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
-							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
-							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
-							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
-							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
-							occlusionMesh.indices = indices;
-							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
-						}
-						else if(!occlusionImage.empty())
-						{
-							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
-						}
+					if(occlusionModel.isValidForProjection())
+					{
+						pcl::IndicesPtr indices(new std::vector<int>);
+						int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
+						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
+						cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
+						occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+						pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+						occlusionMesh.indices = indices;
+						occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+					}
+					else if(!occlusionImage.empty())
+					{
+						UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
 					}
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
@@ -1334,14 +1345,14 @@ int RTABMapApp::Render()
 			}
 		}

-		rtabmap::OdometryEvent odomEvent;
+		rtabmap::SensorEvent sensorEvent;
 		{
-			boost::mutex::scoped_lock  lock(odomMutex_);
-			if(odomEvents_.size())
+			boost::mutex::scoped_lock  lock(sensorMutex_);
+			if(sensorEvents_.size())
 			{
-				LOGI("Process odom events");
-				odomEvent = odomEvents_.back();
-				odomEvents_.clear();
+				LOGI("Process sensor events");
+				sensorEvent = sensorEvents_.back();
+				sensorEvents_.clear();
 				if(cameraJustInitialized_)
 				{
 					notifyCameraStarted = true;
@@ -1361,7 +1372,7 @@ int RTABMapApp::Render()
 			{
 				main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
 			}
-			if(camera_!=0 && cameraJustInitialized_)
+			if(sensorCaptureThread_!=0 && cameraJustInitialized_)
 			{
 				notifyCameraStarted = true;
 				cameraJustInitialized_ = false;
@@ -1562,9 +1573,9 @@ int RTABMapApp::Render()
 			if(clearSceneOnNextRender_)
 			{
 				LOGI("Clearing all rendering data...");
-				odomMutex_.lock();
-				odomEvents_.clear();
-				odomMutex_.unlock();
+				sensorMutex_.lock();
+				sensorEvents_.clear();
+				sensorMutex_.unlock();

 				poseMutex_.lock();
 				poseEvents_.clear();
@@ -1800,7 +1811,7 @@ int RTABMapApp::Render()
 										// Voxelize and filter depending on the previous cloud?
 										pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 										pcl::IndicesPtr indices(new std::vector<int>);
-										if(!data.imageRaw().empty() && !data.depthRaw().empty())
+										if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
 										{
                                            int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
 											cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
@@ -2004,26 +2015,26 @@ int RTABMapApp::Render()
 			}
 			else
 			{
-				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && camera_!=0);
+				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);

 				//just process the last one
-				if(!odomEvent.pose().isNull())
+				if(!sensorEvent.info().odomPose.isNull())
 				{
 					if(odomCloudShown_ && !trajectoryMode_)
 					{
-						if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) || !odomEvent.data().laserScanRaw().isEmpty())
+						if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
 						{
 							pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
 							pcl::IndicesPtr indices(new std::vector<int>);
-							if((!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()))
+							if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
 							{
-                                int meshDecimation = updateMeshDecimation(odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows);
-								cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
+                                int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
+								cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
 							}
 							else
 							{
 								//scan
-								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
+								cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
 								indices->resize(cloud->size());
 								for(unsigned int i=0; i<cloud->size(); ++i)
 								{
@@ -2034,10 +2045,10 @@ int RTABMapApp::Render()
 							if(cloud->size() && indices->size())
 							{
 								LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
-										odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
-										odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
+										sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
+										sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
 									   (int)cloud->width, (int)cloud->height);
-								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
+								main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
 								main_scene_.setCloudVisible(-1, true);
 							}
 							else
@@ -2127,7 +2138,7 @@ int RTABMapApp::Render()

 				lastPostRenderEventTime_ = UTimer::now();

-				if(camera_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
+				if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
 				{
 					UERROR("TangoPoseEventNotReceived");
 					UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
@@ -2319,7 +2330,7 @@ void RTABMapApp::setTrajectoryMode(bool enabled)
 void RTABMapApp::setGraphOptimization(bool enabled)
 {
 	graphOptimization_ = enabled;
-	if((camera_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
+	if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
 	{
 		std::map<int, rtabmap::Transform> poses;
 		std::multimap<int, rtabmap::Link> links;
@@ -3709,19 +3720,12 @@ void RTABMapApp::postCameraPoseEvent(
 		if(qx==0 && qy==0 && qz==0 && qw==0)
 		{
 			// Lost! clear buffer
-			poseBuffer_.clear();
 			camera_->resetOrigin(); // we are lost, create new session on next valid frame
 			return;
 		}
 		rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
-		camera_->poseReceived(pose);
-
-		poseBuffer_.insert(std::make_pair(stamp, pose));
-		if(poseBuffer_.size() > 1000)
-		{
-			poseBuffer_.erase(poseBuffer_.begin());
-		}
+		camera_->poseReceived(pose, stamp);
 	}
 }

@@ -3833,66 +3837,41 @@ void RTABMapApp::postOdometryEvent(
 				{
 					pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;

+					rtabmap::Transform poseWithOriginOffset = pose;
+					if(!camera_->getOriginOffset().isNull())
+					{
+						poseWithOriginOffset = camera_->getOriginOffset() * pose;
+					}
+
 					// Registration depth to rgb
 					if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
 					{
 						UTimer time;
 						rtabmap::Transform motion = rtabmap::Transform::getIdentity();
-						if(depthStamp != stamp && !poseBuffer_.empty())
+						if(depthStamp != stamp)
 						{
 							// Interpolate pose
-							if(!poseBuffer_.empty())
+							rtabmap::Transform poseDepth;
+							cv::Mat cov;
+							if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
+							{
+								UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
+							}
+							else
 							{
-								if(poseBuffer_.rbegin()->first < depthStamp)
-								{
-									UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
-								}
-								else
-								{
-									std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
-									std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
-									rtabmap::Transform poseDepth;
-									if(iterA != poseBuffer_.begin())
-									{
-										iterA = --iterA;
-									}
-									if(iterB == poseBuffer_.end())
-									{
-										iterB = --iterB;
-									}
-									if(iterA == iterB && depthStamp == iterA->first)
-									{
-										poseDepth = iterA->second;
-									}
-									else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
-									{
-										poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
-									}
-									else if(depthStamp < iterA->first)
-									{
-										UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
-									}
-									else
-									{
-										UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
-									}
-									if(!poseDepth.isNull())
-									{
 #ifndef DISABLE_LOG
-										UDEBUG("poseRGB  =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
-										UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
+								UDEBUG("poseRGB  =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
+								UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
 #endif
-										motion = pose.inverse()*poseDepth;
-										// transform in camera frame
+								motion = poseWithOriginOffset.inverse()*poseDepth;
+								// transform in camera frame
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-										motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
+								motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
 #ifndef DISABLE_LOG
-										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+								UDEBUG("motion=%s", motion.prettyPrint().c_str());
 #endif
-									}
-								}
 							}
 						}
 						rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
@@ -3941,11 +3920,6 @@ void RTABMapApp::postOdometryEvent(
                    
                    if(!outputDepth.empty())
                    {
-                    	rtabmap::Transform poseWithOriginOffset = pose;
-                    	if(!camera_->getOriginOffset().isNull())
-                    	{
-                    		poseWithOriginOffset = camera_->getOriginOffset() * pose;
-                    	}
                        rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
                        depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
                        camera_->setOcclusionImage(outputDepth, depthModel);
@@ -3971,8 +3945,7 @@ void RTABMapApp::postOdometryEvent(
                    texCoords[5] = t5;
                    texCoords[6] = t6;
                    texCoords[7] = t7;
-					camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
-					camera_->spinOnce();
+					camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
 				}
 			}
 		}
@@ -3989,17 +3962,17 @@ void RTABMapApp::postOdometryEvent(

 bool RTABMapApp::handleEvent(UEvent * event)
 {
-	if(camera_!=0)
+	if(sensorCaptureThread_!=0)
 	{
 		// called from events manager thread, so protect the data
-		if(event->getClassName().compare("OdometryEvent") == 0)
+		if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			LOGI("Received OdometryEvent!");
-			if(odomMutex_.try_lock())
+			LOGI("Received SensorEvent!");
+			if(sensorMutex_.try_lock())
 			{
-				odomEvents_.clear();
-				odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
-				odomMutex_.unlock();
+				sensorEvents_.clear();
+				sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
+				sensorMutex_.unlock();
 			}
 		}
 		if(event->getClassName().compare("RtabmapEvent") == 0)
@@ -40,7 +40,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "util.h"
 #include "ProgressionStatus.h"

+#include <rtabmap/core/SensorCaptureThread.h>
 #include <rtabmap/core/RtabmapThread.h>
+#include <rtabmap/core/SensorEvent.h>
 #include <rtabmap/utilite/UEventsHandler.h>
 #include <boost/thread/mutex.hpp>
 #include <pcl/pcl_base.h>
@@ -209,6 +211,7 @@ class RTABMapApp : public UEventsHandler {
 private:
  int cameraDriver_;
  rtabmap::CameraMobile * camera_;
+  rtabmap::SensorCaptureThread * sensorCaptureThread_;
  rtabmap::RtabmapThread * rtabmapThread_;
  rtabmap::Rtabmap * rtabmap_;
  rtabmap::LogHandler * logHandler_;
@@ -224,6 +227,7 @@ class RTABMapApp : public UEventsHandler {
  bool cameraColor_;
  bool fullResolution_;
  bool appendMode_;
+  bool useExternalLidar_;
  float maxCloudDepth_;
  float minCloudDepth_;
  int cloudDensityLevel_;
@@ -270,16 +274,15 @@ class RTABMapApp : public UEventsHandler {
    UTimer fpsTime_;

 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
-	std::list<rtabmap::OdometryEvent> odomEvents_;
+	std::list<rtabmap::SensorEvent> sensorEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
-	std::map<double, rtabmap::Transform> poseBuffer_;

 	rtabmap::Transform mapToOdom_;

 	boost::mutex cameraMutex_;
 	boost::mutex rtabmapMutex_;
 	boost::mutex meshesMutex_;
-	boost::mutex odomMutex_;
+	boost::mutex sensorMutex_;
 	boost::mutex poseMutex_;
 	boost::mutex renderingMutex_;

@@ -155,7 +155,7 @@ void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
 }

 void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & depthTexture, int screenWidth, int screenHeight, bool redUnknown) {
-  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+  static_assert(std::extent<decltype(BackgroundRenderer_kVerticesDevice)>::value == kNumVertices * 2, "Incorrect kVertices length");

  GLuint program = shaderPrograms_[depthTexture>0?1:0];
    
@@ -170,7 +170,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
  else
 #endif
    glBindTexture(GL_TEXTURE_2D, texture_id_);
-    
+
  if(depthTexture>0)
  {
    // Texture activate unit 1
@@ -191,7 +191,7 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
  GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
  GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");

-  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVerticesDevice);
  glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);

  glEnableVertexAttribArray(attributeVertices);
@@ -28,9 +28,15 @@

 #include "util.h"

-static const GLfloat BackgroundRenderer_kVertices[] = {
+static const GLfloat BackgroundRenderer_kVerticesDevice[] = {
 	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
 };
+//static const GLfloat BackgroundRenderer_kVerticesView[] = {
+//	0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 1.0f, 1.0f, 1.0f,
+//};
+static const GLfloat BackgroundRenderer_kVerticesView[] = {
+	0.0f, 1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f,
+};
 static const GLfloat BackgroundRenderer_kTexCoord[] = {
   1.0f, 1.0f, 1.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f,
 };
@@ -1 +1,4 @@
-*.jar
+# Ignore everything in this directory
+*
+# Except this file
+!.gitignore
@@ -84,7 +84,7 @@
    <string name="pref_key_depth_from_motion">pref_key_depth_from_motion</string>
    <string name="pref_default_depth_from_motion">false</string>
    <string name="pref_key_arcore_localization_filtering_speed">pref_key_arcore_localization_filtering_speed</string>
-    <string name="pref_default_arcore_localization_filtering_speed">1</string>
+    <string name="pref_default_arcore_localization_filtering_speed">0</string>
    <string name="pref_key_update_rate">pref_key_update_rate</string>
    <string name="pref_default_update_rate">1</string>
    <string name="pref_key_max_speed">pref_key_max_speed</string>
@@ -115,6 +115,9 @@ public class ARCoreSharedCamera {
 	// Image reader that continuously processes CPU images.
 	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
 	private boolean mTOFAvailable = false;
+
+	ByteBuffer mPreviousDepth = null;
+	double mPreviousDepthStamp = 0.0;
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}

@@ -698,6 +701,7 @@ public class ARCoreSharedCamera {
 							mToast.setText(msg);
 						}
 						previousAnchorPose = null;
+						arCoreCorrection = Pose.IDENTITY;
 					}
 				}
 			});
@@ -718,7 +722,8 @@ public class ARCoreSharedCamera {
 				final double speed = Math.sqrt(t[0]*t[0]+t[1]*t[1]+t[2]*t[2])/((double)(frame.getTimestamp()-previousAnchorTimeStamp)/10e8);
 				if(speed>=mARCoreLocalizationFilteringSpeed)
 				{
-					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose).compose(pose.inverse());
+					// Only correct the translation to not lose rotation aligned with gravity
+					arCoreCorrection = arCoreCorrection.compose(previousAnchorPose.compose(pose.inverse()).extractTranslation());
 					t = arCoreCorrection.getTranslation();
 					Log.e(TAG, String.format("POTENTIAL TELEPORTATION!!!!!!!!!!!!!! previous anchor moved (speed=%f), new arcorrection: %f %f %f", speed, t[0], t[1], t[2]));
 					
@@ -744,7 +749,6 @@ public class ARCoreSharedCamera {
 								{
 									mToast.setText(msg);
 								}
-								previousAnchorPose = null;
 							}
 						}
 					});
@@ -755,9 +759,8 @@ public class ARCoreSharedCamera {
 			previousAnchorTimeStamp = frame.getTimestamp();
 			
 			double stamp = (double)frame.getTimestamp()/10e8;
-			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp));
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f arcore %f %f %f cor= %f %f %f stamp=%f", odomPose.tx(), odomPose.ty(), odomPose.tz(), pose.tx(), pose.ty(), pose.tz(), arCoreCorrection.tx(), arCoreCorrection.ty(), arCoreCorrection.tz(), stamp));
 			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), stamp);
-			
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
@@ -813,6 +816,12 @@ public class ARCoreSharedCamera {
 						depth = mTOFImageReader.depth16_raw;
 						depthStamp = (double)mTOFImageReader.timestamp/10e8;
 					}
+
+					if(mPreviousDepth == null)
+					{
+						mPreviousDepth = depth;
+						mPreviousDepthStamp = depthStamp;
+					}
 					
 					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
 							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
@@ -825,13 +834,17 @@ public class ARCoreSharedCamera {
 							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
 							depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
 							stamp,
-							depthStamp,
+							depthStamp>stamp?mPreviousDepthStamp:depthStamp,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
 							points, points.limit()/4,
 							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
 							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
+					
+					
+					mPreviousDepthStamp = depthStamp;
+					mPreviousDepth = depth;
 				}
 				else
 				{
@@ -284,11 +284,11 @@ void setCameraNative(const void *object, int type) {
 }

 void postCameraPoseEventNative(const void *object,
-        float x, float y, float z, float qx, float qy, float qz, float qw)
+        float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
    if(object)
    {
-        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,0.0);
+        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp);
    }
    else
    {
@@ -67,7 +67,7 @@ bool startCameraNative(const void *object);
 void stopCameraNative(const void *object);
 void setCameraNative(const void *object, int type);
 void postCameraPoseEventNative(const void *object,
-                         float x, float y, float z, float qx, float qy, float qz, float qw);
+                         float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
 void postOdometryEventNative(const void *object,
                       float x, float y, float z, float qx, float qy, float qz, float qw,
                       float fx, float fy, float cx, float cy,
@@ -216,18 +216,18 @@ class RTABMap {
        setCameraNative(native_rtabmap, Int32(type))
    }
    
-    func postCameraPoseEvent(pose: simd_float4x4) {
+    func postCameraPoseEvent(pose: simd_float4x4, stamp: TimeInterval) {
        let rotation = GLKMatrix3(
            m: (pose[0,0], pose[0,1], pose[0,2],
            pose[1,0], pose[1,1], pose[1,2],
            pose[2,0], pose[2,1], pose[2,2]))
        let quat = GLKQuaternionMakeWithMatrix3(rotation)
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, stamp)
    }
    
    func notifyLost() {
        // a null transform will make rtabmap creating a new session
-        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0)
+        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0)
    }
    
    func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
@@ -239,7 +239,7 @@ class RTABMap {
        
        let quat = GLKQuaternionMakeWithMatrix3(rotation)
                
-        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w)
+        postCameraPoseEventNative(native_rtabmap, pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w, frame.timestamp)
                
        let confMap = frame.sceneDepth?.confidenceMap
        let depthMap = frame.sceneDepth?.depthMap
@@ -0,0 +1,25 @@
+
+ To reproduce results (based on parameters of this [paper](https://introlab.3it.usherbrooke.ca/mediawiki-introlab/images/b/bc/TRO2013.pdf)):
+ 
+ ```
+ rtabmap-console \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --SURF/HessianThreshold 150\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.2\
+       -gt "~/Downloads/UdeS_1Hz.png"\
+       ~/Downloads/UdeS_1Hz
+ ```
+Adding the ground truth file here is optional to show recall at 100% precision at the end of the process directly without using the octave/MATLAB script below. For NewCollege and CityCentre datasets, `rtabmap-imagesJoiner` can be used to assemble the left and right images together.
+ 
+ To analyze with Octave/MATLAB, drop `LogF.txt` and `LogI.txt` generated files from command above in ShowLogs directly, then execute `showLogs.m`.
@@ -1,4 +0,0 @@
-This directory contains some basic concepts on Bayes filtering.
-Main scripts :
- RecursivesBayes.m
- RecursivesBayesAvpd.m
@@ -32,8 +32,7 @@ if ~isempty(GroundTruth)
    if size(GroundTruth, 1) ~=  length(LogF(:,1)) ||  size(GroundTruth, 1) ~=  length(LogI(:,1))
        error(['The ground truth size doesn''t match the log files (LogI=' num2str(length(LogI(:,1))) ', LogF=' num2str(length(LogF(:,1))) ', GT=' num2str(size(GroundTruth, 1)) ')'])
    end
-    
-    
+
    %[highestHypot, CorrespondingID, GT, Accepted, Good, Index, UnderLoopRatio] descending order
    if(sum(LogI(:,8) == 10) > 0)
        %OLD
@@ -111,14 +110,14 @@ if ~isempty(GroundTruth)
    index = find(PR(:,1) == 1);
    if ~isempty(index)
        maxRecall = PR(index(end),2) * 100;
-        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2)))])
+        display(['Recall max (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(index(end),1)) '), accepted=' num2str(sum(lc(1:index(end),5) & ~lc(1:index(end),7) & lc(1:index(end),2))) '/' num2str(GT_total_positives)])
    else
        display('Recall max (Precision=100%) = 0')
    end
    indexAccepted = find(PR(:,3) == 1);
    if ~isempty(indexAccepted)
        maxRecall = PR(indexAccepted(end),2) * 100;
-        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2)))])
+        display(['Recall max accepted (Precision=100%) = ' num2str(maxRecall) '% (p=' num2str(lc(indexAccepted(end),1)) '), accepted=' num2str(sum(lc(1:indexAccepted(end),5) & ~lc(1:indexAccepted(end),7) & lc(1:indexAccepted(end),2))) '/' num2str(GT_total_positives)])
    else
        display('Recall max accepted (Precision=100%) = 0')
    end
@@ -22,6 +22,8 @@ function [LogF LogI] = showlogs(PathPrefix, GT_file)
 set(0,'defaultAxesFontName', 'Times')
 set(0,'defaultTextFontName', 'Times')

+close all
+
 if nargin < 2, GT_file = ''; end
 if nargin < 1, PathPrefix = '.'; end

@@ -325,7 +327,7 @@ y(LogI(:, 1) == 0) = [];
 x(LogI(:, 1) == 0) = [];
 plot(x,y, 'g.')

-set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
+%set(datacursormode,'UpdateFcn',@(Y,X){sprintf('X: %0.2f',X.Position(1)),sprintf('Y: %0.2f',X.Position(2))})
 % %matched sign words
 % y = LogI(:,2);
 % x = 1:length(y);
@@ -0,0 +1,8 @@
+#!/bin/bash
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )"
+
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/NewCollege ~/loop_closure_detection_datasets/NewCollege.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/CityCentre ~/loop_closure_detection_datasets/CityCentre.png
+$SCRIPT_DIR/run_bow.sh ~/loop_closure_detection_datasets/UdeS_1Hz ~/loop_closure_detection_datasets/UdeS_1Hz.png
+ 
@@ -0,0 +1,32 @@
+#!/bin/bash
+
+DATASET_FOLDER=""
+GT_FILE=""
+if [ $# -eq 2 ] 
+then
+    DATASET_FOLDER=$1
+    GT_FILE=$2
+else
+echo "Usage: run_bow.sh \"dataset folder\" \"ground truth file\""
+exit
+fi
+
+rtabmap-console \
+       -quiet \
+       --Rtabmap/StatisticLogged true\
+       --Rtabmap/StatisticLoggedHeaders false\
+       --Kp/DetectorStrategy 0\
+       --SURF/HessianThreshold 150\
+       --Rtabmap/MemoryThr 300\
+       --Rtabmap/LoopRatio 0.9\
+       --Mem/STMSize 30\
+       --Vis/MaxFeatures 400\
+       --Kp/TfIdfLikelihoodUsed false\
+       --Kp/MaxFeatures 400\
+       --Kp/BadSignRatio 0.25\
+       --Mem/BadSignaturesIgnored true\
+       --Mem/RehearsalSimilarity 0.20\
+       --Mem/RecentWmRatio 0.20\
+       -gt "$GT_FILE"\
+       "$DATASET_FOLDER"
+
@@ -32,7 +32,7 @@ We provide two formats: the first one is more general and the second one is used
  * `device_poses.txt`: VIO poses of each image in `device` frame
  * `camera_poses.txt`: VIO poses of each image in `camera` frame
 * [RTAB-Map Databases](https://drive.google.com/file/d/1TklUcTKFSrcg8b0t0U80G_IpFRMVRlY5/view?usp=drive_link)
-
+* Dataset now also available on [Federated Research Data Repository (FRDR)](https://doi.org/10.20383/103.0931) (if links above don't work)


 ## How reproduce results shown in the paper
@@ -1,30 +0,0 @@
-# - Find Sqlite3
-# This module finds an installed Sqlite3 package.
-#
-# It sets the following variables:
-#  Sqlite3_FOUND       - Set to false, or undefined, if Sqlite3 isn't found.
-#  Sqlite3_INCLUDE_DIR - The Sqlite3 include directory.
-#  Sqlite3_LIBRARY     - The Sqlite3 library to link against.
-
-FIND_PATH(Sqlite3_INCLUDE_DIR sqlite3.h PATHS $ENV{Sqlite3_ROOT_DIR}/include $ENV{Sqlite3_ROOT_DIR})
-
-FIND_LIBRARY(Sqlite3_LIBRARY NAMES sqlite3 PATHS $ENV{Sqlite3_ROOT_DIR}/lib $ENV{Sqlite3_ROOT_DIR})
-
-IF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
-   SET(Sqlite3_FOUND TRUE)
-   SET(Sqlite3_INCLUDE_DIRS ${Sqlite3_INCLUDE_DIR})
-   SET(Sqlite3_LIBRARIES ${Sqlite3_LIBRARY})
-ENDIF (Sqlite3_INCLUDE_DIR AND Sqlite3_LIBRARY)
-
-IF (Sqlite3_FOUND)
-   # show which Sqlite3 was found only if not quiet
-   IF (NOT Sqlite3_FIND_QUIETLY)
-      MESSAGE(STATUS "Found Sqlite3: ${Sqlite3_INCLUDE_DIRS} ${Sqlite3_LIBRARIES}")
-   ENDIF (NOT Sqlite3_FIND_QUIETLY)
-ELSE (Sqlite3_FOUND)
-   # fatal error if Sqlite3 is required but not found
-   IF (Sqlite3_FIND_REQUIRED)
-      MESSAGE(FATAL_ERROR "Could not find Sqlite3")
-   ENDIF (Sqlite3_FIND_REQUIRED)
-ENDIF (Sqlite3_FOUND)
-
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -28,47 +28,32 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #pragma once

 #include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <opencv2/highgui/highgui.hpp>
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/CameraInfo.h"
-#include <set>
-#include <stack>
-#include <list>
-#include <vector>
-
-class UDirectory;
-class UTimer;
+#include <rtabmap/core/SensorCapture.h>
+#include <rtabmap/core/IMU.h>

 namespace rtabmap
 {

+class IMUFilter;
+
 /**
 * Class Camera
 *
 */
-class RTABMAP_CORE_EXPORT Camera
+class RTABMAP_CORE_EXPORT Camera : public SensorCapture
 {
 public:
 	virtual ~Camera();
-	SensorData takeImage(CameraInfo * info = 0);
+
+	SensorData takeImage(SensorCaptureInfo * info = 0) {return takeData(info);}
+	float getImageRate() const {return getFrameRate();}
+	void setImageRate(float imageRate) {setFrameRate(imageRate);}
+	void setInterIMUPublishing(bool enabled, IMUFilter * filter = 0); // Take ownership of filter
+	bool isInterIMUPublishing() const {return publishInterIMU_;}

 	bool initFromFile(const std::string & calibrationPath);
-	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
 	virtual bool isCalibrated() const = 0;
-	virtual std::string getSerial() const = 0;
-	virtual bool odomProvided() const { return false; }
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance) { return false; }

-	//getters
-	float getImageRate() const {return _imageRate;}
-	const Transform & getLocalTransform() const {return _localTransform;}
-
-	//setters
-	void setImageRate(float imageRate) {_imageRate = imageRate;}
-	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
-
-	void resetTimer();
 protected:
 	/**
 	 * Constructor
@@ -78,19 +63,16 @@ protected:
 	 */
 	Camera(float imageRate = 0, const Transform & localTransform = Transform::getIdentity());

-	/**
-	 * returned rgb and depth images should be already rectified if calibration was loaded
-	 */
-	virtual SensorData captureImage(CameraInfo * info = 0) = 0;
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0) = 0;

-	int getNextSeqID() {return ++_seq;}
+	void postInterIMU(const IMU & imu, double stamp);

 private:
-	float _imageRate;
-	Transform _localTransform;
-	cv::Size _targetImageSize;
-	UTimer * _frameRateTimer;
-	int _seq;
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) {return captureImage(info);}
+
+private:
+	IMUFilter * imuFilter_;
+	bool publishInterIMU_;
 };


@@ -27,65 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include <rtabmap/utilite/UEvent.h>
-#include "rtabmap/core/SensorData.h"
-#include "rtabmap/core/CameraInfo.h"
-
-namespace rtabmap
-{
-
-class CameraEvent :
-	public UEvent
-{
-public:
-	enum Code {
-		kCodeData,
-		kCodeNoMoreImages
-	};
-
-public:
-	CameraEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
-		UEvent(kCodeData),
-		data_(image, seq, stamp)
-	{
-		cameraInfo_.cameraName = cameraName;
-	}
-
-	CameraEvent() :
-		UEvent(kCodeNoMoreImages)
-	{
-	}
-
-	CameraEvent(const SensorData & data) :
-		UEvent(kCodeData),
-		data_(data)
-	{
-	}
-
-	CameraEvent(const SensorData & data, const std::string & cameraName) :
-		UEvent(kCodeData),
-		data_(data)
-	{
-		cameraInfo_.cameraName = cameraName;
-	}
-	CameraEvent(const SensorData & data, const CameraInfo & cameraInfo) :
-		UEvent(kCodeData),
-		data_(data),
-		cameraInfo_(cameraInfo)
-	{
-	}
-
-	// Image or descriptors
-	const SensorData & data() const {return data_;}
-	const std::string & cameraName() const {return cameraInfo_.cameraName;}
-	const CameraInfo & info() const {return cameraInfo_;}
-
-	virtual ~CameraEvent() {}
-	virtual std::string getClassName() const {return std::string("CameraEvent");}
-
-private:
-	SensorData data_;
-	CameraInfo cameraInfo_;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorEvent.h"
@@ -27,50 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include <string>
-
-namespace rtabmap
-{
-
-class CameraInfo
-{
-
-public:
-	CameraInfo() :
-		cameraName(""),
-		id(0),
-		stamp(0.0),
-		timeCapture(0.0f),
-		timeDisparity(0.0f),
-		timeMirroring(0.0f),
-		timeStereoExposureCompensation(0.0f),
-		timeImageDecimation(0.0f),
-		timeHistogramEqualization(0.0f),
-		timeScanFromDepth(0.0f),
-		timeUndistortDepth(0.0f),
-		timeBilateralFiltering(0.0f),
-		timeTotal(0.0f),
-		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
-	{
-	}
-	virtual ~CameraInfo() {}
-
-	std::string cameraName;
-	int id;
-	double stamp;
-	float timeCapture;
-	float timeDisparity;
-	float timeMirroring;
-	float timeStereoExposureCompensation;
-	float timeImageDecimation;
-	float timeHistogramEqualization;
-	float timeScanFromDepth;
-	float timeUndistortDepth;
-	float timeBilateralFiltering;
-	float timeTotal;
-	Transform odomPose;
-	cv::Mat odomCovariance;
-	std::vector<float> odomVelocity;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorCaptureInfo.h"
@@ -27,136 +27,4 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #pragma once

-#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
-
-#include <rtabmap/core/Parameters.h>
-#include <rtabmap/core/Transform.h>
-#include <rtabmap/utilite/UThread.h>
-#include <rtabmap/utilite/UEventsSender.h>
-
-namespace clams
-{
-class DiscreteDepthDistortionModel;
-}
-
-namespace rtabmap
-{
-
-class Camera;
-class CameraInfo;
-class SensorData;
-class StereoDense;
-class IMUFilter;
-class Feature2D;
-
-/**
- * Class CameraThread
- *
- */
-class RTABMAP_CORE_EXPORT CameraThread :
-	public UThread,
-	public UEventsSender
-{
-public:
-	// ownership transferred
-	CameraThread(Camera * camera, const ParametersMap & parameters = ParametersMap());
-	/**
-	 * @param camera the camera to take images from
-	 * @param odomSensor an odometry sensor to get a pose
-	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame
-	 */
-	CameraThread(Camera * camera,
-			Camera * odomSensor,
-			const Transform & extrinsics,
-			double poseTimeOffset = 0.0,
-			float poseScaleFactor = 1.0f,
-			bool odomAsGt = false,
-			const ParametersMap & parameters = ParametersMap());
-	CameraThread(Camera * camera,
-				bool odomAsGt,
-				const ParametersMap & parameters = ParametersMap());
-	virtual ~CameraThread();
-
-	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
-	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
-	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
-	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
-	void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
-	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
-	void setImageRate(float imageRate);
-	void setDistortionModel(const std::string & path);
-	void enableBilateralFiltering(float sigmaS, float sigmaR);
-	void disableBilateralFiltering() {_bilateralFiltering = false;}
-	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
-	void disableIMUFiltering();
-	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
-	void disableFeatureDetection();
-
-	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
-	RTABMAP_DEPRECATED void setScanParameters(
-			bool fromDepth,
-			int downsampleStep, // decimation of the depth image in case the scan is from depth image
-			float rangeMin,
-			float rangeMax,
-			float voxelSize,
-			int normalsK,
-			int normalsRadius,
-			bool forceGroundNormalsUp);
-	void setScanParameters(
-			bool fromDepth,
-			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
-			float rangeMin=0.0f,
-			float rangeMax=0.0f,
-			float voxelSize = 0.0f,
-			int normalsK = 0,
-			int normalsRadius = 0.0f,
-			float groundNormalsUp = 0.0f);
-
-	void postUpdate(SensorData * data, CameraInfo * info = 0) const;
-
-	//getters
-	bool isPaused() const {return !this->isRunning();}
-	bool isCapturing() const {return this->isRunning();}
-	bool odomProvided() const;
-
-	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
-	Camera * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
-
-private:
-	virtual void mainLoopBegin();
-	virtual void mainLoop();
-	virtual void mainLoopKill();
-
-private:
-	Camera * _camera;
-	Camera * _odomSensor;
-	Transform _extrinsicsOdomToCamera;
-	bool _odomAsGt;
-	double _poseTimeOffset;
-	float _poseScaleFactor;
-	bool _mirroring;
-	bool _stereoExposureCompensation;
-	bool _colorOnly;
-	int _imageDecimation;
-	int _histogramMethod;
-	bool _stereoToDepth;
-	bool _scanFromDepth;
-	int _scanDownsampleStep;
-	float _scanRangeMin;
-	float _scanRangeMax;
-	float _scanVoxelSize;
-	int _scanNormalsK;
-	float _scanNormalsRadius;
-	float _scanForceGroundNormalsUp;
-	StereoDense * _stereoDense;
-	clams::DiscreteDepthDistortionModel * _distortionModel;
-	bool _bilateralFiltering;
-	float _bilateralSigmaS;
-	float _bilateralSigmaR;
-	IMUFilter * _imuFilter;
-	bool _imuBaseFrameConversion;
-	Feature2D * _featureDetector;
-	bool _depthAsMask;
-};
-
-} // namespace rtabmap
+#include "rtabmap/core/SensorCaptureThread.h"
@@ -86,10 +86,10 @@ public:
 	const DBDriver * driver() const {return _dbDriver;}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
-	SensorData getNextData(CameraInfo * info = 0);
+	SensorData getNextData(SensorCaptureInfo * info = 0);

 private:
 	std::list<std::string> _paths;
@@ -192,16 +192,17 @@ public:
 			const cv::Mat & disparity,
 			float minDisparity);

-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints);
-	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize = cv::Size(), bool ssc = false);
+	static void limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc = false);

 	static cv::Rect computeRoi(const cv::Mat & image, const std::string & roiRatios);
 	static cv::Rect computeRoi(const cv::Mat & image, const std::vector<float> & roiRatios);

 	int getMaxFeatures() const {return maxFeatures_;}
+	bool getSSC() const {return SSC_;}
 	float getMinDepth() const {return _minDepth;}
 	float getMaxDepth() const {return _maxDepth;}
 	int getGridRows() const {return gridRows_;}
@@ -234,6 +235,7 @@ private:
 private:
 	ParametersMap parameters_;
 	int maxFeatures_;
+	bool SSC_;
 	float _maxDepth; // 0=inf
 	float _minDepth;
 	std::vector<float> _roiRatios; // size 4
@@ -1,5 +1,5 @@
 /*
-Copyright (c) 2010-2020, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
@@ -0,0 +1,73 @@
+/*
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#ifndef GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+#define GLOBAL_DESCRIPTOR_EXTRACTOR_H_
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include "rtabmap/core/Parameters.h"
+#include "rtabmap/core/SensorData.h"
+
+
+namespace rtabmap {
+
+// Feature2D
+class RTABMAP_CORE_EXPORT GlobalDescriptorExtractor {
+public:
+	enum Type {
+		kUndef=0,
+		kPyDescriptor=1};
+
+	static std::string typeName(Type type)
+	{
+		switch(type){
+		case kPyDescriptor:
+			return "PyDescriptor";
+		default:
+			return "Unknown";
+		}
+	}
+
+	static GlobalDescriptorExtractor * create(const ParametersMap & parameters = ParametersMap());
+	static GlobalDescriptorExtractor * create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters = ParametersMap()); // for convenience
+
+public:
+	virtual ~GlobalDescriptorExtractor();
+
+	virtual GlobalDescriptor extract(const SensorData & data) const = 0;
+
+	virtual void parseParameters(const ParametersMap & parameters) {}
+	virtual GlobalDescriptorExtractor::Type getType() const = 0;
+
+protected:
+	GlobalDescriptorExtractor(const ParametersMap & parameters = ParametersMap());
+};
+
+}
+
+#endif /* GLOBAL_DESCRIPTOR_EXTRACTOR_H_ */
@@ -28,12 +28,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #ifndef CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_
 #define CORELIB_INCLUDE_RTABMAP_CORE_IMUFILTER_H_

+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
 #include <rtabmap/core/Parameters.h>
 #include <Eigen/Geometry>

 namespace rtabmap {

-class IMUFilter
+class RTABMAP_CORE_EXPORT IMUFilter
 {
 public:
 	enum Type {
@@ -47,7 +47,8 @@ public:
 		kXYZRGB=7,
 		kXYZNormal=8,
 		kXYZINormal=9,
-		kXYZRGBNormal=10};
+		kXYZRGBNormal=10,
+		kXYZIT=11};

 	static std::string formatName(const Format & format);
 	static int channels(const Format & format);
@@ -55,6 +56,7 @@ public:
 	static bool isScanHasNormals(const Format & format);
 	static bool isScanHasRGB(const Format & format);
 	static bool isScanHasIntensity(const Format & format);
+	static bool isScanHasTime(const Format & format);
 	static LaserScan backwardCompatibility(
 			const cv::Mat & oldScanFormat,
 			int maxPoints = 0,
@@ -121,22 +123,27 @@ public:
 	float angleMin() const {return angleMin_;}
 	float angleMax() const {return angleMax_;}
 	float angleIncrement() const {return angleIncrement_;}
+	void setLocalTransform(const Transform & t) {localTransform_ = t;}
 	Transform localTransform() const {return localTransform_;}

 	bool empty() const {return data_.empty();}
 	bool isEmpty() const {return data_.empty();}
-	int size() const {return data_.cols;}
+	int size() const {return data_.total();}
 	int dataType() const {return data_.type();}
 	bool is2d() const {return isScan2d(format_);}
 	bool hasNormals() const {return isScanHasNormals(format_);}
 	bool hasRGB() const {return isScanHasRGB(format_);}
 	bool hasIntensity() const {return isScanHasIntensity(format_);}
+	bool hasTime() const {return isScanHasTime(format_);}
 	bool isCompressed() const {return !data_.empty() && data_.type()==CV_8UC1;}
+	bool isOrganized() const {return data_.rows > 1;}
 	LaserScan clone() const;
+	LaserScan densify() const;

 	int getIntensityOffset() const {return hasIntensity()?(is2d()?2:3):-1;}
 	int getRGBOffset() const {return hasRGB()?(is2d()?2:3):-1;}
 	int getNormalsOffset() const {return hasNormals()?(2 + (is2d()?0:1) + ((hasRGB() || hasIntensity())?1:0)):-1;}
+	int getTimeOffset() const {return hasTime()?4:-1;}

 	float & field(unsigned int pointIndex, unsigned int channelOffset);

@@ -0,0 +1,57 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+#include <rtabmap/core/SensorCapture.h>
+
+namespace rtabmap
+{
+
+/**
+ * Class Lidar
+ *
+ */
+class RTABMAP_CORE_EXPORT Lidar : public SensorCapture
+{
+public:
+	virtual ~Lidar() {}
+
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param lidarRate the frame rate (Hz), 0 for fast as the lidar can
+	 * @param localTransform the transform from base frame to lidar frame
+	 */
+	Lidar(float lidarRate = 0, const Transform & localTransform = Transform::getIdentity()) :
+		SensorCapture(lidarRate, localTransform) {}
+};
+
+
+} // namespace rtabmap
@@ -59,6 +59,7 @@ class RegistrationVis;
 class Stereo;
 class LocalGridMaker;
 class MarkerDetector;
+class GlobalDescriptorExtractor;

 class RTABMAP_CORE_EXPORT Memory
 {
@@ -333,6 +334,7 @@ private:
 	bool _rotateImagesUpsideUp;
 	bool _createOccupancyGrid;
 	int _visMaxFeatures;
+	bool _visSSC;
 	bool _imagesAlreadyRectified;
 	bool _rectifyOnlyFeatures;
 	bool _covOffDiagonalIgnored;
@@ -375,6 +377,8 @@ private:
 	LocalGridMaker * _localMapMaker;

 	MarkerDetector * _markerDetector;
+
+	GlobalDescriptorExtractor * _globalDescriptorExtractor;
 };

 } // namespace rtabmap
@@ -111,6 +111,7 @@ private:
 	bool _alignWithGround;
 	bool _publishRAMUsage;
 	bool _imagesAlreadyRectified;
+	bool _deskewing;
 	Transform _pose;
 	int _resetCurrentCount;
 	double previousStamp_;
@@ -50,6 +50,7 @@ public:
 		localBundleConstraints(0),
 		localBundleTime(0),
 		keyFrameAdded(false),
+		timeDeskewing(0.0f),
 		timeEstimation(0.0f),
 		timeParticleFiltering(0.0f),
 		stamp(0),
@@ -76,6 +77,7 @@ public:
 		output.localBundlePoses = localBundlePoses;
 		output.localBundleModels = localBundleModels;
 		output.keyFrameAdded = keyFrameAdded;
+		output.timeDeskewing = timeDeskewing;
 		output.timeEstimation = timeEstimation;
 		output.timeParticleFiltering = timeParticleFiltering;
 		output.stamp = stamp;
@@ -105,6 +107,7 @@ public:
 	std::map<int, Transform> localBundlePoses;
 	std::map<int, std::vector<CameraModel> > localBundleModels;
 	bool keyFrameAdded;
+	float timeDeskewing;
 	float timeEstimation;
 	float timeParticleFiltering;
 	double stamp;
@@ -197,6 +197,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Rtabmap, LoopThr,           float, 0.11,      "Loop closing threshold.");
    RTABMAP_PARAM(Rtabmap, LoopRatio,         float, 0,         "The loop closure hypothesis must be over LoopRatio x lastHypothesisValue.");
    RTABMAP_PARAM(Rtabmap, LoopGPS,           bool,  true,      uFormat("Use GPS to filter likelihood (if GPS is recorded). Only locations inside the local radius \"%s\" of the current GPS location are considered for loop closure detection.", kRGBDLocalRadius().c_str()));
+    RTABMAP_PARAM(Rtabmap, VirtualPlaceLikelihoodRatio, int, 0,  "Likelihood ratio for virtual place (for no loop closure hypothesis): 0=Mean / StdDev, 1=StdDev / (Max-Mean)");

    // Memory
    RTABMAP_PARAM(Mem, RehearsalSimilarity,         float, 0.6,     "Rehearsal similarity.");
@@ -231,6 +232,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Mem, UseOdomFeatures,             bool, true,     "Use odometry features instead of regenerating them.");
    RTABMAP_PARAM(Mem, UseOdomGravity,              bool, false,    uFormat("Use odometry instead of IMU orientation to add gravity links to new nodes created. We assume that odometry is already aligned with gravity (e.g., we are using a VIO approach). Gravity constraints are used by graph optimization only if \"%s\" is not zero.", kOptimizerGravitySigma().c_str()));
    RTABMAP_PARAM(Mem, CovOffDiagIgnored,           bool, true,     "Ignore off diagonal values of the covariance matrix.");
+    RTABMAP_PARAM(Mem, GlobalDescriptorStrategy,    int, 0,        "Extract global descriptor from sensor data. 0=disabled, 1=PyDescriptor");
    RTABMAP_PARAM(Mem, RotateImagesUpsideUp,        bool, false,    "Rotate images so that upside is up if they are not already. This can be useful in case the robots don't have all same camera orientation but are using the same map, so that not rotation-invariant visual features can still be used across the fleet.");

    // KeypointMemory (Keypoint-based)
@@ -242,6 +244,7 @@ class RTABMAP_CORE_EXPORT Parameters
    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).");
+    RTABMAP_PARAM(Kp, SSC,                      bool, false,  "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
    RTABMAP_PARAM(Kp, BadSignRatio,             float, 0.5,   "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad).");
    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)
@@ -358,6 +361,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(RGBD, OptimizeFromGraphEnd,     bool, false, "Optimize graph from the newest node. If false, the graph is optimized from the oldest node of the current graph (this adds an overhead computation to detect to oldest node of the current graph, but it can be useful to preserve the map referential from the oldest node). Warning when set to false: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).");
    RTABMAP_PARAM(RGBD, OptimizeMaxError,         float, 3.0,   uFormat("Reject loop closures if optimization error ratio is greater than this value (0=disabled). Ratio is computed as absolute error over standard deviation of each link. This will help to detect when a wrong loop closure is added to the graph. Not compatible with \"%s\" if enabled.", kOptimizerRobust().c_str()));
    RTABMAP_PARAM(RGBD, MaxLoopClosureDistance,   float, 0.0,   "Reject loop closures/localizations if the distance from the map is over this distance (0=disabled).");
+    RTABMAP_PARAM(RGBD, ForceOdom3DoF,            bool, true,  uFormat("Force odometry pose to be 3DoF if %s=true.", kRegForce3DoF().c_str()));
    RTABMAP_PARAM(RGBD, StartAtOrigin,            bool, false, uFormat("If true, rtabmap will assume the robot is starting from origin of the map. If false, rtabmap will assume the robot is restarting from the last saved localization pose from previous session (the place where it shut down previously). Used only in localization mode (%s=false).", kMemIncrementalMemory().c_str()));
    RTABMAP_PARAM(RGBD, GoalReachedRadius,        float, 0.5,  "Goal reached radius (m).");
    RTABMAP_PARAM(RGBD, PlanStuckIterations,      int, 0,      "Mark the current goal node on the path as unreachable if it is not updated after X iterations (0=disabled). If all upcoming nodes on the path are unreachabled, the plan fails.");
@@ -436,6 +440,9 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(g2o, Baseline,          double, 0.075,   "When doing bundle adjustment with RGB-D data, we can set a fake baseline (m) to do stereo bundle adjustment (if 0, mono bundle adjustment is done). For stereo data, the baseline in the calibration is used directly.");

    RTABMAP_PARAM(GTSAM, Optimizer,       int, 1,          "0=Levenberg 1=GaussNewton 2=Dogleg");
+    RTABMAP_PARAM(GTSAM, Incremental,     bool, false,     uFormat("Do graph optimization incrementally (iSAM2) to increase optimization speed on loop closures. Note that only GaussNewton and Dogleg optimization algorithms are supported (%s) in this mode.", kGTSAMOptimizer().c_str()));
+    RTABMAP_PARAM(GTSAM, IncRelinearizeThreshold,  double, 0.01, "Only relinearize variables whose linear delta magnitude is greater than this threshold. See GTSAM::ISAM2 doc for more info.");
+    RTABMAP_PARAM(GTSAM, IncRelinearizeSkip,       int, 1, "Only relinearize any variables every X calls to ISAM2::update(). See GTSAM::ISAM2 doc for more info.");

    // Odometry
    RTABMAP_PARAM(Odom, Strategy,               int, 0,       "0=Frame-to-Map (F2M) 1=Frame-to-Frame (F2F) 2=Fovis 3=viso2 4=DVO-SLAM 5=ORB_SLAM2 6=OKVIS 7=LOAM 8=MSCKF_VIO 9=VINS-Fusion 10=OpenVINS 11=FLOAM 12=Open3D");
@@ -458,6 +465,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Odom, ScanKeyFrameThr,        float, 0.9,   "[Geometry] Create a new keyframe when the number of ICP inliers drops under this ratio of points in last frame's scan. Setting the value to 0 means that a keyframe is created for each processed frame.");
    RTABMAP_PARAM(Odom, ImageDecimation,     unsigned int, 1, uFormat("Decimation of the RGB image before registration. If depth size is larger than decimated RGB size, depth is decimated to be always at most equal to RGB size. If %s is true and if depth is smaller than decimated RGB, depth may be interpolated to match RGB size for feature detection.", kVisDepthAsMask().c_str()));
    RTABMAP_PARAM(Odom, AlignWithGround,        bool, false,  "Align odometry with the ground on initialization.");
+    RTABMAP_PARAM(Odom, Deskewing,              bool, true,   "Lidar deskewing. If input lidar has time channel, it will be deskewed with a constant motion model (with IMU orientation and/or guess if provided).");

    // Odometry Frame-to-Map
    RTABMAP_PARAM(OdomF2M, MaxSize,             int, 2000,    "[Visual] Local map size: If > 0 (example 5000), the odometry will maintain a local map of X maximum words.");
@@ -686,6 +694,7 @@ class RTABMAP_CORE_EXPORT Parameters
    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 12=SURF/FREAK 13=GFTT/DAISY 14=SURF/DAISY 15=PyDetector");
 #endif
    RTABMAP_PARAM(Vis, MaxFeatures,               int,   1000,  "0 no limits.");
+    RTABMAP_PARAM(Vis, SSC,                       bool,  false, "If true, SSC (Suppression via Square Covering) is applied to limit keypoints.");
    RTABMAP_PARAM(Vis, MaxDepth,                  float, 0,     "Max depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, MinDepth,                  float, 0,     "Min depth of the features (0 means no limit).");
    RTABMAP_PARAM(Vis, DepthAsMask,               bool,  true,  "Use depth image as mask when extracting features.");
@@ -721,6 +730,10 @@ class RTABMAP_CORE_EXPORT Parameters
 	RTABMAP_PARAM(GMS, WithScale,            bool, false,   "Take scale transformation into account.");
 	RTABMAP_PARAM(GMS, ThresholdFactor,      double, 6.0,   "The higher, the less matches.");

+	// Global descriptor approaches
+	RTABMAP_PARAM_STR(PyDescriptor, Path,    "",          "Path to python script file (see available ones in rtabmap/corelib/src/pydescriptor/*). See the header to see where the script should be used.");
+	RTABMAP_PARAM(PyDescriptor, Dim,         int, 4096,   "Descriptor dimension.");
+
    // ICP registration parameters
 #ifdef RTABMAP_POINTMATCHER
    RTABMAP_PARAM(Icp, Strategy,                  int, 1,       "ICP implementation: 0=Point Cloud Library, 1=libpointmatcher, 2=CCCoreLib (CloudCompare).");
@@ -743,6 +756,7 @@ class RTABMAP_CORE_EXPORT Parameters
    RTABMAP_PARAM(Icp, Epsilon,                   float, 0,     "Set the transformation epsilon (maximum allowable difference between two consecutive transformations) in order for an optimization to be considered as having converged to the final solution.");
    RTABMAP_PARAM(Icp, CorrespondenceRatio,       float, 0.1,   "Ratio of matching correspondences to accept the transform.");
    RTABMAP_PARAM(Icp, Force4DoF,                 bool, false,   uFormat("Limit ICP to x, y, z and yaw DoF. Available if %s > 0.", kIcpStrategy().c_str()));
+    RTABMAP_PARAM(Icp, FiltersEnabled,            int, 3,       "Flag to enable filters: 1=\"from\" cloud only, 2=\"to\" cloud only, 3=both.");
 #ifdef RTABMAP_POINTMATCHER
    RTABMAP_PARAM(Icp, PointToPlane,                bool, true,   "Use point to plane ICP.");
 #else
@@ -922,6 +936,7 @@ public:
    static ParametersMap filterParameters(const ParametersMap & parameters, const std::string & group, bool remove = false);

    static void readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly = false);
+    static void readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly = false);
    static void writeINI(const std::string & configFile, const ParametersMap & parameters);

    /**
@@ -69,6 +69,7 @@ private:
 	float _epsilon;
 	float _correspondenceRatio;
 	bool _force4DoF;
+	int _filtersEnabled;
 	bool _pointToPlane;
 	int _pointToPlaneK;
 	float _pointToPlaneRadius;
@@ -151,6 +151,8 @@ public:

 	float getTimeThreshold() const {return _maxTimeAllowed;} // in ms
 	void setTimeThreshold(float maxTimeAllowed); // in ms
+	int getMemoryThreshold() const {return _maxMemoryAllowed;} // in nodes
+	void setMemoryThreshold(int maxMemoryAllowed); // in nodes

 	void setInitialPose(const Transform & initialPose);
 	int triggerNewMap();
@@ -282,6 +284,7 @@ private:
 	unsigned int _maxMemoryAllowed; // signatures count in WM
 	float _loopThr;
 	float _loopRatio;
+	int _virtualPlaceLikelihoodRatio;
 	float _maxLoopClosureDistance;
 	bool _verifyLoopClosureHypothesis;
 	unsigned int _maxRetrieved;
@@ -322,6 +325,7 @@ private:
 	int _pathStuckIterations;
 	float _pathLinearVelocity;
 	float _pathAngularVelocity;
+	bool _forceOdom3doF;
 	bool _restartAtOrigin;
 	bool _loopCovLimited;
 	bool _loopGPS;
@@ -0,0 +1,93 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <opencv2/highgui/highgui.hpp>
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include "rtabmap/core/SensorData.h"
+#include <set>
+#include <stack>
+#include <list>
+#include <vector>
+
+class UDirectory;
+class UTimer;
+
+namespace rtabmap
+{
+
+/**
+ * Class Camera
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCapture
+{
+public:
+	virtual ~SensorCapture();
+	SensorData takeData(SensorCaptureInfo * info = 0);
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") = 0;
+	virtual std::string getSerial() const = 0;
+	virtual bool odomProvided() const { return false; }
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06) { return false; }
+
+	//getters
+	float getFrameRate() const {return _frameRate;}
+	const Transform & getLocalTransform() const {return _localTransform;}
+
+	//setters
+	void setFrameRate(float frameRate) {_frameRate = frameRate;}
+	void setLocalTransform(const Transform & localTransform) {_localTransform= localTransform;}
+
+	void resetTimer();
+protected:
+	/**
+	 * Constructor
+	 *
+	 * @param frameRate the frame rate (Hz), 0 for fast as the sensor can
+	 * @param localTransform the transform from base frame to sensor frame
+	 */
+	SensorCapture(float frameRate = 0, const Transform & localTransform = Transform::getIdentity());
+
+	/**
+	 * returned rgb and depth images should be already rectified if calibration was loaded
+	 */
+	virtual SensorData captureData(SensorCaptureInfo * info = 0) = 0;
+
+	int getNextSeqID() {return ++_seq;}
+
+private:
+	float _frameRate;
+	Transform _localTransform;
+	UTimer * _frameRateTimer;
+	int _seq;
+};
+
+
+} // namespace rtabmap
@@ -0,0 +1,82 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/Transform.h"
+#include <string>
+
+namespace rtabmap
+{
+
+class SensorCaptureInfo
+{
+
+public:
+	SensorCaptureInfo() :
+		cameraName(""),
+		id(0),
+		stamp(0.0),
+		timeCapture(0.0f),
+		timeDeskewing(0.0f),
+		timeDisparity(0.0f),
+		timeMirroring(0.0f),
+		timeStereoExposureCompensation(0.0f),
+		timeImageDecimation(0.0f),
+		timeHistogramEqualization(0.0f),
+		timeScanFromDepth(0.0f),
+		timeUndistortDepth(0.0f),
+		timeBilateralFiltering(0.0f),
+		timeTotal(0.0f),
+		odomCovariance(cv::Mat::eye(6,6,CV_64FC1))
+	{
+	}
+	virtual ~SensorCaptureInfo() {}
+
+	std::string cameraName;
+	int id;
+	double stamp;
+	float timeCapture;
+	float timeDeskewing;
+	float timeDisparity;
+	float timeMirroring;
+	float timeStereoExposureCompensation;
+	float timeImageDecimation;
+	float timeHistogramEqualization;
+	float timeScanFromDepth;
+	float timeUndistortDepth;
+	float timeBilateralFiltering;
+	float timeTotal;
+	Transform odomPose;
+	cv::Mat odomCovariance;
+	std::vector<float> odomVelocity;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureInfo CameraInfo;
+
+} // namespace rtabmap
@@ -0,0 +1,216 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include "rtabmap/core/rtabmap_core_export.h" // DLL export/import defines
+
+#include <rtabmap/core/Parameters.h>
+#include <rtabmap/core/Transform.h>
+#include <rtabmap/utilite/UThread.h>
+#include <rtabmap/utilite/UEventsSender.h>
+
+namespace clams
+{
+class DiscreteDepthDistortionModel;
+}
+
+namespace rtabmap
+{
+
+class Camera;
+class Lidar;
+class SensorCapture;
+class SensorCaptureInfo;
+class SensorData;
+class StereoDense;
+class IMUFilter;
+class Feature2D;
+
+/**
+ * Class CameraThread
+ *
+ */
+class RTABMAP_CORE_EXPORT SensorCaptureThread :
+	public UThread,
+	public UEventsSender
+{
+public:
+	// ownership transferred
+	SensorCaptureThread(
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose (can be again the camera)
+	 * @param odomAsGt set odometry sensor pose as ground truth instead of odometry
+	 * @param extrinsics the static transform between odometry sensor's left lens frame to camera's left lens frame  (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from. If the camera is providing a pose, it can be used for deskewing
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the lidar)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			SensorCapture * odomSensor,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	/**
+	 * @param lidar the lidar to take scans from
+	 * @param camera the camera to take images from
+	 * @param odomSensor an odometry sensor to get a pose and used for deskewing (can be again the camera or lidar)
+	 * @param extrinsics the static transform between odometry frame to camera frame (without optical rotation)
+	 */
+	SensorCaptureThread(
+			Lidar * lidar,
+			Camera * camera,
+			SensorCapture * odomSensor,
+			const Transform & extrinsics,
+			double poseTimeOffset = 0.0,
+			float poseScaleFactor = 1.0f,
+			double poseWaitTime = 0.1,
+			const ParametersMap & parameters = ParametersMap());
+	virtual ~SensorCaptureThread();
+
+	void setMirroringEnabled(bool enabled) {_mirroring = enabled;}
+	void setStereoExposureCompensation(bool enabled) {_stereoExposureCompensation = enabled;}
+	void setColorOnly(bool colorOnly) {_colorOnly = colorOnly;}
+	void setImageDecimation(int decimation) {_imageDecimation = decimation;}
+	void setHistogramMethod(int histogramMethod) {_histogramMethod = histogramMethod;}
+	void setStereoToDepth(bool enabled) {_stereoToDepth = enabled;}
+	void setFrameRate(float frameRate);
+	RTABMAP_DEPRECATED void setImageRate(float frameRate) {setFrameRate(frameRate);}
+	void setDistortionModel(const std::string & path);
+	void setOdomAsGroundTruth(bool enabled) {_odomAsGt = enabled;}
+	void enableBilateralFiltering(float sigmaS, float sigmaR);
+	void disableBilateralFiltering() {_bilateralFiltering = false;}
+	void enableIMUFiltering(int filteringStrategy=1, const ParametersMap & parameters = ParametersMap(), bool baseFrameConversion = false);
+	void disableIMUFiltering();
+	void enableFeatureDetection(const ParametersMap & parameters = ParametersMap());
+	void disableFeatureDetection();
+
+	// Use new version of this function with groundNormalsUp=0.8 for forceGroundNormalsUp=True and groundNormalsUp=0.0 for forceGroundNormalsUp=False.
+	RTABMAP_DEPRECATED void setScanParameters(
+			bool fromDepth,
+			int downsampleStep, // decimation of the depth image in case the scan is from depth image
+			float rangeMin,
+			float rangeMax,
+			float voxelSize,
+			int normalsK,
+			float normalsRadius,
+			bool forceGroundNormalsUp,
+			bool deskewing);
+	void setScanParameters(
+			bool fromDepth,
+			int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
+			float rangeMin=0.0f,
+			float rangeMax=0.0f,
+			float voxelSize = 0.0f,
+			int normalsK = 0,
+			float normalsRadius = 0.0f,
+			float groundNormalsUp = 0.0f,
+			bool deskewing = false);
+
+	void postUpdate(SensorData * data, SensorCaptureInfo * info = 0) const;
+
+	//getters
+	bool isPaused() const {return !this->isRunning();}
+	bool isCapturing() const {return this->isRunning();}
+	bool odomProvided() const;
+
+	Camera * camera() {return _camera;} // return null if not set, valid until CameraThread is deleted
+	SensorCapture * odomSensor() {return _odomSensor;} // return null if not set, valid until CameraThread is deleted
+	Lidar * lidar() {return _lidar;} // return null if not set, valid until CameraThread is deleted
+
+private:
+	virtual void mainLoopBegin();
+	virtual void mainLoop();
+	virtual void mainLoopKill();
+
+private:
+	Camera * _camera;
+	SensorCapture * _odomSensor;
+	Lidar * _lidar;
+	Transform _extrinsicsOdomToCamera;
+	bool _odomAsGt;
+	double _poseTimeOffset;
+	float _poseScaleFactor;
+	double _poseWaitTime;
+	bool _mirroring;
+	bool _stereoExposureCompensation;
+	bool _colorOnly;
+	int _imageDecimation;
+	int _histogramMethod;
+	bool _stereoToDepth;
+	bool _scanDeskewing;
+	bool _scanFromDepth;
+	int _scanDownsampleStep;
+	float _scanRangeMin;
+	float _scanRangeMax;
+	float _scanVoxelSize;
+	int _scanNormalsK;
+	float _scanNormalsRadius;
+	float _scanForceGroundNormalsUp;
+	StereoDense * _stereoDense;
+	clams::DiscreteDepthDistortionModel * _distortionModel;
+	bool _bilateralFiltering;
+	float _bilateralSigmaS;
+	float _bilateralSigmaR;
+	IMUFilter * _imuFilter;
+	bool _imuBaseFrameConversion;
+	Feature2D * _featureDetector;
+	bool _depthAsMask;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorCaptureThread CameraThread;
+
+} // namespace rtabmap
@@ -0,0 +1,94 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#pragma once
+
+#include <rtabmap/core/SensorCaptureInfo.h>
+#include <rtabmap/utilite/UEvent.h>
+#include "rtabmap/core/SensorData.h"
+
+namespace rtabmap
+{
+
+class SensorEvent :
+	public UEvent
+{
+public:
+	enum Code {
+		kCodeData,
+		kCodeNoMoreImages
+	};
+
+public:
+	SensorEvent(const cv::Mat & image, int seq=0, double stamp = 0.0, const std::string & cameraName = std::string()) :
+		UEvent(kCodeData),
+		data_(image, seq, stamp)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+
+	SensorEvent() :
+		UEvent(kCodeNoMoreImages)
+	{
+	}
+
+	SensorEvent(const SensorData & data) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+	}
+
+	SensorEvent(const SensorData & data, const std::string & cameraName) :
+		UEvent(kCodeData),
+		data_(data)
+	{
+		sensorCaptureInfo_.cameraName = cameraName;
+	}
+	SensorEvent(const SensorData & data, const SensorCaptureInfo & sensorCaptureInfo) :
+		UEvent(kCodeData),
+		data_(data),
+		sensorCaptureInfo_(sensorCaptureInfo)
+	{
+	}
+
+	// Image or descriptors
+	const SensorData & data() const {return data_;}
+	const std::string & cameraName() const {return sensorCaptureInfo_.cameraName;}
+	const SensorCaptureInfo & info() const {return sensorCaptureInfo_;}
+
+	virtual ~SensorEvent() {}
+	virtual std::string getClassName() const {return std::string("SensorEvent");}
+
+private:
+	SensorData data_;
+	SensorCaptureInfo sensorCaptureInfo_;
+};
+
+//backward compatibility
+RTABMAP_DEPRECATED typedef SensorEvent CameraEvent;
+
+} // namespace rtabmap
@@ -90,9 +90,10 @@ public:
 	void removeLink(int idTo);
 	void removeVirtualLinks();

-	void addLandmark(const Link & landmark) {_landmarks.insert(std::make_pair(landmark.to(), landmark));}
+	void addLandmark(const Link & landmark);
 	const std::map<int, Link> & getLandmarks() const {return _landmarks;}
-	void removeLandmarks() {_landmarks.clear();}
+	void removeLandmarks();
+	void removeLandmark(int landmarkId);

 	void setSaved(bool saved) {_saved = saved;}
 	void setModified(bool modified) {_modified = modified; _linksModified = modified;}
@@ -157,6 +157,7 @@ class RTABMAP_CORE_EXPORT Statistics
 	RTABMAP_STATS(Timing, Memory_update, ms);
 	RTABMAP_STATS(Timing, Neighbor_link_refining, ms);
 	RTABMAP_STATS(Timing, Proximity_by_time, ms);
+	RTABMAP_STATS(Timing, Proximity_by_space_search, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space_visual, ms);
 	RTABMAP_STATS(Timing, Proximity_by_space, ms);
 	RTABMAP_STATS(Timing, Cleaning_neighbors, ms);
@@ -57,9 +57,12 @@ public:

 	void setOutputMode(int outputMode = 0);
 	void setDepthProfile(int confThreshold = 200, int lrcThreshold = 5);
+	void setExtendedDisparity(bool extendedDisparity);
+	void setSubpixelMode(bool enabled, int fractionalBits = 3);
+	void setCompanding(bool enabled, int width=96);
 	void setRectification(bool useSpecTranslation, float alphaScaling = 0.0f);
 	void setIMU(bool imuPublished, bool publishInterIMU);
-	void setIrBrightness(float dotProjectormA = 0.0f, float floodLightmA = 200.0f);
+	void setIrIntensity(float dotIntensity = 0.0f, float floodIntensity = 0.0f);
 	void setDetectFeatures(int detectFeatures = 0);
 	void setBlobPath(const std::string & blobPath);
 	void setGFTTDetector(bool useHarrisDetector = false, float minDistance = 7.0f, int numTargetFeatures = 1000);
@@ -70,7 +73,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_DEPTHAI
@@ -82,12 +85,15 @@ private:
 	int confThreshold_;
 	int lrcThreshold_;
 	int resolution_;
+	bool extendedDisparity_;
+	int subpixelFractionalBits_;
+	int compandingWidth_;
 	bool useSpecTranslation_;
 	float alphaScaling_;
 	bool imuPublished_;
 	bool publishInterIMU_;
-	float dotProjectormA_;
-	float floodLightmA_;
+	float dotIntensity_;
+	float floodIntensity_;
 	int detectFeatures_;
 	bool useHarrisDetector_;
 	float minDistance_;
@@ -97,9 +103,7 @@ private:
 	int nmsRadius_;
 	std::string blobPath_;
 	std::shared_ptr<dai::Device> device_;
-	std::shared_ptr<dai::DataOutputQueue> leftOrColorQueue_;
-	std::shared_ptr<dai::DataOutputQueue> rightOrDepthQueue_;
-	std::shared_ptr<dai::DataOutputQueue> featuresQueue_;
+	std::shared_ptr<dai::DataOutputQueue> cameraQueue_;
 	std::map<double, cv::Vec3f> accBuffer_;
 	std::map<double, cv::Vec3f> gyroBuffer_;
 	UMutex imuMutex_;
@@ -61,7 +61,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_FREENECT
@@ -77,7 +77,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_FREENECT2
@@ -118,7 +118,7 @@ public:
 	}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	bool readPoses(
@@ -63,7 +63,7 @@ public:
 	void setPreferences(int rgb_resolution, int framerate, int depth_resolution);

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	void close();
@@ -72,7 +72,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	void close();
@@ -67,7 +67,7 @@ protected:
 	/**
 	 * returned rgb and depth images should be already rectified if calibration was loaded
 	 */
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_MYNTEYE
@@ -69,7 +69,7 @@ public:
 	void setDepthDecimation(int decimation);

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_OPENNI2
@@ -51,7 +51,7 @@ public:
 	virtual std::string getSerial() const {return "";} // unknown with OpenCV

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	bool _asus;
@@ -85,7 +85,7 @@ public:
    virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
    pcl::Grabber* interface_;
@@ -53,7 +53,7 @@ public:
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);cameraDepth_.setMaxFrames(value);}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	CameraImages cameraDepth_;
@@ -72,7 +72,7 @@ public:
 	virtual bool odomProvided() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_REALSENSE
@@ -68,7 +68,7 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const;
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.06);

 	// parameters are set during initialization
 	// D400 series
@@ -77,7 +77,7 @@ public:
 	void setResolution(int width, int height, int fps = 30);
 	void setDepthResolution(int width, int height, int fps = 30);
 	void setGlobalTimeSync(bool enabled);
-	void publishInterIMU(bool enabled);
+
 	/**
 	 * Dual mode (D400+T265 or L500+T265)
 	 * @param enabled enable dual mode
@@ -105,7 +105,7 @@ private:
 #endif

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_REALSENSE2
@@ -142,7 +142,6 @@ private:
 	int cameraDepthHeight_;
 	int cameraDepthFps_;
 	bool globalTimeSync_;
-	bool publishInterIMU_;
 	bool dualMode_;
 	Transform dualExtrinsics_;
 	std::string jsonConfig_;
@@ -51,7 +51,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_DC1394
@@ -53,7 +53,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_FLYCAPTURE2
@@ -64,7 +64,7 @@ public:
 	virtual void setMaxFrames(int value) {CameraImages::setMaxFrames(value);camera2_->setMaxFrames(value);}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	CameraImages * camera2_;
@@ -60,7 +60,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	cv::VideoCapture capture_;
@@ -71,7 +71,7 @@ public:
 	void setResolution(int width, int height) {_width=width, _height=height;}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	cv::VideoCapture capture_;
@@ -76,12 +76,12 @@ public:
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
 	virtual bool odomProvided() const;
-	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance);
+	virtual bool getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime = 0.0);

-	void publishInterIMU(bool enabled);
+	void postInterIMUPublic(const IMU & imu, double stamp);

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_ZED
@@ -100,7 +100,6 @@ private:
 	bool computeOdometry_;
 	bool lost_;
 	bool force3DoF_;
-	bool publishInterIMU_;
 	ZedIMUThread * imuPublishingThread_;
 #endif
 };
@@ -63,7 +63,7 @@ public:
 	virtual std::string getSerial() const;

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 #ifdef RTABMAP_ZEDOC
@@ -64,7 +64,7 @@ public:
 	void setResolution(int width, int height) {_width=width, _height=height;}

 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(SensorCaptureInfo * info = 0);

 private:
 	// File type
@@ -0,0 +1,94 @@
+/*
+Copyright (c) 2010-2022, Mathieu Labbe
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+#ifndef CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_
+
+// Should be first on windows to avoid "WinSock.h has already been included" error
+#include <pcl/io/vlp_grabber.h>
+
+#include <rtabmap/core/Lidar.h>
+#include <rtabmap/utilite/USemaphore.h>
+
+namespace rtabmap {
+
+struct PointXYZIT {
+	float x;
+	float y;
+	float z;
+	float i;
+	float t;
+};
+
+class RTABMAP_CORE_EXPORT LidarVLP16 :public Lidar, public pcl::VLPGrabber {
+public:
+	LidarVLP16(
+			const std::string& pcapFile,
+			bool organized = false,
+			bool stampLast = true,
+			float frameRate = 0.0f,
+			Transform localTransform = Transform::getIdentity());
+	LidarVLP16(
+			const boost::asio::ip::address& ipAddress,
+			const std::uint16_t port = 2368,
+			bool organized = false,
+			bool useHostTime = true,
+			bool stampLast = true,
+			float frameRate = 0.0f,
+			Transform localTransform = Transform::getIdentity());
+	virtual ~LidarVLP16();
+
+	SensorData takeScan(SensorCaptureInfo * info = 0) {return takeData(info);}
+
+	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "") override;
+	virtual std::string getSerial() const override {return getName();}
+
+	void setOrganized(bool enable);
+
+private:
+	void buildTimings(bool dualMode);
+    virtual void toPointClouds (HDLDataPacket *dataPacket) override;
+
+protected:
+    virtual SensorData captureData(SensorCaptureInfo * info = 0) override;
+
+private:
+    // timing offset lookup table
+    std::vector< std::vector<float> > timingOffsets_;
+    bool timingOffsetsDualMode_;
+    double startSweepTime_;
+    double startSweepTimeHost_;
+    bool organized_;
+    bool useHostTime_;
+    bool stampLast_;
+    SensorData lastScan_;
+    std::vector<std::vector<PointXYZIT> > accumulatedScans_;
+    USemaphore scanReady_;
+    UMutex lastScanMutex_;
+};
+
+} /* namespace rtabmap */
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LIDAR_LIDARVLP16_H_ */
@@ -30,6 +30,10 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/core/Optimizer.h>

+namespace gtsam {
+	class ISAM2;
+}
+
 namespace rtabmap {

 class RTABMAP_CORE_EXPORT OptimizerGTSAM : public Optimizer
@@ -38,13 +42,8 @@ public:
 	static bool available();

 public:
-	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap()) :
-		Optimizer(parameters),
-		optimizer_(Parameters::defaultGTSAMOptimizer())
-	{
-		parseParameters(parameters);
-	}
-	virtual ~OptimizerGTSAM() {}
+	OptimizerGTSAM(const ParametersMap & parameters = ParametersMap());
+	virtual ~OptimizerGTSAM();

 	virtual Type type() const {return kTypeGTSAM;}

@@ -60,7 +59,25 @@ public:
 			int * iterationsDone = 0);

 private:
-	int optimizer_;
+	int internalOptimizerType_;
+
+	gtsam::ISAM2 * isam2_;
+	struct ConstraintToFactor {
+		ConstraintToFactor(int _from, int _to, std::uint64_t _factorIndice)
+		{
+			from = _from;
+			to = _to;
+			factorIndice = _factorIndice;
+		}
+		int from;
+		int to;
+		std::uint64_t factorIndice;
+	};
+
+	std::vector<ConstraintToFactor> lastAddedConstraints_;
+	int lastSwitchId_;
+	std::set<int> addedPoses_;
+	std::pair<int, std::uint64_t> lastRootFactorIndex_;
 };

 } /* namespace rtabmap */
@@ -164,6 +164,9 @@ void RTABMAP_CORE_EXPORT NMS(
 		cv::Mat & descriptorsOut,
 		int border, int dist_thresh, int img_width, int img_height);

+std::vector<int> RTABMAP_CORE_EXPORT SSC(
+	const std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, float tolerance, int cols, int rows);
+
 /**
 * @brief Rotate images and camera model so that the top of the image is up.
 *
@@ -455,6 +455,19 @@ RTABMAP_DEPRECATED pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_CORE_EXPORT loadC
 		int downsampleStep = 1,
 		float voxelSize = 0.0f);

+/**
+ * @brief Lidar deskewing
+ * @param input lidar, format should have time channel
+ * @param input stamp of the lidar
+ * @param velocity in base frame
+ * @param velocity stamp at which it has been computed
+ * @return lidar deskewed
+ */
+LaserScan RTABMAP_CORE_EXPORT deskew(
+		const LaserScan & input,
+		double inputStamp,
+		const rtabmap::Transform & velocity);
+
 } // namespace util3d
 } // namespace rtabmap

@@ -13,8 +13,10 @@ SET(SRC_FILES
    
    Recovery.cpp
    
+    SensorCapture.cpp
+    SensorCaptureThread.cpp
+    
    Camera.cpp
-    CameraThread.cpp
    CameraModel.cpp
    
    camera/CameraFreenect.cpp
@@ -115,6 +117,8 @@ SET(SRC_FILES
    
    MarkerDetector.cpp
    
+    GlobalDescriptorExtractor.cpp
+    
    GainCompensator.cpp

    rtflann/ext/lz4.c
@@ -132,6 +136,13 @@ SET(SRC_FILES
    opencv/five-point.cpp
 )

+IF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
+SET(SRC_FILES
+    ${SRC_FILES}
+    lidar/LidarVLP16.cpp
+)
+ENDIF(PCL_VERSION VERSION_GREATER_EQUAL "1.8")
+
 IF(OpenCV_VERSION_MAJOR EQUAL 2)
 SET(SRC_FILES
    ${SRC_FILES}
@@ -173,14 +184,14 @@ SET(PUBLIC_LIBRARIES
 	${PCL_LIBRARIES}
 )

-IF(Sqlite3_FOUND)
+IF(SQLite3_FOUND)
    SET(INCLUDE_DIRS
 		${INCLUDE_DIRS}
-		${Sqlite3_INCLUDE_DIRS}
+		${SQLite3_INCLUDE_DIRS}
 	)
 	SET(LIBRARIES
 		${LIBRARIES}
-		${Sqlite3_LIBRARIES}
+		${SQLite3_LIBRARIES}
 	)
 ELSE()
 	SET(SRC_FILES
@@ -224,6 +235,7 @@ IF(WITH_PYTHON AND Python3_FOUND)
 		python/PythonInterface.cpp
 		python/PyMatcher.cpp
 		python/PyDetector.cpp
+		python/PyDescriptor.cpp
 	)
 	 SET(INCLUDE_DIRS
 	    ${TORCH_INCLUDE_DIRS}
@@ -26,42 +26,25 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

 #include "rtabmap/core/Camera.h"
+#include "rtabmap/core/IMUFilter.h"

-#include <rtabmap/utilite/UEventsManager.h>
-#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UStl.h>
-#include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UFile.h>
 #include <rtabmap/utilite/UDirectory.h>
-#include <rtabmap/utilite/UTimer.h>
-
-#include <opencv2/imgproc/imgproc.hpp>
-
-#include <iostream>
-#include <cmath>
+#include <rtabmap/utilite/UEventsManager.h>

 namespace rtabmap
 {

 Camera::Camera(float imageRate, const Transform & localTransform) :
-	_imageRate(imageRate),
-	_localTransform(localTransform*CameraModel::opticalRotation()),
-	_targetImageSize(0,0),
-	_frameRateTimer(new UTimer()),
-	_seq(0)
-{
-}
+		SensorCapture(imageRate, localTransform*CameraModel::opticalRotation()),
+		imuFilter_(0),
+		publishInterIMU_(false)
+{}

 Camera::~Camera()
 {
-	UDEBUG("");
-	delete _frameRateTimer;
-	UDEBUG("");
-}
-
-void Camera::resetTimer()
-{
-	_frameRateTimer->start();
+	delete imuFilter_;
 }

 bool Camera::initFromFile(const std::string & calibrationPath)
@@ -69,54 +52,32 @@ bool Camera::initFromFile(const std::string & calibrationPath)
 	return init(UDirectory::getDir(calibrationPath), uSplit(UFile::getName(calibrationPath), '.').front());
 }

-SensorData Camera::takeImage(CameraInfo * info)
+void Camera::setInterIMUPublishing(bool enabled, IMUFilter * filter)
 {
-	bool warnFrameRateTooHigh = false;
-	float actualFrameRate = 0;
-	float imageRate = _imageRate;
-	if(imageRate>0)
-	{
-		int sleepTime = (1000.0f/imageRate - 1000.0f*_frameRateTimer->getElapsedTime());
-		if(sleepTime > 2)
-		{
-			uSleep(sleepTime-2);
-		}
-		else if(sleepTime < 0)
-		{
-			warnFrameRateTooHigh = true;
-			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
-		}
+	publishInterIMU_ = enabled;
+	delete imuFilter_;
+	imuFilter_ = filter;
+}

-		// Add precision at the cost of a small overhead
-		while(_frameRateTimer->getElapsedTime() < 1.0/double(imageRate)-0.000001)
-		{
-			//
-		}
-
-		double slept = _frameRateTimer->getElapsedTime();
-		_frameRateTimer->start();
-		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(imageRate));
-	}
-
-	UTimer timer;
-	SensorData data = this->captureImage(info);
-	double captureTime = timer.ticks();
-	if(warnFrameRateTooHigh)
+void Camera::postInterIMU(const IMU & imu, double stamp)
+{
+	if(imuFilter_)
 	{
-		UWARN("Camera: Cannot reach target image rate %f Hz, current rate is %f Hz and capture time = %f s.",
-				imageRate, actualFrameRate, captureTime);
+		imuFilter_->update(
+				imu.angularVelocity()[0], imu.angularVelocity()[1], imu.angularVelocity()[2],
+				imu.linearAcceleration()[0], imu.linearAcceleration()[1], imu.linearAcceleration()[2],
+				stamp);
+		cv::Vec4d q;
+		imuFilter_->getOrientation(q[0],q[1],q[2],q[3]);
+		UEventsManager::post(new IMUEvent(IMU(
+				q, cv::Mat(),
+				imu.angularVelocity(), imu.angularVelocityCovariance(),
+				imu.linearAcceleration(), imu.linearAccelerationCovariance(),
+				imu.localTransform()),
+				stamp));
+		return;
 	}
-	else
-	{
-		UDEBUG("Time capturing image = %fs", captureTime);
-	}
-	if(info)
-	{
-		info->id = data.id();
-		info->stamp = data.stamp();
-		info->timeCapture = captureTime;
-	}
-	return data;
+	UEventsManager::post(new IMUEvent(imu, stamp));
 }

 } // namespace rtabmap
@@ -6728,7 +6728,7 @@ void DBDriverSqlite3::stepGlobalDescriptor(sqlite3_stmt * ppStmt,

 	//data
 	std::vector<unsigned char> dataBytes = rtabmap::compressData(descriptor.data());
-	if(infoBytes.empty())
+	if(dataBytes.empty())
 	{
 		rc = sqlite3_bind_null(ppStmt, index++);
 	}
@@ -25,6 +25,7 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/DBReader.h"
 #include "rtabmap/core/DBDriver.h"

@@ -34,7 +35,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UConversion.h>
 #include <rtabmap/utilite/UEventsManager.h>

-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/util3d.h"
@@ -268,7 +268,7 @@ std::string DBReader::getSerial() const
 	return "DBReader";
 }

-SensorData DBReader::captureImage(CameraInfo * info)
+SensorData DBReader::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data = this->getNextData(info);
 	if(data.id()>0 && _stopId>0 && data.id() > _stopId)
@@ -370,7 +370,7 @@ SensorData DBReader::captureImage(CameraInfo * info)
 	return data;
 }

-SensorData DBReader::getNextData(CameraInfo * info)
+SensorData DBReader::getNextData(SensorCaptureInfo * info)
 {
 	SensorData data;
 	if(_dbDriver)
@@ -268,70 +268,111 @@ void Feature2D::filterKeypointsByDisparity(
 	}
 }

-void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, int maxKeypoints, const cv::Size & imageSize, bool ssc)
 {
 	cv::Mat descriptors;
-	limitKeypoints(keypoints, descriptors, maxKeypoints);
+	limitKeypoints(keypoints, descriptors, maxKeypoints, imageSize, ssc);
 }

-void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
 {
 	std::vector<cv::Point3f> keypoints3D;
-	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints);
+	limitKeypoints(keypoints, keypoints3D, descriptors, maxKeypoints, imageSize, ssc);
 }

-void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints)
+void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vector<cv::Point3f> & keypoints3D, cv::Mat & descriptors, int maxKeypoints, const cv::Size & imageSize, bool ssc)
 {
 	UASSERT_MSG((int)keypoints.size() == descriptors.rows || descriptors.rows == 0, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
 	UASSERT_MSG(keypoints.size() == keypoints3D.size() || keypoints3D.size() == 0, uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
-		// Remove words under the new hessian threshold
-
-		// Sort words by hessian
-		std::multimap<float, int> hessianMap; // <hessian,id>
-		for(unsigned int i = 0; i <keypoints.size(); ++i)
-		{
-			//Keep track of the data, to be easier to manage the data in the next step
-			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
-		}
-
-		// Remove them from the signature
-		int removed = (int)hessianMap.size()-maxKeypoints;
-		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-		std::vector<cv::KeyPoint> kptsTmp(maxKeypoints);
+		int removed;
+		std::vector<cv::KeyPoint> kptsTmp;
 		std::vector<cv::Point3f> kpts3DTmp;
-		if(!keypoints3D.empty())
-		{
-			kpts3DTmp.resize(maxKeypoints);
-		}
 		cv::Mat descriptorsTmp;
-		if(descriptors.rows)
+		if(ssc)
 		{
-			descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
-		}
-		for(unsigned int k=0; k < kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
-		{
-			kptsTmp[k] = keypoints[iter->second];
-			if(keypoints3D.size())
+			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			static constexpr float tolerance = 0.1;
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			removed = keypoints.size()-ResultVec.size();
+			// retrieve final keypoints
+			kptsTmp.resize(ResultVec.size());
+			if(!keypoints3D.empty())
 			{
-				kpts3DTmp[k] = keypoints3D[iter->second];
+				kpts3DTmp.resize(ResultVec.size());
 			}
 			if(descriptors.rows)
 			{
-				if(descriptors.type() == CV_32FC1)
+				descriptorsTmp = cv::Mat(ResultVec.size(), descriptors.cols, descriptors.type());
+			}
+			for(unsigned int k=0; k<ResultVec.size(); ++k)
+			{
+				kptsTmp[k] = keypoints[ResultVec[k]];
+				if(keypoints3D.size())
 				{
-					memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+					kpts3DTmp[k] = keypoints3D[ResultVec[k]];
 				}
-				else
+				if(descriptors.rows)
 				{
-					memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
+					if(descriptors.type() == CV_32FC1)
+					{
+						memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(ResultVec[k]), descriptors.cols*sizeof(float));
+					}
+					else
+					{
+						memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(ResultVec[k]), descriptors.cols*sizeof(char));
+					}
 				}
 			}
 		}
-		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), kptsTmp.size()?kptsTmp.back().response:0.0f);
+		else
+		{
+			ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+			// Remove words under the new hessian threshold
+
+			// Sort words by hessian
+			std::multimap<float, int> hessianMap; // <hessian,id>
+			for(unsigned int i = 0; i <keypoints.size(); ++i)
+			{
+				//Keep track of the data, to be easier to manage the data in the next step
+				hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+			}
+
+			// Remove them from the signature
+			removed = (int)hessianMap.size()-maxKeypoints;
+			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+			kptsTmp.resize(maxKeypoints);
+			if(!keypoints3D.empty())
+			{
+				kpts3DTmp.resize(maxKeypoints);
+			}
+			if(descriptors.rows)
+			{
+				descriptorsTmp = cv::Mat(maxKeypoints, descriptors.cols, descriptors.type());
+			}
+			for(unsigned int k=0; k<kptsTmp.size() && iter!=hessianMap.rend(); ++k, ++iter)
+			{
+				kptsTmp[k] = keypoints[iter->second];
+				if(keypoints3D.size())
+				{
+					kpts3DTmp[k] = keypoints3D[iter->second];
+				}
+				if(descriptors.rows)
+				{
+					if(descriptors.type() == CV_32FC1)
+					{
+						memcpy(descriptorsTmp.ptr<float>(k), descriptors.ptr<float>(iter->second), descriptors.cols*sizeof(float));
+					}
+					else
+					{
+						memcpy(descriptorsTmp.ptr<char>(k), descriptors.ptr<char>(iter->second), descriptors.cols*sizeof(char));
+					}
+				}
+			}
+		}
+		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, (int)kptsTmp.size(), !ssc&&kptsTmp.size()?kptsTmp.back().response:0.0f);
 		ULOGGER_DEBUG("removing words time = %f s", timer.ticks());
 		keypoints = kptsTmp;
 		keypoints3D = kpts3DTmp;
@@ -342,31 +383,46 @@ void Feature2D::limitKeypoints(std::vector<cv::KeyPoint> & keypoints, std::vecto
 	}
 }

-void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints)
+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, bool ssc)
 {
 	if(maxKeypoints > 0 && (int)keypoints.size() > maxKeypoints)
 	{
 		UTimer timer;
-		ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", (int)keypoints.size());
-		// Remove words under the new hessian threshold
-
-		// Sort words by hessian
-		std::multimap<float, int> hessianMap; // <hessian,id>
-		for(unsigned int i = 0; i <keypoints.size(); ++i)
-		{
-			//Keep track of the data, to be easier to manage the data in the next step
-			hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
-		}
-
-		// Keep keypoints with highest response
-		int removed = (int)hessianMap.size()-maxKeypoints;
-		std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
-		inliers.resize(keypoints.size(), false);
 		float minimumHessian = 0.0f;
-		for(int k=0; k < maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
+		int removed;
+		inliers.resize(keypoints.size(), false);
+		if(ssc)
 		{
-			inliers[iter->second] = true;
-			minimumHessian = iter->first;
+			ULOGGER_DEBUG("too much words (%d), removing words with SSC", keypoints.size());
+			static constexpr float tolerance = 0.1;
+			auto ResultVec = util2d::SSC(keypoints, maxKeypoints, tolerance, imageSize.width, imageSize.height);
+			removed = keypoints.size()-ResultVec.size();
+			for(unsigned int k=0; k<ResultVec.size(); ++k)
+			{
+				inliers[ResultVec[k]] = true;
+			}
+		}
+		else
+		{
+			ULOGGER_DEBUG("too much words (%d), removing words with the hessian threshold", keypoints.size());
+			// Remove words under the new hessian threshold
+
+			// Sort words by hessian
+			std::multimap<float, int> hessianMap; // <hessian,id>
+			for(unsigned int i = 0; i<keypoints.size(); ++i)
+			{
+				//Keep track of the data, to be easier to manage the data in the next step
+				hessianMap.insert(std::pair<float, int>(fabs(keypoints[i].response), i));
+			}
+
+			// Keep keypoints with highest response
+			removed = (int)hessianMap.size()-maxKeypoints;
+			std::multimap<float, int>::reverse_iterator iter = hessianMap.rbegin();
+			for(int k=0; k<maxKeypoints && iter!=hessianMap.rend(); ++k, ++iter)
+			{
+				inliers[iter->second] = true;
+				minimumHessian = iter->first;
+			}
 		}
 		ULOGGER_DEBUG("%d keypoints removed, (kept %d), minimum response=%f", removed, maxKeypoints, minimumHessian);
 		ULOGGER_DEBUG("filter keypoints time = %f s", timer.ticks());
@@ -378,7 +434,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	}
 }

-void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols)
+void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std::vector<bool> & inliers, int maxKeypoints, const cv::Size & imageSize, int gridRows, int gridCols, bool ssc)
 {
 	if(maxKeypoints <= 0 || (int)keypoints.size() <= maxKeypoints)
 	{
@@ -406,7 +462,7 @@ void Feature2D::limitKeypoints(const std::vector<cv::KeyPoint> & keypoints, std:
 	for(size_t i=0; i<keypointsPerCell.size(); ++i)
 	{
 		std::vector<bool> inliersCell;
-		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell);
+		limitKeypoints(keypointsPerCell[i], inliersCell, maxKeypointsPerCell, cv::Size(colSize, rowSize), ssc);
 		for(size_t j=0; j<inliersCell.size(); ++j)
 		{
 			if(inliersCell[j])
@@ -432,6 +488,7 @@ cv::Rect Feature2D::computeRoi(const cv::Mat & image, const std::vector<float> &
 /////////////////////
 Feature2D::Feature2D(const ParametersMap & parameters) :
 		maxFeatures_(Parameters::defaultKpMaxFeatures()),
+		SSC_(Parameters::defaultKpSSC()),
 		_maxDepth(Parameters::defaultKpMaxDepth()),
 		_minDepth(Parameters::defaultKpMinDepth()),
 		_roiRatios(std::vector<float>(4, 0.0f)),
@@ -453,6 +510,7 @@ void Feature2D::parseParameters(const ParametersMap & parameters)
 	uInsert(parameters_, parameters);

 	Parameters::parse(parameters, Parameters::kKpMaxFeatures(), maxFeatures_);
+	Parameters::parse(parameters, Parameters::kKpSSC(), SSC_);
 	Parameters::parse(parameters, Parameters::kKpMaxDepth(), _maxDepth);
 	Parameters::parse(parameters, Parameters::kKpMinDepth(), _minDepth);
 	Parameters::parse(parameters, Parameters::kKpSubPixWinSize(), _subPixWinSize);
@@ -736,7 +794,7 @@ std::vector<cv::KeyPoint> Feature2D::generateKeypoints(const cv::Mat & image, co
 			subKeypoints = this->generateKeypointsImpl(image, roi, mask);
 			if (this->getType() != Feature2D::Type::kFeaturePyDetector)
 			{
-				limitKeypoints(subKeypoints, maxFeatures);
+				limitKeypoints(subKeypoints, maxFeatures, roi.size(), this->getSSC());
 			}
 			if(roi.x || roi.y)
 			{
@@ -2142,7 +2200,7 @@ std::vector<cv::KeyPoint> ORBOctree::generateKeypointsImpl(const cv::Mat & image

 	if((int)keypoints.size() > this->getMaxFeatures())
 	{
-		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures());
+		limitKeypoints(keypoints, descriptors_, this->getMaxFeatures(), roi.size(), this->getSSC());
 	}
 #else
 	UWARN("RTAB-Map is not built with ORB OcTree option enabled so ORB OcTree feature cannot be used!");
@@ -0,0 +1,76 @@
+/*
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+
+#include "rtabmap/core/GlobalDescriptorExtractor.h"
+
+#ifdef RTABMAP_PYTHON
+#include "python/PyDescriptor.h"
+#endif
+
+namespace rtabmap {
+
+GlobalDescriptorExtractor::GlobalDescriptorExtractor(const ParametersMap & parameters)
+{
+}
+GlobalDescriptorExtractor::~GlobalDescriptorExtractor()
+{
+}
+GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(const ParametersMap & parameters)
+{
+	int type = Parameters::defaultMemGlobalDescriptorStrategy();
+	Parameters::parse(parameters, Parameters::kMemGlobalDescriptorStrategy(), type);
+	return create((GlobalDescriptorExtractor::Type)type, parameters);
+}
+GlobalDescriptorExtractor * GlobalDescriptorExtractor::create(GlobalDescriptorExtractor::Type type, const ParametersMap & parameters)
+{
+	UDEBUG("Creating global descriptor of type %d", (int)type);
+#ifndef RTABMAP_PYTHON
+	if(type == GlobalDescriptorExtractor::kPyDescriptor)
+	{
+		UWARN("PyDescriptor cannot be used as rtabmap is not built with Python3 support.");
+		type = GlobalDescriptorExtractor::kUndef;
+	}
+#endif
+
+	GlobalDescriptorExtractor * GlobalDescriptorExtractor = 0;
+	switch(type)
+	{
+#ifdef RTABMAP_PYTHON
+	case GlobalDescriptorExtractor::kPyDescriptor:
+		GlobalDescriptorExtractor = new PyDescriptor(parameters);
+		break;
+#endif
+	default:
+		type = GlobalDescriptorExtractor::kUndef;
+		break;
+	}
+	return GlobalDescriptorExtractor;
+}
+
+}
+
@@ -121,7 +121,7 @@ bool GlobalMap::update(const std::map<int, Transform> & poses)
 		}
 		else
 		{
-			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", jter->first);
+			UDEBUG("Updated pose for node %d is not found, some points may not be copied. Use negative ids to just update cell values without adding new ones.", iter->first);
 		}
 	}

@@ -65,6 +65,9 @@ std::string LaserScan::formatName(const Format & format)
 		case kXYZRGBNormal:
 			name = "XYZRGBNormal";
 			break;
+		case kXYZIT:
+			name = "XYZIT";
+			break;
 		default:
 			name = "Unknown";
 			break;
@@ -88,6 +91,7 @@ int LaserScan::channels(const Format & format)
 			channels = 4;
 			break;
 		case kXYNormal:
+		case kXYZIT:
 			channels = 5;
 			break;
 		case kXYZNormal:
@@ -119,7 +123,11 @@ bool LaserScan::isScanHasRGB(const Format & format)
 }
 bool LaserScan::isScanHasIntensity(const Format & format)
 {
-	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal;
+	return format==kXYZI || format==kXYZINormal || format == kXYI || format == kXYINormal || format==kXYZIT;
+}
+bool LaserScan::isScanHasTime(const Format & format)
+{
+	return format==kXYZIT;
 }

 LaserScan LaserScan::backwardCompatibility(
@@ -213,7 +221,14 @@ LaserScan::LaserScan(
 		const LaserScan & scan,
 		int maxPoints,
 		float maxRange,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	UASSERT(scan.empty() || scan.format() != kUnknown);
 	init(scan.data(), scan.format(), 0, maxRange, 0, 0, 0, maxPoints, localTransform);
@@ -224,7 +239,14 @@ LaserScan::LaserScan(
 		int maxPoints,
 		float maxRange,
 		Format format,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(scan.data(), format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
 }
@@ -234,7 +256,14 @@ LaserScan::LaserScan(
 		int maxPoints,
 		float maxRange,
 		Format format,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(data, format, 0, maxRange, 0, 0, 0, maxPoints, localTransform);
 }
@@ -246,7 +275,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	UASSERT(scan.empty() || scan.format() != kUnknown);
 	init(scan.data(), scan.format(), minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
@@ -260,7 +296,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(scan.data(), format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
 }
@@ -273,7 +316,14 @@ LaserScan::LaserScan(
 		float angleMin,
 		float angleMax,
 		float angleIncrement,
-		const Transform & localTransform)
+		const Transform & localTransform) :
+	format_(kUnknown),
+	maxPoints_(0),
+	rangeMin_(0),
+	rangeMax_(0),
+	angleMin_(0),
+	angleMax_(0),
+	angleIncrement_(0)
 {
 	init(data, format, minRange, maxRange, angleMin, angleMax, angleIncrement, 0, localTransform);
 }
@@ -289,8 +339,7 @@ void LaserScan::init(
 		int maxPoints,
 		const Transform & localTransform)
 {
-	UASSERT(data.empty() || data.rows == 1);
-	UASSERT(data.empty() || data.type() == CV_8UC1 || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
+	UASSERT(data.empty() || (data.type() == CV_8UC1 && data.rows == 1) || data.type() == CV_32FC2 || data.type() == CV_32FC3 || data.type() == CV_32FC(4) || data.type() == CV_32FC(5) || data.type() == CV_32FC(6)  || data.type() == CV_32FC(7));
 	UASSERT(!localTransform.isNull());

 	bool is2D = false;
@@ -307,6 +356,10 @@ void LaserScan::init(
 		// 3D scan
 		UASSERT(rangeMax>=rangeMin);
 		maxPoints_ = maxPoints;
+		if(maxPoints_ == 0 && data.rows>1)
+		{
+			maxPoints_ = data.rows * data.cols;
+		}
 	}

 	data_ = data;
@@ -320,18 +373,18 @@ void LaserScan::init(

 	if(!data.empty() && !isCompressed())
 	{
-		if(is2D && data_.cols > maxPoints_)
+		if(is2D && (int)data_.total() > maxPoints_)
 		{
-			UWARN("The number of points (%d) in the scan is over the maximum "
+			UWARN("The number of points (%ld) in the scan is over the maximum "
 				  "points (%d) defined by angle settings (min=%f max=%f inc=%f). "
 				  "The scan info may be wrong!",
-				  data_.cols, maxPoints_, angleMin_, angleMax_, angleIncrement_);
+				  data_.total(), maxPoints_, angleMin_, angleMax_, angleIncrement_);
 		}
-		else if(!is2D && maxPoints_>0 && data_.cols > maxPoints_)
+		else if(!is2D && maxPoints_>0 && (int)data_.total() > maxPoints_)
 		{
-			UDEBUG("The number of points (%d) in the scan is over the maximum "
+			UDEBUG("The number of points (%ld) in the scan is over the maximum "
 				  "points (%d) defined by max points setting.",
-				  data_.cols, maxPoints_);
+				  data_.total(), maxPoints_);
 		}

 		if(format == kUnknown)
@@ -350,7 +403,7 @@ void LaserScan::init(
 			UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
-			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
+			UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal || format == kXYZIT)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 			UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%s", LaserScan::formatName(format).c_str()).c_str());
 		}
@@ -366,11 +419,36 @@ LaserScan LaserScan::clone() const
 	return LaserScan(data_.clone(), maxPoints_, rangeMax_, format_, localTransform_.clone());
 }

+LaserScan LaserScan::densify() const
+{
+	if(!isOrganized())
+	{
+		return *this;
+	}
+	cv::Mat output(1, data_.total(), data_.type());
+	int oi = 0;
+	for(int i=0; i<data_.rows; ++i)
+	{
+		for(int j=0; j<data_.cols; ++j)
+		{
+			const float * ptr = data_.ptr<float>(i, j);
+			float * outputPtr = output.ptr<float>(0, oi);
+			if(! (std::isnan(ptr[0]) || std::isnan(ptr[1]) || (!is2d() && std::isnan(ptr[2]))))
+			{
+				memcpy(outputPtr, ptr, data_.elemSize());
+				++oi;
+			}
+		}
+	}
+	return LaserScan(cv::Mat(output, cv::Range::all(), cv::Range(0,oi)), maxPoints_, rangeMax_, format_, localTransform_.clone());
+}
+
 float & LaserScan::field(unsigned int pointIndex, unsigned int channelOffset)
 {
-	UASSERT(pointIndex < (unsigned int)data_.cols);
+	UASSERT(pointIndex < (unsigned int)data_.total());
 	UASSERT(channelOffset < (unsigned int)data_.channels());
-	return data_.ptr<float>(0, pointIndex)[channelOffset];
+	unsigned int row = pointIndex / data_.cols;
+	return data_.ptr<float>(row, pointIndex - row * data_.cols)[channelOffset];
 }

 LaserScan & LaserScan::operator+=(const LaserScan & scan)
@@ -381,7 +459,7 @@ LaserScan & LaserScan::operator+=(const LaserScan & scan)

 LaserScan LaserScan::operator+(const LaserScan & scan)
 {
-	UASSERT(this->empty() || scan.empty() || this->format() == scan.format());
+	UASSERT(this->empty() || scan.empty() || (this->format() == scan.format() && !this->isOrganized() && !scan.isOrganized()));
 	LaserScan dest;
 	if(!scan.empty())
 	{
@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/EpipolarGeometry.h>
 #include "rtabmap/core/VisualWord.h"
 #include "rtabmap/core/Features2d.h"
+#include "rtabmap/core/GlobalDescriptorExtractor.h"
 #include "rtabmap/core/RegistrationIcp.h"
 #include "rtabmap/core/Registration.h"
 #include "rtabmap/core/RegistrationVis.h"
@@ -110,6 +111,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_rotateImagesUpsideUp(Parameters::defaultMemRotateImagesUpsideUp()),
 	_createOccupancyGrid(Parameters::defaultRGBDCreateOccupancyGrid()),
 	_visMaxFeatures(Parameters::defaultVisMaxFeatures()),
+	_visSSC(Parameters::defaultVisSSC()),
 	_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
 	_rectifyOnlyFeatures(Parameters::defaultRtabmapRectifyOnlyFeatures()),
 	_covOffDiagonalIgnored(Parameters::defaultMemCovOffDiagIgnored()),
@@ -132,6 +134,7 @@ Memory::Memory(const ParametersMap & parameters) :
 	_feature2D = Feature2D::create(parameters);
 	_vwd = new VWDictionary(parameters);
 	_registrationPipeline = Registration::create(parameters);
+	_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters);
 	if(!_registrationPipeline->isImageRequired())
 	{
 		// make sure feature matching is used instead of optical flow to compute the guess
@@ -601,6 +604,7 @@ void Memory::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(params, Parameters::kMemRotateImagesUpsideUp(), _rotateImagesUpsideUp);
 	Parameters::parse(params, Parameters::kRGBDCreateOccupancyGrid(), _createOccupancyGrid);
 	Parameters::parse(params, Parameters::kVisMaxFeatures(), _visMaxFeatures);
+	Parameters::parse(params, Parameters::kVisSSC(), _visSSC);
 	Parameters::parse(params, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
 	Parameters::parse(params, Parameters::kRtabmapRectifyOnlyFeatures(), _rectifyOnlyFeatures);
 	Parameters::parse(params, Parameters::kMemCovOffDiagIgnored(), _covOffDiagonalIgnored);
@@ -761,6 +765,22 @@ void Memory::parseParameters(const ParametersMap & parameters)
 		_markerDetector->parseParameters(params);
 	}

+	int globalDescriptorStrategy = -1;
+	Parameters::parse(params, Parameters::kMemGlobalDescriptorStrategy(), globalDescriptorStrategy);
+	if(globalDescriptorStrategy != -1 &&
+			(_globalDescriptorExtractor==0 || (int)_globalDescriptorExtractor->getType() != globalDescriptorStrategy))
+	{
+		if(_globalDescriptorExtractor)
+		{
+			delete _globalDescriptorExtractor;
+		}
+		_globalDescriptorExtractor = GlobalDescriptorExtractor::create(parameters_);
+	}
+	else if(_globalDescriptorExtractor)
+	{
+		_globalDescriptorExtractor->parseParameters(params);
+	}
+
 	// do this after all params are parsed
 	// SLAM mode vs Localization mode
 	iter = params.find(Parameters::kMemIncrementalMemory());
@@ -1197,7 +1217,10 @@ void Memory::moveSignatureToWMFromSTM(int id, int * reducedTo)
 					}
 				}

-				this->moveToTrash(s, false);
+				// Setting true to make sure we save all visual
+				// words that could be referenced in a previously
+				// transferred node in LTM (#979)
+				this->moveToTrash(s, true);
 				s = 0;
 			}
 		}
@@ -2770,6 +2793,19 @@ void Memory::removeLink(int oldId, int newId)
 			UERROR("Signatures %d and %d don't have bidirectional link!", oldS->id(), newS->id());
 		}
 	}
+	else if(this->_getSignature(newId<0?oldId:newId))
+	{
+		int landmarkId = newId<0?newId:oldId;
+		Signature * s = this->_getSignature(newId<0?oldId:newId);
+		s->removeLandmark(newId<0?newId:oldId);
+		_linksChanged = true;
+		// Update landmark index
+		std::map<int, std::set<int> >::iterator nter = _landmarksIndex.find(landmarkId);
+		if(nter!=_landmarksIndex.end())
+		{
+			nter->second.erase(s->id());
+		}
+	}
 	else
 	{
 		if(!newS)
@@ -5097,9 +5133,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		UASSERT(keypoints3D.empty() || keypoints3D.size() == keypoints.size());

 		int maxFeatures = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visMaxFeatures:_feature2D->getMaxFeatures();
+		bool ssc = _rawDescriptorsKept&&!pose.isNull()&&_feature2D->getMaxFeatures()>0&&_feature2D->getMaxFeatures()<_visMaxFeatures?_visSSC:_feature2D->getSSC();
 		if((int)keypoints.size() > maxFeatures)
 		{
-			_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
+			if(data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures, data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(), ssc);
+			else
+				_feature2D->limitKeypoints(keypoints, keypoints3D, descriptors, maxFeatures);
 		}
 		t = timer.ticks();
 		if(stats) stats->addStatistic(Statistics::kTimingMemKeypoints_detection(), t*1000.0f);
@@ -5313,21 +5353,41 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 		{
 			UASSERT((int)keypoints.size() == descriptors.rows);
 			int inliersCount = 0;
-			if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
+			if((_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1) &&
+				(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
+					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1))
 			{
-				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(), decimatedData.imageRaw().size(), _feature2D->getGridRows(), _feature2D->getGridCols());
-				for(size_t i=0; i<inliers.size(); ++i)
-				{
-					if(inliers[i])
-					{
-						++inliersCount;
-					}
-				}
+				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+					decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+					decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+					data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+					_feature2D->getGridRows(), _feature2D->getGridCols(), _feature2D->getSSC());
 			}
 			else
 			{
-				Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
-				inliersCount = _feature2D->getMaxFeatures();
+				if(_feature2D->getGridRows() > 1 || _feature2D->getGridCols() > 1)
+				{
+					UWARN("Ignored %s and %s parameters as they cannot be used for multi-cameras setup or uncalibrated camera.",
+							Parameters::kKpGridCols().c_str(), Parameters::kKpGridRows().c_str());
+				}
+				if(decimatedData.cameraModels().size()==1 || decimatedData.stereoCameraModels().size()==1 ||
+					data.cameraModels().size()==1 || data.stereoCameraModels().size()==1)
+				{
+					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures(),
+						decimatedData.cameraModels().size()?decimatedData.cameraModels()[0].imageSize():
+						decimatedData.stereoCameraModels().size()?decimatedData.stereoCameraModels()[0].left().imageSize():
+						data.cameraModels().size()?data.cameraModels()[0].imageSize():data.stereoCameraModels()[0].left().imageSize(),
+						_feature2D->getSSC());
+				}
+				else
+				{
+					Feature2D::limitKeypoints(keypoints, inliers, _feature2D->getMaxFeatures());
+				}
+			}
+			for(size_t i=0; i<inliers.size(); ++i)
+			{
+				if(inliers[i])
+					++inliersCount;
 			}

 			descriptorsForQuantization = cv::Mat(inliersCount, descriptors.cols, descriptors.type());
@@ -5921,7 +5981,17 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
 	s->sensorData().setGroundTruth(data.groundTruth());
 	s->sensorData().setGPS(data.gps());
 	s->sensorData().setEnvSensors(data.envSensors());
-	s->sensorData().setGlobalDescriptors(data.globalDescriptors());
+
+	std::vector<GlobalDescriptor> globalDescriptors = data.globalDescriptors();
+	if(_globalDescriptorExtractor)
+	{
+		GlobalDescriptor gdescriptor = _globalDescriptorExtractor->extract(inputData);
+		if(!gdescriptor.data().empty())
+		{
+			globalDescriptors.push_back(gdescriptor);
+		}
+	}
+	s->sensorData().setGlobalDescriptors(globalDescriptors);

 	t = timer.ticks();
 	if(stats) stats->addStatistic(Statistics::kTimingMemCompressing_data(), t*1000.0f);
@@ -140,6 +140,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 		_alignWithGround(Parameters::defaultOdomAlignWithGround()),
 		_publishRAMUsage(Parameters::defaultRtabmapPublishRAMUsage()),
 		_imagesAlreadyRectified(Parameters::defaultRtabmapImagesAlreadyRectified()),
+		_deskewing(Parameters::defaultOdomDeskewing()),
 		_pose(Transform::getIdentity()),
 		_resetCurrentCount(0),
 		previousStamp_(0),
@@ -169,6 +170,7 @@ Odometry::Odometry(const rtabmap::ParametersMap & parameters) :
 	Parameters::parse(parameters, Parameters::kOdomAlignWithGround(), _alignWithGround);
 	Parameters::parse(parameters, Parameters::kRtabmapPublishRAMUsage(), _publishRAMUsage);
 	Parameters::parse(parameters, Parameters::kRtabmapImagesAlreadyRectified(), _imagesAlreadyRectified);
+	Parameters::parse(parameters, Parameters::kOdomDeskewing(), _deskewing);

 	if(_imageDecimation == 0)
 	{
@@ -620,6 +622,75 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 	}

 	UTimer time;
+
+	// Deskewing lidar
+	if( _deskewing &&
+		!data.laserScanRaw().empty() &&
+		data.laserScanRaw().hasTime() &&
+		dt > 0 &&
+		!guess.isNull())
+	{
+		UDEBUG("Deskewing begin");
+		// Recompute velocity
+		float vx,vy,vz, vroll,vpitch,vyaw;
+		guess.getTranslationAndEulerAngles(vx,vy,vz, vroll,vpitch,vyaw);
+
+		// transform to velocity
+		vx /= dt;
+		vy /= dt;
+		vz /= dt;
+		vroll /= dt;
+		vpitch /= dt;
+		vyaw /= dt;
+
+		if(!imus_.empty())
+		{
+			float scanTime =
+				data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
+				data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+
+			// replace orientation velocity based on IMU (if available)
+			Transform imuFirstScan = Transform::getTransform(imus_,
+					data.stamp() +
+					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()]);
+			Transform imuLastScan = Transform::getTransform(imus_,
+					data.stamp() +
+					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()]);
+			if(!imuFirstScan.isNull() && !imuLastScan.isNull())
+			{
+				Transform orientation = imuFirstScan.inverse() * imuLastScan;
+				orientation.getEulerAngles(vroll, vpitch, vyaw);
+				if(_force3DoF)
+				{
+					vroll=0;
+					vpitch=0;
+					vyaw /= scanTime;
+				}
+				else
+				{
+					vroll /= scanTime;
+					vpitch /= scanTime;
+					vyaw /= scanTime;
+				}
+			}
+		}
+
+		Transform velocity(vx,vy,vz,vroll,vpitch,vyaw);
+		LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
+		if(!scanDeskewed.isEmpty())
+		{
+			data.setLaserScan(scanDeskewed);
+		}
+		info->timeDeskewing = time.ticks();
+		UDEBUG("Deskewing end");
+	}
+	if(data.laserScanRaw().isOrganized())
+	{
+		// Laser scans should be dense passing this point
+		data.setLaserScan(data.laserScanRaw().densify());
+	}
+
+
 	Transform t;
 	if(_imageDecimation > 1 && !data.imageRaw().empty())
 	{
@@ -915,6 +986,7 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
 		{
 			UWARN("Odometry automatically reset to latest pose!");
 			this->reset(_pose);
+			_resetCurrentCount = _resetCountdown;
 			if(info)
 			{
 				*info = OdometryInfo();
@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/OdometryThread.h"
 #include "rtabmap/core/Odometry.h"
 #include "rtabmap/core/odometry/OdometryMono.h"
 #include "rtabmap/core/OdometryInfo.h"
-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/utilite/ULogger.h"

@@ -58,12 +58,12 @@ bool OdometryThread::handleEvent(UEvent * event)
 {
 	if(this->isRunning())
 	{
-		if(event->getClassName().compare("CameraEvent") == 0)
+		if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			CameraEvent * cameraEvent = (CameraEvent*)event;
-			if(cameraEvent->getCode() == CameraEvent::kCodeData)
+			SensorEvent * sensorEvent = (SensorEvent*)event;
+			if(sensorEvent->getCode() == SensorEvent::kCodeData)
 			{
-				this->addData(cameraEvent->data());
+				this->addData(sensorEvent->data());
 			}
 		}
 		else if(event->getClassName().compare("IMUEvent") == 0)
@@ -1172,11 +1172,8 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
 	return out;
 }

-
-void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
+void readINIImpl(const CSimpleIniA & ini, const std::string & configFilePath, ParametersMap & parameters, bool modifiedOnly)
 {
-	CSimpleIniA ini;
-	ini.LoadFile(configFile.c_str());
 	const CSimpleIniA::TKeyVal * keyValMap = ini.GetSection("Core");
 	if(keyValMap)
 	{
@@ -1191,12 +1188,12 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 				{
 					if(!RTABMAP_VERSION_COMPARE(std::atoi(version[0].c_str()), std::atoi(version[1].c_str()), std::atoi(version[2].c_str())))
 					{
-						if(configFile.find(".rtabmap") != std::string::npos)
+						if(configFilePath.find(".rtabmap") != std::string::npos)
 						{
 							UWARN("Version in the config file \"%s\" is more recent (\"%s\") than "
 								   "current RTAB-Map version used (\"%s\"). The config file will be upgraded "
 								   "to new version.",
-								   configFile.c_str(),
+								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);
 						}
@@ -1205,7 +1202,7 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 							UERROR("Version in the config file \"%s\" is more recent (\"%s\") than "
 								   "current RTAB-Map version used (\"%s\"). New parameters (if there are some) will "
 								   "be ignored.",
-								   configFile.c_str(),
+								   configFilePath.c_str(),
 								   (*iter).second,
 								   RTABMAP_VERSION);
 						}
@@ -1255,11 +1252,26 @@ void Parameters::readINI(const std::string & configFile, ParametersMap & paramet
 	else
 	{
 		ULOGGER_WARN("Section \"Core\" in %s doesn't exist... "
-				    "Ignore this warning if the ini file does not exist yet. "
-				    "The ini file will be automatically created when rtabmap will close.", configFile.c_str());
+					"Ignore this warning if the ini file does not exist yet. "
+					"The ini file will be automatically created when rtabmap will close.", configFilePath.c_str());
 	}
 }

+
+void Parameters::readINI(const std::string & configFile, ParametersMap & parameters, bool modifiedOnly)
+{
+	CSimpleIniA ini;
+	ini.LoadFile(configFile.c_str());
+	readINIImpl(ini, configFile, parameters, modifiedOnly);
+}
+
+void Parameters::readINIStr(const std::string & configContent, ParametersMap & parameters, bool modifiedOnly)
+{
+	CSimpleIniA ini;
+	ini.LoadData(configContent);
+	readINIImpl(ini, "", parameters, modifiedOnly);
+}
+
 void Parameters::writeINI(const std::string & configFile, const ParametersMap & parameters)
 {
 	CSimpleIniA ini;
@@ -160,7 +160,7 @@ bool databaseRecovery(
 		DBReader dbReader(databasePath, 0, odometryIgnored);
 		dbReader.init();

-		CameraInfo info;
+		SensorCaptureInfo info;
 		SensorData data = dbReader.takeImage(&info);
 		int processed = 0;
 		if (progressState)
@@ -69,6 +69,7 @@ RegistrationIcp::RegistrationIcp(const ParametersMap & parameters, Registration
 	_epsilon(Parameters::defaultIcpEpsilon()),
 	_correspondenceRatio(Parameters::defaultIcpCorrespondenceRatio()),
 	_force4DoF(Parameters::defaultIcpForce4DoF()),
+	_filtersEnabled(Parameters::defaultIcpFiltersEnabled()),
 	_pointToPlane(Parameters::defaultIcpPointToPlane()),
 	_pointToPlaneK(Parameters::defaultIcpPointToPlaneK()),
 	_pointToPlaneRadius(Parameters::defaultIcpPointToPlaneRadius()),
@@ -115,6 +116,7 @@ void RegistrationIcp::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kIcpEpsilon(), _epsilon);
 	Parameters::parse(parameters, Parameters::kIcpCorrespondenceRatio(), _correspondenceRatio);
 	Parameters::parse(parameters, Parameters::kIcpForce4DoF(), _force4DoF);
+	Parameters::parse(parameters, Parameters::kIcpFiltersEnabled(), _filtersEnabled);
 	Parameters::parse(parameters, Parameters::kIcpOutlierRatio(), _outlierRatio);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlane(), _pointToPlane);
 	Parameters::parse(parameters, Parameters::kIcpPointToPlaneK(), _pointToPlaneK);
@@ -337,6 +339,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	UDEBUG("Downsampling step=%d", _downsamplingStep);
 	UDEBUG("Force 3DoF=%s", this->force3DoF()?"true":"false");
 	UDEBUG("Force 4DoF=%s", _force4DoF?"true":"false");
+	UDEBUG("Enabled filters: from=%s to=%s", _filtersEnabled&1?"true":"false", _filtersEnabled&2?"true":"false");
 	UDEBUG("Min Complexity=%f", _pointToPlaneMinComplexity);
 	UDEBUG("libpointmatcher (knn=%d, outlier ratio=%f)", _libpointmatcherKnn, _outlierRatio);
 	UDEBUG("Strategy=%d", _strategy);
@@ -360,7 +363,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 	int maxLaserScansFrom = dataFrom.laserScanRaw().maxPoints()>0?dataFrom.laserScanRaw().maxPoints():dataFrom.laserScanRaw().size();
 	int maxLaserScansTo = dataTo.laserScanRaw().maxPoints()>0?dataTo.laserScanRaw().maxPoints():dataTo.laserScanRaw().size();

-	if(!dataFrom.laserScanRaw().empty())
+	if(!dataFrom.laserScanRaw().empty() && (_filtersEnabled & 1))
 	{
 		int pointsBeforeFiltering = dataFrom.laserScanRaw().size();
 		LaserScan fromScan = util3d::commonFiltering(dataFrom.laserScanRaw(),
@@ -401,7 +404,7 @@ Transform RegistrationIcp::computeTransformationImpl(
 		float ratio = float(dataFrom.laserScanRaw().size()) / float(pointsBeforeFiltering);
 		maxLaserScansFrom = int(float(maxLaserScansFrom) * ratio);
 	}
-	if(!dataTo.laserScanRaw().empty())
+	if(!dataTo.laserScanRaw().empty() && (_filtersEnabled & 2))
 	{
 		int pointsBeforeFiltering = dataTo.laserScanRaw().size();
 		LaserScan toScan = util3d::commonFiltering(dataTo.laserScanRaw(),
@@ -102,6 +102,7 @@ RegistrationVis::RegistrationVis(const ParametersMap & parameters, Registration
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpNndrRatio(), _featureParameters.at(Parameters::kVisCorNNDR())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpDetectorStrategy(), _featureParameters.at(Parameters::kVisFeatureType())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), _featureParameters.at(Parameters::kVisMaxFeatures())));
+	uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), _featureParameters.at(Parameters::kVisSSC())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), _featureParameters.at(Parameters::kVisMaxDepth())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpMinDepth(), _featureParameters.at(Parameters::kVisMinDepth())));
 	uInsert(_featureParameters, ParametersPair(Parameters::kKpRoiRatios(), _featureParameters.at(Parameters::kVisRoiRatios())));
@@ -234,6 +235,10 @@ void RegistrationVis::parseParameters(const ParametersMap & parameters)
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxFeatures(), parameters.at(Parameters::kVisMaxFeatures())));
 	}
+	if(uContains(parameters, Parameters::kVisSSC()))
+	{
+		uInsert(_featureParameters, ParametersPair(Parameters::kKpSSC(), parameters.at(Parameters::kVisSSC())));
+	}
 	if(uContains(parameters, Parameters::kVisMaxDepth()))
 	{
 		uInsert(_featureParameters, ParametersPair(Parameters::kKpMaxDepth(), parameters.at(Parameters::kVisMaxDepth())));
@@ -103,6 +103,7 @@ Rtabmap::Rtabmap() :
 	_maxMemoryAllowed(Parameters::defaultRtabmapMemoryThr()), // 0=inf
 	_loopThr(Parameters::defaultRtabmapLoopThr()),
 	_loopRatio(Parameters::defaultRtabmapLoopRatio()),
+	_virtualPlaceLikelihoodRatio(Parameters::defaultRtabmapVirtualPlaceLikelihoodRatio()),
 	_maxLoopClosureDistance(Parameters::defaultRGBDMaxLoopClosureDistance()),
 	_verifyLoopClosureHypothesis(Parameters::defaultVhEpEnabled()),
 	_maxRetrieved(Parameters::defaultRtabmapMaxRetrieved()),
@@ -143,6 +144,7 @@ Rtabmap::Rtabmap() :
 	_pathStuckIterations(Parameters::defaultRGBDPlanStuckIterations()),
 	_pathLinearVelocity(Parameters::defaultRGBDPlanLinearVelocity()),
 	_pathAngularVelocity(Parameters::defaultRGBDPlanAngularVelocity()),
+	_forceOdom3doF(Parameters::defaultRGBDForceOdom3DoF()),
 	_restartAtOrigin(Parameters::defaultRGBDStartAtOrigin()),
 	_loopCovLimited(Parameters::defaultRGBDLoopCovLimited()),
 	_loopGPS(Parameters::defaultRtabmapLoopGPS()),
@@ -566,6 +568,8 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRtabmapMemoryThr(), _maxMemoryAllowed);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopThr(), _loopThr);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopRatio(), _loopRatio);
+	Parameters::parse(parameters, Parameters::kRtabmapVirtualPlaceLikelihoodRatio(), _virtualPlaceLikelihoodRatio);
+
 	Parameters::parse(parameters, Parameters::kRGBDMaxLoopClosureDistance(), _maxLoopClosureDistance);
 	Parameters::parse(parameters, Parameters::kVhEpEnabled(), _verifyLoopClosureHypothesis);
 	Parameters::parse(parameters, Parameters::kRtabmapMaxRetrieved(), _maxRetrieved);
@@ -616,6 +620,7 @@ void Rtabmap::parseParameters(const ParametersMap & parameters)
 	Parameters::parse(parameters, Parameters::kRGBDPlanStuckIterations(), _pathStuckIterations);
 	Parameters::parse(parameters, Parameters::kRGBDPlanLinearVelocity(), _pathLinearVelocity);
 	Parameters::parse(parameters, Parameters::kRGBDPlanAngularVelocity(), _pathAngularVelocity);
+	Parameters::parse(parameters, Parameters::kRGBDForceOdom3DoF(), _forceOdom3doF);
 	Parameters::parse(parameters, Parameters::kRGBDStartAtOrigin(), _restartAtOrigin);
 	Parameters::parse(parameters, Parameters::kRGBDLoopCovLimited(), _loopCovLimited);
 	Parameters::parse(parameters, Parameters::kRtabmapLoopGPS(), _loopGPS);
@@ -1094,6 +1099,13 @@ void Rtabmap::resetMemory()
 	{
 		UERROR("RTAB-Map is not initialized. No memory to reset...");
 	}
+
+	if(_graphOptimizer)
+	{
+		delete _graphOptimizer;
+		_graphOptimizer = Optimizer::create(_parameters);
+	}
+
 	this->setupLogFiles(true);
 }

@@ -1177,6 +1189,7 @@ bool Rtabmap::process(
 	double timeMemoryUpdate = 0;
 	double timeNeighborLinkRefining = 0;
 	double timeProximityByTimeDetection = 0;
+	double timeProximityBySpaceSearch = 0;
 	double timeProximityBySpaceVisualDetection = 0;
 	double timeProximityBySpaceDetection = 0;
 	double timeCleaningNeighbors = 0;
@@ -1244,6 +1257,12 @@ bool Rtabmap::process(
 	{
 		if(!odomPose.isNull())
 		{
+			// If we are doing 2D mapping, make sure the pose is 3DoF so that landmark logic works.
+			if(_forceOdom3doF && _graphOptimizer->isSlam2d() && !odomPose.is3DoF())
+			{
+				odomPose = odomPose.to3DoF();
+			}
+
 			// this will make sure that all inverse operations will work!
 			if(!odomPose.isInvertible())
 			{
@@ -1338,6 +1357,13 @@ bool Rtabmap::process(
 				{
 					iter->second = mapCorrectionInv * iter->second;
 				}
+
+				std::map<int, Transform> nodesOnly(_optimizedPoses.lower_bound(1), _optimizedPoses.end());
+				_lastLocalizationNodeId = graph::findNearestNode(nodesOnly, _lastLocalizationPose);
+				UWARN("Transformed map accordingly to last localization pose saved in database (%s=true)! nearest id = %d of last pose = %s",
+						Parameters::kRGBDOptimizeFromGraphEnd().c_str(),
+						_lastLocalizationNodeId,
+						_lastLocalizationPose.prettyPrint().c_str());
 			}
 		}

@@ -2509,38 +2535,38 @@ bool Rtabmap::process(
 			// insert them first to make sure they are loaded.
 			reactivatedIds.insert(reactivatedIds.begin(), retrievalLocalIds.begin(), retrievalLocalIds.end());
 		}
-
-		//============================================================
-		// RETRIEVAL 3/3 : Load signatures from the database
-		//============================================================
-		if(reactivatedIds.size())
-		{
-			// Not important if the loop closure hypothesis don't have all its neighbors loaded,
-			// only a loop closure link is added...
-			signaturesRetrieved = _memory->reactivateSignatures(
-					reactivatedIds,
-					_maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved
-					timeRetrievalDbAccess);
-
-			ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
-
-			timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
-			UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
-
-			// Immunize just retrieved signatures
-			immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
-
-			if(!signaturesRetrieved.empty() && !_globalScanMap.empty())
-			{
-				UWARN("Some signatures have been retrieved from memory management, clearing global scan map...");
-				_globalScanMap.clear();
-				_globalScanMapPoses.clear();
-			}
-		}
-		timeReactivations = timer.ticks();
-		ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
 	}

+	//============================================================
+	// RETRIEVAL 3/3 : Load signatures from the database
+	//============================================================
+	if(reactivatedIds.size())
+	{
+		// Not important if the loop closure hypothesis don't have all its neighbors loaded,
+		// only a loop closure link is added...
+		signaturesRetrieved = _memory->reactivateSignatures(
+				reactivatedIds,
+				_maxRetrieved+(unsigned int)retrievalLocalIds.size(), // add path retrieved
+				timeRetrievalDbAccess);
+
+		ULOGGER_INFO("retrieval of %d (db time = %fs)", (int)signaturesRetrieved.size(), timeRetrievalDbAccess);
+
+		timeRetrievalDbAccess += timeGetNeighborsTimeDb + timeGetNeighborsSpaceDb;
+		UINFO("total timeRetrievalDbAccess=%fs", timeRetrievalDbAccess);
+
+		// Immunize just retrieved signatures
+		immunizedLocations.insert(signaturesRetrieved.begin(), signaturesRetrieved.end());
+
+		if(!signaturesRetrieved.empty() && !_globalScanMap.empty())
+		{
+			UWARN("Some signatures have been retrieved from memory management, clearing global scan map...");
+			_globalScanMap.clear();
+			_globalScanMapPoses.clear();
+		}
+	}
+	timeReactivations = timer.ticks();
+	ULOGGER_INFO("timeReactivations=%fs", timeReactivations);
+
 	//============================================================
 	// Proximity detections
 	//============================================================
@@ -2598,22 +2624,39 @@ bool Rtabmap::process(
 				// 1) compare visually with nearest locations
 				//
 				UDEBUG("Proximity detection (local loop closure in SPACE using matching images, local radius=%fm)", _localRadius);
-				std::map<int, float> nearestIds;
-				if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
-				{
-					nearestIds = _memory->getNeighborsIdRadius(signature->id(), _localRadius, _optimizedPoses, _proximityMaxGraphDepth);
-				}
-				else
-				{
-					nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
-				}
+				std::map<int, float> nearestIds = graph::findNearestNodes(signature->id(), _optimizedPoses, _localRadius);
 				UDEBUG("nearestIds=%d/%d", (int)nearestIds.size(), (int)_optimizedPoses.size());
 				std::map<int, Transform> nearestPoses;
+				std::multimap<int, int> links;
+				if(_memory->isIncremental() && _proximityMaxGraphDepth>0)
+				{
+					// get bidirectional links
+					for(std::multimap<int, Link>::iterator iter=_constraints.begin(); iter!=_constraints.end(); ++iter)
+					{
+						if(uContains(_optimizedPoses, iter->second.from()) && uContains(_optimizedPoses, iter->second.to()))
+						{
+							links.insert(std::make_pair(iter->second.from(), iter->second.to()));
+							links.insert(std::make_pair(iter->second.to(), iter->second.from())); // <->
+						}
+					}
+				}
 				for(std::map<int, float>::iterator iter=nearestIds.lower_bound(1); iter!=nearestIds.end(); ++iter)
 				{
 					if(_memory->getStMem().find(iter->first) == _memory->getStMem().end())
 					{
-						nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+						if(_memory->isIncremental() && _proximityMaxGraphDepth > 0)
+						{
+							std::list<std::pair<int, Transform> > path = graph::computePath(_optimizedPoses, links, signature->id(), iter->first);
+							UDEBUG("Graph depth to %d = %ld", iter->first, path.size());
+							if(!path.empty() && (int)path.size() <= _proximityMaxGraphDepth)
+							{
+								nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+							}
+						}
+						else
+						{
+							nearestPoses.insert(std::make_pair(iter->first, _optimizedPoses.at(iter->first)));
+						}
 					}
 				}
 				UDEBUG("nearestPoses=%d", (int)nearestPoses.size());
@@ -2645,6 +2688,9 @@ bool Rtabmap::process(
 				}
 				UDEBUG("nearestPaths=%d proximityMaxPaths=%d", (int)nearestPaths.size(), _proximityMaxPaths);

+				timeProximityBySpaceSearch = timer.ticks();
+				ULOGGER_INFO("timeProximityBySpaceSearch=%fs", timeProximityBySpaceSearch);
+
 				float proximityFilteringRadius = _proximityFilteringRadius;
 				if(_maxLoopClosureDistance>0.0f && (proximityFilteringRadius <= 0.0f || _maxLoopClosureDistance<proximityFilteringRadius))
 				{
@@ -3054,6 +3100,7 @@ bool Rtabmap::process(
 					UINFO("Landmark %d observed again! Seen the first time by node %d.", -iter->first, *_memory->getLandmarksIndex().find(iter->first)->second.begin());
 					landmarksDetected.insert(std::make_pair(iter->first, _memory->getLandmarksIndex().find(iter->first)->second));
 					rejectedGlobalLoopClosure = false; // If it was true, it will be set back to false if landmarks are rejected on graph optimization
+					loopClosureLinksAdded.push_back(std::make_pair(signature->id(), iter->first));
 				}
 			}
 		}
@@ -4072,6 +4119,7 @@ bool Rtabmap::process(
 			statistics_.addStatistic(Statistics::kTimingMemory_update(), timeMemoryUpdate*1000);
 			statistics_.addStatistic(Statistics::kTimingNeighbor_link_refining(), timeNeighborLinkRefining*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_time(), timeProximityByTimeDetection*1000);
+			statistics_.addStatistic(Statistics::kTimingProximity_by_space_search(), timeProximityBySpaceSearch*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_space_visual(), timeProximityBySpaceVisualDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingProximity_by_space(), timeProximityBySpaceDetection*1000);
 			statistics_.addStatistic(Statistics::kTimingReactivation(), timeReactivations*1000);
@@ -4306,6 +4354,7 @@ bool Rtabmap::process(
 				}
 				else
 				{
+					UDEBUG("Clearing _lastLocalizationNodeId(%d)", _lastLocalizationNodeId);
 					_lastLocalizationNodeId = 0;
 				}
 			}
@@ -4693,6 +4742,16 @@ void Rtabmap::setTimeThreshold(float maxTimeAllowed)
 		ULOGGER_WARN("Time threshold set to %fms, it is not in seconds!", _maxTimeAllowed);
 	}
 }
+void Rtabmap::setMemoryThreshold(int maxMemoryAllowed)
+{
+	//must be positive, 0 mean inf memory allowed (no memory limit)
+	_maxMemoryAllowed = maxMemoryAllowed;
+	if(_maxMemoryAllowed < 0)
+	{
+		ULOGGER_WARN("maxMemoryAllowed < 0, then setting it to 0 (inf).");
+		_maxMemoryAllowed = 0;
+	}
+}

 void Rtabmap::setWorkingDirectory(std::string path)
 {
@@ -5225,34 +5284,34 @@ void Rtabmap::adjustLikelihood(std::map<int, float> & likelihood) const
 	for(std::map<int, float>::iterator iter=++likelihood.begin(); iter!= likelihood.end(); ++iter)
 	{
 		float value = iter->second;
-		if(value > mean+stdDev && mean)
+		iter->second = 1.0f;
+		if(value > mean+stdDev)
 		{
-			iter->second = (value-(stdDev-epsilon))/mean;
-			if(value > max)
+			if(_virtualPlaceLikelihoodRatio==0 && mean)
 			{
-				max = value;
-				maxId = iter->first;
+				iter->second = (value-(stdDev-epsilon))/mean;
+			}
+			else if(_virtualPlaceLikelihoodRatio!=0 && stdDev)
+			{
+				iter->second = (value-mean)/stdDev;
 			}
 		}
-		else if(value == 1.0f && stdDev == 0)
+
+		if(value > max)
 		{
-			iter->second = 1.0f;
-			if(value > max)
-			{
-				max = value;
-				maxId = iter->first;
-			}
-		}
-		else
-		{
-			iter->second = 1.0f;
+			max = value;
+			maxId = iter->first;
 		}
 	}

-	if(stdDev > epsilon && max)
+	if(_virtualPlaceLikelihoodRatio==0 && stdDev > epsilon && max)
 	{
 		likelihood.begin()->second = mean/stdDev + 1.0f;
 	}
+	else if(_virtualPlaceLikelihoodRatio!=0 && max > mean)
+	{
+		likelihood.begin()->second = stdDev/(max-mean) + 1.0f;
+	}
 	else
 	{
 		likelihood.begin()->second = 2.0f; //2 * std dev
@@ -25,11 +25,11 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

+#include <rtabmap/core/SensorEvent.h>
 #include "rtabmap/core/Rtabmap.h"
 #include "rtabmap/core/RtabmapThread.h"
 #include "rtabmap/core/RtabmapEvent.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraEvent.h"
 #include "rtabmap/core/ParamEvent.h"
 #include "rtabmap/core/OdometryEvent.h"
 #include "rtabmap/core/UserDataEvent.h"
@@ -371,11 +371,11 @@ bool RtabmapThread::handleEvent(UEvent* event)
 			// IMU events are published at high frequency, early exit
 			return false;
 		}
-		else if(event->getClassName().compare("CameraEvent") == 0)
+		else if(event->getClassName().compare("SensorEvent") == 0)
 		{
-			UDEBUG("CameraEvent");
-			CameraEvent * e = (CameraEvent*)event;
-			if(e->getCode() == CameraEvent::kCodeData)
+			UDEBUG("SensorEvent");
+			SensorEvent * e = (SensorEvent*)event;
+			if(e->getCode() == SensorEvent::kCodeData)
 			{
 				if (_rtabmap->isRGBDMode())
 				{
@@ -0,0 +1,114 @@
+/*
+Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+    * Redistributions of source code must retain the above copyright
+      notice, this list of conditions and the following disclaimer.
+    * Redistributions in binary form must reproduce the above copyright
+      notice, this list of conditions and the following disclaimer in the
+      documentation and/or other materials provided with the distribution.
+    * Neither the name of the Universite de Sherbrooke nor the
+      names of its contributors may be used to endorse or promote products
+      derived from this software without specific prior written permission.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
+ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
+DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+#include "rtabmap/core/SensorCapture.h"
+
+#include <rtabmap/utilite/UEventsManager.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UDirectory.h>
+#include <rtabmap/utilite/UTimer.h>
+
+#include <opencv2/imgproc/imgproc.hpp>
+
+#include <iostream>
+#include <cmath>
+
+namespace rtabmap
+{
+
+SensorCapture::SensorCapture(float frameRate, const Transform & localTransform) :
+	_frameRate(frameRate),
+	_localTransform(localTransform),
+	_frameRateTimer(new UTimer()),
+	_seq(0)
+{
+}
+
+SensorCapture::~SensorCapture()
+{
+	delete _frameRateTimer;
+}
+
+void SensorCapture::resetTimer()
+{
+	_frameRateTimer->start();
+}
+
+SensorData SensorCapture::takeData(SensorCaptureInfo * info)
+{
+	bool warnFrameRateTooHigh = false;
+	float actualFrameRate = 0;
+	float frameRate = _frameRate;
+	if(frameRate>0)
+	{
+		int sleepTime = (1000.0f/frameRate - 1000.0f*_frameRateTimer->getElapsedTime());
+		if(sleepTime > 2)
+		{
+			uSleep(sleepTime-2);
+		}
+		else if(sleepTime < 0)
+		{
+			warnFrameRateTooHigh = true;
+			actualFrameRate = 1.0/(_frameRateTimer->getElapsedTime());
+		}
+
+		// Add precision at the cost of a small overhead
+		while(_frameRateTimer->getElapsedTime() < 1.0/double(frameRate)-0.000001)
+		{
+			//
+		}
+
+		double slept = _frameRateTimer->getElapsedTime();
+		_frameRateTimer->start();
+		UDEBUG("slept=%fs vs target=%fs", slept, 1.0/double(frameRate));
+	}
+
+	UTimer timer;
+	SensorData data = this->captureData(info);
+	double captureTime = timer.ticks();
+	if(warnFrameRateTooHigh)
+	{
+		UWARN("Camera: Cannot reach target frame rate %f Hz, current rate is %f Hz and capture time = %f s.",
+				frameRate, actualFrameRate, captureTime);
+	}
+	else
+	{
+		UDEBUG("Time capturing data = %fs", captureTime);
+	}
+	if(info)
+	{
+		info->id = data.id();
+		info->stamp = data.stamp();
+		info->timeCapture = captureTime;
+	}
+	return data;
+}
+
+} // namespace rtabmap
@@ -25,9 +25,10 @@ ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

-#include "rtabmap/core/CameraThread.h"
+#include "rtabmap/core/SensorCaptureThread.h"
 #include "rtabmap/core/Camera.h"
-#include "rtabmap/core/CameraEvent.h"
+#include "rtabmap/core/Lidar.h"
+#include "rtabmap/core/SensorEvent.h"
 #include "rtabmap/core/CameraRGBD.h"
 #include "rtabmap/core/util2d.h"
 #include "rtabmap/core/util3d.h"
@@ -50,143 +51,149 @@ namespace rtabmap
 {

 // ownership transferred
-CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
-		_camera(camera),
-		_odomSensor(0),
-		_odomAsGt(false),
-		_poseTimeOffset(0.0),
-		_poseScaleFactor(1.0f),
-		_mirroring(false),
-		_stereoExposureCompensation(false),
-		_colorOnly(false),
-		_imageDecimation(1),
-		_histogramMethod(0),
-		_stereoToDepth(false),
-		_scanFromDepth(false),
-		_scanDownsampleStep(1),
-		_scanRangeMin(0.0f),
-		_scanRangeMax(0.0f),
-		_scanVoxelSize(0.0f),
-		_scanNormalsK(0),
-		_scanNormalsRadius(0.0f),
-		_scanForceGroundNormalsUp(false),
-		_stereoDense(StereoDense::create(parameters)),
-		_distortionModel(0),
-		_bilateralFiltering(false),
-		_bilateralSigmaS(10),
-		_bilateralSigmaR(0.1),
-		_imuFilter(0),
-		_imuBaseFrameConversion(false),
-		_featureDetector(0),
-		_depthAsMask(Parameters::defaultVisDepthAsMask())
+SensorCaptureThread::SensorCaptureThread(
+		Camera * camera,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
 {
-	UASSERT(_camera != 0);
+	UASSERT(camera != 0);
 }

 // ownership transferred
-CameraThread::CameraThread(
+SensorCaptureThread::SensorCaptureThread(
 		Camera * camera,
-		Camera * odomSensor,
+		SensorCapture * odomSensor,
 		const Transform & extrinsics,
 		double poseTimeOffset,
 		float poseScaleFactor,
-		bool odomAsGt,
+		double poseWaitTime,
 		const ParametersMap & parameters) :
-			_camera(camera),
-			_odomSensor(odomSensor),
-			_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
-			_odomAsGt(odomAsGt),
-			_poseTimeOffset(poseTimeOffset),
-			_poseScaleFactor(poseScaleFactor),
-			_mirroring(false),
-			_stereoExposureCompensation(false),
-			_colorOnly(false),
-			_imageDecimation(1),
-			_histogramMethod(0),
-			_stereoToDepth(false),
-			_scanFromDepth(false),
-			_scanDownsampleStep(1),
-			_scanRangeMin(0.0f),
-			_scanRangeMax(0.0f),
-			_scanVoxelSize(0.0f),
-			_scanNormalsK(0),
-			_scanNormalsRadius(0.0f),
-			_scanForceGroundNormalsUp(false),
-			_stereoDense(StereoDense::create(parameters)),
-			_distortionModel(0),
-			_bilateralFiltering(false),
-			_bilateralSigmaS(10),
-			_bilateralSigmaR(0.1),
-			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
 {
-	UASSERT(_camera != 0 && _odomSensor != 0 && !_extrinsicsOdomToCamera.isNull());
-	UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
-	UDEBUG("_poseTimeOffset        =%f", _poseTimeOffset);
-	UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
-	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+	UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull());
 }

-// ownership transferred
-CameraThread::CameraThread(
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
+{
+	UASSERT(lidar != 0);
+}
+
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
 		Camera * camera,
-		bool odomAsGt,
 		const ParametersMap & parameters) :
-			_camera(camera),
-			_odomSensor(0),
-			_odomAsGt(odomAsGt),
-			_poseTimeOffset(0.0),
-			_poseScaleFactor(1.0f),
-			_mirroring(false),
-			_stereoExposureCompensation(false),
-			_colorOnly(false),
-			_imageDecimation(1),
-			_histogramMethod(0),
-			_stereoToDepth(false),
-			_scanFromDepth(false),
-			_scanDownsampleStep(1),
-			_scanRangeMin(0.0f),
-			_scanRangeMax(0.0f),
-			_scanVoxelSize(0.0f),
-			_scanNormalsK(0),
-			_scanNormalsRadius(0.0f),
-			_scanForceGroundNormalsUp(false),
-			_stereoDense(StereoDense::create(parameters)),
-			_distortionModel(0),
-			_bilateralFiltering(false),
-			_bilateralSigmaS(10),
-			_bilateralSigmaR(0.1),
-			_imuFilter(0),
-			_imuBaseFrameConversion(false),
-			_featureDetector(0),
-			_depthAsMask(Parameters::defaultVisDepthAsMask())
+			SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
 {
-	UASSERT(_camera != 0);
-	UDEBUG("_odomAsGt              =%s", _odomAsGt?"true":"false");
+	UASSERT(lidar != 0 && camera != 0);
 }

-CameraThread::~CameraThread()
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		SensorCapture * odomSensor,
+		double poseTimeOffset,
+		float poseScaleFactor,
+		double poseWaitTime,
+		const ParametersMap & parameters) :
+			SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
+{
+	UASSERT(lidar != 0 && odomSensor != 0);
+}
+
+SensorCaptureThread::SensorCaptureThread(
+		Lidar * lidar,
+		Camera * camera,
+		SensorCapture * odomSensor,
+		const Transform & extrinsics,
+		double poseTimeOffset,
+		float poseScaleFactor,
+		double poseWaitTime,
+		const ParametersMap & parameters) :
+				_camera(camera),
+				_odomSensor(odomSensor),
+				_lidar(lidar),
+				_extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
+				_odomAsGt(false),
+				_poseTimeOffset(poseTimeOffset),
+				_poseScaleFactor(poseScaleFactor),
+				_poseWaitTime(poseWaitTime),
+				_mirroring(false),
+				_stereoExposureCompensation(false),
+				_colorOnly(false),
+				_imageDecimation(1),
+				_histogramMethod(0),
+				_stereoToDepth(false),
+				_scanDeskewing(false),
+				_scanFromDepth(false),
+				_scanDownsampleStep(1),
+				_scanRangeMin(0.0f),
+				_scanRangeMax(0.0f),
+				_scanVoxelSize(0.0f),
+				_scanNormalsK(0),
+				_scanNormalsRadius(0.0f),
+				_scanForceGroundNormalsUp(false),
+				_stereoDense(StereoDense::create(parameters)),
+				_distortionModel(0),
+				_bilateralFiltering(false),
+				_bilateralSigmaS(10),
+				_bilateralSigmaR(0.1),
+				_imuFilter(0),
+				_imuBaseFrameConversion(false),
+				_featureDetector(0),
+				_depthAsMask(Parameters::defaultVisDepthAsMask())
+{
+	UASSERT(_camera != 0 || _lidar != 0);
+	if(_lidar && _camera)
+	{
+		_camera->setFrameRate(0);
+	}
+	if(_odomSensor)
+	{
+		if(_camera)
+		{
+			if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull())
+			{
+				_extrinsicsOdomToCamera.setIdentity();
+			}
+			UASSERT(!_extrinsicsOdomToCamera.isNull());
+			UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
+		}
+		UDEBUG("_poseTimeOffset        =%f", _poseTimeOffset);
+		UDEBUG("_poseScaleFactor       =%f", _poseScaleFactor);
+		UDEBUG("_poseWaitTime          =%f", _poseWaitTime);
+	}
+}
+
+SensorCaptureThread::~SensorCaptureThread()
 {
 	join(true);
+	if(_odomSensor != _camera && _odomSensor != _lidar)
+	{
+		delete _odomSensor;
+	}
 	delete _camera;
-	delete _odomSensor;
+	delete _lidar;
 	delete _distortionModel;
 	delete _stereoDense;
 	delete _imuFilter;
 	delete _featureDetector;
 }

-void CameraThread::setImageRate(float imageRate)
+void SensorCaptureThread::setFrameRate(float frameRate)
 {
-	if(_camera)
+	if(_lidar)
 	{
-		_camera->setImageRate(imageRate);
+		_lidar->setFrameRate(frameRate);
+	}
+	else if(_camera)
+	{
+		_camera->setFrameRate(frameRate);
 	}
 }

-void CameraThread::setDistortionModel(const std::string & path)
+void SensorCaptureThread::setDistortionModel(const std::string & path)
 {
 	if(_distortionModel)
 	{
@@ -206,7 +213,7 @@ void CameraThread::setDistortionModel(const std::string & path)
 	}
 }

-void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
+void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 {
 	UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
 	_bilateralFiltering = true;
@@ -214,26 +221,27 @@ void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
 	_bilateralSigmaR = sigmaR;
 }

-void CameraThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
+void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
 {
 	delete _imuFilter;
 	_imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
 	_imuBaseFrameConversion = baseFrameConversion;
 }

-void CameraThread::disableIMUFiltering()
+void SensorCaptureThread::disableIMUFiltering()
 {
 	delete _imuFilter;
 	_imuFilter = 0;
 }

-void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
+void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters)
 {
 	delete _featureDetector;
 	ParametersMap params = parameters;
 	ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
 	uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
 	uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
+	uInsert(params, ParametersPair(Parameters::kKpSSC(), uValue(params, Parameters::kVisSSC(), defaultParams.at(Parameters::kVisSSC()))));
 	uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
 	uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
 	uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
@@ -245,35 +253,38 @@ void CameraThread::enableFeatureDetection(const ParametersMap & parameters)
 	_featureDetector = Feature2D::create(params);
 	_depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
 }
-void CameraThread::disableFeatureDetection()
+void SensorCaptureThread::disableFeatureDetection()
 {
 	delete _featureDetector;
 	_featureDetector = 0;
 }

-void CameraThread::setScanParameters(
+void SensorCaptureThread::setScanParameters(
 	bool fromDepth,
 	int downsampleStep,
 	float rangeMin,
 	float rangeMax,
 	float voxelSize,
 	int normalsK,
-	int normalsRadius,
-	bool forceGroundNormalsUp)
+	float normalsRadius,
+	bool forceGroundNormalsUp,
+	bool deskewing)
 {
-	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f);
+	setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing);
 }

-void CameraThread::setScanParameters(
+void SensorCaptureThread::setScanParameters(
 			bool fromDepth,
 			int downsampleStep, // decimation of the depth image in case the scan is from depth image
 			float rangeMin,
 			float rangeMax,
 			float voxelSize,
 			int normalsK,
-			int normalsRadius,
-			float groundNormalsUp)
+			float normalsRadius,
+			float groundNormalsUp,
+			bool deskewing)
 {
+	_scanDeskewing = deskewing;
 	_scanFromDepth = fromDepth;
 	_scanDownsampleStep=downsampleStep;
 	_scanRangeMin = rangeMin;
@@ -284,29 +295,174 @@ void CameraThread::setScanParameters(
 	_scanForceGroundNormalsUp = groundNormalsUp;
 }

-bool CameraThread::odomProvided() const
+bool SensorCaptureThread::odomProvided() const
 {
-	return _camera && (_camera->odomProvided() || (_odomSensor && _odomSensor->odomProvided()));
+	if(_odomAsGt)
+	{
+		return false;
+	}
+	return _odomSensor != 0;
 }

-void CameraThread::mainLoopBegin()
+void SensorCaptureThread::mainLoopBegin()
 {
 	ULogger::registerCurrentThread("Camera");
-	_camera->resetTimer();
+	if(_lidar)
+	{
+		_lidar->resetTimer();
+	}
+	else if(_camera)
+	{
+		_camera->resetTimer();
+	}
+	if(_imuFilter)
+	{
+		// In case we paused the camera and moved somewhere else, restart filtering.
+		_imuFilter->reset();
+	}
 }

-void CameraThread::mainLoop()
+void SensorCaptureThread::mainLoop()
 {
+	UASSERT(_lidar || _camera);
 	UTimer totalTime;
-	CameraInfo info;
-	SensorData data = _camera->takeImage(&info);
-
-	if(_odomSensor)
+	SensorCaptureInfo info;
+	SensorData data;
+	SensorData cameraData;
+	double lidarStamp = 0.0;
+	double cameraStamp = 0.0;
+	if(_lidar)
 	{
+		data = _lidar->takeData(&info);
+		if(data.stamp() == 0.0)
+		{
+			UWARN("Could not capture scan!");
+		}
+		else
+		{
+			lidarStamp = data.stamp();
+			if(_camera)
+			{
+				cameraData = _camera->takeData();
+				if(cameraData.stamp() == 0.0)
+				{
+					UWARN("Could not capture image!");
+				}
+				else
+				{
+					double stampStart = UTimer::now();
+					while(cameraData.stamp() < data.stamp() &&
+						!isKilled() &&
+						UTimer::now() - stampStart < _poseWaitTime &&
+						!cameraData.imageRaw().empty())
+					{
+						// Make sure the camera frame is newer than lidar frame so
+						// that if there are imus published by the cameras, we can get
+						// them all in odometry before deskewing.
+						cameraData = _camera->takeData();
+					}
+
+					cameraStamp = cameraData.stamp();
+					if(cameraData.stamp() < data.stamp())
+					{
+						UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.",
+								cameraData.stamp(),
+								data.stamp(),
+								_poseWaitTime);
+					}
+
+					if(!cameraData.stereoCameraModels().empty())
+					{
+						data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true);
+					}
+					else
+					{
+						data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true);
+					}
+				}
+			}
+		}
+	}
+	else if(_camera)
+	{
+		data = _camera->takeData(&info);
+		if(data.stamp() == 0.0)
+		{
+			UWARN("Could not capture image!");
+		}
+		else
+		{
+			cameraStamp = cameraData.stamp();
+		}
+	}
+
+	if(_odomSensor && data.stamp() != 0.0)
+	{
+		if(lidarStamp!=0.0 && _scanDeskewing)
+		{
+			UDEBUG("Deskewing begin");
+			if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime())
+			{
+				float scanTime =
+					data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
+					data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+
+				Transform poseFirstScan;
+				Transform poseLastScan;
+				cv::Mat cov;
+				double firstStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
+				double lastStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()];
+				if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) &&
+				   _odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0))
+				{
+					if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
+					{
+						poseFirstScan.x() *= _poseScaleFactor;
+						poseFirstScan.y() *= _poseScaleFactor;
+						poseFirstScan.z() *= _poseScaleFactor;
+						poseLastScan.x() *= _poseScaleFactor;
+						poseLastScan.y() *= _poseScaleFactor;
+						poseLastScan.z() *= _poseScaleFactor;
+					}
+
+					UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull());
+
+					Transform transform = poseFirstScan.inverse() * poseLastScan;
+
+					// convert to velocity
+					float x,y,z,roll,pitch,yaw;
+					transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
+					x/=scanTime;
+					y/=scanTime;
+					z/=scanTime;
+					roll /= scanTime;
+					pitch /= scanTime;
+					yaw /= scanTime;
+
+					Transform velocity(x,y,z,roll,pitch,yaw);
+					UTimer timeDeskewing;
+					LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
+					info.timeDeskewing = timeDeskewing.ticks();
+					if(!scanDeskewed.isEmpty())
+					{
+						data.setLaserScan(scanDeskewed);
+					}
+				}
+				else if(!data.laserScanRaw().empty())
+				{
+					UWARN("Failed to get poses for stamps %f and %f! Lidar won't be deskewed!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset);
+				}
+			}
+			else if(!data.laserScanRaw().empty())
+			{
+				UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str());
+			}
+			UDEBUG("Deskewing end");
+		}
+
 		Transform pose;
-		Transform poseToLeftCam;
 		cv::Mat covariance;
-		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance))
+		if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
 		{
 			info.odomPose = pose;
 			info.odomCovariance = covariance;
@@ -316,21 +472,47 @@ void CameraThread::mainLoop()
 				info.odomPose.y() *= _poseScaleFactor;
 				info.odomPose.z() *= _poseScaleFactor;
 			}
-			// Adjust local transform of the camera based on the pose frame
-			if(!data.cameraModels().empty())
+
+			if(cameraStamp != 0.0)
 			{
-				UASSERT(data.cameraModels().size()==1);
-				CameraModel model = data.cameraModels()[0];
-				model.setLocalTransform(_extrinsicsOdomToCamera);
-				data.setCameraModel(model);
-			}
-			else if(!data.stereoCameraModels().empty())
-			{
-				UASSERT(data.stereoCameraModels().size()==1);
-				StereoCameraModel model = data.stereoCameraModels()[0];
-				model.setLocalTransform(_extrinsicsOdomToCamera);
-				data.setStereoCameraModel(model);
+				Transform cameraCorrection = Transform::getIdentity();
+				if(lidarStamp > 0.0 && lidarStamp != cameraStamp)
+				{
+					if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
+					{
+						cameraCorrection = info.odomPose.inverse() * pose;
+					}
+					else
+					{
+						UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp);
+					}
+				}
+
+				// Adjust local transform of the camera based on the pose frame
+				if(!data.cameraModels().empty())
+				{
+					UASSERT(data.cameraModels().size()==1);
+					CameraModel model = data.cameraModels()[0];
+					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
+					data.setCameraModel(model);
+				}
+				else if(!data.stereoCameraModels().empty())
+				{
+					UASSERT(data.stereoCameraModels().size()==1);
+					StereoCameraModel model = data.stereoCameraModels()[0];
+					model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
+					data.setStereoCameraModel(model);
+				}
 			}
+
+			// Fake IMU to intialize gravity (assuming pose is aligned with gravity!)
+			Eigen::Quaterniond q = info.odomPose.getQuaterniond();
+			data.setIMU(IMU(
+					cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(),
+					cv::Vec3d(), cv::Mat(),
+					cv::Vec3d(), cv::Mat(),
+					Transform::getIdentity()));
+			this->disableIMUFiltering();
 		}
 		else
 		{
@@ -347,19 +529,19 @@ void CameraThread::mainLoop()
 	if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
 	{
 		postUpdate(&data, &info);
-		info.cameraName = _camera->getSerial();
+		info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
 		info.timeTotal = totalTime.ticks();
-		this->post(new CameraEvent(data, info));
+		this->post(new SensorEvent(data, info));
 	}
 	else if(!this->isKilled())
 	{
-		UWARN("no more images...");
+		UWARN("no more data...");
 		this->kill();
-		this->post(new CameraEvent());
+		this->post(new SensorEvent());
 	}
 }

-void CameraThread::mainLoopKill()
+void SensorCaptureThread::mainLoopKill()
 {
 	if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
 	{
@@ -384,7 +566,7 @@ void CameraThread::mainLoopKill()
 	}
 }

-void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
+void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const
 {
 	UASSERT(dataPtr!=0);
 	SensorData & data = *dataPtr;
@@ -219,19 +219,70 @@ void Signature::removeVirtualLinks()
 	}
 }

+void Signature::addLandmark(const Link & landmark)
+{
+	UDEBUG("Add landmark %d to %d (type=%d/%s var=%f,%f)", landmark.to(), this->id(), (int)landmark.type(), landmark.typeName().c_str(), landmark.transVariance(), landmark.rotVariance());
+	UASSERT_MSG(landmark.from() == this->id(), uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
+	UASSERT_MSG(landmark.to() < 0, uFormat("%d->%d for signature %d (type=%d)", landmark.from(), landmark.to(), this->id(), landmark.type()).c_str());
+	UASSERT_MSG(_landmarks.find(landmark.to()) == _landmarks.end(), uFormat("Landmark %d (type=%d) already added to signature %d!", landmark.to(), landmark.type(), this->id()).c_str());
+	_landmarks.insert(std::make_pair(landmark.to(), landmark));
+	_linksModified = true;
+}
+
+void Signature::removeLandmarks()
+{
+	size_t sizeBefore = _landmarks.size();
+	_landmarks.clear();
+	if(_landmarks.size() != sizeBefore)
+		_linksModified = true;
+}
+
+void Signature::removeLandmark(int landmarkId)
+{
+	int count = (int)_landmarks.erase(landmarkId);
+	if(count)
+	{
+		UDEBUG("Removed landmark %d from %d", landmarkId, this->id());
+		_linksModified = true;
+	}
+}
+
 float Signature::compareTo(const Signature & s) const
 {
+	UASSERT(this->sensorData().globalDescriptors().size() == s.sensorData().globalDescriptors().size());
+
 	float similarity = 0.0f;
-	const std::multimap<int, int> & words = s.getWords();
+	int totalDescs = 0;

-	if(!s.isBadSignature() && !this->isBadSignature())
+	for(size_t i=0; i<this->sensorData().globalDescriptors().size(); ++i)
 	{
-		std::list<std::pair<int, std::pair<int, int> > > 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);
+		if(this->sensorData().globalDescriptors()[i].type()==1 && s.sensorData().globalDescriptors()[i].type()==1)
+		{
+			// rescale dot product from -1<->1 to 0<->1 (we assume normalized vectors!)
+			float dotProd = (this->sensorData().globalDescriptors()[i].data().dot(s.sensorData().globalDescriptors()[i].data()) + 1.0f) / 2.0f;
+			UASSERT_MSG(dotProd>=0, "Global descriptors should be normalized!");
+			similarity += dotProd;
+			totalDescs += 1;
+		}
+	}

-		similarity = float(pairs.size()) / float(totalWords);
+	if(totalDescs)
+	{
+		similarity /= totalDescs;
+	}
+	else
+	{
+		const std::multimap<int, int> & words = s.getWords();
+
+		if(!s.isBadSignature() && !this->isBadSignature())
+		{
+			std::list<std::pair<int, std::pair<int, int> > > 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);
+
+			similarity = float(pairs.size()) / float(totalWords);
+		}
 	}
 	return similarity;
 }
@@ -58,12 +58,15 @@ CameraDepthAI::CameraDepthAI(
 	confThreshold_(200),
 	lrcThreshold_(5),
 	resolution_(resolution),
+	extendedDisparity_(false),
+	subpixelFractionalBits_(0),
+	compandingWidth_(0),
 	useSpecTranslation_(false),
 	alphaScaling_(0.0),
 	imuPublished_(true),
 	publishInterIMU_(false),
-	dotProjectormA_(0.0),
-	floodLightmA_(200.0),
+	dotIntensity_(0.0),
+	floodIntensity_(0.0),
 	detectFeatures_(0),
 	useHarrisDetector_(false),
 	minDistance_(7.0),
@@ -107,6 +110,58 @@ void CameraDepthAI::setDepthProfile(int confThreshold, int lrcThreshold)
 #endif
 }

+void CameraDepthAI::setExtendedDisparity(bool extendedDisparity)
+{
+#ifdef RTABMAP_DEPTHAI
+	extendedDisparity_ = extendedDisparity;
+	if(extendedDisparity_)
+	{
+		if(subpixelFractionalBits_>0)
+		{
+			UWARN("Extended disparity has been enabled while subpixel being also enabled, disabling subpixel...");
+			subpixelFractionalBits_ = 0;
+		}
+		if(compandingWidth_>0)
+		{
+			UWARN("Extended disparity has been enabled while companding being also enabled, disabling companding...");
+			compandingWidth_ = 0;
+		}
+	}
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setSubpixelMode(bool enabled, int fractionalBits)
+{
+#ifdef RTABMAP_DEPTHAI
+	UASSERT(fractionalBits>=3 && fractionalBits<=5);
+	subpixelFractionalBits_ = enabled?fractionalBits:0;
+	if(subpixelFractionalBits_ != 0 && extendedDisparity_)
+	{
+		UWARN("Subpixel has been enabled while extended disparity being also enabled, disabling extended disparity...");
+		extendedDisparity_ = false;
+	}
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
+void CameraDepthAI::setCompanding(bool enabled, int width)
+{
+#ifdef RTABMAP_DEPTHAI
+	UASSERT(width == 64 || width == 96);
+	compandingWidth_ = enabled?width:0;
+	if(compandingWidth_ != 0 && extendedDisparity_)
+	{
+		UWARN("Companding has been enabled while extended disparity being also enabled, disabling extended disparity...");
+		extendedDisparity_ = false;
+	}
+#else
+	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
+#endif
+}
+
 void CameraDepthAI::setRectification(bool useSpecTranslation, float alphaScaling)
 {
 #ifdef RTABMAP_DEPTHAI
@@ -127,11 +182,11 @@ void CameraDepthAI::setIMU(bool imuPublished, bool publishInterIMU)
 #endif
 }

-void CameraDepthAI::setIrBrightness(float dotProjectormA, float floodLightmA)
+void CameraDepthAI::setIrIntensity(float dotIntensity, float floodIntensity)
 {
 #ifdef RTABMAP_DEPTHAI
-	dotProjectormA_ = dotProjectormA;
-	floodLightmA_ = floodLightmA;
+	dotIntensity_ = dotIntensity;
+	floodIntensity_ = floodIntensity;
 #else
 	UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
 #endif
@@ -235,51 +290,45 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		imu = p.create<dai::node::IMU>();
 	std::shared_ptr<dai::node::FeatureTracker> gfttDetector;
 	std::shared_ptr<dai::node::ImageManip> manip;
-	std::shared_ptr<dai::node::NeuralNetwork> superPointNetwork;
+	std::shared_ptr<dai::node::NeuralNetwork> neuralNetwork;
 	if(detectFeatures_ == 1)
 	{
 		gfttDetector = p.create<dai::node::FeatureTracker>();
 	}
-	else if(detectFeatures_ == 2)
+	else if(detectFeatures_ >= 2)
 	{
 		if(!blobPath_.empty())
 		{
 			manip = p.create<dai::node::ImageManip>();
-			superPointNetwork = p.create<dai::node::NeuralNetwork>();
+			neuralNetwork = p.create<dai::node::NeuralNetwork>();
 		}
 		else
 		{
-			UWARN("Missing SuperPoint blob file!");
+			UWARN("Missing MyriadX blob file!");
 			detectFeatures_ = 0;
 		}
 	}

-	auto xoutLeftOrColor = p.create<dai::node::XLinkOut>();
-	auto xoutDepthOrRight = p.create<dai::node::XLinkOut>();
+	auto sync = p.create<dai::node::Sync>();
+	auto xoutCamera = p.create<dai::node::XLinkOut>();
 	std::shared_ptr<dai::node::XLinkOut> xoutIMU;
 	if(imuPublished_)
 		xoutIMU = p.create<dai::node::XLinkOut>();
-	std::shared_ptr<dai::node::XLinkOut> xoutFeatures;
-	if(detectFeatures_)
-		xoutFeatures = p.create<dai::node::XLinkOut>();

 	// XLinkOut
-	xoutLeftOrColor->setStreamName(outputMode_<2?"rectified_left":"rectified_color");
-	xoutDepthOrRight->setStreamName(outputMode_?"depth":"rectified_right");
+	xoutCamera->setStreamName("camera");
 	if(imuPublished_)
 		xoutIMU->setStreamName("imu");
-	if(detectFeatures_)
-		xoutFeatures->setStreamName("features");

 	monoLeft->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
 	monoRight->setResolution((dai::MonoCameraProperties::SensorResolution)resolution_);
 	monoLeft->setCamera("left");
 	monoRight->setCamera("right");
-	if(detectFeatures_ == 2)
+	if(detectFeatures_ >= 2)
 	{
 		if(this->getImageRate() <= 0 || this->getImageRate() > 15)
 		{
-			UWARN("On-device SuperPoint enabled, image rate is limited to 15 FPS!");
+			UWARN("On-device SuperPoint or HF-Net enabled, image rate is limited to 15 FPS!");
 			monoLeft->setFps(15);
 			monoRight->setFps(15);
 		}
@@ -295,7 +344,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		stereo->setDepthAlign(dai::CameraBoardSocket::CAM_A);
 	else
 		stereo->setDepthAlign(dai::StereoDepthProperties::DepthAlign::RECTIFIED_LEFT);
-	stereo->setExtendedDisparity(false);
+	stereo->setExtendedDisparity(extendedDisparity_);
 	stereo->setRectifyEdgeFillColor(0); // black, to better see the cutout
 	stereo->enableDistortionCorrection(true);
 	stereo->setDisparityToDepthUseSpecTranslation(useSpecTranslation_);
@@ -303,8 +352,9 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 	if(alphaScaling_ > -1.0f)
 		stereo->setAlphaScaling(alphaScaling_);
 	stereo->initialConfig.setConfidenceThreshold(confThreshold_);
-	stereo->initialConfig.setLeftRightCheck(true);
-	stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
+	stereo->initialConfig.setLeftRightCheck(lrcThreshold_>=0);
+	if(lrcThreshold_>=0)
+		stereo->initialConfig.setLeftRightCheckThreshold(lrcThreshold_);
 	stereo->initialConfig.setMedianFilter(dai::MedianFilter::KERNEL_7x7);
 	auto config = stereo->initialConfig.get();
 	config.censusTransform.kernelSize = dai::StereoDepthConfig::CensusTransform::KernelSize::KERNEL_7x9;
@@ -325,6 +375,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		if(alphaScaling_ > -1.0f)
 			colorCam->setCalibrationAlpha(alphaScaling_);
 	}
+	this->setImageRate(0);

 	// Using VideoEncoder on PoE devices, Subpixel is not supported
 	if(deviceToUse.protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
@@ -336,51 +387,58 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		if(outputMode_ < 2)
 		{
 			stereo->rectifiedLeft.link(leftOrColorEnc->input);
+			leftOrColorEnc->bitstream.link(sync->inputs["left"]);
 		}
 		else
 		{
 			colorCam->video.link(leftOrColorEnc->input);
+			leftOrColorEnc->bitstream.link(sync->inputs["color"]);
 		}
 		if(outputMode_)
 		{
 			depthOrRightEnc->setQuality(100);
 			stereo->disparity.link(depthOrRightEnc->input);
+			depthOrRightEnc->bitstream.link(sync->inputs["depth"]);
 		}
 		else
 		{
 			stereo->rectifiedRight.link(depthOrRightEnc->input);
+			depthOrRightEnc->bitstream.link(sync->inputs["right"]);
 		}
-		leftOrColorEnc->bitstream.link(xoutLeftOrColor->input);
-		depthOrRightEnc->bitstream.link(xoutDepthOrRight->input);
 	}
 	else
 	{
-		stereo->setSubpixel(true);
-		stereo->setSubpixelFractionalBits(4);
+		stereo->setSubpixel(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5);
+		if(subpixelFractionalBits_>=3 && subpixelFractionalBits_<=5)
+			stereo->setSubpixelFractionalBits(subpixelFractionalBits_);
 		config = stereo->initialConfig.get();
-		config.costMatching.disparityWidth = dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64;
-		config.costMatching.enableCompanding = true;
+		config.costMatching.enableCompanding = compandingWidth_>0;
+		if(compandingWidth_>0)
+			config.costMatching.disparityWidth = compandingWidth_==64?dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_64:dai::StereoDepthConfig::CostMatching::DisparityWidth::DISPARITY_96;
 		stereo->initialConfig.set(config);
 		if(outputMode_ < 2)
 		{
-			stereo->rectifiedLeft.link(xoutLeftOrColor->input);
+			stereo->rectifiedLeft.link(sync->inputs["left"]);
 		}
 		else
 		{
 			monoLeft->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
 			monoRight->setResolution(dai::MonoCameraProperties::SensorResolution::THE_400_P);
-			colorCam->video.link(xoutLeftOrColor->input);
+			colorCam->video.link(sync->inputs["color"]);
 		}
 		if(outputMode_)
-			stereo->depth.link(xoutDepthOrRight->input);
+			stereo->depth.link(sync->inputs["depth"]);
 		else
-			stereo->rectifiedRight.link(xoutDepthOrRight->input);
+			stereo->rectifiedRight.link(sync->inputs["right"]);
 	}

+	sync->setSyncThreshold(std::chrono::milliseconds(int(500 / monoLeft->getFps())));
+	sync->out.link(xoutCamera->input);
+
 	if(imuPublished_)
 	{
-		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 100 hz rate
-		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 100);
+		// enable ACCELEROMETER_RAW and GYROSCOPE_RAW at 200 hz rate
+		imu->enableIMUSensor({dai::IMUSensor::ACCELEROMETER_RAW, dai::IMUSensor::GYROSCOPE_RAW}, 200);
 		// above this threshold packets will be sent in batch of X, if the host is not blocked and USB bandwidth is available
 		imu->setBatchReportThreshold(1);
 		// maximum number of IMU packets in a batch, if it's reached device will block sending until host can receive it
@@ -403,27 +461,35 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		cfg.featureMaintainer.minimumDistanceBetweenFeatures = minDistance_ * minDistance_;
 		gfttDetector->initialConfig.set(cfg);
 		stereo->rectifiedLeft.link(gfttDetector->inputImage);
-		gfttDetector->outputFeatures.link(xoutFeatures->input);
+		gfttDetector->outputFeatures.link(sync->inputs["feat"]);
 	}
-	else if(detectFeatures_ == 2)
+	else if(detectFeatures_ >= 2)
 	{
 		manip->setKeepAspectRatio(false);
 		manip->setMaxOutputFrameSize(320 * 200);
 		manip->initialConfig.setResize(320, 200);
-		superPointNetwork->setBlobPath(blobPath_);
-		superPointNetwork->setNumInferenceThreads(2);
-		superPointNetwork->setNumNCEPerInferenceThread(1);
-		superPointNetwork->input.setBlocking(false);
+		neuralNetwork->setBlobPath(blobPath_);
+		neuralNetwork->setNumInferenceThreads(2);
+		neuralNetwork->setNumNCEPerInferenceThread(1);
+		neuralNetwork->input.setBlocking(false);
 		stereo->rectifiedLeft.link(manip->inputImage);
-		manip->out.link(superPointNetwork->input);
-		superPointNetwork->out.link(xoutFeatures->input);
+		manip->out.link(neuralNetwork->input);
+		neuralNetwork->out.link(sync->inputs["feat"]);
 	}

 	device_.reset(new dai::Device(p, deviceToUse));

+	UINFO("Available camera sensors: ");
+	for(auto& sensor : device_->getCameraSensorNames()) {
+		UINFO("Socket: CAM_%c - %s", 'A'+(unsigned char)sensor.first, sensor.second.c_str());
+	}
+
 	UINFO("Loading eeprom calibration data");
 	dai::CalibrationHandler calibHandler = device_->readCalibration();

+	auto eeprom = calibHandler.getEepromData();
+	UINFO("Product name: %s, board name: %s", eeprom.productName.c_str(), eeprom.boardName.c_str());
+
 	auto cameraId = outputMode_<2?dai::CameraBoardSocket::CAM_B:dai::CameraBoardSocket::CAM_A;
 	cv::Mat cameraMatrix, distCoeffs, newCameraMatrix;

@@ -462,7 +528,6 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		//		matrix[0][0], matrix[0][1], matrix[0][2], matrix[0][3],
 		//		matrix[1][0], matrix[1][1], matrix[1][2], matrix[1][3],
 		//		matrix[2][0], matrix[2][1], matrix[2][2], matrix[2][3]);
-		auto eeprom = calibHandler.getEepromData();
 		if(eeprom.boardName == "OAK-D" ||
 		   eeprom.boardName == "BW1098OBC")
 		{
@@ -503,6 +568,7 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 		UINFO("IMU disabled");
 	}

+	cameraQueue_ = device_->getOutputQueue("camera", 8, false);
 	if(imuPublished_)
 	{
 		imuLocalTransform_ = this->getLocalTransform() * imuLocalTransform_;
@@ -534,16 +600,16 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
 			}
 		});
 	}
-	leftOrColorQueue_ = device_->getOutputQueue(outputMode_<2?"rectified_left":"rectified_color", 8, false);
-	rightOrDepthQueue_ = device_->getOutputQueue(outputMode_?"depth":"rectified_right", 8, false);
-	if(detectFeatures_)
-		featuresQueue_ = device_->getOutputQueue("features", 8, false);

-	std::vector<std::tuple<std::string, int, int>> irDrivers = device_->getIrDrivers();
-	if(!irDrivers.empty())
+	if(!device_->getIrDrivers().empty())
 	{
-		device_->setIrLaserDotProjectorBrightness(dotProjectormA_);
-		device_->setIrFloodLightBrightness(floodLightmA_);
+		UINFO("Setting IR intensity");
+		device_->setIrLaserDotProjectorIntensity(dotIntensity_);
+		device_->setIrFloodLightIntensity(floodIntensity_);
+	}
+	else if(dotIntensity_ > 0 || floodIntensity_ > 0)
+	{
+		UWARN("No IR drivers were detected! IR intensity cannot be set.");
 	}

 	uSleep(2000); // avoid bad frames on start
@@ -572,21 +638,16 @@ std::string CameraDepthAI::getSerial() const
 	return "";
 }

-SensorData CameraDepthAI::captureImage(CameraInfo * info)
+SensorData CameraDepthAI::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_DEPTHAI

+	auto messageGroup = cameraQueue_->get<dai::MessageGroup>();
+	auto rectifLeftOrColor = messageGroup->get<dai::ImgFrame>(outputMode_<2?"left":"color");
+	auto rectifRightOrDepth = messageGroup->get<dai::ImgFrame>(outputMode_?"depth":"right");
+
 	cv::Mat leftOrColor, depthOrRight;
-	auto rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
-	auto rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
-
-	while(rectifLeftOrColor->getSequenceNum() < rectifRightOrDepth->getSequenceNum())
-		rectifLeftOrColor = leftOrColorQueue_->get<dai::ImgFrame>();
-	while(rectifLeftOrColor->getSequenceNum() > rectifRightOrDepth->getSequenceNum())
-		rectifRightOrDepth = rightOrDepthQueue_->get<dai::ImgFrame>();
-
-	double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
 	if(device_->getDeviceInfo().protocol == X_LINK_TCP_IP || mxidOrName_.find(".") != std::string::npos)
 	{
 		leftOrColor = cv::imdecode(rectifLeftOrColor->getData(), cv::IMREAD_ANYCOLOR);
@@ -604,6 +665,7 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 		depthOrRight = rectifRightOrDepth->getCvFrame();
 	}

+	double stamp = std::chrono::duration<double>(rectifLeftOrColor->getTimestampDevice(dai::CameraExposureOffset::MIDDLE).time_since_epoch()).count();
 	if(outputMode_)
 		data = SensorData(leftOrColor, depthOrRight, stereoModel_.left(), this->getNextSeqID(), stamp);
 	else
@@ -660,28 +722,30 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)

 	if(detectFeatures_ == 1)
 	{
-		auto features = featuresQueue_->get<dai::TrackedFeatures>();
-		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
-			features = featuresQueue_->get<dai::TrackedFeatures>();
-		auto detectedFeatures = features->trackedFeatures;
-
+		auto features = messageGroup->get<dai::TrackedFeatures>("feat")->trackedFeatures;
 		std::vector<cv::KeyPoint> keypoints;
-		for(auto& feature : detectedFeatures)
+		for(auto& feature : features)
 			keypoints.emplace_back(cv::KeyPoint(feature.position.x, feature.position.y, 3));
 		data.setFeatures(keypoints, std::vector<cv::Point3f>(), cv::Mat());
 	}
-	else if(detectFeatures_ == 2)
+	else if(detectFeatures_ >= 2)
 	{
-		auto features = featuresQueue_->get<dai::NNData>();
-		while(features->getSequenceNum() < rectifLeftOrColor->getSequenceNum())
-			features = featuresQueue_->get<dai::NNData>();
-
-		auto heatmap = features->getLayerFp16("heatmap");
-		auto desc = features->getLayerFp16("desc");
-
-		cv::Mat scores(200, 320, CV_32FC1, heatmap.data());
-		cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);
+		auto features = messageGroup->get<dai::NNData>("feat");
+		std::vector<float> scores_dense, local_descriptor_map, global_descriptor;
+		if(detectFeatures_ == 2)
+		{
+			scores_dense = features->getLayerFp16("heatmap");
+			local_descriptor_map = features->getLayerFp16("desc");
+		}
+		else if(detectFeatures_ == 3)
+		{
+			scores_dense = features->getLayerFp16("pred/local_head/detector/Squeeze");
+			local_descriptor_map = features->getLayerFp16("pred/local_head/descriptor/transpose");
+			global_descriptor = features->getLayerFp16("pred/global_head/l2_normalize_1");
+		}

+		cv::Mat scores(200, 320, CV_32FC1, scores_dense.data());
+		cv::resize(scores, scores, targetSize_, 0, 0, cv::INTER_CUBIC);	
 		if(nms_)
 		{
 			cv::Mat dilated_scores(targetSize_, CV_32FC1);
@@ -714,10 +778,11 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 			keypoints.emplace_back(cv::KeyPoint(kpt, 8, -1, response));
 		}

-		cv::Mat coarse_desc(25, 40, CV_32FC(256), desc.data());
-		coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
-			cv::normalize(descriptor, descriptor);
-		});
+		cv::Mat coarse_desc(25, 40, CV_32FC(256), local_descriptor_map.data());
+		if(detectFeatures_ == 2)
+			coarse_desc.forEach<cv::Vec<float, 256>>([&](cv::Vec<float, 256>& descriptor, const int position[]) -> void {
+				cv::normalize(descriptor, descriptor);
+			});
 		cv::Mat mapX(keypoints.size(), 1, CV_32FC1);
 		cv::Mat mapY(keypoints.size(), 1, CV_32FC1);
 		for(size_t i=0; i<keypoints.size(); ++i)
@@ -734,6 +799,8 @@ SensorData CameraDepthAI::captureImage(CameraInfo * info)
 		descriptors = descriptors.reshape(1);

 		data.setFeatures(keypoints, std::vector<cv::Point3f>(), descriptors);
+		if(detectFeatures_ == 3)
+			data.addGlobalDescriptor(GlobalDescriptor(1, cv::Mat(1, global_descriptor.size(), CV_32FC1, global_descriptor.data()).clone()));
 	}

 #else
@@ -418,7 +418,7 @@ std::string CameraFreenect::getSerial() const
 	return "";
 }

-SensorData CameraFreenect::captureImage(CameraInfo * info)
+SensorData CameraFreenect::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT
@@ -334,7 +334,7 @@ std::string CameraFreenect2::getSerial() const
 	return "";
 }

-SensorData CameraFreenect2::captureImage(CameraInfo * info)
+SensorData CameraFreenect2::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
 #ifdef RTABMAP_FREENECT2
--- a/Show More
+++ b/Show More
Author	SHA1	Message	Date
matlabbe	0f961783c1	merged master->rolling	2024-05-27 11:42:12 -07:00
matlabbe	32e03aaf68	ORB_SLAM3: fixed error if left image is color. SensorCaptureThread: stop thread if data cannot be captured (e.g., end of dataset) instead of skipping indefinitly with error log.	2024-05-26 18:04:07 -07:00
matlabbe	03afb94d92	arcore_java: fixed high speed filtering jumping (#1281 )	2024-05-25 15:54:57 -07:00
matlabbe	c5c190a2a0	arcore_java: disabled max speed localization filtering by default #1281	2024-05-25 14:42:54 -07:00
matlabbe	bc66060fab	arcore: fixed pose not updated correctly	2024-05-25 13:50:01 -07:00
Borong Yuan	fbeabf0751	Add ANMS (SSC method) (#1276 )	2024-05-19 18:21:00 -07:00
matlabbe	0d4e4730c7	Merge branch 'borongyuan-likelihood_experiment'	2024-05-19 17:46:47 -07:00
matlabbe	995f65b2ec	Added parameter Rtabmap/VirtualPlaceLikelihoodRatio to select how likelihood is normalized/adjusted (0=default old approach used in paper).	2024-05-19 17:46:00 -07:00
matlabbe	c034a9631c	Merge branch 'likelihood_experiment' of https://github.com/borongyuan/rtabmap into borongyuan-likelihood_experiment	2024-05-19 14:57:58 -07:00
matlabbe	256dcbded4	Added script to reproduce results from loop closure detection paper. Added option to show Recall at 100% precision directly from rtabmap-console. Fixed retrieval not working on loop closure detection mode-only. Fixed showLogs.m error with Octave.	2024-05-19 14:54:37 -07:00
matlabbe	812caeeb9a	docker: added libtbb-dev	2024-05-12 22:15:20 -07:00
matlabbe	a66564f98f	docker/jammy/jammy-iron: added WITH_TBB option for opencv	2024-05-12 20:36:27 -07:00
matlabbe	375597af00	docker/focal: added WITH_TBB option for opencv	2024-05-12 19:58:12 -07:00
matlabbe	72d7418011	Fixed build with pcl<1.8	2024-05-12 19:13:05 -07:00
matlabbe	9505a21a7e	DBViewer/Matcher tool: Fixed crash when python feature/matcher is used	2024-04-28 17:40:37 -07:00
matlabbe	36304b2cd1	frontiers docker: added missing pybind-dev dep	2024-04-28 11:46:37 -07:00
Borong Yuan	f3cfa7831d	adjust likelihood using z-score	2024-04-28 12:22:26 +08:00
matlabbe	99c4612c64	Fixed #1268 . Fixed ui warnings of labels with same name.	2024-04-27 14:58:33 -07:00
matlabbe	27dea71398	Apply fix from #979	2024-04-15 20:51:50 -07:00
matlabbe	726bceb816	fixing bionic build	2024-04-14 19:46:34 -07:00
matlabbe	700704bec9	LiDAR capture support in standalone library (#1264 ) * Working rtabmap_lidar-mapping example (live and pcap) * finalizing merge, added some deprecated * fixed build * Working deskewing for Lidar + Camera/IMU (no camera pose correction yet) and Lidar + Odom Sensor in main UI. * backward compatibility * fixed some not used variable warnings, fixed qt build for lidar mapping example * Refactored CameraMobile, added AREngine background support, fixed LidarVPL16 build error with PCL 1.8 * ARCoreJava: buffer last depth image in case its stamp i higher than pose stamp. CameraMobile: added pose buffer. SensorCaptureThread: to get pose, odomSensor should be explicitly set, but can be same as lidar or camera inputs. * Working external lidar on iOS * util3d::commonFiltering()/adjustNormalsToViewPoint() added organized cloud support. MainWindow: updated odomSensor setup * fixed winsock include order * reverted camera tool * disable imu filtering when odom sensor is used * Updated package version * fixed windows build * fixing more windows build erros	2024-04-14 19:06:04 -07:00
matlabbe	6a6913c939	Fixed #1262	2024-04-13 16:42:58 -07:00
Borong Yuan	a55b4203cd	copy global descriptor (#1263 )	2024-04-13 14:56:28 -07:00
matlabbe	a3267c9da6	UI: Added option to change map resolution on UI side, save/restore thresholds in figures, fixed basic panel's slam/rgbd mode checkboxes not changing the state of corresponding parameters.	2024-04-12 17:12:23 -07:00
matlabbe	fbbab1c077	Update README.md	2024-04-11 17:12:11 -07:00
mathieu86	4e7110930c	DbViewer: switch between left and right views when double-clicking on single nodes	2024-04-11 13:01:00 -07:00
matlabbe	9266926a4a	docker: enabled focal linux/arm/v7	2024-04-10 09:13:24 -07:00
Dominic Létourneau	5a159cef04	Create scheduled-stats.yml (#1260 ) GitHub Stats for IntRoLab.	2024-04-09 09:09:21 -07:00
matlabbe	5ed1bf0ca8	GlobalDescriptor (PyDescriptor / netvlad) (#1163 ) * First commit for pydescriptor * Fixed Python refactoring errors * GUI: added PyDescriptor parameters * reordered python3 includes * updated rtabmap_netvlad.py test main * fixed build from last merge * integrated https://github.com/introlab/rtabmap/pull/1255 * rescaled dot product result Closing https://github.com/introlab/rtabmap/issues/1105	2024-04-07 15:48:16 -07:00
matlabbe	c33e995e83	Landmark constraints can now be rejected (in case we see same landmark at two different spots or constraint is poorly estimated). Optimizer/Robust can now be used with landmark constraints with orientation.	2024-04-06 13:41:03 -07:00
matlabbe	92edae35fc	Fixed map empty on start if there is no global loop closure yet in localization mode when RGBD/OptimizeFromGraphEnd is true	2024-04-06 11:46:26 -07:00
matlabbe	d794669ef9	DBViewer: added buttons to edit/add priors	2024-04-02 17:37:52 -07:00
matlabbe	9cb0046734	fixing build with gtsam 4.0	2024-04-02 15:32:04 -07:00
matlabbe	76f63ab79a	Fixing build with gtsam 4.2	2024-04-02 13:29:51 -07:00
matlabbe	063ca9c8ee	fixed gtsam 4.3 build error (https://github.com/introlab/rtabmap/pull/1033#issuecomment-2031002034 )	2024-04-01 23:05:16 -07:00
matlabbe	11adbdcc9f	Fixed #1253	2024-03-31 14:53:00 -07:00
matlabbe	8612ebba93	fixed https://github.com/introlab/rtabmap_ros/issues/1134	2024-03-31 13:06:52 -07:00
matlabbe	a0476af896	UI: Exposed more depthai options, setting default rectification alpha to -1 to avoid OAK-D Pro crash	2024-03-30 18:08:32 -07:00
matlabbe	3b735907c8	DbViewer tool: added database's parameter overriding from arguments	2024-03-29 13:30:12 -07:00
matlabbe	0ba79be461	depthai >=2.24 requirement	2024-03-28 14:27:17 -07:00
matlabbe	cc17ebe92b	iSAM2 integration (#1249 ) * isam2 integration * fonctional iSAM2, added related parameters * updated default params after testing large-scale enviroment * proximity search optimization * boost<1.68 fix * boost version * Fixed map not showing in graphview * iSAM2: supporting MM/localization/multi-session modes	2024-03-27 17:18:02 -07:00
Borong Yuan	bfeb487dba	OAK Internal Sync & DepthAI HF-Net Support (#1193 ) * Using Sync Node * Using normalized IR intensity * Set IMU output frequency to 200 Hz * set sync threshold properly * add depthai hfnet local head * fix DB error * add depthai hfnet global head * fix data delay if image rate is set manually	2024-03-27 12:29:06 -07:00
matlabbe	bc6d390fb6	bionic docker: fixing cmake version for arm	2024-03-24 18:49:11 -07:00
matlabbe	f8a63ecf10	Fixing bionic docker (with cmake>=3.14)	2024-03-23 14:41:10 -07:00
matlabbe	7183b43c2a	missed part from last commit	2024-03-23 14:34:54 -07:00
matlabbe	fa3ece5183	Increased minimal cmake version to 3.14 to use cmake's official FindSQLite3.cmake (#1250 )	2024-03-23 14:32:18 -07:00
matlabbe	2f132b3a40	GUI: adjusted max limit of Optimizer/Epsilon	2024-03-19 16:34:11 -07:00
matlabbe	79dc180bf4	Fixed Odom/ResetCountdown not resetting anymore if next frame is also bad.	2024-03-17 17:19:08 -07:00
Chris Lalancette	f8c4b62591	Remove dependency on qt_gui_cpp. (#1072 ) There is no dependency in this package on qt_gui_cpp. Instead, just use a dependency on qtbase5-dev Signed-off-by: Chris Lalancette <clalancette@gmail.com>	2023-06-27 13:07:47 -07:00