iOS various updates (#1340)

* Added Data Recording Mode. Added option to filter ARKit localization jumps.

* Implemented max acc relocalization filtering (working on iOS)

* Fixed android build, added re-localization max acceleration parameter

* ARCore Java: Fixed pose of depth not available at stamp requested

* Android log fix

* Added libLAS support

* ios: added LAS support

* updated dep install script to skip libraries already installed

* CameraMobile: Fixed origin not updated if updateOnRender() is used

* Default max opt error increased to 2x to reduce number of loop closures rejected. OptimizerGTSAM: updated gravity noise model to use same sigma for both parameters.

* Updated license

* bump 0.21.7

* updated license date

CMakeLists.txt

@@ -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 6)
+SET(RTABMAP_PATCH_VERSION 7)
 SET(RTABMAP_VERSION
   ${RTABMAP_MAJOR_VERSION}.${RTABMAP_MINOR_VERSION}.${RTABMAP_PATCH_VERSION})
   
@@ -177,6 +177,7 @@ option(WITH_TORCH         "Include Torch support (SuperPoint)"   OFF)
 option(WITH_PYTHON        "Include Python3 support (PyMatcher, PyDetector)"  OFF)
 option(WITH_PYTHON_THREADING  "Use more than one Python interpreter."  OFF)
 option(WITH_PDAL          "Include PDAL support"                 ON)
+option(WITH_LIBLAS        "Include libLAS support"               OFF)
 option(WITH_CUDASIFT      "Include CudaSift support (this fork https://github.com/matlabbe/CudaSift)" OFF) 
 option(WITH_FREENECT      "Include Freenect support"             ON)
 option(WITH_FREENECT2     "Include Freenect2 support"            ON)
@@ -416,6 +417,13 @@ IF(WITH_PDAL)
     ENDIF(PDAL_FOUND)
 ENDIF(WITH_PDAL)
 
+IF(WITH_LIBLAS)
+    FIND_PACKAGE(libLAS QUIET)
+    IF(libLAS_FOUND)
+       MESSAGE(STATUS "Found libLAS ${libLAS_VERSION}: ${libLAS_INCLUDE_DIRS}")
+    ENDIF(libLAS_FOUND)
+ENDIF(WITH_LIBLAS)
+
 IF(WITH_CUDASIFT)
     FIND_PACKAGE(CudaSift 3 QUIET)
     IF(CudaSift_FOUND)
@@ -950,6 +958,9 @@ ENDIF(NOT opengv_FOUND OR NOT WITH_OPENGV)
 IF(NOT PDAL_FOUND)
    SET(PDAL "//")
 ENDIF(NOT PDAL_FOUND)
+IF(NOT libLAS_FOUND)
+   SET(LIBLAS "//")
+ENDIF(NOT libLAS_FOUND)
 IF(NOT CudaSift_FOUND)
    SET(CUDASIFT "//")
 ENDIF(NOT CudaSift_FOUND)
@@ -1427,6 +1438,14 @@ ELSE()
 MESSAGE(STATUS "  With PDAL                 = NO (PDAL not found)")
 ENDIF()
 
+IF(libLAS_FOUND)
+MESSAGE(STATUS "  With libLAS ${libLAS_VERSION}         = YES (License: BSD)")
+ELSEIF(NOT WITH_LIBLAS)
+MESSAGE(STATUS "  With libLAS               = NO (WITH_LIBLAS=OFF)")
+ELSE()
+MESSAGE(STATUS "  With libLAS               = NO (libLAS not found)")
+ENDIF()
+
 IF(CudaSift_FOUND)
 MESSAGE(STATUS "  With CudaSift             = YES (License: MIT)")
 ELSEIF(NOT WITH_CUDASIFT)

Version.h.in

@@ -58,6 +58,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 @FASTCV@#define RTABMAP_FASTCV
 @OPENGV@#define RTABMAP_OPENGV
 @PDAL@#define RTABMAP_PDAL
+@LIBLAS@#define RTABMAP_LIBLAS
 @CUDASIFT@#define RTABMAP_CUDASIFT
 @LOAM@#define RTABMAP_LOAM
 @FLOAM@#define RTABMAP_FLOAM

app/android/jni/CameraARCore.cpp

@@ -37,8 +37,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
-	CameraMobile(smoothing),
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing, float upstreamRelocalizationAccThr):
+	CameraMobile(smoothing, upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),
@@ -125,6 +125,8 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
+	CameraMobile::init(calibrationFolder, cameraName);
+
 	UScopeMutex lock(arSessionMutex_);
 
 	ArInstallStatus install_status;
@@ -272,54 +274,6 @@ void CameraARCore::close()
 	CameraMobile::close();
 }
 
-LaserScan CameraARCore::scanFromPointCloudData(
-		const float * pointCloudData,
-		int points,
-		const Transform & pose,
-		const CameraModel & model,
-		const cv::Mat & rgb,
-		std::vector<cv::KeyPoint> * kpts,
-		std::vector<cv::Point3f> * kpts3D)
-{
-	if(pointCloudData && points>0)
-	{
-		cv::Mat scanData(1, points, CV_32FC4);
-		float * ptr = scanData.ptr<float>();
-		for(unsigned int i=0;i<points; ++i)
-		{
-			cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
-			pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
-			ptr[i*4] = pt.x;
-			ptr[i*4 + 1] = pt.y;
-			ptr[i*4 + 2] = pt.z;
-
-			//get color from rgb image
-			cv::Point3f org= pt;
-			pt = util3d::transformPoint(pt, opticalRotationInv);
-			int u,v;
-			model.reproject(pt.x, pt.y, pt.z, u, v);
-			unsigned char r=255,g=255,b=255;
-			if(model.inFrame(u, v))
-			{
-				b=rgb.at<cv::Vec3b>(v,u).val[0];
-				g=rgb.at<cv::Vec3b>(v,u).val[1];
-				r=rgb.at<cv::Vec3b>(v,u).val[2];
-				if(kpts)
-					kpts->push_back(cv::KeyPoint(u,v,3));
-				if(kpts3D)
-					kpts3D->push_back(org);
-			}
-			*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
-
-			//confidence
-			//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
-
-		}
-		return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
-	}
-	return LaserScan();
-}
-
 void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
 {
 	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
@@ -385,12 +339,6 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose)
 							   /*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);
 
@@ -402,11 +350,12 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose)
 		ArCamera_getPose(arSession_, ar_camera, arPose_);
 		ArPose_getPoseRaw(arSession_, arPose_, pose_raw);
 		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;
+		pose = rtabmap::rtabmap_world_T_opengl_world * poseArCore * rtabmap::opengl_world_T_rtabmap_world;
 
-		if(poseArCore.isNull())
+		if(pose.isNull())
 		{
 			LOGE("CameraARCore: Pose is null");
+			return data;
 		}
 
 		// Get calibration parameters
@@ -530,7 +479,11 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose)
 #endif
 							if(pointCloudData && points>0)
 							{
-								scan = scanFromPointCloudData(pointCloudData, points, poseArCore, model, rgb, &kpts, &kpts3);
+								cv::Mat pointCloudDataMat(1, points, CV_32FC4, (void *)pointCloudData);
+								scan = scanFromPointCloudData(pointCloudDataMat, pose, model, rgb, &kpts, &kpts3);
+#ifndef DISABLE_LOG
+								LOGI("valid scan points = %d", scan.size());
+#endif
 							}
 						}
 						else
@@ -541,15 +494,9 @@ SensorData CameraARCore::updateDataOnRender(Transform & pose)
 						data = SensorData(scan, rgb, depthFromMotion_?getOcclusionImage():cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 
-						if(!poseArCore.isNull())
+						if(!pose.isNull())
 						{
-							pose = poseArCore;
 							this->poseReceived(pose, stamp);
-							// adjust origin
-							if(!getOriginOffset().isNull())
-							{
-								pose = getOriginOffset() * pose;
-							}
 						}
 					}
 				}

app/android/jni/CameraARCore.h

@@ -50,17 +50,7 @@ namespace rtabmap {
 
 class CameraARCore : public CameraMobile {
 public:
-	static LaserScan scanFromPointCloudData(
-			const float * pointCloudData,
-			int points,
-			const Transform & pose,
-			const CameraModel & model,
-			const cv::Mat & rgb,
-			std::vector<cv::KeyPoint> * kpts = 0,
-			std::vector<cv::Point3f> * kpts3D = 0);
-
-public:
-	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraARCore();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraAREngine.cpp

@@ -40,8 +40,8 @@ namespace rtabmap {
 //////////////////////////////
 // CameraAREngine
 //////////////////////////////
-CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing):
-	CameraMobile(smoothing),
+CameraAREngine::CameraAREngine(void* env, void* context, void* activity, bool smoothing, float upstreamRelocalizationAccThr):
+	CameraMobile(smoothing, upstreamRelocalizationAccThr),
 	env_(env),
 	context_(context),
 	activity_(activity),
@@ -66,6 +66,8 @@ bool CameraAREngine::init(const std::string & calibrationFolder, const std::stri
 {
 	close();
 
+	CameraMobile::init(calibrationFolder, cameraName);
+
 	UScopeMutex lock(arSessionMutex_);
 
 	HwArInstallStatus install_status;
@@ -236,12 +238,6 @@ SensorData CameraAREngine::updateDataOnRender(Transform & pose)
 							   /*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);
 
@@ -334,11 +330,6 @@ SensorData CameraAREngine::updateDataOnRender(Transform & pose)
 					{
 						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;
-						}
 					}
 				}
 			}

app/android/jni/CameraAREngine.h

@@ -46,7 +46,7 @@ namespace rtabmap {
 
 class CameraAREngine : public CameraMobile {
 public:
-	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false);
+	CameraAREngine(void* env, void* context, void* activity, bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraAREngine();
 
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

app/android/jni/CameraMobile.cpp

@@ -18,6 +18,7 @@ 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
+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
@@ -52,7 +53,7 @@ const rtabmap::Transform CameraMobile::opticalRotationInv = Transform(
 	    0.0f,  0.0f, -1.0f, 0.0f,
 		1.0f,  0.0f,  0.0f, 0.0f);
 
-CameraMobile::CameraMobile(bool smoothing) :
+CameraMobile::CameraMobile(bool smoothing, float upstreamRelocalizationAccThr) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		textureId_(0),
@@ -60,7 +61,10 @@ CameraMobile::CameraMobile(bool smoothing) :
 		stampEpochOffset_(0.0),
 		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
-		originUpdate_(false)
+		originUpdate_(true),
+		upstreamRelocalizationAccThr_(upstreamRelocalizationAccThr),
+		previousAnchorStamp_(0.0),
+		dataGoodTracking_(true)
 {
 }
 
@@ -72,37 +76,44 @@ CameraMobile::~CameraMobile() {
 bool CameraMobile::init(const std::string &, const std::string &)
 {
 	deviceTColorCamera_ = opticalRotation;
+	// clear semaphore
+	if(dataReady_.value() > 0) {
+		dataReady_.acquire(dataReady_.value());
+	}
 	return true;
 }
 
 void CameraMobile::close()
 {
+	UScopeMutex lock(dataMutex_);
+
 	firstFrame_ = true;
 	lastKnownGPS_ = GPS();
 	lastEnvSensors_.clear();
 	originOffset_ = Transform();
-	originUpdate_ = false;
+	originUpdate_ = true;
 	dataPose_ = Transform();
 	data_ = SensorData();
+    dataGoodTracking_ = true;
+    previousAnchorPose_.setNull();
+    previousAnchorLinearVelocity_.clear();
+    previousAnchorStamp_ = 0.0;
 
     if(textureId_ != 0)
     {
         glDeleteTextures(1, &textureId_);
         textureId_ = 0;
     }
+    // in case someone is waiting on captureImage()
+    dataReady_.release();
 }
 
 void CameraMobile::resetOrigin()
 {
-	firstFrame_ = true;
-	lastKnownGPS_ = GPS();
-	lastEnvSensors_.clear();
-	dataPose_ = Transform();
-	data_ = SensorData();
 	originUpdate_ = true;
 }
 
-bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
+bool CameraMobile::getPose(double epochStamp, Transform & pose, cv::Mat & covariance, double maxWaitTime)
 {
 	pose.setNull();
 
@@ -113,27 +124,27 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance,
 	{
 		poseMutex_.lock();
 		int waitTry = 0;
-		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < stamp && waitTry < maxWaitTimeMs)
+		while(maxWaitTimeMs>0 && poseBuffer_.rbegin()->first < epochStamp && waitTry < maxWaitTimeMs)
 		{
 			poseMutex_.unlock();
 			++waitTry;
 			uSleep(1);
 			poseMutex_.lock();
 		}
-		if(poseBuffer_.rbegin()->first < stamp)
+		if(poseBuffer_.rbegin()->first < epochStamp)
 		{
 			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);
+				UWARN("Could not find poses to interpolate at time %f after waiting %d ms (latest is %f)...", epochStamp, maxWaitTimeMs, poseBuffer_.rbegin()->first);
 			}
 			else
 			{
-				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", stamp, poseBuffer_.rbegin()->first);
+				UWARN("Could not find poses to interpolate at time %f (latest is %f)...", epochStamp, poseBuffer_.rbegin()->first);
 			}
 		}
 		else
 		{
-			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(stamp);
+			std::map<double, Transform>::const_iterator iterB = poseBuffer_.lower_bound(epochStamp);
 			std::map<double, Transform>::const_iterator iterA = iterB;
 			if(iterA != poseBuffer_.begin())
 			{
@@ -143,17 +154,17 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance,
 			{
 				iterB = --iterB;
 			}
-			if(iterA == iterB && stamp == iterA->first)
+			if(iterA == iterB && epochStamp == iterA->first)
 			{
 				pose = iterA->second;
 			}
-			else if(stamp >= iterA->first && stamp <= iterB->first)
+			else if(epochStamp >= iterA->first && epochStamp <= iterB->first)
 			{
-				pose = iterA->second.interpolate((stamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+				pose = iterA->second.interpolate((epochStamp-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);
+				UWARN("Could not find pose data to interpolate at time %f (earliest is %f). Are sensors synchronized?", epochStamp, iterA->first);
 			}
 		}
 		poseMutex_.unlock();
@@ -163,25 +174,103 @@ bool CameraMobile::getPose(double stamp, Transform & pose, cv::Mat & covariance,
 
 void CameraMobile::poseReceived(const Transform & pose, double deviceStamp)
 {
+	// Pose reveived is the pose of the device in rtabmap coordinate
 	if(!pose.isNull())
 	{
 		Transform p = pose;
-		if(originUpdate_)
-		{
-			originOffset_ = p.translation().inverse();
-			originUpdate_ = false;
-		}
 		
 		if(stampEpochOffset_ == 0.0)
 		{
 			stampEpochOffset_ = UTimer::now() - deviceStamp;
 		}
+
+		if(originUpdate_)
+		{
+			firstFrame_ = true;
+			lastKnownGPS_ = GPS();
+			lastEnvSensors_.clear();
+			dataGoodTracking_ = true;
+			previousAnchorPose_.setNull();
+			previousAnchorLinearVelocity_.clear();
+			previousAnchorStamp_ = 0.0;
+			originOffset_ = pose.translation().inverse();
+			originUpdate_ = false;
+		}
         
 		double epochStamp = stampEpochOffset_ + deviceStamp;
-
 		if(!originOffset_.isNull())
 		{
-			p = originOffset_*p;
+			// Filter re-localizations from poses received
+			rtabmap::Transform rawPose = originOffset_ * pose.translation(); // remove rotation to keep position in fixed frame
+			// Remove upstream localization corrections by integrating pose from previous frame anchor
+            bool showLog = false;
+            if(upstreamRelocalizationAccThr_>0.0f && !previousAnchorPose_.isNull())
+			{
+				float dt = epochStamp - previousAnchorStamp_;
+                std::vector<float> currentLinearVelocity(3);
+                float dx = rawPose.x()-previousAnchorPose_.x();
+                float dy = rawPose.y()-previousAnchorPose_.y();
+                float dz = rawPose.z()-previousAnchorPose_.z();
+                currentLinearVelocity[0] = dx / dt;
+                currentLinearVelocity[1] = dy / dt;
+                currentLinearVelocity[2] = dz / dt;
+				if(!previousAnchorLinearVelocity_.empty() && uNorm(dx, dy, dz)>0.02)
+				{
+                    float ax = (currentLinearVelocity[0] - previousAnchorLinearVelocity_[0]) / dt;
+                    float ay = (currentLinearVelocity[1] - previousAnchorLinearVelocity_[1]) / dt;
+                    float az = (currentLinearVelocity[2] - previousAnchorLinearVelocity_[2]) / dt;
+					float acceleration = sqrt(ax*ax + ay*ay + az*az);
+					if(acceleration>=upstreamRelocalizationAccThr_)
+					{
+						// Only correct the translation to not lose rotation aligned
+						// with gravity.
+                        
+                        // Use constant motion model to update current pose.
+						rtabmap::Transform offset(previousAnchorLinearVelocity_[0] * dt,
+                                                  previousAnchorLinearVelocity_[1] * dt,
+                                                  previousAnchorLinearVelocity_[2] * dt,
+                                                  0, 0, 0, 1);
+						rtabmap::Transform newRawPose = offset * previousAnchorPose_;
+						currentLinearVelocity = previousAnchorLinearVelocity_;
+						originOffset_.x() += newRawPose.x() - rawPose.x();
+                        originOffset_.y() += newRawPose.y() - rawPose.y();
+                        originOffset_.z() += newRawPose.z() - rawPose.z();
+                        UERROR("Upstream re-localization has been suppressed because of "
+						     "high acceleration detected (%f m/s^2) causing a jump!",
+								acceleration);
+						dataGoodTracking_ = false;
+                        post(new CameraInfoEvent(0, "UpstreamRelocationFiltered", uFormat("%.1f m/s^2", acceleration).c_str()));
+                        showLog = true;
+					}
+				}
+				previousAnchorLinearVelocity_ = currentLinearVelocity;
+			}
+
+            p = originOffset_*pose;
+            previousAnchorPose_ = p;
+            previousAnchorStamp_ = epochStamp;
+            
+            if(upstreamRelocalizationAccThr_>0.0f) {
+                relocalizationDebugBuffer_.insert(std::make_pair(epochStamp, std::make_pair(pose, p)));
+                if(relocalizationDebugBuffer_.size() > 60)
+                {
+                    relocalizationDebugBuffer_.erase(relocalizationDebugBuffer_.begin());
+                }
+                if(showLog) {
+                    std::stringstream stream;
+                    for(auto iter=relocalizationDebugBuffer_.begin(); iter!=relocalizationDebugBuffer_.end(); ++iter)
+                    {
+                        stream << iter->first - relocalizationDebugBuffer_.begin()->first
+                        << " " << iter->second.first.x()
+                        << " " << iter->second.first.y()
+                        << " " << iter->second.first.z()
+                        << " " << iter->second.second.x()
+                        << " " << iter->second.second.y()
+                        << " " << iter->second.second.z() << std::endl;
+                    }
+                    UERROR("timestamp original_xyz corrected_xyz:\n%s", stream.str().c_str());
+                }
+            }
 		}
 
 		{
@@ -217,22 +306,16 @@ void CameraMobile::update(const SensorData & data, const Transform & pose, const
 {
 	UScopeMutex lock(dataMutex_);
 
-	bool notify = !data_.isValid();
+    LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
 
-	LOGD("CameraMobile::update pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
+	bool notify = !data_.isValid();
+    
 	data_ = data;
     dataPose_ = pose;
 
     viewMatrix_ = viewMatrix;
     projectionMatrix_ = projectionMatrix;
-    
-    // adjust origin
-    if(!originOffset_.isNull())
-    {
-        dataPose_ = originOffset_ * dataPose_;
-        viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-    }
-    
+        
     if(textureId_ == 0)
     {
     	glGenTextures(1, &textureId_);
@@ -272,12 +355,15 @@ void CameraMobile::update(const SensorData & data, const Transform & pose, const
         }
     }
 
-	postUpdate();
-	
-	if(notify)
-	{
-		dataReady_.release();
-	}
+    if(data_.isValid())
+    {
+        postUpdate();
+
+        if(notify)
+        {
+            dataReady_.release();
+        }
+    }
 }
 
 void CameraMobile::updateOnRender()
@@ -286,7 +372,6 @@ void CameraMobile::updateOnRender()
 	bool notify = !data_.isValid();
 
 	data_ = updateDataOnRender(dataPose_);
-
 	if(data_.isValid())
 	{
 		postUpdate();
@@ -309,6 +394,14 @@ void CameraMobile::postUpdate()
 {
 	if(data_.isValid())
 	{
+		// adjust origin
+		if(!originOffset_.isNull())
+		{
+			dataPose_ = originOffset_ * dataPose_;
+			viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
+			occlusionModel_.setLocalTransform(originOffset_ * occlusionModel_.localTransform());
+		}
+
 		if(lastKnownGPS_.stamp() > 0.0 && data_.stamp()-lastKnownGPS_.stamp()<1.0)
 		{
 			data_.setGPS(lastKnownGPS_);
@@ -424,11 +517,20 @@ void CameraMobile::postUpdate()
 SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 {
 	SensorData data;
-	if(dataReady_.acquire(1, 5000))
+    bool firstFrame = true;
+    bool dataGoodTracking = true;
+    rtabmap::Transform dataPose;
+	if(dataReady_.acquire(1, 15000))
 	{
 		UScopeMutex lock(dataMutex_);
 		data = data_;
+        dataPose = dataPose_;
+        firstFrame = firstFrame_;
+        dataGoodTracking = dataGoodTracking_;
+        firstFrame_ = false;
+        dataGoodTracking_ = true;
 		data_ = SensorData();
+        dataPose_.setNull();
 	}
 	if(data.isValid())
 	{
@@ -438,18 +540,26 @@ SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 		if(info)
 		{
 			// linear cov = 0.0001
-			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame_?9999.0:0.0001);
-			if(!firstFrame_)
+			info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.00001);
+			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;
+                // roll/pitch should be fairly accurate with VIO input
+				info->odomCovariance.at<double>(3,3) *= 0.01; // roll
+				info->odomCovariance.at<double>(4,4) *= 0.01; // pitch
+                if(!dataGoodTracking)
+                {
+                    UERROR("not good tracking!");
+                    // add slightly more error on translation
+                    // 0.001
+                    info->odomCovariance.at<double>(0,0) *= 10; // x
+                    info->odomCovariance.at<double>(1,1) *= 10; // y
+                    info->odomCovariance.at<double>(2,2) *= 10; // z
+                    info->odomCovariance.at<double>(5,5) *= 10; // yaw
+                }
 			}
-			info->odomPose = dataPose_;
+			info->odomPose = dataPose;
 		}
-
-		firstFrame_ = false;
 	}
 	else
 	{
@@ -460,7 +570,6 @@ SensorData CameraMobile::captureImage(SensorCaptureInfo * info)
 
 LaserScan CameraMobile::scanFromPointCloudData(
         const cv::Mat & pointCloudData,
-        int points,
         const Transform & pose,
         const CameraModel & model,
         const cv::Mat & rgb,
@@ -477,7 +586,7 @@ LaserScan CameraMobile::scanFromPointCloudData(
         UASSERT(pointCloudData.depth() == CV_32F && ic >= 3);
         
         int oi = 0;
-        for(unsigned int i=0;i<points; ++i)
+        for(unsigned int i=0;i<pointCloudData.cols; ++i)
         {
             cv::Point3f pt(inPtr[i*ic], inPtr[i*ic + 1], inPtr[i*ic + 2]);
             pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);

app/android/jni/CameraMobile.h

@@ -79,7 +79,6 @@ public:
 public:
     static LaserScan scanFromPointCloudData(
             const cv::Mat & pointCloudData,
-            int points,
             const Transform & pose,
             const CameraModel & model,
             const cv::Mat & rgb,
@@ -88,7 +87,7 @@ public:
             int kptsSize = 3);
 
 public:
-	CameraMobile(bool smoothing = false);
+	CameraMobile(bool smoothing = false, float upstreamRelocalizationAccThr = 0.0f);
 	virtual ~CameraMobile();
 
 	// abstract functions
@@ -96,16 +95,17 @@ public:
 	virtual void close(); // inherited classes should call its parent at the end of their close().
 	virtual std::string getSerial() const {return "CameraMobile";}
 
+	// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch), viewMatrix in opengl frame (without origin offset)
 	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;
 
 	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)
+	// original pose of device in rtabmap frame (without origin offset), stamp of the device (may be not epoch)
+	void poseReceived(const Transform & pose, double deviceStamp);
 	double getStampEpochOffset() const {return stampEpochOffset_;}
 
 	const CameraModel & getCameraModel() const {return model_;}
@@ -150,11 +150,17 @@ private:
 	EnvSensors lastEnvSensors_;
 	Transform originOffset_;
 	bool originUpdate_;
+	float upstreamRelocalizationAccThr_;
+	rtabmap::Transform previousAnchorPose_;
+	std::vector<float> previousAnchorLinearVelocity_;
+	double previousAnchorStamp_;
+    std::map<double, std::pair<rtabmap::Transform, rtabmap::Transform> > relocalizationDebugBuffer_;
 
 	USemaphore dataReady_;
 	UMutex dataMutex_;
 	SensorData data_;
 	Transform dataPose_;
+    bool dataGoodTracking_;
 
 	UMutex poseMutex_;
 	std::map<double, Transform> poseBuffer_; // <stamp, Pose>

app/android/jni/CameraTango.cpp

@@ -194,6 +194,8 @@ bool CameraTango::init(const std::string & calibrationFolder, const std::string
 {
 	close();
 
+	CameraMobile::init(calibrationFolder, cameraName);
+
 	TangoSupport_initialize(TangoService_getPoseAtTime, TangoService_getCameraIntrinsics);
 
 	// Connect to Tango
@@ -657,12 +659,6 @@ void CameraTango::cloudReceived(const cv::Mat & cloud, double timestamp)
 					//LOGD("tango    = %s", poseDevice.prettyPrint().c_str());
 					//LOGD("opengl(t)= %s", (opengl_world_T_tango_world * poseDevice).prettyPrint().c_str());
 
-					// adjust origin
-					if(!getOriginOffset().isNull())
-					{
-						odom = getOriginOffset() * odom;
-					}
-
 					// occlusion depth
 					if(!depth.empty())
 					{
@@ -832,10 +828,6 @@ SensorData CameraTango::updateDataOnRender(Transform & pose)
 					float cy = static_cast<float>(color_camera_intrinsics.cy);
 
 					viewMatrix_ = glm::make_mat4(matrix_transform.matrix);
-					if(!getOriginOffset().isNull())
-					{
-						viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
-					}
 
 					projectionMatrix_ = tango_gl::Camera::ProjectionMatrixForCameraIntrinsics(
 							image_width, image_height, fx, fy, cx, cy, 0.3, 50);

app/android/jni/RTABMapApp.cpp

@@ -73,9 +73,14 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
+#ifdef RTABMAP_PDAL
+#include <rtabmap/core/PDALWriter.h>
+#elif defined(RTABMAP_LIBLAS)
+#include <rtabmap/core/LASWriter.h>
+#endif
 
 #define LOW_RES_PIX 2
-//#define DEBUG_RENDERING_PERFORMANCE
+#define DEBUG_RENDERING_PERFORMANCE
 
 const int g_optMeshId = -100;
 
@@ -134,7 +139,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
+	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_ && !dataRecorderMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0"));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
@@ -188,10 +193,18 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded()));
 	if(dataRecorderMode_)
 	{
-		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1")));
-		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0"))); // deactivate rehearsal
-		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
-		uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true")));
+        // Example taken from https://github.com/introlab/rtabmap_ros/blob/master/rtabmap_launch/launch/data_recorder.launch
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), "1.0")); // deactivate rehearsal
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), "-1"));           // deactivate keypoints extraction
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "0"));      // deactivate global retrieval
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));    // deactivate local retrieval
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false"));    // don't create map labels
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMemoryThr(), "2"));         // keep the WM empty
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemSTMSize(), "1"));               // STM=1 -->
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), "false"));
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), "0"));
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), "0"));
+        uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true")));
 	}
 
 	return parameters;
@@ -231,6 +244,8 @@ RTABMapApp::RTABMapApp() :
 		renderingTextureDecimation_(4),
 		backgroundColor_(0.2f),
         depthConfidence_(2),
+        upstreamRelocalizationMaxAcc_(0.0f),
+		exportPointCloudFormat_("ply"),
 		dataRecorderMode_(false),
 		clearSceneOnNextRender_(false),
 		openingDatabase_(false),
@@ -307,12 +322,14 @@ void RTABMapApp::setupSwiftCallbacks(void * classPtr,
                                                               int,
                                                               float, float, float, float,
                                                               int, int,
-                                                              float, float, float, float, float, float))
+                                                              float, float, float, float, float, float),
+                                     void(*cameraInfoEventCallback)(void *, int, const char*, const char*))
 {
     swiftClassPtr_ = classPtr;
     progressionStatus_.setSwiftCallback(classPtr, progressCallback);
     swiftInitCallback = initCallback;
     swiftStatsUpdatedCallback = statsUpdatedCallback;
+    swiftCameraInfoEventCallback = cameraInfoEventCallback;
 }
 #endif
 
@@ -386,6 +403,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
     lastPostRenderEventTime_ = 0.0;
     lastPoseEventTime_ = 0.0;
     bufferedStatsData_.clear();
+    graphOptimization_ = true;
 
 	this->registerToEventsManager();
 
@@ -475,7 +493,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	rtabmap_ = new rtabmap::Rtabmap();
 	rtabmap::ParametersMap parameters = getRtabmapParameters();
 
-	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory)));
+    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory && !dataRecorderMode_)));
 	LOGI("Initializing database...");
 	rtabmap_->init(parameters, databasePath);
 	rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
@@ -745,8 +763,19 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 	{
 		boost::mutex::scoped_lock  lock(cameraMutex_);
 		if(camera_)
-		{
-			camera_->resetOrigin();
+        {
+            camera_->resetOrigin();
+            if(dataRecorderMode_)
+            {
+                // Don't update faster than we record, so that we see is what is recorded
+                camera_->setFrameRate(rtabmapThread_->getDetectorRate());
+                rtabmapThread_->setDetectorRate(0);
+            }
+            else
+            {
+                // set default 10
+                camera_->setFrameRate(10);
+            }
 		}
 	}
 
@@ -908,7 +937,7 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -916,14 +945,14 @@ bool RTABMapApp::startCamera()
 	else if(cameraDriver_ == 2)
 	{
 #ifdef RTABMAP_ARENGINE
-		camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_);
+		camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_, upstreamRelocalizationMaxAcc_);
 #else
 		UERROR("RTAB-Map is not built with AREngine support!");
 #endif
 	}
 	else if(cameraDriver_ == 3)
 	{
-		camera_ = new rtabmap::CameraMobile(smoothing_);
+		camera_ = new rtabmap::CameraMobile(smoothing_, upstreamRelocalizationMaxAcc_);
 	}
 
 	if(camera_ == 0)
@@ -931,6 +960,13 @@ bool RTABMapApp::startCamera()
 		UERROR("Unknown or not supported camera driver! %d", cameraDriver_);
 		return false;
 	}
+    
+    if(rtabmapThread_ && dataRecorderMode_)
+    {
+        // Don't update faster than we record, so that we see is what is recorded
+        camera_->setFrameRate(rtabmapThread_->getDetectorRate());
+        rtabmapThread_->setDetectorRate(0);
+    }
 
 	if(camera_->init())
 	{
@@ -967,6 +1003,7 @@ void RTABMapApp::stopCamera()
 		boost::mutex::scoped_lock  lock(cameraMutex_);
 		if(sensorCaptureThread_!=0)
 		{
+            camera_->close();
 			sensorCaptureThread_->join(true);
 			delete sensorCaptureThread_; // camera_ is closed and deleted inside
 			sensorCaptureThread_ = 0;
@@ -1291,7 +1328,7 @@ int RTABMapApp::Render()
 					camera_->updateOnRender();
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Camera updateOnRender %fs", time.ticks());
+				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
 				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
 				{
@@ -1308,7 +1345,7 @@ int RTABMapApp::Render()
 						arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
 					}
 				}
-				if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+				if(!visualizingMesh_ && !dataRecorderMode_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
 				{
 					rtabmap::CameraModel occlusionModel;
 					cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
@@ -1330,7 +1367,7 @@ int RTABMapApp::Render()
 					}
 				}
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Update background and occlusion mesh %fs", time.ticks());
+				LOGD("Update background and occlusion mesh %fs", time.ticks());
 #endif
 			}
 		}
@@ -1746,7 +1783,7 @@ int RTABMapApp::Render()
 				// Transform pose in OpenGL world
 				for(std::map<int, rtabmap::Transform>::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter)
 				{
-					if(!graphOptimization_)
+					if(!graphOptimization_ && !dataRecorderMode_)
 					{
 						std::map<int, rtabmap::Transform>::iterator jter = rawPoses_.find(iter->first);
 						if(jter != rawPoses_.end())
@@ -2014,7 +2051,8 @@ int RTABMapApp::Render()
 					}
 				}
 			}
-			else
+			
+            if(dataRecorderMode_ || !rtabmapEvents.size())
 			{
 				main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
 
@@ -2417,6 +2455,11 @@ void RTABMapApp::setAppendMode(bool enabled)
 	}
 }
 
+void RTABMapApp::setUpstreamRelocalizationAccThr(float value)
+{
+    upstreamRelocalizationMaxAcc_ = value;
+}
+
 void RTABMapApp::setDataRecorderMode(bool enabled)
 {
 	if(dataRecorderMode_ != enabled)
@@ -2492,6 +2535,22 @@ void RTABMapApp::setDepthConfidence(int value)
     }
 }
 
+void RTABMapApp::setExportPointCloudFormat(const std::string & format)
+{
+#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
+    if(format == "las") {
+        exportPointCloudFormat_ = format;
+    }
+    else
+#endif
+    if(format != "ply") {
+        UERROR("Not supported point cloud format %s", format.c_str());
+    }
+    else {
+        exportPointCloudFormat_ = format;
+    }
+}
+
 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
 {
 	std::string compatibleKey = key;
@@ -2599,11 +2658,14 @@ void RTABMapApp::save(const std::string & databasePath)
     std::multimap<int, rtabmap::Link> links = rtabmap_->getLocalConstraints();
 	rtabmap_->close(true, databasePath);
 	rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath);
-	rtabmap_->setOptimizedPoses(poses, links);
 	if(dataRecorderMode_)
 	{
 		clearSceneOnNextRender_ = true;
 	}
+    else
+    {
+        rtabmap_->setOptimizedPoses(poses, links);
+    }
 }
 
 bool RTABMapApp::recover(const std::string & from, const std::string & to)
@@ -3543,18 +3605,43 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 
 	if(polygonMesh->cloud.data.size())
 	{
-		// Point cloud PLY
-		std::string filePath = directory + UDirectory::separator() + name + ".ply";
-		LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, (int)polygonMesh->polygons.size(), filePath.c_str());
-		success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0;
-		if(success)
-		{
-			LOGI("Saved ply to %s!", filePath.c_str());
-		}
-		else
-		{
-			UERROR("Failed saving ply to %s!", filePath.c_str());
-		}
+#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
+        if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") {
+            // Point cloud LAS
+            std::string filePath = directory + UDirectory::separator() + name + ".las";
+            LOGI("Saving las (%d vertices) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, filePath.c_str());
+            pcl::PointCloud<pcl::PointXYZRGB> output;
+            pcl::fromPCLPointCloud2(polygonMesh->cloud, output);
+#ifdef RTABMAP_PDAL
+            success = rtabmap::savePDALFile(filePath, output) == 0;
+#else
+            success = rtabmap::saveLASFile(filePath, output) == 0;
+#endif
+            if(success)
+            {
+                LOGI("Saved las to %s!", filePath.c_str());
+            }
+            else
+            {
+                UERROR("Failed saving las to %s!", filePath.c_str());
+            }
+        }
+        else
+#endif
+        {
+            // Point cloud PLY
+            std::string filePath = directory + UDirectory::separator() + name + ".ply";
+            LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, (int)polygonMesh->polygons.size(), 	filePath.c_str());
+            success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0;
+            if(success)
+            {
+                LOGI("Saved ply to %s!", filePath.c_str());
+            }
+            else
+            {
+                UERROR("Failed saving ply to %s!", filePath.c_str());
+            }
+        }
 	}
 	else if(textureMesh->cloud.data.size())
 	{
@@ -3712,24 +3799,6 @@ int RTABMapApp::postProcessing(int approach)
 	return returnedValue;
 }
 
-void RTABMapApp::postCameraPoseEvent(
-		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
-{
-	boost::mutex::scoped_lock  lock(cameraMutex_);
-	if(cameraDriver_ == 3 && camera_)
-	{
-		if(qx==0 && qy==0 && qz==0 && qw==0)
-		{
-			// Lost! clear buffer
-			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, stamp);
-	}
-}
-
 void RTABMapApp::postOdometryEvent(
 		rtabmap::Transform pose,
 		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
@@ -3742,7 +3811,7 @@ void RTABMapApp::postOdometryEvent(
         const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
         const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
         const float * points, int pointsLen, int pointsChannels,
-        const rtabmap::Transform & viewMatrix,
+        rtabmap::Transform viewMatrix,
         float p00, float p11, float p02, float p12, float p22, float p32, float p23,
         float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
 {
@@ -3750,6 +3819,12 @@ void RTABMapApp::postOdometryEvent(
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(cameraDriver_ == 3 && camera_)
 	{
+        if(pose.isNull())
+        {
+            // We are lost, trigger a new map on next update
+            camera_->resetOrigin();
+            return;
+        }
 		if(rgb_fx > 0.0f && rgb_fy > 0.0f && rgb_cx > 0.0f && rgb_cy > 0.0f && stamp > 0.0f && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight)
 		{
 #ifndef DISABLE_LOG
@@ -3760,7 +3835,7 @@ void RTABMapApp::postOdometryEvent(
 			   (depth==0 || depthFormat == AIMAGE_FORMAT_DEPTH16))
 #else //__APPLE__
             if(rgbFormat == 875704422 &&
-               (depth==0 || depthFormat == 1717855600))
+               (depth==0 || depthFormat == 1717855600))
 #endif
 			{
 				cv::Mat outputRGB;
@@ -3836,13 +3911,11 @@ void RTABMapApp::postOdometryEvent(
 
 				if(!outputRGB.empty())
 				{
+                    // Convert in our coordinate frame
 					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;
-					}
+					// We should update the pose before querying poses for depth below (if not same stamp than rgb)
+					camera_->poseReceived(pose, stamp);
 
 					// Registration depth to rgb
 					if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
@@ -3852,19 +3925,24 @@ void RTABMapApp::postOdometryEvent(
 						if(depthStamp != stamp)
 						{
 							// Interpolate pose
+							rtabmap::Transform poseRgb;
 							rtabmap::Transform poseDepth;
 							cv::Mat cov;
-							if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
+							if(!camera_->getPose(camera_->getStampEpochOffset()+stamp, poseRgb, cov, 0.0))
 							{
-								UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
+								UERROR("Could not find pose at rgb stamp %f (epoch %f)!", stamp, camera_->getStampEpochOffset()+stamp);
+							}
+							else if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
+							{
+								UERROR("Could not find pose at depth stamp %f (epoch %f) last rgb is %f!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
 							}
 							else
 							{
 #ifndef DISABLE_LOG
-								UDEBUG("poseRGB  =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
+								UDEBUG("poseRGB  =%s (stamp=%f)", poseRgb.prettyPrint().c_str(), stamp);
 								UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
 #endif
-								motion = poseWithOriginOffset.inverse()*poseDepth;
+								motion = poseRgb.inverse()*poseDepth;
 								// transform in camera frame
 #ifndef DISABLE_LOG
 								UDEBUG("motion=%s", motion.prettyPrint().c_str());
@@ -3910,19 +3988,19 @@ void RTABMapApp::postOdometryEvent(
 						if(outputDepth.empty())
 						{
                             int kptsSize = fullResolution_ ? 12 : 6;
-							scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB, &kpts, &kpts3, kptsSize);
+							scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pose, model, outputRGB, &kpts, &kpts3, kptsSize);
 						}
 						else
 						{
 							// We will recompute features if depth is available
-							scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB);
+							scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pose, model, outputRGB);
 						}
 					}
                     
                     if(!outputDepth.empty())
                     {
                         rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
-                        depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
+                        depthModel.setLocalTransform(pose*model.localTransform());
                         camera_->setOcclusionImage(outputDepth, depthModel);
                     }
                     
@@ -4031,6 +4109,15 @@ bool RTABMapApp::handleEvent(UEvent * event)
 			}
 			jvm->DetachCurrentThread();
 		}
+#else
+        if(swiftClassPtr_)
+        {
+            std::function<void()> actualCallback = [&](){
+                swiftCameraInfoEventCallback(swiftClassPtr_, tangoEvent->type(), tangoEvent->key().c_str(), tangoEvent->value().c_str());
+            };
+            actualCallback();
+            success = true;
+        }
 #endif
 		if(!success)
 		{

app/android/jni/RTABMapApp.h

@@ -70,7 +70,8 @@ class RTABMapApp : public UEventsHandler {
                                                     int,
                                                     float, float, float, float,
                                                     int, int,
-                                                    float, float, float, float, float, float));
+                                                    float, float, float, float, float, float),
+                           void(*cameraInfoCallback)(void *, int, const char*, const char*));
     
 #endif
   ~RTABMapApp();
@@ -138,6 +139,7 @@ class RTABMapApp : public UEventsHandler {
   void setSmoothing(bool enabled);
   void setDepthFromMotion(bool enabled);
   void setAppendMode(bool enabled);
+  void setUpstreamRelocalizationAccThr(float value);
   void setDataRecorderMode(bool enabled);
   void setMaxCloudDepth(float value);
   void setMinCloudDepth(float value);
@@ -150,6 +152,7 @@ class RTABMapApp : public UEventsHandler {
   void setRenderingTextureDecimation(int value);
   void setBackgroundColor(float gray);
   void setDepthConfidence(int value);
+  void setExportPointCloudFormat(const std::string & format);
   int setMappingParameter(const std::string & key, const std::string & value);
   void setGPS(const rtabmap::GPS & gps);
   void addEnvSensor(int type, float value);
@@ -178,9 +181,6 @@ class RTABMapApp : public UEventsHandler {
   bool writeExportedMesh(const std::string & directory, const std::string & name);
   int postProcessing(int approach);
 
-  void postCameraPoseEvent(
-  		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
-
   void postOdometryEvent(
 		rtabmap::Transform pose,
 		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
@@ -193,7 +193,7 @@ class RTABMapApp : public UEventsHandler {
         const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
         const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
         const float * points, int pointsLen, int pointsChannels,
-		const rtabmap::Transform & viewMatrix, //view matrix
+		rtabmap::Transform viewMatrix, //view matrix
         float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
         float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
 
@@ -239,6 +239,8 @@ class RTABMapApp : public UEventsHandler {
   int renderingTextureDecimation_;
   float backgroundColor_;
   int depthConfidence_;
+  float upstreamRelocalizationMaxAcc_;
+  std::string exportPointCloudFormat_;
 
   rtabmap::ParametersMap mappingParameters_;
 
@@ -309,6 +311,7 @@ class RTABMapApp : public UEventsHandler {
                              float, float, float, float,
                              int, int,
                              float, float, float, float, float, float);
+    void(*swiftCameraInfoEventCallback)(void *, int, const char *, const char *);
     
 #endif
 };

app/android/jni/jni_interface.cpp

@@ -550,6 +550,20 @@ Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
 		UERROR("native_application is null!");
 	}
 }
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setUpstreamRelocalizationAccThr(
+		JNIEnv*, jclass, jlong native_application, float value)
+{
+	if(native_application)
+	{
+		return native(native_application)->setUpstreamRelocalizationAccThr(value);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
@@ -866,22 +880,6 @@ Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 	}
 }
 
-JNIEXPORT void JNICALL
-Java_com_introlab_rtabmap_RTABMapLib_postCameraPoseEvent(
-		JNIEnv* env, jclass, jlong native_application,
-		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
-{
-	if(native_application)
-	{
-		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw, stamp);
-	}
-	else
-	{
-		UERROR("native_application is null!");
-		return;
-	}
-}
-
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 		JNIEnv* env, jclass, jlong native_application,

app/android/res/layout/activity_settings.xml

@@ -103,12 +103,12 @@
                 android:summary="@string/pref_summary_depth_from_motion"
                 android:defaultValue="@string/pref_default_depth_from_motion"/>
              <ListPreference
-		        android:key="@string/pref_key_arcore_localization_filtering_speed"
-		        android:title="@string/pref_title_arcore_localization_filtering_speed"
-		        android:summary="@string/pref_summary_arcore_localization_filtering_speed"
-		        android:entries="@array/pref_arcore_localization_filtering_speed_keys"
-		   	    android:entryValues="@array/pref_arcore_localization_filtering_speed_values"
-		   		android:defaultValue="@string/pref_default_arcore_localization_filtering_speed"/>
+		        android:key="@string/pref_key_arcore_relocalization_acc_thr"
+		        android:title="@string/pref_title_arcore_relocalization_acc_thr"
+		        android:summary="@string/pref_summary_arcore_relocalization_acc_thr"
+		        android:entries="@array/pref_arcore_relocalization_acc_thr_keys"
+		   	    android:entryValues="@array/pref_arcore_relocalization_acc_thr_values"
+		   		android:defaultValue="@string/pref_default_arcore_relocalization_acc_thr"/>
 		    <com.introlab.rtabmap.CustomSwitchPreference
 		        android:key="@string/pref_key_smoothing"
 		        android:title="@string/pref_title_smoothing"

app/android/res/values/strings.xml

@@ -83,8 +83,8 @@
     <string name="pref_default_camera_driver">-1</string>
     <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">0</string>
+    <string name="pref_key_arcore_relocalization_acc_thr">pref_key_arcore_relocalization_acc_thr</string>
+    <string name="pref_default_arcore_relocalization_acc_thr">58.8399</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>
@@ -100,7 +100,7 @@
     <string name="pref_key_min_inliers">pref_key_min_inliers</string>
     <string name="pref_default_min_inliers">25</string>
     <string name="pref_key_opt_error">pref_key_opt_error</string>
-    <string name="pref_default_opt_error">1</string>
+    <string name="pref_default_opt_error">2</string>
     <string name="pref_key_features_voc">pref_key_features_voc</string>
     <string name="pref_default_features_voc">200</string>
     <string name="pref_key_features">pref_key_features</string>
@@ -329,8 +329,8 @@
     <string name="pref_summary_camera_driver">AR sdk used for capturing 6DoF poses and images. A TOF camera is required to record a 3D model.</string>
     <string name="pref_title_depth_from_motion">Depth From Motion</string>
     <string name="pref_summary_depth_from_motion">Use ARCore\'s depth API to compute depth image from motion. If the phone has a TOF camera and is supported by ARCore, results should be better. Currently supported only with ARCore NDK driver.</string>
-    <string name="pref_title_arcore_localization_filtering_speed">ARCore Localization Filtering Speed</string>
-    <string name="pref_summary_arcore_localization_filtering_speed">Filter ARCore\'s localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set a speed threshold to detect those events.</string>
+    <string name="pref_title_arcore_relocalization_acc_thr">ARCore Re-Localization Acceleration Threshold</string>
+    <string name="pref_summary_arcore_relocalization_acc_thr">Filter ARCore\'s re-localizations to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events.</string>
     <string name="pref_title_append">Append Mode</string>
     <string name="pref_summary_append">When resuming mapping, wait for a relocalization on the current map before starting a new map.</string>
     <string name="pref_title_resolution">HD Mode</string>
@@ -412,23 +412,29 @@
         <item>"1"</item>
         <item>"0.5"</item>
     </string-array>
-    <string-array name="pref_arcore_localization_filtering_speed_keys">
+    <string-array name="pref_arcore_relocalization_acc_thr_keys">
         <item>"Disabled"</item>
-        <item>"5 m/s"</item>
-        <item>"4 m/s"</item>
-        <item>"3 m/s"</item>
-        <item>"2 m/s"</item>
-        <item>"1 m/s"</item>
-        <item>"0.5 m/s"</item>
+        <item>"10 g"</item>
+        <item>"9 g"</item>
+        <item>"8 g"</item>
+        <item>"7 g"</item>
+        <item>"6 g"</item>
+        <item>"5 g"</item>
+        <item>"4 g"</item>
+        <item>"3 g"</item>
+        <item>"2 g"</item>
     </string-array>
-    <string-array name="pref_arcore_localization_filtering_speed_values">
+    <string-array name="pref_arcore_relocalization_acc_thr_values">
         <item>"0"</item>
-        <item>"5"</item>
-        <item>"4"</item>
-        <item>"3"</item>
-        <item>"2"</item>
-        <item>"1"</item>
-        <item>"0.5"</item>
+        <item>"98.0665"</item>
+        <item>"88.25985"</item>
+        <item>"78.4532"</item>
+        <item>"68.64655"</item>
+        <item>"58.8399"</item>
+        <item>"49.03325"</item>
+        <item>"39.2266"</item>
+        <item>"29.41995"</item>
+        <item>"19.6133"</item>
     </string-array>
     <string-array name="pref_max_speed_keys">
         <item>"No Limit"</item>

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

@@ -56,9 +56,8 @@ public class ARCoreSharedCamera {
       };
 	
 	private static RTABMapActivity mActivity;
-	public ARCoreSharedCamera(RTABMapActivity c, float arCoreLocalizationFilteringSpeed) {
+	public ARCoreSharedCamera(RTABMapActivity c) {
 		mActivity = c;
-		mARCoreLocalizationFilteringSpeed = arCoreLocalizationFilteringSpeed;
 	}
 
 	// Depth TOF Image.
@@ -72,11 +71,7 @@ public class ARCoreSharedCamera {
 	// ARCore session that supports camera sharing.
 	private Session sharedSession;
 	
-	private Pose previousAnchorPose = null;
-	private long previousAnchorTimeStamp;
-	private Pose arCoreCorrection = Pose.IDENTITY;
 	private Pose odomPose = Pose.IDENTITY;
-	private float mARCoreLocalizationFilteringSpeed = 1.0f;
 
 	// Camera capture session. Used by both non-AR and AR modes.
 	private CameraCaptureSession captureSession;
@@ -111,6 +106,7 @@ public class ARCoreSharedCamera {
 	private CaptureRequest.Builder previewCaptureRequestBuilder;
 
 	private int cameraTextureId = -1;
+	private boolean firstFrameReceived = false;
 
 	// Image reader that continuously processes CPU images.
 	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
@@ -118,6 +114,8 @@ public class ARCoreSharedCamera {
 
 	ByteBuffer mPreviousDepth = null;
 	double mPreviousDepthStamp = 0.0;
+	ByteBuffer mPreviousDepth2 = null;
+	double mPreviousDepthStamp2 = 0.0;
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
@@ -633,6 +631,7 @@ public class ARCoreSharedCamera {
 		}
 		
 		mTOFImageReader.close();
+		firstFrameReceived = false;
 
 		if(cameraTextureId>=0)
 		{
@@ -681,14 +680,15 @@ public class ARCoreSharedCamera {
 			Log.e(TAG, String.format("camera is null!"));
 			return;
 		}
+		boolean lost = false;
 		if (camera.getTrackingState() != TrackingState.TRACKING) {
 			final String trackingState = camera.getTrackingState().toString();
 			Log.e(TAG, String.format("Tracking lost! state=%s", trackingState));
 			// This will force a new session on the next frame received
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, 0,0,0,0,0,0,0,0);
+			lost = true;
 			mActivity.runOnUiThread(new Runnable() {
 				public void run() {
-					if(mToast!=null && previousAnchorPose != null)
+					if(mToast!=null && firstFrameReceived)
 					{
 						String msg = "Tracking lost! If you are mapping, you will need to relocalize before continuing.";
 						if(mToast.getView() == null || !mToast.getView().isShown())
@@ -700,67 +700,14 @@ public class ARCoreSharedCamera {
 						{
 							mToast.setText(msg);
 						}
-						previousAnchorPose = null;
-						arCoreCorrection = Pose.IDENTITY;
 					}
 				}
 			});
-			return;
 		}
 
 		if (frame.getTimestamp() != 0) {
 			Pose pose = camera.getPose();
-			
-			// Remove ARCore SLAM corrections by integrating pose from previous frame anchor
-			if(previousAnchorPose == null || mARCoreLocalizationFilteringSpeed==0)
-			{
-				odomPose = pose;
-			}
-			else
-			{
-				float[] t = previousAnchorPose.inverse().compose(pose).getTranslation();
-				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)
-				{
-					// 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]));
-					
-					t = odomPose.getTranslation();
-					float[] t2 = (arCoreCorrection.compose(pose)).getTranslation();
-					float[] t3 = previousAnchorPose.getTranslation();
-					float[] t4 = pose.getTranslation();
-					Log.e(TAG, String.format("Odom = %f %f %f -> %f %f %f ArCore= %f %f %f -> %f %f %f", t[0], t[1], t[2], t2[0], t2[1], t2[2], t3[0], t3[1], t3[2], t4[0], t4[1], t4[2]));
-				
-					mActivity.runOnUiThread(new Runnable() {
-						public void run() {
-							if(mToast!=null)
-							{
-								String msg = String.format("ARCore localization has been suppressed "
-										+ "because of high speed detected (%f m/s) causing a jump! You can change "
-										+ "ARCore localization filtering speed in Settings->Mapping if you are "
-										+ "indeed moving as fast.", speed);
-								if(mToast.getView() == null || !mToast.getView().isShown())
-								{
-									mToast.makeText(mActivity.getApplicationContext(), msg, Toast.LENGTH_LONG).show();
-								}
-								else
-								{
-									mToast.setText(msg);
-								}
-							}
-						}
-					});
-				}
-				odomPose = arCoreCorrection.compose(pose);
-			}
-			previousAnchorPose = pose;
-			previousAnchorTimeStamp = frame.getTimestamp();
-			
 			double stamp = (double)frame.getTimestamp()/10e8;
-			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();
@@ -803,7 +750,7 @@ public class ARCoreSharedCamera {
 		        camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
    
 		        float[] viewMatrix = new float[16];
-		        arCoreCorrection.compose(camera.getDisplayOrientedPose()).inverse().toMatrix(viewMatrix, 0);
+		        camera.getDisplayOrientedPose().inverse().toMatrix(viewMatrix, 0);
 		        float[] quat = new float[4];
 		        rotationMatrixToQuaternion(viewMatrix, quat);
 
@@ -822,35 +769,47 @@ public class ARCoreSharedCamera {
 						mPreviousDepth = depth;
 						mPreviousDepthStamp = depthStamp;
 					}
+					if(mPreviousDepth2 == null)
+					{
+						mPreviousDepth2 = depth;
+						mPreviousDepthStamp2 = 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));
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f previous=%f rgb=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp, mPreviousDepthStamp, stamp));
 
 					RTABMapLib.postOdometryEventDepth(
 							RTABMapActivity.nativeApplication,
-							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
+							lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(), 
 							fl[0], fl[1], pp[0], pp[1], 
 							depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
 							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>stamp?mPreviousDepthStamp:depthStamp,
+							depthStamp<=stamp?depthStamp:mPreviousDepthStamp<=stamp?mPreviousDepthStamp:mPreviousDepthStamp2,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							depthStamp>stamp?mPreviousDepth:depth, depthStamp>stamp?mPreviousDepth.limit():depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depthStamp<=stamp?depth:mPreviousDepthStamp<=stamp?mPreviousDepth:mPreviousDepth2,
+							depthStamp<=stamp?depth.limit():mPreviousDepthStamp<=stamp?mPreviousDepth.limit():mPreviousDepth2.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;
+					// triple buffer in case delay of rgb frame is > 60 ms
+					if(depthStamp != mPreviousDepthStamp)
+					{
+						mPreviousDepthStamp2 = mPreviousDepthStamp;
+						mPreviousDepth2 = mPreviousDepth;
+						mPreviousDepthStamp = depthStamp;
+						mPreviousDepth = depth;
+					}
 				}
 				else
 				{
 					RTABMapLib.postOdometryEvent(
 							RTABMapActivity.nativeApplication,
-							odomPose.tx(), odomPose.ty(), odomPose.tz(), odomPose.qx(), odomPose.qy(), odomPose.qz(), odomPose.qw(), 
+							lost?0:pose.tx(), lost?0:pose.ty(), lost?0:pose.tz(), lost?0:pose.qx(), lost?0:pose.qy(), lost?0:pose.qz(), lost?0:pose.qw(), 
 							fl[0], fl[1], pp[0], pp[1], 
 							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
 							stamp, 
@@ -861,6 +820,7 @@ public class ARCoreSharedCamera {
                             texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
 				}
 				
+				firstFrameReceived = !lost;
 				image.close();
 				cloud.close();
 				

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

@@ -323,6 +323,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		// touch point.
 		Display display = getWindowManager().getDefaultDisplay();
 		display.getSize(mScreenSize);
+		Log.i(TAG, String.format("Screen resolution: %dx%d", mScreenSize.x, mScreenSize.y));
 
 		getWindow().addFlags(WindowManager.LayoutParams.FLAG_KEEP_SCREEN_ON);
 		getWindow().addFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_STATUS);
@@ -1142,6 +1143,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			RTABMapLib.setDepthFromMotion(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_depth_from_motion), Boolean.parseBoolean(getString(R.string.pref_default_depth_from_motion))));
 			RTABMapLib.setCameraColor(nativeApplication, !sharedPref.getBoolean(getString(R.string.pref_key_fisheye), Boolean.parseBoolean(getString(R.string.pref_default_fisheye))));
 			RTABMapLib.setAppendMode(nativeApplication, sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append))));
+			RTABMapLib.setUpstreamRelocalizationAccThr(nativeApplication, Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_arcore_relocalization_acc_thr), getString(R.string.pref_default_arcore_relocalization_acc_thr))));
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/DetectionRate", mUpdateRate);
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/TimeThr", mTimeThr);
 			RTABMapLib.setMappingParameter(nativeApplication, "Rtabmap/MemoryThr", memThr);
@@ -1383,9 +1385,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							if(cameraStartSucess && mCameraDriver == 3)
 							{
 								synchronized (this) {
-									SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity());
-									String arCoreLocalizationFiltering = sharedPref.getString(getString(R.string.pref_key_arcore_localization_filtering_speed), getString(R.string.pref_default_arcore_localization_filtering_speed));
-									mArCoreCamera = new ARCoreSharedCamera(getActivity(), Float.parseFloat(arCoreLocalizationFiltering));
+									mArCoreCamera = new ARCoreSharedCamera(getActivity());
 									mArCoreCamera.setToast(mToast);
 									mProgressDialog.setTitle("");
 									mProgressDialog.setMessage(message);
@@ -1775,16 +1775,19 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			long currentTime = System.currentTimeMillis()/1000;
 			if(loopClosureId > 0)
 			{
+				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
 				mToast.setText(String.format("Loop closure detected! (%d/%d inliers)", inliers, matches));
 				mToast.show();
 			}
 			else if(landmarkDetected != 0)
 			{
+				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
 				mToast.setText(String.format("Marker %d detected!", landmarkDetected));
 				mToast.show();
 			}
 			else if(rejected > 0)
 			{
+				if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
 				if(inliers >= Integer.parseInt(mMinInliers))
 				{
 					if(optimizationMaxError > 0.0f)
@@ -1806,6 +1809,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 			{
 				if(currentTime - mLastFastMovementNotificationStamp > 3)
 				{
+					if (mToast != null  && mToast.getView().isShown()) mToast.cancel();
 					mToast.setText("Move slower... blurry images are not added to map (\"Settings->Mapping...->Maximum Motion Speed\" is enabled).");
 					mToast.show();
 				}
@@ -2057,7 +2061,14 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		 * "Unknown"
 		 */
 		String str = null;
-		if(key.equals("TangoServiceException"))
+		if(key.equals("UpstreamRelocationFiltered"))
+		{
+			if(mItemDebugVisibility != null && mItemDebugVisibility.isChecked()) {
+				str = String.format("%s re-localization filtered because an acceleration of %s has been detected, which is over current threshold set in the settings.",
+					mCameraDriver == 2?"AREngine":"ARCore", value);
+			}
+		}
+		else if(key.equals("TangoServiceException"))
 			str = String.format("Tango service exception: %s", value);
 		else if(key.equals("FisheyeOverExposed"))
 			;//str = String.format("The fisheye image is over exposed with average pixel value %s px.", value);
@@ -3024,7 +3035,6 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 		}
 		else
 		{
-		
 			RTABMapLib.openDatabase(nativeApplication, tmpDatabase, databaseInMemory, false, true);
 	
 			if(!(mState == State.STATE_CAMERA || mState ==State.STATE_MAPPING))

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

@@ -76,6 +76,7 @@ public class RTABMapLib
     public static native void setDepthFromMotion(long nativeApplication, boolean enabled);
     public static native void setCameraColor(long nativeApplication, boolean enabled);
     public static native void setAppendMode(long nativeApplication, boolean enabled);
+    public static native void setUpstreamRelocalizationAccThr(long nativeApplication, float value);
     public static native void setDataRecorderMode(long nativeApplication, boolean enabled);
     public static native void setMaxCloudDepth(long nativeApplication, float value);
     public static native void setMinCloudDepth(long nativeApplication, float value);
@@ -135,7 +136,6 @@ public class RTABMapLib
     public static native float getUpdateTime(long nativeApplication);
     public static native int getLoopClosureId(long nativeApplication);
     
-    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
     public static native void postOdometryEvent(long nativeApplication,
     		float x, float y, float z, float qx, float qy, float qz, float qw,
     		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,

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

@@ -222,7 +222,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         ((Preference)findPreference(getString(R.string.pref_key_background_color))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_background_color))).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         ((Preference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_rendering_texture_decimation))).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
           
-        ((Preference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_localization_filtering_speed))).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
+        ((Preference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_arcore_relocalization_acc_thr))).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         ((Preference)findPreference(getString(R.string.pref_key_max_speed))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_speed))).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr));
@@ -285,7 +285,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref
         	if(key.compareTo(getString(R.string.pref_key_background_color))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_background_color));
         	if(key.compareTo(getString(R.string.pref_key_rendering_texture_decimation))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_rendering_texture_decimation));
         	
-        	if(key.compareTo(getString(R.string.pref_key_arcore_localization_filtering_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_localization_filtering_speed));
+        	if(key.compareTo(getString(R.string.pref_key_arcore_relocalization_acc_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_arcore_relocalization_acc_thr));
         	if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate));
         	if(key.compareTo(getString(R.string.pref_key_max_speed))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_speed));
         	if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr));

app/ios/RTABMapApp.xcodeproj/project.pbxproj

@@ -3,7 +3,7 @@
 	archiveVersion = 1;
 	classes = {
 	};
-	objectVersion = 52;
+	objectVersion = 54;
 	objects = {
 
 /* Begin PBXBuildFile section */
@@ -20,6 +20,7 @@
 		4EE016C3259BE464008CCE65 /* Main.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016BF259BE464008CCE65 /* Main.storyboard */; };
 		4EE016C4259BE464008CCE65 /* LaunchScreen.storyboard in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C1259BE464008CCE65 /* LaunchScreen.storyboard */; };
 		4EE016C7259BE46F008CCE65 /* Assets.xcassets in Resources */ = {isa = PBXBuildFile; fileRef = 4EE016C6259BE46F008CCE65 /* Assets.xcassets */; };
+		4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFAA9422CAE4E960055DA51 /* liblas.a */; };
 		4EFD0B36259D4DE900575D88 /* libboost_filesystem.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */; };
 		4EFD0B37259D4DE900575D88 /* libboost_program_options.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */; };
 		4EFD0B38259D4DE900575D88 /* libboost_regex.a in Frameworks */ = {isa = PBXBuildFile; fileRef = 4EFD0B2D259D4DE900575D88 /* libboost_regex.a */; };
@@ -170,6 +171,7 @@
 		4EE016C2259BE464008CCE65 /* Base */ = {isa = PBXFileReference; lastKnownFileType = file.storyboard; name = Base; path = RTABMapApp/Base.lproj/LaunchScreen.storyboard; sourceTree = "<group>"; };
 		4EE016C6259BE46F008CCE65 /* Assets.xcassets */ = {isa = PBXFileReference; lastKnownFileType = folder.assetcatalog; name = Assets.xcassets; path = RTABMapApp/Assets.xcassets; sourceTree = "<group>"; };
 		4EE016E1259BE96A008CCE65 /* RTABMapApp-Bridging-Header.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; name = "RTABMapApp-Bridging-Header.h"; path = "RTABMapApp/RTABMapApp-Bridging-Header.h"; sourceTree = "<group>"; };
+		4EFAA9422CAE4E960055DA51 /* liblas.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = liblas.a; path = RTABMapApp/Libraries/lib/liblas.a; sourceTree = "<group>"; };
 		4EFD0B2B259D4DE900575D88 /* libboost_filesystem.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_filesystem.a; path = RTABMapApp/Libraries/lib/libboost_filesystem.a; sourceTree = "<group>"; };
 		4EFD0B2C259D4DE900575D88 /* libboost_program_options.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_program_options.a; path = RTABMapApp/Libraries/lib/libboost_program_options.a; sourceTree = "<group>"; };
 		4EFD0B2D259D4DE900575D88 /* libboost_regex.a */ = {isa = PBXFileReference; lastKnownFileType = archive.ar; name = libboost_regex.a; path = RTABMapApp/Libraries/lib/libboost_regex.a; sourceTree = "<group>"; };
@@ -343,6 +345,7 @@
 			isa = PBXFrameworksBuildPhase;
 			buildActionMask = 2147483647;
 			files = (
+				4EFAA9432CAE4E960055DA51 /* liblas.a in Frameworks */,
 				4EFD0F50259D67D900575D88 /* liblibwebp.a in Frameworks */,
 				4EFD0F51259D67D900575D88 /* liblibpng.a in Frameworks */,
 				4EFD0F52259D67D900575D88 /* liblibjpeg-turbo.a in Frameworks */,
@@ -493,6 +496,7 @@
 		4EFD0B2A259D4DE900575D88 /* Frameworks */ = {
 			isa = PBXGroup;
 			children = (
+				4EFAA9422CAE4E960055DA51 /* liblas.a */,
 				4EFD0F4D259D67D900575D88 /* liblibjpeg-turbo.a */,
 				4EFD0F4C259D67D900575D88 /* liblibpng.a */,
 				4EFD0F4F259D67D900575D88 /* liblibprotobuf.a */,
@@ -957,6 +961,7 @@
 				ENABLE_STRICT_OBJC_MSGSEND = YES;
 				GCC_C_LANGUAGE_STANDARD = gnu11;
 				GCC_NO_COMMON_BLOCKS = YES;
+				GCC_PREPROCESSOR_DEFINITIONS = "";
 				GCC_WARN_64_TO_32_BIT_CONVERSION = YES;
 				GCC_WARN_ABOUT_RETURN_TYPE = YES_ERROR;
 				GCC_WARN_UNDECLARED_SELECTOR = YES;
@@ -967,6 +972,7 @@
 				MTL_ENABLE_DEBUG_INFO = NO;
 				MTL_FAST_MATH = YES;
 				SDKROOT = iphoneos;
+				SWIFT_ACTIVE_COMPILATION_CONDITIONS = "";
 				SWIFT_COMPILATION_MODE = wholemodule;
 				SWIFT_OPTIMIZATION_LEVEL = "-O";
 				VALIDATE_PRODUCT = YES;
@@ -1007,7 +1013,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.0;
+				MARKETING_VERSION = 0.21.7;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;
 				PRODUCT_NAME = "$(TARGET_NAME)";
@@ -1064,7 +1070,7 @@
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/lib",
 					"$(PROJECT_DIR)/RTABMapApp/Libraries/share/OpenCV/3rdparty/lib",
 				);
-				MARKETING_VERSION = 0.21.0;
+				MARKETING_VERSION = 0.21.7;
 				ONLY_ACTIVE_ARCH = YES;
 				OTHER_CFLAGS = "";
 				PRODUCT_BUNDLE_IDENTIFIER = com.introlab.rtabmap;

app/ios/RTABMapApp.xcodeproj/xcshareddata/xcschemes/RTABMapApp.xcscheme

@@ -23,7 +23,7 @@
       </BuildActionEntries>
    </BuildAction>
    <TestAction
-      buildConfiguration = "Debug"
+      buildConfiguration = "Release"
       selectedDebuggerIdentifier = "Xcode.DebuggerFoundation.Debugger.LLDB"
       selectedLauncherIdentifier = "Xcode.DebuggerFoundation.Launcher.LLDB"
       shouldUseLaunchSchemeArgsEnv = "YES">
@@ -51,6 +51,13 @@
             ReferencedContainer = "container:RTABMapApp.xcodeproj">
          </BuildableReference>
       </BuildableProductRunnable>
+      <EnvironmentVariables>
+         <EnvironmentVariable
+            key = "IDEPreferLogStreaming"
+            value = "YES"
+            isEnabled = "YES">
+         </EnvironmentVariable>
+      </EnvironmentVariables>
    </LaunchAction>
    <ProfileAction
       buildConfiguration = "Release"
@@ -70,7 +77,7 @@
       </BuildableProductRunnable>
    </ProfileAction>
    <AnalyzeAction
-      buildConfiguration = "Debug">
+      buildConfiguration = "Release">
    </AnalyzeAction>
    <ArchiveAction
       buildConfiguration = "Release"

app/ios/RTABMapApp/Base.lproj/Main.storyboard

@@ -1,9 +1,9 @@
 <?xml version="1.0" encoding="UTF-8"?>
-<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="19455" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
-    <device id="retina3_5" orientation="portrait" appearance="light"/>
+<document type="com.apple.InterfaceBuilder3.CocoaTouch.Storyboard.XIB" version="3.0" toolsVersion="32700.99.1234" targetRuntime="iOS.CocoaTouch" propertyAccessControl="none" useAutolayout="YES" useTraitCollections="YES" useSafeAreas="YES" colorMatched="YES" initialViewController="zah-iI-EPt">
+    <device id="retina6_12" orientation="portrait" appearance="light"/>
     <dependencies>
         <deployment identifier="iOS"/>
-        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="19454"/>
+        <plugIn identifier="com.apple.InterfaceBuilder.IBCocoaTouchPlugin" version="22684"/>
         <capability name="Image references" minToolsVersion="12.0"/>
         <capability name="Safe area layout guides" minToolsVersion="9.0"/>
         <capability name="System colors in document resources" minToolsVersion="11.0"/>
@@ -15,14 +15,14 @@
             <objects>
                 <glkViewController preferredFramesPerSecond="30" id="zah-iI-EPt" customClass="ViewController" customModule="RTABMapApp" customModuleProvider="target" sceneMemberID="viewController">
                     <glkView key="view" opaque="NO" clipsSubviews="YES" multipleTouchEnabled="YES" contentMode="center" drawableDepthFormat="24" enableSetNeedsDisplay="NO" id="sqF-4e-2HU">
-                        <rect key="frame" x="0.0" y="0.0" width="320" height="480"/>
+                        <rect key="frame" x="0.0" y="0.0" width="393" height="852"/>
                         <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
                         <subviews>
                             <stackView opaque="NO" contentMode="scaleToFill" axis="vertical" translatesAutoresizingMaskIntoConstraints="NO" id="kPv-DO-hpd">
-                                <rect key="frame" x="250" y="180" width="60" height="120"/>
+                                <rect key="frame" x="323" y="366" width="60" height="120"/>
                                 <subviews>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="mJN-Ru-yPg" userLabel="StopButton">
-                                        <rect key="frame" x="0.0" y="0.5" width="60" height="59"/>
+                                        <rect key="frame" x="0.0" y="1" width="60" height="58.666666666666671"/>
                                         <constraints>
                                             <constraint firstAttribute="height" constant="60" id="g9J-0P-CmQ"/>
                                             <constraint firstAttribute="width" constant="60" id="m06-eJ-dIn"/>
@@ -34,7 +34,7 @@
                                         </connections>
                                     </button>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="7Lv-u6-mGh" userLabel="RecordButton">
-                                        <rect key="frame" x="0.0" y="60.5" width="60" height="59"/>
+                                        <rect key="frame" x="0.0" y="61.000000000000007" width="60" height="58.666666666666664"/>
                                         <constraints>
                                             <constraint firstAttribute="height" constant="60" id="DQI-eQ-v3X"/>
                                             <constraint firstAttribute="width" constant="60" id="Oeu-YT-wMz"/>
@@ -50,10 +50,10 @@
                                 </subviews>
                             </stackView>
                             <stackView opaque="NO" contentMode="scaleToFill" spacing="20" translatesAutoresizingMaskIntoConstraints="NO" id="ozY-dg-YOd">
-                                <rect key="frame" x="160" y="20" width="140" height="60"/>
+                                <rect key="frame" x="233" y="79" width="140" height="60"/>
                                 <subviews>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="9kh-4C-KyP" userLabel="LibraryButton">
-                                        <rect key="frame" x="0.0" y="1" width="60" height="57.5"/>
+                                        <rect key="frame" x="0.0" y="1.6666666666666643" width="60" height="57"/>
                                         <constraints>
                                             <constraint firstAttribute="width" constant="60" id="Ekq-3J-kUL"/>
                                             <constraint firstAttribute="height" constant="60" id="fqe-e7-0uk"/>
@@ -67,7 +67,7 @@
                                         </connections>
                                     </button>
                                     <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" buttonType="system" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="y53-OI-O76" userLabel="MenuButton">
-                                        <rect key="frame" x="80" y="0.5" width="60" height="59"/>
+                                        <rect key="frame" x="80" y="1" width="60" height="58.666666666666671"/>
                                         <constraints>
                                             <constraint firstAttribute="height" constant="60" id="L5T-8h-ale"/>
                                             <constraint firstAttribute="width" constant="60" id="pLI-ns-KHQ"/>
@@ -78,14 +78,14 @@
                                 </subviews>
                             </stackView>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Label" textAlignment="natural" lineBreakMode="wordWrap" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="KlF-4H-k6I">
-                                <rect key="frame" x="16" y="20" width="31" height="14.5"/>
+                                <rect key="frame" x="16" y="79" width="31" height="14.333333333333329"/>
                                 <color key="backgroundColor" red="0.0" green="0.0" blue="0.0" alpha="0.29820365646258501" colorSpace="custom" customColorSpace="sRGB"/>
                                 <fontDescription key="fontDescription" type="system" pointSize="12"/>
                                 <color key="textColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <nil key="highlightedColor"/>
                             </label>
                             <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="773-n4-lob" userLabel="ViewButton">
-                                <rect key="frame" x="248" y="417.5" width="52" height="40.5"/>
+                                <rect key="frame" x="321" y="756.33333333333337" width="52" height="40"/>
                                 <constraints>
                                     <constraint firstAttribute="width" constant="52" id="epT-x2-WRc"/>
                                     <constraint firstAttribute="height" constant="44" id="wAe-I1-qhs"/>
@@ -96,7 +96,7 @@
                                 </state>
                             </button>
                             <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="012-W3-dOS" userLabel="CloseVisualization">
-                                <rect key="frame" x="75" y="434" width="170.5" height="22"/>
+                                <rect key="frame" x="111.33333333333333" y="772" width="170.33333333333337" height="22"/>
                                 <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <state key="normal" title="Close Visualization" image="xmark.square" catalog="system">
                                     <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
@@ -106,7 +106,7 @@
                                 </connections>
                             </button>
                             <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="9oq-op-NGV" userLabel="StopCamera">
-                                <rect key="frame" x="98" y="433" width="124.5" height="22"/>
+                                <rect key="frame" x="134.33333333333334" y="771" width="124.33333333333334" height="22"/>
                                 <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <state key="normal" title="Stop Camera" image="xmark.square" catalog="system">
                                     <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
@@ -116,9 +116,9 @@
                                 </connections>
                             </button>
                             <button opaque="NO" contentMode="scaleAspectFit" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="Jj4-ow-MwD" userLabel="NewScanButtonLarge">
-                                <rect key="frame" x="73.5" y="224" width="173" height="32"/>
+                                <rect key="frame" x="86.333333333333329" y="407" width="220.66666666666669" height="38.333333333333314"/>
                                 <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
-                                <state key="normal" title="Start the Camera">
+                                <state key="normal" title="New Mapping Session">
                                     <color key="titleColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                     <imageReference key="image" image="plus.app" catalog="system" symbolScale="large"/>
                                     <preferredSymbolConfiguration key="preferredSymbolConfiguration" configurationType="font">
@@ -130,7 +130,7 @@
                                 </connections>
                             </button>
                             <button opaque="NO" contentMode="scaleToFill" contentHorizontalAlignment="center" contentVerticalAlignment="center" lineBreakMode="middleTruncation" translatesAutoresizingMaskIntoConstraints="NO" id="3Sc-lx-YKH">
-                                <rect key="frame" x="161" y="96" width="149" height="22"/>
+                                <rect key="frame" x="234" y="155" width="149" height="23.666666666666657"/>
                                 <color key="tintColor" white="1" alpha="1" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <state key="normal" title="Export OBJ-PLY" image="square.and.arrow.up" catalog="system"/>
                                 <connections>
@@ -138,7 +138,7 @@
                                 </connections>
                             </button>
                             <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="80" minValue="0.0" maxValue="120" translatesAutoresizingMaskIntoConstraints="NO" id="30J-Tx-NRW" userLabel="OrthoClipDistanceSlider">
-                                <rect key="frame" x="-2" y="344" width="122" height="31"/>
+                                <rect key="frame" x="-2" y="682" width="122" height="31"/>
                                 <constraints>
                                     <constraint firstAttribute="width" constant="118" id="dsv-im-K52"/>
                                 </constraints>
@@ -147,7 +147,7 @@
                                 </connections>
                             </slider>
                             <slider opaque="NO" contentMode="scaleToFill" placeholderIntrinsicWidth="114" placeholderIntrinsicHeight="30" contentHorizontalAlignment="center" contentVerticalAlignment="center" value="90" minValue="0.0" maxValue="180" translatesAutoresizingMaskIntoConstraints="NO" id="Z1x-Af-hdf" userLabel="GridRotationSlider">
-                                <rect key="frame" x="99" y="389" width="122" height="31"/>
+                                <rect key="frame" x="135.66666666666666" y="727" width="121.99999999999997" height="31"/>
                                 <constraints>
                                     <constraint firstAttribute="width" constant="118" id="KDl-lP-Jj7"/>
                                 </constraints>
@@ -155,8 +155,8 @@
                                     <action selector="rotateGridAction:" destination="zah-iI-EPt" eventType="valueChanged" id="eiC-QR-TYH"/>
                                 </connections>
                             </slider>
-                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Toast Label" textAlignment="natural" lineBreakMode="tailTruncation" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="JMv-UE-cvK" userLabel="ToastLabel">
-                                <rect key="frame" x="116.5" y="330" width="87.5" height="20.5"/>
+                            <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Toast Label" textAlignment="center" lineBreakMode="wordWrap" numberOfLines="4" baselineAdjustment="alignBaselines" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="JMv-UE-cvK" userLabel="ToastLabel">
+                                <rect key="frame" x="51" y="668.33333333333337" width="291" height="20.333333333333371"/>
                                 <color key="backgroundColor" white="0.0" alpha="0.3002232142857143" colorSpace="custom" customColorSpace="genericGamma22GrayColorSpace"/>
                                 <accessibility key="accessibilityConfiguration">
                                     <accessibilityTraits key="traits" staticText="YES" notEnabled="YES"/>
@@ -170,6 +170,7 @@
                         <constraints>
                             <constraint firstItem="KlF-4H-k6I" firstAttribute="leading" secondItem="qAR-pI-uT2" secondAttribute="leading" constant="16" id="14l-HO-Yqo"/>
                             <constraint firstItem="kPv-DO-hpd" firstAttribute="centerY" secondItem="sqF-4e-2HU" secondAttribute="centerY" id="4S8-5c-tBm"/>
+                            <constraint firstItem="JMv-UE-cvK" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leadingMargin" constant="35" id="Awg-pU-JkT"/>
                             <constraint firstItem="KlF-4H-k6I" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="CS3-Rk-Wxs"/>
                             <constraint firstItem="012-W3-dOS" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="IsC-yB-kNs"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="trailing" secondItem="773-n4-lob" secondAttribute="trailing" constant="20" id="N8s-Yg-VsL"/>
@@ -178,6 +179,7 @@
                             <constraint firstItem="Jj4-ow-MwD" firstAttribute="centerX" secondItem="sqF-4e-2HU" secondAttribute="centerX" id="OLx-LR-8xj"/>
                             <constraint firstItem="012-W3-dOS" firstAttribute="top" secondItem="Z1x-Af-hdf" secondAttribute="bottom" constant="15" id="RpS-k0-btM"/>
                             <constraint firstItem="ozY-dg-YOd" firstAttribute="top" secondItem="qAR-pI-uT2" secondAttribute="top" constant="20" id="TFT-we-noh"/>
+                            <constraint firstAttribute="trailingMargin" secondItem="JMv-UE-cvK" secondAttribute="trailing" constant="35" id="Ya5-GD-1rM"/>
                             <constraint firstItem="30J-Tx-NRW" firstAttribute="leading" secondItem="sqF-4e-2HU" secondAttribute="leading" id="ZBG-GX-P1X"/>
                             <constraint firstItem="3Sc-lx-YKH" firstAttribute="top" secondItem="ozY-dg-YOd" secondAttribute="bottom" constant="16" id="abR-Tj-Nua"/>
                             <constraint firstItem="qAR-pI-uT2" firstAttribute="bottom" secondItem="012-W3-dOS" secondAttribute="bottom" constant="24" id="dcl-mV-bH1"/>
@@ -220,17 +222,17 @@
             <objects>
                 <viewController storyboardIdentifier="unsupportedDeviceMessage" id="v3G-nW-gVc" userLabel="Unsupported View Controller" sceneMemberID="viewController">
                     <view key="view" contentMode="scaleToFill" id="hzM-1k-POo">
-                        <rect key="frame" x="0.0" y="0.0" width="320" height="480"/>
+                        <rect key="frame" x="0.0" y="0.0" width="393" height="852"/>
                         <autoresizingMask key="autoresizingMask" widthSizable="YES" heightSizable="YES"/>
                         <subviews>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" text="Unsupported Device" textAlignment="center" lineBreakMode="tailTruncation" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="kZt-ct-4KD">
-                                <rect key="frame" x="21.5" y="165" width="277.5" height="30"/>
+                                <rect key="frame" x="28" y="341.33333333333331" width="337.33333333333331" height="33.666666666666686"/>
                                 <fontDescription key="fontDescription" style="UICTFontTextStyleTitle1"/>
                                 <nil key="textColor"/>
                                 <nil key="highlightedColor"/>
                             </label>
                             <label opaque="NO" userInteractionEnabled="NO" contentMode="left" horizontalHuggingPriority="251" verticalHuggingPriority="251" textAlignment="center" lineBreakMode="tailTruncation" numberOfLines="0" baselineAdjustment="alignBaselines" enabled="NO" adjustsFontSizeToFit="NO" translatesAutoresizingMaskIntoConstraints="NO" id="4Js-Hw-3QU">
-                                <rect key="frame" x="21.5" y="203" width="277.5" height="74"/>
+                                <rect key="frame" x="28" y="383" width="337.33333333333331" height="86.333333333333314"/>
                                 <string key="text">This sample app requires a LiDAR-capable device, such as the second-generation iPad Pro 11-inch and fourth-generation iPad Pro 12.9-inch.</string>
                                 <fontDescription key="fontDescription" style="UICTFontTextStyleBody"/>
                                 <nil key="textColor"/>
@@ -256,16 +258,16 @@
         </scene>
     </scenes>
     <resources>
-        <image name="ellipsis.circle" catalog="system" width="128" height="121"/>
-        <image name="eye" catalog="system" width="128" height="81"/>
-        <image name="folder" catalog="system" width="128" height="97"/>
+        <image name="ellipsis.circle" catalog="system" width="128" height="123"/>
+        <image name="eye" catalog="system" width="128" height="79"/>
+        <image name="folder" catalog="system" width="128" height="96"/>
         <image name="plus.app" catalog="system" width="128" height="114"/>
-        <image name="record.circle" catalog="system" width="128" height="121"/>
-        <image name="square.and.arrow.up" catalog="system" width="115" height="128"/>
-        <image name="stop.circle" catalog="system" width="128" height="121"/>
+        <image name="record.circle" catalog="system" width="128" height="123"/>
+        <image name="square.and.arrow.up" catalog="system" width="108" height="128"/>
+        <image name="stop.circle" catalog="system" width="128" height="123"/>
         <image name="xmark.square" catalog="system" width="128" height="114"/>
         <systemColor name="systemRedColor">
-            <color red="1" green="0.23137254901960785" blue="0.18823529411764706" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
+            <color red="1" green="0.23137254900000001" blue="0.18823529410000001" alpha="1" colorSpace="custom" customColorSpace="sRGB"/>
         </systemColor>
     </resources>
 </document>

app/ios/RTABMapApp/NativeWrapper.cpp

@@ -33,11 +33,12 @@ void setupCallbacksNative(const void *object, void * classPtr,
                                                    int,
                                                    float, float, float, float,
                                                    int, int,
-                                                   float, float, float, float, float, float))
+                                                   float, float, float, float, float, float),
+                          void(*cameraInfoEventCallback)(void *, int, const char*, const char*))
 {
     if(object)
     {
-        native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback);
+        native(object)->setupSwiftCallbacks(classPtr, progressCallback, initCallback, statsUpdatedCallback, cameraInfoEventCallback);
     }
     else
     {
@@ -283,20 +284,6 @@ 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, double stamp)
-{
-    if(object)
-    {
-        native(object)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw,stamp);
-    }
-    else
-    {
-        UERROR("object is null!");
-        return;
-    }
-}
-
 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,
@@ -312,7 +299,7 @@ void postOdometryEventNative(const void *object,
     if(object)
     {
         native(object)->postOdometryEvent(
-                rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+                (qx==0.0f && qy==0.0f && qz==0.0f && qw==0.0f)?rtabmap::Transform():rtabmap::Transform(x,y,z,qx,qy,qz,qw),
                 fx,fy,cx,cy, 0,0,0,0,
                 rtabmap::Transform(), rtabmap::Transform(),
                 stamp, 0,
@@ -454,6 +441,13 @@ void setLocalizationModeNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setDataRecorderModeNative(const void *object, bool enabled)
+{
+    if(object)
+        native(object)->setDataRecorderMode(enabled);
+    else
+        UERROR("object is null!");
+}
 void setTrajectoryModeNative(const void *object, bool enabled)
 {
     if(object)
@@ -510,6 +504,13 @@ void setAppendModeNative(const void *object, bool enabled)
     else
         UERROR("object is null!");
 }
+void setUpstreamRelocalizationAccThrNative(const void * object, float value)
+{
+    if(object)
+        native(object)->setUpstreamRelocalizationAccThr(value);
+    else
+        UERROR("object is null!");
+}
 void setMaxCloudDepthNative(const void *object, float value)
 {
     if(object)
@@ -587,6 +588,15 @@ void setDepthConfidenceNative(const void *object, int value)
     else
         UERROR("object is null!");
 }
+
+void setExportPointCloudFormatNative(const void *object, const char * format)
+{
+    if(object)
+        native(object)->setExportPointCloudFormat(format);
+    else
+        UERROR("object is null!");
+}
+
 int setMappingParameterNative(const void *object, const char * key, const char * value)
 {
     if(object)

app/ios/RTABMapApp/NativeWrapper.hpp

@@ -29,7 +29,8 @@ void setupCallbacksNative(const void *object, void * classPtr,
                                                    int,
                                                    float, float, float, float,
                                                    int, int,
-                                                   float, float, float, float, float, float));
+                                                   float, float, float, float, float, float),
+                          void(*cameraInfoEventCallback)(void *, int, const char*, const char*));
 void destroyNativeApplication(const void *object);
 void setScreenRotationNative(const void *object, int displayRotation);
 int openDatabaseNative(const void *object, const char * databasePath, bool databaseInMemory, bool optimize, bool clearDatabase);
@@ -66,8 +67,6 @@ int renderNative(const void *object);
 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, 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,
@@ -92,6 +91,7 @@ void setLightingNative(const void *object, bool enabled);
 void setBackfaceCullingNative(const void *object, bool enabled);
 void setWireframeNative(const void *object, bool enabled);
 void setLocalizationModeNative(const void *object, bool enabled);
+void setDataRecorderModeNative(const void *object, bool enabled);
 void setTrajectoryModeNative(const void *object, bool enabled);
 void setGraphOptimizationNative(const void *object, bool enabled);
 void setNodesFilteringNative(const void *object, bool enabled);
@@ -100,6 +100,7 @@ void setGridVisibleNative(const void *object, bool visible);
 void setFullResolutionNative(const void *object, bool enabled);
 void setSmoothingNative(const void *object, bool enabled);
 void setAppendModeNative(const void *object, bool enabled);
+void setUpstreamRelocalizationAccThrNative(const void *object, float value);
 void setMaxCloudDepthNative(const void *object, float value);
 void setMinCloudDepthNative(const void *object, float value);
 void setCloudDensityLevelNative(const void *object, int value);
@@ -111,6 +112,7 @@ void setMaxGainRadiusNative(const void *object, float value);
 void setRenderingTextureDecimationNative(const void *object, int value);
 void setBackgroundColorNative(const void *object, float gray);
 void setDepthConfidenceNative(const void *object, int value);
+void setExportPointCloudFormatNative(const void *object, const char * format);
 int setMappingParameterNative(const void *object, const char * key, const char * value);
 
 typedef struct ImageNative

app/ios/RTABMapApp/RTABMap.swift

@@ -85,6 +85,25 @@ class RTABMap {
 
                     observer.statsUpdated(mySelf, nodes: Int(nodes), words: Int(words), points: Int(points), polygons: Int(polygons), updateTime: updateTime, loopClosureId: Int(loopClosureId), highestHypId: Int(highestHypId), databaseMemoryUsed: Int(databaseMemoryUsed), inliers: Int(inliers), matches: Int(matches), featuresExtracted: Int(featuresExtracted), hypothesis: hypothesis, nodesDrawn: Int(nodesDrawn), fps: fps, rejected: Int(rejected), rehearsalValue: rehearsalValue, optimizationMaxError: optimizationMaxError, optimizationMaxErrorRatio: optimizationMaxErrorRatio, distanceTravelled: distanceTravelled, fastMovement: Int(fastMovement), landmarkDetected: Int(landmarkDetected), x: x, y: y, z: z, roll: roll, pitch: pitch, yaw: yaw)
                 }
+             },
+             //cameraInfoEventCallback
+             {(observer, type, key, value) -> Void in
+                         // Extract pointer to `self` from void pointer:
+                let mySelf = Unmanaged<RTABMap>.fromOpaque(observer!).takeUnretainedValue()
+                         // Call instance method:
+                         //mySelf.TestMethod();
+                for (id, observation) in mySelf.observations {
+                    // If the observer is no longer in memory, we
+                    // can clean up the observation for its ID
+                    guard let observer = observation.observer else {
+                        mySelf.observations.removeValue(forKey: id)
+                        continue
+                    }
+					
+                    let strKey = String(cString: key!)
+                    let strValue = String(cString: value!)
+                    observer.cameraInfoEventReceived(mySelf, type: Int(type), key: strKey, value: strValue)
+                }
              })
     }
     
@@ -215,21 +234,7 @@ class RTABMap {
     func setCamera(type: Int) {
         setCameraNative(native_rtabmap, Int32(type))
     }
-    
-    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, stamp)
-    }
-    
-    func notifyLost() {
-        // a null transform will make rtabmap creating a new session
-        postCameraPoseEventNative(native_rtabmap, 0,0,0,0,0,0,0,0)
-    }
-    
+
     func postOdometryEvent(frame: ARFrame, orientation: UIInterfaceOrientation, viewport: CGSize) {
         let pose = frame.camera.transform   // ViewMatrix
         let rotation = GLKMatrix3(
@@ -239,12 +244,10 @@ 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, frame.timestamp)
-                
         let confMap = frame.sceneDepth?.confidenceMap
         let depthMap = frame.sceneDepth?.depthMap
         let points = frame.rawFeaturePoints?.points
-                
+
         if points != nil && (depthMap != nil || points!.count>0)
         {
             let v = frame.camera.viewMatrix(for: orientation)
@@ -313,9 +316,22 @@ class RTABMap {
                 if(frame.lightEstimate != nil) {
                     addEnvSensor(type: 4, value: Float(frame.lightEstimate!.ambientIntensity))
                 }
-                                
+                
+                var lost = false
+                switch frame.camera.trackingState {
+                case .normal:
+                    lost = false
+                case .limited(.excessiveMotion):
+                    lost = false
+                case .limited(.insufficientFeatures):
+                    lost = false
+                default:
+                    lost = true
+                }
+                // Notify lost with pose=null
+
                 postOdometryEventNative(native_rtabmap,
-                                        pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w,
+                                        !lost ? pose[3,0]:0, !lost ? pose[3,1]:0, !lost ? pose[3,2]:0, !lost ? quat.x:0, !lost ? quat.y:0, !lost ? quat.z:0, !lost ? quat.w:0,
                                         frame.camera.intrinsics[0,0], // fx
                                         frame.camera.intrinsics[1,1], // fy
                                         frame.camera.intrinsics[2,0], // cx
@@ -390,8 +406,8 @@ class RTABMap {
     func setLocalizationMode(enabled: Bool) {
         setLocalizationModeNative(native_rtabmap, enabled)
     }
-    func setTrajectoryMode(enabled: Bool) {
-        setTrajectoryModeNative(native_rtabmap, enabled)
+    func setDataRecorderMode(enabled: Bool) {
+        setDataRecorderModeNative(native_rtabmap, enabled)
     }
     func setGraphOptimization(enabled: Bool) {
         setGraphOptimizationNative(native_rtabmap, enabled)
@@ -414,6 +430,9 @@ class RTABMap {
     func setAppendMode(enabled: Bool) {
         setAppendModeNative(native_rtabmap, enabled)
     }
+    func setUpstreamRelocalizationAccThr(value: Float) {
+        setUpstreamRelocalizationAccThrNative(native_rtabmap, value)
+    }
     func setMaxCloudDepth(value: Float) {
         setMaxCloudDepthNative(native_rtabmap, value)
     }
@@ -447,6 +466,11 @@ class RTABMap {
     func setDepthConfidence(value: Int) {
         setDepthConfidenceNative(native_rtabmap, Int32(value))
     }
+    func setExportPointCloudFormat(format: String) {
+        format.utf8CString.withUnsafeBufferPointer { bufferFormat in
+            return setExportPointCloudFormatNative(native_rtabmap, bufferFormat.baseAddress)
+        }
+    }
     func setMappingParameter(key: String, value: String) {
         key.utf8CString.withUnsafeBufferPointer { bufferKey in
             value.utf8CString.withUnsafeBufferPointer { bufferValue in
@@ -523,6 +547,7 @@ protocol RTABMapObserver: class {
                       roll: Float,
                       pitch: Float,
                       yaw: Float)
+    func cameraInfoEventReceived(_ rtabmap: RTABMap, type: Int, key: String, value: String)
 }
 
 extension String {

app/ios/RTABMapApp/ViewController.swift

@@ -41,6 +41,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     private var mTimeThr: Int = 0
     private var mMaxFeatures: Int = 0
     private var mLoopThr = 0.11
+    private var mDataRecording = false
     
     private var mReviewRequested = false
     
@@ -63,7 +64,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         case .STATE_CAMERA:
             return "Camera Preview"
         case .STATE_MAPPING:
-            return "Mapping"
+            return mDataRecording ? "Data Recording" : "Mapping"
         case .STATE_PROCESSING:
             return "Processing"
         case .STATE_VISUALIZING:
@@ -428,6 +429,16 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
     }
     
+    func cameraInfoEventReceived(_ rtabmap: RTABMap, type: Int, key: String, value: String) {
+        if(self.debugShown && key == "UpstreamRelocationFiltered")
+        {
+            DispatchQueue.main.async {
+                self.dismiss(animated: true)
+                self.showToast(message: "ARKit re-localization filtered because an acceleration of \(value) has been detected, which is over current threshold set in the settings.", seconds: 3)
+            }
+        }
+    }
+    
     func getMemoryUsage() -> UInt64 {
         var taskInfo = mach_task_basic_info()
         var count = mach_msg_type_number_t(MemoryLayout<mach_task_basic_info>.size)/4
@@ -460,7 +471,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         if(mMapNodes > 0 && self.openedDatabasePath == nil)
         {
             let msg = "RTAB-Map has been pushed to background while mapping. Do you want to save the map now?"
-            let alert = UIAlertController(title: "Mapping Stopped!", message: msg, preferredStyle: .alert)
+            let alert = UIAlertController(title: mDataRecording ? "Data Recording Stopped" : "Mapping Stopped!", message: msg, preferredStyle: .alert)
             let alertActionNo = UIAlertAction(title: "Ignore", style: .cancel) {
                 (UIAlertAction) -> Void in
             }
@@ -582,6 +593,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         mState = state;
 
         var actionNewScanEnabled: Bool
+        var actionNewDataRecording: Bool
         var actionSaveEnabled: Bool
         var actionResumeEnabled: Bool
         var actionExportEnabled: Bool
@@ -602,7 +614,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             exportOBJPLYButton.isHidden = true
             orthoDistanceSlider.isHidden = cameraMode != 3
             orthoGridSlider.isHidden = cameraMode != 3
-            actionNewScanEnabled = true
+            actionNewScanEnabled = !mDataRecording
+            actionNewDataRecording = mDataRecording
             actionSaveEnabled = false
             actionResumeEnabled = false
             actionExportEnabled = false
@@ -621,7 +634,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             exportOBJPLYButton.isHidden = true
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
-            actionNewScanEnabled = true
+            actionNewScanEnabled = !mDataRecording
+            actionNewDataRecording = mDataRecording
             actionSaveEnabled = false
             actionResumeEnabled = false
             actionExportEnabled = false
@@ -643,6 +657,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             orthoDistanceSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
             orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
             actionNewScanEnabled = false
+            actionNewDataRecording = false
             actionSaveEnabled = false
             actionResumeEnabled = false
             actionExportEnabled = false
@@ -662,6 +677,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
             actionNewScanEnabled = true
+            actionNewDataRecording = true
             actionSaveEnabled = mMapNodes>0
             actionResumeEnabled = mMapNodes>0
             actionExportEnabled = mMapNodes>0
@@ -681,6 +697,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
             orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
             actionNewScanEnabled = true
+            actionNewDataRecording = true
             actionSaveEnabled = mState != .STATE_WELCOME && mMapNodes>0
             actionResumeEnabled = mState != .STATE_WELCOME && mMapNodes>0
             actionExportEnabled = mState != .STATE_WELCOME && mMapNodes>0
@@ -762,9 +779,16 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 self.optimization(approach: -1)
             }),
             optimizeAdvancedMenu])
+
+        // Advanced menu
+        let advancedMenu = UIMenu(title: "Advanced...", children: [
+            UIAction(title: "New Data Recording", image: UIImage(systemName: "plus.app"), attributes: actionNewDataRecording ? [] : .disabled, state: .off, handler: { _ in
+            	self.newScan(dataRecordingMode: true)
+        	})
+        ])
                 
         var fileMenuChildren: [UIMenuElement] = []
-        fileMenuChildren.append(UIAction(title: "New Scan", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in
+        fileMenuChildren.append(UIAction(title: "New Mapping Session", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in
             self.newScan()
         }))
         if(actionOptimizeEnabled) {
@@ -787,6 +811,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         fileMenuChildren.append(UIAction(title: "Append Scan", image: UIImage(systemName: "play.fill"), attributes: actionResumeEnabled ? [] : .disabled, state: .off, handler: { _ in
             self.resumeScan()
         }))
+        fileMenuChildren.append(advancedMenu)
         
         // File menu
         let fileMenu = UIMenu(title: "File", options: .displayInline, children: fileMenuChildren)
@@ -1001,16 +1026,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             status = "Camera Is Occluded Or Lighting Is Too Dark"
         }
 
-        if accept
+        if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation
         {
-            if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation
-            {
-                rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size)
-            }
-        }
-        else
-        {
-            rtabmap?.notifyLost();
+            rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size)
         }
         
         if !status.isEmpty {
@@ -1292,7 +1310,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
     }
     
     @IBAction func singleTapped(_ gestureRecognizer: UITapGestureRecognizer) {
-        if gestureRecognizer.state == UIGestureRecognizer.State.recognized
+        if gestureRecognizer.state == .recognized
         {
             resetNoTouchTimer(!mHudVisible)
             
@@ -1322,6 +1340,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         rtabmap!.setFullResolution(enabled: defaults.bool(forKey: "HDMode"));
         rtabmap!.setSmoothing(enabled: defaults.bool(forKey: "Smoothing"));
         rtabmap!.setAppendMode(enabled: defaults.bool(forKey: "AppendMode"));
+        rtabmap!.setUpstreamRelocalizationAccThr(value: defaults.float(forKey: "UpstreamRelocalizationFilteringAccThr"));
+        rtabmap!.setExportPointCloudFormat(format: defaults.string(forKey: "ExportPointCloudFormat")!);
         
         mTimeThr = (defaults.string(forKey: "TimeLimit")! as NSString).integerValue
         mMaxFeatures = (defaults.string(forKey: "MaxFeaturesExtractedLoopClosure")! as NSString).integerValue
@@ -1379,8 +1399,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         let bgColor = defaults.float(forKey: "BackgroundColor");
         rtabmap!.setBackgroundColor(gray: bgColor);
         
+        let format = defaults.string(forKey: "ExportPointCloudFormat")!;
         DispatchQueue.main.async {
             self.statusLabel.textColor = bgColor>=0.6 ? UIColor(white: 0.0, alpha: 1) : UIColor(white: 1.0, alpha: 1)
+            self.exportOBJPLYButton.setTitle("Export OBJ-\(format == "las" ? "LAS" : "PLY")", for: .normal)
         }
     
         rtabmap!.setClusterRatio(value: defaults.float(forKey: "NoiseFilteringRatio"));
@@ -1409,22 +1431,24 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
             rtabmap!.postExportation(visualize: false)
         }
         
-        let alertController = UIAlertController(title: "Append Mode", message: "The camera preview will not be aligned to map on start, move to a previously scanned area, then push Record. When a loop closure is detected, new scans will be appended to map.", preferredStyle: .alert)
-
-        let okAction = UIAlertAction(title: "OK", style: .default) { (action) in
+        if(!mDataRecording) {
+            let alertController = UIAlertController(title: "Append Mode", message: "The camera preview will not be aligned to map on start, move to a previously scanned area, then push Record. When a loop closure is detected, new scans will be appended to map.", preferredStyle: .alert)
+            
+            let okAction = UIAlertAction(title: "OK", style: .default) { (action) in
+            }
+            alertController.addAction(okAction)
+            
+            present(alertController, animated: true)
         }
-        alertController.addAction(okAction)
-        
-        present(alertController, animated: true)
         
         setGLCamera(type: 0);
         startCamera();
     }
     
-    func newScan()
+    func newScan(dataRecordingMode: Bool = false)
     {
         print("databases.size() = \(databases.size())")
-        if(databases.count >= 5 && !mReviewRequested && self.depthSupported)
+        if(databases.count >= 10 && !mReviewRequested && self.depthSupported)
         {
             SKStoreReviewController.requestReviewInCurrentScene()
             mReviewRequested = true
@@ -1438,7 +1462,6 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         mMapNodes = 0;
         self.openedDatabasePath = nil
         let tmpDatabase = self.getDocumentDirectory().appendingPathComponent(self.RTABMAP_TMP_DB)
-        let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory")
         if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING) &&
            FileManager.default.fileExists(atPath: tmpDatabase.path) &&
            tmpDatabase.fileSize > 1024*1024) // > 1MB
@@ -1454,7 +1477,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     catch {
                         print("Could not clear tmp database: \(error)")
                     }
-                    self.newScan()
+                    self.newScan(dataRecordingMode: dataRecordingMode)
                 }
                 alert.addAction(alertActionNo)
                 let alertActionCancel = UIAlertAction(title: "Cancel", style: .cancel) {
@@ -1545,10 +1568,25 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         }
         else
         {
+            let inMemory = UserDefaults.standard.bool(forKey: "DatabaseInMemory") && !dataRecordingMode
+            mDataRecording = dataRecordingMode
+            self.rtabmap!.setDataRecorderMode(enabled: dataRecordingMode)
+            self.optimizedGraphShown = true // Always reset to true when opening a database
             self.rtabmap!.openDatabase(databasePath: tmpDatabase.path, databaseInMemory: inMemory, optimize: false, clearDatabase: true)
-
+            
             if(!(self.mState == State.STATE_CAMERA || self.mState == State.STATE_MAPPING))
             {
+                if(mDataRecording) {
+                    let alertController = UIAlertController(title: "Data Recording Mode", message: "This mode should be only used if you want to record raw ARKit data as long as possible without any feedback: loop closure detection and map rendering are disabled. The database size in Debug display shows how much data has been recorded so far.", preferredStyle: .alert)
+                    
+                    let okAction = UIAlertAction(title: "OK", style: .default) { (action) in
+                    }
+                    alertController.addAction(okAction)
+                    
+                    present(alertController, animated: true)
+                    self.debugShown = true
+                }
+                
                 self.setGLCamera(type: 0);
                 self.startCamera();
             }
@@ -1575,6 +1613,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     alert.addAction(yes)
                     let no = UIAlertAction(title: "No", style: .cancel) {
                         (UIAlertAction) -> Void in
+                        if(self.mDataRecording) {
+                            self.save() // We cannot skip saving after data recording
+                        }
                     }
                     alert.addAction(no)
                     
@@ -1583,10 +1624,17 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                     self.saveDatabase(fileName: fileName);
                 }
             }
+            else
+            {
+                self.save()
+            }
         }
 
         //Step : 3
         var placeholder = Date().getFormattedDate(format: "yyMMdd-HHmmss")
+        if(mDataRecording) {
+            placeholder += "-recording"
+        }
         if self.openedDatabasePath != nil && !self.openedDatabasePath!.path.isEmpty
         {
             var components = self.openedDatabasePath!.lastPathComponent.components(separatedBy: ".")
@@ -1604,7 +1652,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         //Step : 4
         alert.addAction(save)
         //Cancel action
-        alert.addAction(UIAlertAction(title: "Cancel", style: .default) { (alertAction) in })
+        if(!mDataRecording) {
+            alert.addAction(UIAlertAction(title: "Cancel", style: .default) { (alertAction) in })
+        }
 
         self.present(alert, animated: true) {
             alert.textFields?.first?.selectAll(nil)
@@ -1649,7 +1699,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                 print("Could not clear tmp database: \(error)")
             }
             self.updateDatabases()
-            self.updateState(state: previousState)
+            self.updateState(state: self.mDataRecording ? .STATE_WELCOME : previousState)
         })
     }
     
@@ -1854,6 +1904,10 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         locationManager?.stopUpdatingLocation()
         rtabmap?.setPausedMapping(paused: true)
         rtabmap?.stopCamera()
+        if(mDataRecording) {
+            // this will show the trajectory before saving
+            self.rtabmap!.setGraphOptimization(enabled: false)
+        }
         setGLCamera(type: 2)
         if(mState == .STATE_VISUALIZING_CAMERA)
         {
@@ -1863,34 +1917,42 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         
         if !ignoreSaving
         {
-            dismiss(animated: true, completion: {
-                var msg = "Do you want to do standard graph optimization and make a nice assembled mesh now? This can be also done later using \"Optimize\" and \"Assemble\" menus."
-                let depthUsed = self.depthSupported && UserDefaults.standard.bool(forKey: "LidarMode")
-                if !depthUsed
-                {
-                    msg = "Do you want to do standard graph optimization now? This can be also done later using \"Optimize\" menu."
-                }
-                let alert = UIAlertController(title: "Mapping Stopped! Optimize Now?", message: msg, preferredStyle: .alert)
-                if depthUsed {
-                    let alertActionOnlyGraph = UIAlertAction(title: "Only Optimize", style: .default)
+            if(mDataRecording)
+            {
+                // Go directly to save
+                self.save()
+            }
+            else
+            {
+                dismiss(animated: true, completion: {
+                    var msg = "Do you want to do standard graph optimization and make a nice assembled mesh now? This can be also done later using \"Optimize\" and \"Assemble\" menus."
+                    let depthUsed = self.depthSupported && UserDefaults.standard.bool(forKey: "LidarMode")
+                    if !depthUsed
                     {
-                        (UIAlertAction) -> Void in
-                        self.optimization(withStandardMeshExport: false, approach: -1)
+                        msg = "Do you want to do standard graph optimization now? This can be also done later using \"Optimize\" menu."
                     }
-                    alert.addAction(alertActionOnlyGraph)
-                }
-                let alertActionNo = UIAlertAction(title: "Save First", style: .cancel) {
-                    (UIAlertAction) -> Void in
-                    self.save()
-                }
-                alert.addAction(alertActionNo)
-                let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
-                    (UIAlertAction) -> Void in
-                    self.optimization(withStandardMeshExport: depthUsed, approach: -1)
-                }
-                alert.addAction(alertActionYes)
-                self.present(alert, animated: true, completion: nil)
-            })
+                    let alert = UIAlertController(title: "Mapping Stopped! Optimize Now?", message: msg, preferredStyle: .alert)
+                    if depthUsed {
+                        let alertActionOnlyGraph = UIAlertAction(title: "Only Optimize", style: .default)
+                        {
+                            (UIAlertAction) -> Void in
+                            self.optimization(withStandardMeshExport: false, approach: -1)
+                        }
+                        alert.addAction(alertActionOnlyGraph)
+                    }
+                    let alertActionNo = UIAlertAction(title: "Save First", style: .cancel) {
+                        (UIAlertAction) -> Void in
+                        self.save()
+                    }
+                    alert.addAction(alertActionNo)
+                    let alertActionYes = UIAlertAction(title: "Yes", style: .default) {
+                        (UIAlertAction) -> Void in
+                        self.optimization(withStandardMeshExport: depthUsed, approach: -1)
+                    }
+                    alert.addAction(alertActionYes)
+                    self.present(alert, animated: true, completion: nil)
+                })
+            }
         }
         else if(mMapNodes == 0)
         {
@@ -1937,6 +1999,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
         updateState(state: .STATE_PROCESSING);
         var status = 0
         DispatchQueue.background(background: {
+            self.optimizedGraphShown = true // Always reset to true when opening a database
             status = self.rtabmap!.openDatabase(databasePath: self.openedDatabasePath!.path, databaseInMemory: true, optimize: false, clearDatabase: false)
         }, completion:{
             // main thread

app/ios/RTABMapApp/install_deps.sh

@@ -4,17 +4,13 @@ set -euxo pipefail
 
 # Tested on Apple Silicon Mac, with cmake 3.19.2.
 
-mkdir Libraries
+mkdir -p Libraries
 cd Libraries
 pwd=$(pwd)
 prefix=$pwd
 sysroot=iphoneos
 #sysroot=iphonesimulator
 
-
-# Install directory for all dependencies
-mkdir -p $prefix
-
 # openmp
 # based on https://github.com/Homebrew/homebrew-core/blob/HEAD/Formula/libomp.rb
 #curl -L https://github.com/llvm/llvm-project/releases/download/llvmorg-11.1.0/openmp-11.1.0.src.tar.xz -o openmp-11.1.0.src.tar.xz
@@ -30,41 +26,60 @@ mkdir -p $prefix
 #cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 
 # Boost
-echo "wget boost..."
-curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz
-tar -xzf boost_1_59_0.tar.gz
+if [ ! -e $prefix/include/boost ]
+then
+if [ ! -e boost_1_59_0 ]
+then
+  echo "wget boost..."
+  curl -L https://downloads.sourceforge.net/project/boost/boost/1.59.0/boost_1_59_0.tar.gz -o boost_1_59_0.tar.gz
+  tar -xzf boost_1_59_0.tar.gz
+fi
 cd boost_1_59_0
 curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/489ff2869eccd6f8d03ffb9090ef839108762741/BoostConfig.cmake.in -o BoostConfig.cmake.in
 curl -L https://gist.github.com/matlabbe/0bce8feeb73a499a76afbbcc5c687221/raw/b07fe7d4e5dfe5f1d110c733e5cf660d79a26378/CMakeLists.txt -o CMakeLists.txt
-mkdir build
+mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
 #rm -r boost_1_59_0.tar.gz boost_1_59_0
+fi
 
 # eigen
-echo "wget eigen..."
-curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
-tar -xzf 3.3.9.tar.gz
+if [ ! -e $prefix/include/eigen3 ]
+then
+if [ ! -e eigen-3.3.9 ]
+then
+  echo "wget eigen..."
+  curl -L https://gitlab.com/libeigen/eigen/-/archive/3.3.9/eigen-3.3.9.tar.gz -o 3.3.9.tar.gz
+  tar -xzf 3.3.9.tar.gz
+fi
 cd eigen-3.3.9
-mkdir build
+mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
 #rm -r 3.3.9.tar.gz eigen-3.3.9
+fi
 
 # FLANN
-echo "wget flann..."
-git clone https://github.com/flann-lib/flann.git
+if [ ! -e $prefix/include/flann ]
+then
+if [ ! -e flann ]
+then
+  echo "wget flann..."
+  git clone https://github.com/flann-lib/flann.git -b 1.8.4
+fi
 cd flann
-git checkout 1.8.4
-curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
-git apply flann_ios.patch
-mkdir build
+if [ ! -e flann_ios.patch ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/c858ba36fb85d5e44d8667dfb3543e12/raw/8fc40aa9bc3267604869444020476a49f14ab424/flann_ios.patch  -o flann_ios.patch
+  git apply flann_ios.patch
+fi
+mkdir -p build
 cd build
 # comment "add_subdirectory( test )" in top CMakeLists.txt
 # comment "add_subdirectory( doc )" in top CMakeLists.txt
@@ -73,76 +88,131 @@ cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED=
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
 #rm -r flann
+fi
 
 # GTSAM
-git clone https://bitbucket.org/gtborg/gtsam.git
-cd gtsam
-git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
+if [ ! -e $prefix/include/gtsam ]
+then
+if [ ! -e gtsam ]
+then
+  git clone https://bitbucket.org/gtborg/gtsam.git
+  cd gtsam
+  git checkout fbb9d3bdda8b88df51896bc401bfd170573e66f5
+else
+  cd gtsam
+fi
 # patch
-curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
-git apply gtsam_GKlib_ios_fix.patch
-mkdir build
+if [ ! -e gtsam_GKlib_ios_fix.patch ]
+then
+  curl -L https://gist.github.com/matlabbe/76d658dddb841b3355ae3a6e32850cd8/raw/7033cba1c89097b0c830651d7277c04dc92cbdd9/gtsam_GKlib_ios_fix.patch -o gtsam_GKlib_ios_fix.patch
+  git apply gtsam_GKlib_ios_fix.patch
+fi
+mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DMETIS_SHARED=OFF -DGTSAM_BUILD_STATIC_LIBRARY=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_WRAP_SERIALIZATION=OFF -DGTSAM_BUILD_WRAP=OFF -DGTSAM_INSTALL_CPPUNITLITE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
 #rm -rf gtsam
+fi
 
 # g2o
-git clone https://github.com/RainerKuemmerle/g2o.git
-cd g2o
-git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
+if [ ! -e $prefix/include/g2o ]
+then
+if [ ! -e g2o ]
+then
+  git clone https://github.com/RainerKuemmerle/g2o.git
+  cd g2o
+  git checkout a3f7706bdbb849b2808dc3e1b7aee189f63b498e
+else
+  cd g2o
+fi
 # patch
-curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
-git apply g2o_ios_fix.patch
-mkdir build
+if [ ! -e g2o_ios_fix.patch ]
+then
+  curl -L https://gist.github.com/matlabbe/b9ccfeae8f0744b275cab23510872680/raw/a58e06accba3976420d4b61341685c123193810e/g2o_ios_fix.patch -o g2o_ios_fix.patch
+  git apply g2o_ios_fix.patch
+fi
+mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_LGPL_SHARED_LIBS=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
 cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
 cd $pwd
 #rm -rf g2o
+fi
 
 # VTK
-git clone https://github.com/Kitware/VTK.git
-cd VTK
-git checkout tags/v8.2.0
+if [ ! -e $prefix/lib/vtk.framework ]
+then
+if [ ! -e VTK ]
+then
+  git clone https://github.com/Kitware/VTK.git
+  cd VTK
+  git checkout tags/v8.2.0
+else
+  cd VTK
+fi
 git cherry-pick bf3ae8072df2393c7270509bae41be0776826346
-mkdir build
+mkdir -p build
 cd build
 cmake -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_FRAMEWORK_INSTALL_PREFIX=$prefix/lib -DIOS_DEVICE_ARCHITECTURES="arm64" -DIOS_SIMULATOR_ARCHITECTURES="" -DBUILD_EXAMPLES=OFF -DBUILD_TESTING=OFF -DVTK_IOS_BUILD=ON -DModule_vtkFiltersModeling=ON ..
 # For iphonesimulator: add -DIOS_DEVICE_ARCHITECTURES=""
 cmake --build . --config Release
 cd $pwd
 #rm -rf VTK
+fi
 
 # PCL
-git clone https://github.com/PointCloudLibrary/pcl.git
-cd pcl
-git checkout tags/pcl-1.11.1
+if [ ! -e $prefix/include/pcl-1.11 ]
+then
+if [ ! -e pcl ]
+then
+  git clone https://github.com/PointCloudLibrary/pcl.git
+  cd pcl
+  git checkout tags/pcl-1.11.1
+else
+  cd pcl
+fi
 # patch
-curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
-git apply pcl_1_11_1_vtk_ios_support.patch
-mkdir build
+if [ ! -e pcl_1_11_1_vtk_ios_support.patch ]
+then
+  curl -L https://gist.github.com/matlabbe/f3ba9366eb91e1b855dadd2ddce5746d/raw/6869cf26211ab15492599e557b0e729b23b2c119/pcl_1_11_1_vtk_ios_support.patch -o pcl_1_11_1_vtk_ios_support.patch
+  git apply pcl_1_11_1_vtk_ios_support.patch
+fi
+mkdir -p build
 cd build
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DBUILD_apps=OFF -DBUILD_examples=OFF -DBUILD_tools=OFF -DBUILD_visualization=OFF -DBUILD_tracking=OFF -DBUILD_people=OFF -DBUILD_global_tests=OFF -DWITH_QT=OFF -DWITH_OPENGL=OFF -DWITH_VTK=ON -DPCL_SHARED_LIBS=OFF -DPCL_ENABLE_SSE=OFF -DCMAKE_FIND_ROOT_PATH=$prefix ..
 cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
 #rm -rf pcl
+fi
 
 # OpenCV
-git clone https://github.com/opencv/opencv_contrib.git
-cd opencv_contrib
-git checkout tags/3.4.2
+if [ ! -e $prefix/include/opencv2 ]
+then
+if [ ! -e opencv_contrib ]
+then
+  git clone https://github.com/opencv/opencv_contrib.git
+  cd opencv_contrib
+  git checkout tags/3.4.2
+fi
 cd $pwd
-git clone https://github.com/opencv/opencv.git
-cd opencv
-git checkout tags/3.4.2
-curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
-git apply opencv_ios.patch
-mkdir build
+if [ ! -e opencv ]
+then
+  git clone https://github.com/opencv/opencv.git
+  cd opencv
+  git checkout tags/3.4.2
+else
+  cd opencv
+fi
+if [ ! -e opencv_ios.patch ]
+then
+  curl -L https://gist.githubusercontent.com/matlabbe/fdc3ab4854f3a68fbde7277f543b4e5b/raw/f340839c09165056d3845645df24b76507542fd2/opencv_ios.patch -o opencv_ios.patch
+  git apply opencv_ios.patch
+fi
+mkdir -p build
 cd build
 # add "add_definitions(-DPNG_ARM_NEON_OPT=0)" in 3rdparty/libpng/CMakeLists.txt
 cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DOPENCV_EXTRA_MODULES_PATH=$prefix/opencv_contrib/modules -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_CUDA=OFF -DBUILD_opencv_apps=OFF -DBUILD_opencv_xobjdetect=OFF -DBUILD_opencv_stereo=OFF ..
@@ -150,13 +220,31 @@ cmake --build . --config Release
 cmake --build . --config Release --target install
 cd $pwd
 #rm -rf opencv opencv_contrib
+fi
 
-mkdir rtabmap
+# LAS
+if [ ! -e $prefix/include/liblas ]
+then
+if [ ! -e libLAS ]
+then
+  git clone https://github.com/libLAS/libLAS.git
+fi
+cd libLAS
+sed -i '' 's/SHARED/STATIC/g' src/CMakeLists.txt
+mkdir -p build
+cd build
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_UTILITIES=OFF -DWITH_TESTS=OFF -DWITH_GEOTIFF=OFF ..
+cmake --build . --config Release -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cmake --build . --config Release --target install -- CODE_SIGN_IDENTITY="" CODE_SIGNING_REQUIRED="NO" CODE_SIGN_ENTITLEMENTS=""  CODE_SIGNING_ALLOWED="NO"
+cd $pwd
+fi
+
+mkdir -p rtabmap
 cd rtabmap
 cmake -DANDROID_PREBUILD=ON ../../../../..
 cmake --build . --config Release
-mkdir ios
+mkdir -p ios
 cd ios
-cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF ../../../../../..
+cmake -G Xcode -DCMAKE_SYSTEM_NAME=iOS -DCMAKE_OSX_ARCHITECTURES=arm64 -DCMAKE_OSX_SYSROOT=$sysroot -DBUILD_SHARED_LIBS=OFF -DCMAKE_BUILD_TYPE=Release -DCMAKE_OSX_DEPLOYMENT_TARGET=12.0 -DCMAKE_INSTALL_PREFIX=$prefix -DCMAKE_FIND_ROOT_PATH=$prefix -DWITH_QT=OFF -DBUILD_APP=OFF -DBUILD_TOOLS=OFF -DWITH_TORO=OFF -DWITH_VERTIGO=OFF -DWITH_MADGWICK=OFF -DWITH_ORB_OCTREE=OFF  -DBUILD_EXAMPLES=OFF -DWITH_LIBLAS=ON ../../../../../..
 cmake --build . --config Release
 cmake --build . --config Release --target install

app/ios/Settings.bundle/Assembling.plist

@@ -346,6 +346,32 @@
 				<integer>0</integer>
 			</array>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>The format of exported point cloud data.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>Export Point Cloud Format</string>
+			<key>Key</key>
+			<string>ExportPointCloudFormat</string>
+			<key>DefaultValue</key>
+			<string>ply</string>
+			<key>Titles</key>
+			<array>
+				<string>PLY</string>
+				<string>LAS</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>ply</string>
+				<string>las</string>
+			</array>
+		</dict>
 	</array>
 </dict>
 </plist>

app/ios/Settings.bundle/License.plist

@@ -13,7 +13,7 @@
 			<string>
 ======= RTAB-Map =======
 RTAB-Map - https://github.com/introlab/rtabmap
-Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
+Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Universite de Sherbrooke, all rights reserved.
 Copyright (c) XXX, contributors, all rights reserved.
 
 Redistribution and use in source and binary forms, with or without
@@ -165,6 +165,53 @@ 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.
 
+======= libLAS =======
+
+Copyright (c) 2007, Martin Isenburg, isenburg at cs.unc.edu
+
+Copyright (c) 2008, Howard Butler, hobu.inc at gmail.com
+
+Copyright (c) 2008, Mateusz Loskot, mateusz at loskot.net
+
+Copyright (c) 2008, Phil Vachon, philippe at cowpig.ca
+
+Copyright (c) 2008, Frank Warmerdam, warmerdam at pobox.com
+
+Copyright (c) 2008, Martin Rodriguez, mrodriguez at stereocarto.com
+
+Copyright (c) 2016, Oscar Martinez Rubi o.rubi at esciencecenter.nl
+
+Copyright (c) 2016, Romulo Goncalves r.goncalves at esciencecenter.nl
+
+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 Martin Isenburg or Iowa Department
+      of Natural Resources 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 OWNER 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.
 
 			</string>
 		</dict>

app/ios/Settings.bundle/Mapping.plist

@@ -70,6 +70,48 @@
 			<key>DefaultValue</key>
 			<false/>
 		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSGroupSpecifier</string>
+			<key>FooterText</key>
+			<string>Filter ARKit's re-localization events to avoid jumps in odometry when creating a map with RTAB-Map, which would cause large drift errors that are difficult to correct. Set an acceleration threshold to detect those events.</string>
+		</dict>
+		<dict>
+			<key>Type</key>
+			<string>PSMultiValueSpecifier</string>
+			<key>Title</key>
+			<string>ARKit Re-Localization Acceleration Threshold</string>
+			<key>Key</key>
+			<string>UpstreamRelocalizationFilteringAccThr</string>
+			<key>DefaultValue</key>
+			<string>58.8399</string>
+			<key>Titles</key>
+			<array>
+				<string>Disabled</string>
+				<string>10 g</string>
+				<string>9 g</string>
+				<string>8 g</string>
+				<string>7 g</string>
+				<string>6 g</string>
+				<string>5 g</string>
+				<string>4 g</string>
+				<string>3 g</string>
+				<string>2 g</string>
+			</array>
+			<key>Values</key>
+			<array>
+				<string>0</string>
+				<string>98.0665</string>
+				<string>88.25985</string>
+				<string>78.4532</string>
+				<string>68.64655</string>
+				<string>58.8399</string>
+				<string>49.03325</string>
+				<string>39.2266</string>
+				<string>29.41995</string>
+				<string>19.6133</string>
+			</array>
+		</dict>
 		<dict>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
@@ -368,7 +410,7 @@
 			<key>Key</key>
 			<string>MaxOptimizationError</string>
 			<key>DefaultValue</key>
-			<integer>1</integer>
+			<integer>2</integer>
 			<key>Titles</key>
 			<array>
 				<string>4x</string>

app/ios/Settings.bundle/Root.plist

@@ -60,7 +60,7 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.0</real>
+				<real>0</real>
 				<real>5</real>
 				<real>4.5</real>
 				<real>4</real>
@@ -74,7 +74,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.0</real>
+			<real>0</real>
 			<key>Key</key>
 			<string>MinDepth</string>
 			<key>Title</key>
@@ -95,8 +95,8 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.0</real>
-				<real>0.29999999999999999</real>
+				<real>0</real>
+				<real>0.3</real>
 				<real>0.5</real>
 				<real>0.75</real>
 				<real>1</real>
@@ -130,7 +130,7 @@
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don't affect post-processing.</string>
+			<string>Increase minimum polygon angle to force scanning perpendicular to surfaces. Set a decimation factor to reduce the number of polygons. The higher the factor, the less polygons will remain. Note that the decimation factor is done after decreasing point cloud density. Decimation factor and texture resolution don&apos;t affect post-processing.</string>
 			<key>Type</key>
 			<string>PSGroupSpecifier</string>
 		</dict>
@@ -196,7 +196,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.0</real>
+			<real>0</real>
 			<key>Key</key>
 			<string>MeshDecimationFactor</string>
 			<key>Title</key>
@@ -220,18 +220,18 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.98999999999999999</real>
-				<real>0.94999999999999996</real>
-				<real>0.90000000000000002</real>
-				<real>0.80000000000000004</real>
-				<real>0.69999999999999996</real>
-				<real>0.59999999999999998</real>
+				<real>0.99</real>
+				<real>0.95</real>
+				<real>0.9</real>
+				<real>0.8</real>
+				<real>0.7</real>
+				<real>0.6</real>
 				<real>0.5</real>
-				<real>0.40000000000000002</real>
-				<real>0.29999999999999999</real>
-				<real>0.20000000000000001</real>
-				<real>0.10000000000000001</real>
-				<real>0.0</real>
+				<real>0.4</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0</real>
 			</array>
 		</dict>
 		<dict>
@@ -266,7 +266,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.20000000000000001</real>
+			<real>0.2</real>
 			<key>Key</key>
 			<string>BackgroundColor</string>
 			<key>Title</key>
@@ -288,16 +288,16 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.90000000000000002</real>
-				<real>0.80000000000000004</real>
-				<real>0.69999999999999996</real>
-				<real>0.59999999999999998</real>
+				<real>0.9</real>
+				<real>0.8</real>
+				<real>0.7</real>
+				<real>0.6</real>
 				<real>0.5</real>
-				<real>0.40000000000000002</real>
-				<real>0.29999999999999999</real>
-				<real>0.20000000000000001</real>
-				<real>0.10000000000000001</real>
-				<real>0.0</real>
+				<real>0.4</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0</real>
 			</array>
 		</dict>
 		<dict>
@@ -402,19 +402,19 @@
 			<string>PSMultiValueSpecifier</string>
 			<key>Values</key>
 			<array>
-				<real>0.29999999999999999</real>
-				<real>0.20000000000000001</real>
-				<real>0.10000000000000001</real>
-				<real>0.050000000000000003</real>
-				<real>0.025000000000000001</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0.05</real>
+				<real>0.025</real>
 				<real>0.02</real>
-				<real>0.014999999999999999</real>
+				<real>0.015</real>
 				<real>0.01</real>
 			</array>
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<real>0.050000000000000003</real>
+			<real>0.05</real>
 			<key>Key</key>
 			<string>NoiseFilteringRatio</string>
 			<key>Title</key>
@@ -439,22 +439,22 @@
 			<key>Values</key>
 			<array>
 				<real>1</real>
-				<real>0.90000000000000002</real>
-				<real>0.80000000000000004</real>
-				<real>0.69999999999999996</real>
-				<real>0.59999999999999998</real>
+				<real>0.9</real>
+				<real>0.8</real>
+				<real>0.7</real>
+				<real>0.6</real>
 				<real>0.5</real>
-				<real>0.40000000000000002</real>
-				<real>0.29999999999999999</real>
-				<real>0.20000000000000001</real>
-				<real>0.10000000000000001</real>
-				<real>0.050000000000000003</real>
+				<real>0.4</real>
+				<real>0.3</real>
+				<real>0.2</real>
+				<real>0.1</real>
+				<real>0.05</real>
 				<real>0.01</real>
 			</array>
 		</dict>
 		<dict>
 			<key>FooterText</key>
-			<string>Copyright (c) 2010-2023, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
+			<string>Copyright (c) 2010-2024, Mathieu Labbe - IntRoLab - Université de Sherbrooke. All rights reserved.</string>
 			<key>Title</key>
 			<string>About</string>
 			<key>Type</key>
@@ -462,7 +462,7 @@
 		</dict>
 		<dict>
 			<key>DefaultValue</key>
-			<string>0.21.0</string>
+			<string>0.21.7</string>
 			<key>Key</key>
 			<string>Version</string>
 			<key>Title</key>

corelib/include/rtabmap/core/LASWriter.h

@@ -0,0 +1,43 @@
+/*
+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 CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
+#define CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+
+namespace rtabmap {
+
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZ> & cloud,          const std::vector<int> & cameraIds = std::vector<int>());
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,       const std::vector<int> & cameraIds = std::vector<int>(), const std::vector<float> & intensities = std::vector<float>());
+int saveLASFile(const std::string & filePath, const pcl::PointCloud<pcl::PointXYZI> & cloud,         const std::vector<int> & cameraIds = std::vector<int>());
+
+}
+
+
+#endif /* CORELIB_INCLUDE_RTABMAP_CORE_LASWRITER_H_ */

corelib/src/CMakeLists.txt

@@ -539,6 +539,22 @@ IF(PDAL_FOUND)
 	ENDIF(PDAL_VERSION VERSION_LESS "1.7")
 ENDIF(PDAL_FOUND)
 
+IF(libLAS_FOUND)
+	SET(INCLUDE_DIRS
+		${INCLUDE_DIRS}
+		${libLAS_INCLUDE_DIRS}
+	)
+	SET(LIBRARIES
+		${LIBRARIES}
+		${libLAS_LIBRARIES}
+	)
+	SET(SRC_FILES
+		${SRC_FILES}
+		LASWriter.cpp
+	)
+ENDIF(libLAS_FOUND)
+
+
 IF(CudaSift_FOUND)
 	#target
 	SET(LIBRARIES

corelib/src/LASWriter.cpp

@@ -0,0 +1,169 @@
+/*
+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/LASWriter.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/ULogger.h>
+#include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UConversion.h>
+
+#include <fstream>
+#include <liblas/liblas.hpp>
+
+namespace rtabmap {
+
+int saveLASFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZ> & cloud,
+		const std::vector<int> & cameraIds)
+{
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
+	std::ofstream ofs;
+	ofs.open(filePath, std::ios::out | std::ios::binary);
+
+	liblas::Header header;
+	header.SetScale(0.001, 0.001, 0.001);
+
+	header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
+
+	try {
+		liblas::Writer writer(ofs, header);
+
+		for(size_t i=0; i<cloud.size(); ++i)
+		{
+			liblas::Point point(&header);
+			point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
+			if(!cameraIds.empty())
+			{
+				point.SetPointSourceID(cameraIds.at(i));
+			}
+			
+			writer.WritePoint(point);
+		}
+	}
+	catch(liblas::configuration_error & e)
+	{
+		UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
+		return 1;
+	}
+
+	return 0; //success
+}
+
+int saveLASFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
+		const std::vector<int> & cameraIds,
+		const std::vector<float> & intensities)
+{
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+	UASSERT_MSG(intensities.empty() || intensities.size() == cloud.size(),
+				uFormat("intensities=%d cloud=%d", (int)intensities.size(), (int)cloud.size()).c_str());
+
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
+	std::ofstream ofs;
+	ofs.open(filePath, std::ios::out | std::ios::binary);
+
+	liblas::Header header;
+
+	header.SetScale(0.001, 0.001, 0.001);
+	header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
+
+	try {
+		liblas::Writer writer(ofs, header);
+
+		for(size_t i=0; i<cloud.size(); ++i)
+		{
+			liblas::Point point(&header);
+			point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
+			liblas::Color color(cloud.at(i).r*65535/255, cloud.at(i).g*65535/255, cloud.at(i).b*65535/255);
+			point.SetColor(color);
+			if(!cameraIds.empty())
+			{
+				point.SetPointSourceID(cameraIds.at(i));
+			}
+			if(!intensities.empty())
+			{
+				point.SetIntensity(intensities.at(i));
+			}
+			
+			writer.WritePoint(point);
+		}
+	}
+	catch(liblas::configuration_error & e)
+	{
+		UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
+		return 1;
+	}
+
+	return 0; //success
+}
+
+int saveLASFile(const std::string & filePath,
+		const pcl::PointCloud<pcl::PointXYZI> & cloud,
+		const std::vector<int> & cameraIds)
+{
+	UASSERT_MSG(cameraIds.empty() || cameraIds.size() == cloud.size(),
+			uFormat("cameraIds=%d cloud=%d", (int)cameraIds.size(), (int)cloud.size()).c_str());
+
+	std::ofstream ofs;
+	ofs.open(filePath, std::ios::out | std::ios::binary);
+
+	liblas::Header header;
+	header.SetScale(0.001, 0.001, 0.001);
+
+	header.SetCompressed(uToLowerCase(UFile::getExtension(filePath)) == "laz");
+
+	try {
+		liblas::Writer writer(ofs, header);
+
+		for(size_t i=0; i<cloud.size(); ++i)
+		{
+			liblas::Point point(&header);
+			point.SetCoordinates(cloud.at(i).x, cloud.at(i).y, cloud.at(i).z);
+			if(!cameraIds.empty())
+			{
+				point.SetPointSourceID(cameraIds.at(i));
+			}
+			
+			writer.WritePoint(point);
+		}
+	}
+	catch(liblas::configuration_error & e)
+	{
+		UERROR("\"laz\" format not available, use \"las\" instead: %s", e.what());
+		return 1;
+	}
+
+	return 0; //success
+}
+
+}
+

corelib/src/Parameters.cpp

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

corelib/src/optimizer/OptimizerGTSAM.cpp

@@ -217,7 +217,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 			{
 				gtsam::noiseModel::Diagonal::shared_ptr priorNoise = gtsam::noiseModel::Diagonal::Variances(
 						(gtsam::Vector(6) <<
-								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), hasGPSPrior?1e-2:std::numeric_limits<double>::min(), // roll, pitch, fixed yaw if there are no priors
+								(hasGravityConstraints?2:1e-2), (hasGravityConstraints?2:1e-2), (hasGPSPrior?1e-2:std::numeric_limits<double>::min()), // roll, pitch, fixed yaw if there are no priors
 								(hasGPSPrior?2:1e-2), hasGPSPrior?2:1e-2, hasGPSPrior?2:1e-2 // xyz
 								).finished());
 				graph.add(gtsam::PriorFactor<gtsam::Pose3>(rootId, gtsam::Pose3(initialPose.toEigen4d()), priorNoise));
@@ -473,7 +473,7 @@ std::map<int, Transform> OptimizerGTSAM::optimize(
 				{
 					Vector3 r = gtsam::Pose3(iter->second.transform().toEigen4d()).rotation().xyz();
 					gtsam::Unit3 nG = gtsam::Rot3::RzRyRx(r.x(), r.y(), 0).rotate(gtsam::Unit3(0,0,-1));
-					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), 10));
+					gtsam::SharedNoiseModel model = gtsam::noiseModel::Isotropic::Sigmas(gtsam::Vector2(gravitySigma(), gravitySigma()));
 					graph.add(Pose3GravityFactor(iter->first, nG, model, Unit3(0,0,1)));
 					lastAddedConstraints_.push_back(ConstraintToFactor(iter->first, iter->first, -1));
 				}

guilib/src/AboutDialog.cpp

@@ -107,6 +107,13 @@ AboutDialog::AboutDialog(QWidget * parent) :
 	_ui->label_pdal->setText("No");
 	_ui->label_pdal_license->setEnabled(false);
 #endif
+#ifdef RTABMAP_LIBLAS
+	_ui->label_liblas->setText("Yes");
+	_ui->label_liblas_license->setEnabled(true);
+#else
+	_ui->label_liblas->setText("No");
+	_ui->label_liblas_license->setEnabled(false);
+#endif
 #ifdef RTABMAP_CUDASIFT
 	_ui->label_cudasift->setText("Yes");
 	_ui->label_cudasift_license->setEnabled(true);

guilib/src/DatabaseViewer.cpp

@@ -7620,6 +7620,7 @@ void DatabaseViewer::updateGraphView()
 
 		// remove intermediate nodes?
 		if(ui_->checkBox_ignoreIntermediateNodes->isVisible() &&
+		   ui_->checkBox_ignoreIntermediateNodes->isEnabled() &&
 		   ui_->checkBox_ignoreIntermediateNodes->isChecked())
 		{
 			for(std::multimap<int, Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)

guilib/src/ExportCloudsDialog.cpp

@@ -79,6 +79,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 #ifdef RTABMAP_PDAL
 #include <rtabmap/core/PDALWriter.h>
+#elif defined(RTABMAP_LIBLAS)
+#include <rtabmap/core/LASWriter.h>
 #endif
 
 namespace rtabmap {
@@ -211,7 +213,7 @@ ExportCloudsDialog::ExportCloudsDialog(QWidget *parent) :
 	connect(_ui->checkBox_camProjKeepPointsNotSeenByCameras, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->checkBox_camProjRecolorPoints, SIGNAL(stateChanged(int)), this, SIGNAL(configChanged()));
 	connect(_ui->comboBox_camProjExportCamera, SIGNAL(currentIndexChanged(int)), this, SIGNAL(configChanged()));
-#ifndef RTABMAP_PDAL
+#if !defined(RTABMAP_PDAL) && !defined(RTABMAP_LIBLAS)
 	_ui->comboBox_camProjExportCamera->setEnabled(false);
 	_ui->label_camProjExportCamera->setEnabled(false);
 	_ui->label_camProjExportCamera->setText(_ui->label_camProjExportCamera->text() + " (PDAL dependency required)");
@@ -4046,6 +4048,8 @@ void ExportCloudsDialog::saveClouds(
 			extensions += QString(" *.") + iter->c_str();
 		}
 		extensions += ")";
+#elif defined(RTABMAP_LIBLAS)
+		QString extensions = tr("Point cloud data (*.ply *.pcd *.las *.laz)");
 #else
 		QString extensions = tr("Point cloud data (*.ply *.pcd)");
 #endif
@@ -4160,7 +4164,7 @@ void ExportCloudsDialog::saveClouds(
 						success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
 					}
 				}
-#ifdef RTABMAP_PDAL
+#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
 				else if(!QFileInfo(path).suffix().isEmpty())
 				{
 					std::vector<int> cameraIds(pointToPixels.size(), 0);
@@ -4173,19 +4177,37 @@ void ExportCloudsDialog::saveClouds(
 					}
 					if(cloudIWithNormals.get())
 					{
+#ifdef RTABMAP_PDAL
 						success = savePDALFile(path.toStdString(), *cloudIWithNormals, cameraIds, binaryMode) == 0;
+#else
+						UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation.");
+						success = false;
+#endif
 					}
 					else if(cloudIWithoutNormals.get())
 					{
+#ifdef RTABMAP_PDAL
 						success = savePDALFile(path.toStdString(), *cloudIWithoutNormals, cameraIds, binaryMode) == 0;
+#else
+						success = saveLASFile(path.toStdString(), *cloudIWithoutNormals, cameraIds) == 0;
+#endif
 					}
 					else if(cloudRGBWithoutNormals.get())
 					{
+#ifdef RTABMAP_PDAL
 						success = savePDALFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds, binaryMode) == 0;
+#else
+						success = saveLASFile(path.toStdString(), *cloudRGBWithoutNormals, cameraIds) == 0;
+#endif
 					}
 					else
 					{
+#ifdef RTABMAP_PDAL
 						success = savePDALFile(path.toStdString(), *clouds.begin()->second, cameraIds, binaryMode) == 0;
+#else
+						UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation.");
+						success = false;
+#endif
 					}
 				}
 #endif
@@ -4230,6 +4252,8 @@ void ExportCloudsDialog::saveClouds(
 			items.push_back(iter->c_str());
 		}
 		extensions += ")...";
+#elif defined(RTABMAP_LIBLAS)
+		QString extensions = tr("Save clouds to (*.ply *.pcd *.las *.laz)...");
 #else
 		QString extensions = tr("Save clouds to (*.ply *.pcd)...");
 #endif
@@ -4343,24 +4367,42 @@ void ExportCloudsDialog::saveClouds(
 									success = pcl::io::savePLYFile(pathFile.toStdString(), *transformedCloud, binaryMode) == 0;
 								}
 							}
-#ifdef RTABMAP_PDAL
+#if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
 							else if(!suffix.isEmpty())
 							{
 								if(cloudIWithNormals.get())
 								{
+#ifdef RTABMAP_PDAL
 									success = savePDALFile(pathFile.toStdString(), *cloudIWithNormals) == 0;
+#else
+									UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation.");
+									success = false;
+#endif
 								}
 								else if(cloudIWithoutNormals.get())
 								{
+#ifdef RTABMAP_PDAL
 									success = savePDALFile(pathFile.toStdString(), *cloudIWithoutNormals) == 0;
+#else
+									success = saveLASFile(pathFile.toStdString(), *cloudIWithoutNormals) == 0;
+#endif
 								}
 								else if(cloudRGBWithoutNormals.get())
 								{
+#ifdef RTABMAP_PDAL
 									success = savePDALFile(pathFile.toStdString(), *cloudRGBWithoutNormals) == 0;
+#else
+									success = saveLASFile(pathFile.toStdString(), *cloudRGBWithoutNormals) == 0;
+#endif
 								}
 								else
 								{
+#ifdef RTABMAP_PDAL
 									success = savePDALFile(pathFile.toStdString(), *transformedCloud) == 0;
+#else
+									UERROR("Normals cannot be save with current libLAS implementation, disable normals estimation.");
+									success = false;
+#endif
 								}
 							}
 #endif

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>rtabmap</name>
-  <version>0.21.6</version>
+  <version>0.21.7</version>
   <description>RTAB-Map's standalone library. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

tools/Export/main.cpp

@@ -256,6 +256,8 @@ int main(int argc, char * argv[])
 		{
 #ifdef RTABMAP_PDAL
 			las = true;
+#elif defined(RTABMAP_LIBLAS)
+			printf("\"--las\" option cannot be used with libLAS because the cloud has normals, build RTAB-Map with PDAL support to export in las with normals. Will export in PLY...\n");
 #else
 			printf("\"--las\" option cannot be used because RTAB-Map is not built with PDAL support. Will export in PLY...\n");
 #endif