iOS feb 2025 various updates (#1451)

* Using shadows on all texts and icons to see them better when background is white. Buffering database previews to show faster the Library. Updated how RAM usage is computed and now show max memory. Added warnings when RAM usage is low.

* Added measuring tool

* fixed map not shown when closing visualization

* Added measure text size setting, changed look and feel of point to point

* Fixed measuring point not showing

* Added fix for #1401

* Addressing #1407

* Export: Added colored OBJ options

* fixed index_t not existing on focal

* Fixed Settings not applied after restoring to all default settings witohut restarting the app (Marker detection not working issue #1455 )

* Marker detection: Fixing wrong depth used when rgb and depth image sizes cannot be divided exactly one from the the other #1455

* Added Marker Max Range option (default 1m)

* Added OBJ texture policy option (keep color on textureless polygons)

* Added texture/color blending option directly in the app. Added LAZ export option.

* Hiding measuring button if not mesh, dont zip if exporting to laz, updating Export XXX button based on the current context, fixed always blending texture/color on not visualization mode

* updated default marker max range to 2m

* bump ios app version

* fixing pcl 1.8.1 build

* fixed android build

app/android/jni/RTABMapApp.cpp

@@ -132,6 +132,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 
 	parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
 
+    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
@@ -150,6 +151,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
+    parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisPnPVarianceMedianRatio(), std::string("2")));
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
 	parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
@@ -264,13 +266,43 @@ RTABMapApp::RTABMapApp() :
 		lastPoseEventTime_(0.0),
 		visualizingMesh_(false),
 		exportedMeshUpdated_(false),
-		optMesh_(new pcl::TextureMesh),
+		measuresUpdated_(false),
+        targetPoint_(new pcl::PointCloud<pcl::PointXYZRGB>),
+        quadSample_(new pcl::PointCloud<pcl::PointXYZ>),
+        quadSamplePolygons_(2),
+        metricSystem_(true),
+        measuringTextSize_(0.05f),
+		snapAxisThr_(0.95),
+        measuringMode_(0),
+        addMeasureClicked_(false),
+        teleportClicked_(false),
+        removeMeasureClicked_(false),
+		optTextureMesh_(new pcl::TextureMesh),
 		optRefId_(0),
 		optRefPose_(0),
 		mapToOdom_(rtabmap::Transform::getIdentity())
 
 {
-	mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
+    pcl::PointXYZRGB ptWhite;
+    ptWhite.r = ptWhite.g = ptWhite.b = 255;
+    targetPoint_->push_back(ptWhite);
+    snapAxes_.push_back(cv::Vec3f(1,0,0));
+    snapAxes_.push_back(cv::Vec3f(0,1,0));
+    snapAxes_.push_back(cv::Vec3f(0,0,1));
+
+    float quadSize = 0.05f;
+    quadSample_->push_back(pcl::PointXYZ(-quadSize, -quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(quadSize, -quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(quadSize, quadSize, 0.0f));
+    quadSample_->push_back(pcl::PointXYZ(-quadSize, quadSize, 0.0f));
+    quadSamplePolygons_[0].vertices.resize(3);
+    quadSamplePolygons_[0].vertices[0] = 0;
+    quadSamplePolygons_[0].vertices[1] = 1;
+    quadSamplePolygons_[0].vertices[2] = 2;
+    quadSamplePolygons_[1].vertices.resize(3);
+    quadSamplePolygons_[1].vertices[0] = 0;
+    quadSamplePolygons_[1].vertices[1] = 2;
+    quadSamplePolygons_[1].vertices[2] = 3;
 
 #ifdef __ANDROID__
 	env->GetJavaVM(&jvm);
@@ -278,19 +310,6 @@ RTABMapApp::RTABMapApp() :
 #endif
     
 	LOGI("RTABMapApp::RTABMapApp()");
-	createdMeshes_.clear();
-	rawPoses_.clear();
-	clearSceneOnNextRender_ = true;
-	openingDatabase_ = false;
-	exporting_ = false;
-	postProcessing_=false;
-	totalPoints_ = 0;
-	totalPolygons_ = 0;
-	lastDrawnCloudsCount_ = 0;
-	renderingTime_ = 0.0f;
-	lastPostRenderEventTime_ = 0.0;
-	lastPoseEventTime_ = 0.0;
-	bufferedStatsData_.clear();
 #ifdef __ANDROID__
 	progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
 #endif
@@ -404,13 +423,16 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
     lastPoseEventTime_ = 0.0;
     bufferedStatsData_.clear();
     graphOptimization_ = true;
+    measuresUpdated_ = !measures_.empty();
+    measures_.clear();
 
 	this->registerToEventsManager();
 
 	int status = 0;
 
 	// Open visualization while we load (if there is an optimized mesh saved in database)
-	optMesh_.reset(new pcl::TextureMesh);
+	optTextureMesh_.reset(new pcl::TextureMesh);
+    optMesh_ = rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	optRefId_ = 0;
 	if(optRefPose_)
@@ -418,6 +440,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 		delete optRefPose_;
 		optRefPose_ = 0;
 	}
+    visualizingMesh_ = false;
 	cv::Mat cloudMat;
 	std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
@@ -436,19 +459,20 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe
 			if(!cloudMat.empty())
 			{
 				LOGI("Open: Found optimized mesh! Visualizing it.");
-				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+                optMesh_ = rtabmap::Mesh();
 				optTexture_ = textures;
-				if(!optTexture_.empty())
+                if(!optTexture_.empty())
 				{
 					LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
 					status=3;
 				}
-				else if(optMesh_->tex_polygons.size())
+				else if(optTextureMesh_->tex_polygons.size())
 				{
 					LOGI("Open: Polygon mesh");
 					status=2;
 				}
-				else if(!optMesh_->cloud.data.empty())
+				else if(!optTextureMesh_->cloud.data.empty())
 				{
 					LOGI("Open: Point cloud");
 					status=1;
@@ -1330,7 +1354,8 @@ int RTABMapApp::Render()
 #ifdef DEBUG_RENDERING_PERFORMANCE
 				LOGD("Camera updateOnRender %fs", time.ticks());
 #endif
-				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
+                // We detect if we are in measuring mode if rtabmap is not running
+				if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0 && !(rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
 				{
 					main_scene_.background_renderer_ = new BackgroundRenderer();
 					main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
@@ -1424,40 +1449,189 @@ int RTABMapApp::Render()
 			{
 				main_scene_.clear();
 				exportedMeshUpdated_ = false;
+                measuresUpdated_ = measures_.size()>0;
 			}
 			if(!main_scene_.hasCloud(g_optMeshId))
 			{
 				LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...",
-						optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
-						optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
-						optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
-						(int)optMesh_->tex_materials.size(),
+						optTextureMesh_->cloud.point_step==0?0:(int)optTextureMesh_->cloud.data.size()/optTextureMesh_->cloud.point_step,
+						optTextureMesh_->tex_polygons.size()!=1?0:(int)optTextureMesh_->tex_polygons[0].size(),
+						optTextureMesh_->tex_coordinates.size()!=1?0:(int)optTextureMesh_->tex_coordinates[0].size(),
+						(int)optTextureMesh_->tex_materials.size(),
 						optTexture_.cols, optTexture_.rows);
-				if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
+				if(optTextureMesh_->tex_polygons.size() && optTextureMesh_->tex_polygons[0].size())
 				{
-					rtabmap::Mesh mesh;
-					mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
-					mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
-					mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
-					mesh.polygons = optMesh_->tex_polygons[0];
-					mesh.pose.setIdentity();
-					if(optMesh_->tex_coordinates.size())
+                    optMesh_ = rtabmap::Mesh();
+                    optMesh_.gains[0] = optMesh_.gains[1] = optMesh_.gains[2] = 1.0;
+                    optMesh_.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
+                    optMesh_.normals.reset(new pcl::PointCloud<pcl::Normal>);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.cloud);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *optMesh_.normals);
+                    bool hasColors = false;
+                    for(unsigned int i=0; i<optTextureMesh_->cloud.fields.size(); ++i)
+                    {
+                        if(optTextureMesh_->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    if(!hasColors)
+                    {
+                        std::uint8_t r = 255, g = 255, b = 255;    // White
+                        std::uint32_t rgb = ((std::uint32_t)r << 16 | (std::uint32_t)g << 8 | (std::uint32_t)b);
+                        for(size_t i=0; i<optMesh_.cloud->size(); ++i)
+                        {
+                            optMesh_.cloud->at(i).rgb = *reinterpret_cast<float*>(&rgb);
+                        }
+                    }
+                    optMesh_.polygons = optTextureMesh_->tex_polygons[0];
+					if(optTextureMesh_->tex_coordinates.size())
 					{
-						mesh.texCoords = optMesh_->tex_coordinates[0];
-						mesh.texture = optTexture_;
+                        optMesh_.texCoords = optTextureMesh_->tex_coordinates[0];
+                        optMesh_.texture = optTexture_;
 					}
-					main_scene_.addMesh(g_optMeshId, mesh, rtabmap::opengl_world_T_rtabmap_world, true);
+					main_scene_.addMesh(g_optMeshId, optMesh_, rtabmap::opengl_world_T_rtabmap_world, true);
 				}
 				else
 				{
 					pcl::IndicesPtr indices(new std::vector<int>); // null
 					pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
-					pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
+					pcl::fromPCLPointCloud2(optTextureMesh_->cloud, *cloud);
 					main_scene_.addCloud(g_optMeshId, cloud, indices, rtabmap::opengl_world_T_rtabmap_world);
 				}
+                
+                if(!measures_.empty())
+                {
+                    measuresUpdated_ = true;
+                }
 			}
+            
+            if(camera_ != 0 && (rtabmapThread_ == 0 || !rtabmapThread_->isRunning()))
+            {
+                updateMeasuringState();
+            }
+            else
+            {
+                main_scene_.removeLine(55555);
+                main_scene_.removeQuad(55555);
+                main_scene_.removeQuad(55556);
+                main_scene_.removeCircle(55555);
+                main_scene_.removeCircle(55556);
+                main_scene_.removeText(55555);
+                main_scene_.removeCloudOrMesh(-99999);
+                measuringTmpPts_.clear();
+                measuringTmpNormals_.clear();
+            }
+
+            if(measuresUpdated_)
+            {
+                std::list<Measure> measures = measures_;
+                measuresUpdated_ = false;
+                main_scene_.clearLines();
+                main_scene_.clearTexts();
+                main_scene_.clearQuads();
+                main_scene_.clearCircles();
+                int lineId = 0;
+                int textId = 0;
+                int quadId = 0;
+                int circleId = 0;
+                float sphereRadius = 0.02f;
+                float quadSize=0.05f;
+                float quadAlpha = 0.3f;
+
+                tango_gl::Color color(1.0f, 0.0f, 1.0f);
+                tango_gl::Color xColor(1.0f, 0.0f, 0.0f);
+                tango_gl::Color yColor(0.0f, 1.0f, 0.0f);
+                tango_gl::Color zColor(0.0f, 0.0f, 1.0f);
+
+                float restrictiveSnapThr = 0.9999;
+                for(std::list<Measure>::iterator iter=measures.begin(); iter!=measures.end(); ++iter)
+                {
+                    // Determinate color based on current snap axes
+                    tango_gl::Color quadColor = color;
+                    bool sameNormal = false;
+                    if(iter->n1().dot(iter->n2()) > 0.99) // Same normal, plane to plane
+                    {
+                        sameNormal = true;
+                        Eigen::Vector3f n(iter->n1()[0], iter->n1()[1], iter->n1()[2]);
+                        float n1ProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+                        float n1ProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+                        float n1ProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+                        if(fabs(n1ProdX) > restrictiveSnapThr)
+                        {
+                            quadColor = xColor;
+                        }
+                        else if(fabs(n1ProdY) > restrictiveSnapThr)
+                        {
+                            quadColor = yColor;
+                        }
+                        else if(fabs(n1ProdZ) > restrictiveSnapThr)
+                        {
+                            quadColor = zColor;
+                        }
+                    }
+                    
+                    const Measure & m = *iter;
+                    LOGI("dist=%f, %f,%f,%f -> %f,%f,%f", m.length(),
+                            m.pt1().x, m.pt1().y, m.pt1().z,
+                            m.pt2().x, m.pt2().y, m.pt2().z);
+                    cv::Point3f pt1 = rtabmap::util3d::transformPoint(m.pt1(), rtabmap::opengl_world_T_rtabmap_world);
+                    cv::Point3f pt2 = rtabmap::util3d::transformPoint(m.pt2(), rtabmap::opengl_world_T_rtabmap_world);
+                    main_scene_.addLine(++lineId, pt1, pt2, quadColor);
+
+                    if (fabs(iter->n1()[2]) < 0.00001 && sameNormal)
+                    {
+                        // Add a line so that in orthogonal view, we can see better where the lines are starting/finishing
+                        cv::Point3f n = cv::Vec3f(0,0,1).cross(iter->n1());
+
+                        n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world) * (quadSize/2);
+                        cv::Point3f pa = pt1 + n;
+                        cv::Point3f pb = pt1 - n;
+                        main_scene_.addLine(++lineId, pa, pb, quadColor);
+                        cv::Point3f pc = pt2 + n;
+                        cv::Point3f pd = pt2 - n;
+                        main_scene_.addLine(++lineId, pc, pd, quadColor);
+                    }
+
+                    float diff = m.length();
+                    std::string text = uFormat("%0.2f m", diff);
+                    if(!metricSystem_)
+                    {
+                        static const double METERS_PER_FOOT = 0.3048;
+                        static double INCHES_PER_FOOT = 12.0;
+                        double lengthInFeet = diff / METERS_PER_FOOT;
+                        int feet = (int)lengthInFeet;
+                        float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
+                        if(feet > 0)
+                        {
+                            text = uFormat("%d' %0.1f\"", feet, inches);
+                        }
+                        else
+                        {
+                            text = uFormat("%0.1f\"", inches);
+                        }
+                    }
+                    main_scene_.addText(++textId, text, rtabmap::Transform((pt1.x+pt2.x)/2.0f, (pt1.y+pt2.y)/2.0f, (pt1.z+pt2.z)/2.0f, 0, 0,0), measuringTextSize_, quadColor);
+                    
+                    cv::Vec3f n1 = rtabmap::util3d::transformPoint(m.n1(), rtabmap::opengl_world_T_rtabmap_world);
+                    cv::Vec3f n2 = rtabmap::util3d::transformPoint(m.n2(), rtabmap::opengl_world_T_rtabmap_world);
+                    Eigen::Quaternionf q1, q2;
+                    q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n1[0],n1[1],n1[2]));
+                    q2.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(n2[0],n2[1],n2[2]));
+                    
+                    if(sameNormal)
+                    {
+                        main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
+                        main_scene_.addQuad(++quadId, quadSize, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
+                    }
+                    else
+                    {
+                        main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt1.x, pt1.y, pt1.z, q1.x(), q1.y(), q1.z(), q1.w()), quadColor, quadAlpha);
+                        main_scene_.addCircle(++circleId, quadSize/2, rtabmap::Transform(pt2.x, pt2.y, pt2.z, q2.x(), q2.y(), q2.z(), q2.w()), quadColor, quadAlpha);
+                    }
+                }
+            }
 
 			if(!openingDatabase_)
 			{
@@ -1576,7 +1750,8 @@ int RTABMapApp::Render()
 			if(main_scene_.hasCloud(g_optMeshId))
 			{
 				main_scene_.clear();
-				optMesh_.reset(new pcl::TextureMesh);
+				optTextureMesh_.reset(new pcl::TextureMesh);
+                optMesh_ = rtabmap::Mesh();
 				optTexture_ = cv::Mat();
 			}
 
@@ -1639,6 +1814,8 @@ int RTABMapApp::Render()
 				lastPostRenderEventTime_ = 0.0;
 				lastPoseEventTime_ = 0.0;
 				bufferedStatsData_.clear();
+                measuresUpdated_ = !measures_.empty();
+                measures_.clear();
 			}
 
 			// Did we lose OpenGL context? If so, recreate the context;
@@ -2270,6 +2447,395 @@ int RTABMapApp::Render()
 	}
 }
 
+void RTABMapApp::updateMeasuringState()
+{
+    rtabmap::Transform openglCam = main_scene_.GetOpenGLCameraPose();
+    rtabmap::Transform rtabmapCam = rtabmap::rtabmap_world_T_opengl_world * openglCam * rtabmap::opengl_world_T_rtabmap_world;
+    Eigen::Vector3f origin(openglCam.x(), openglCam.y(), openglCam.z());
+    Eigen::Vector3f rtabmapOrigin(rtabmapCam.x(), rtabmapCam.y(), rtabmapCam.z());
+    rtabmap::Transform v = openglCam.rotation() * rtabmap::Transform(0,0,-1,0,0,0);
+    Eigen::Vector3f dir(v.x(), v.y(), v.z());
+    v = rtabmapCam.rotation() * rtabmap::Transform(1,0,0,0,0,0);
+    Eigen::Vector3f rtabmapDir(v.x(), v.y(), v.z());
+    tango_gl::Color color(1.0f, 0.0f, 1.0f);
+    tango_gl::Color xColor(1.0f, 0.0f, 0.0f); // in rtabmap world
+    tango_gl::Color yColor(0.0f, 1.0f, 0.0f); // in rtabmap world
+    tango_gl::Color zColor(0.0f, 0.0f, 1.0f); // in rtabmap world
+    float circleRadius = 0.025;
+    float quadSize=0.05f;
+    float quadAlpha = 0.3f;
+    
+    main_scene_.removeQuad(55555);
+    main_scene_.removeQuad(55556);
+    main_scene_.removeCircle(55555);
+    main_scene_.removeCircle(55556);
+    main_scene_.removeLine(55555);
+    main_scene_.removeText(55555);
+
+    if(removeMeasureClicked_)
+    {
+        if(!measuringTmpPts_.empty())
+        {
+            measuringTmpPts_.clear();
+            measuringTmpNormals_.clear();
+        }
+        else
+        {
+            bool removed = false;
+            for(std::list<Measure>::iterator iter=measures_.begin(); iter!=measures_.end() && !removed; ++iter)
+            {
+                const Measure & m = *iter;
+
+                for(int i=0; i<2;++i)
+                {
+                    // intersecting the quad?
+                    cv::Point3f pt = i==0?m.pt1():m.pt2();
+                    cv::Point3f n = i==0?m.n1():m.n2();
+                    pt = rtabmap::util3d::transformPoint(pt, rtabmap::opengl_world_T_rtabmap_world);
+                    n = rtabmap::util3d::transformPoint(n, rtabmap::opengl_world_T_rtabmap_world);
+                    Eigen::Quaternionf q;
+                    q.setFromTwoVectors(Eigen::Vector3f(0,0,-1), Eigen::Vector3f(n.x,n.y,n.z));
+                    float dTmp;
+                    Eigen::Vector3f nTmp;
+                    int indexTmp;
+                    if(rtabmap::util3d::intersectRayMesh(
+                                            origin,
+                                            dir,
+                                            *rtabmap::util3d::transformPointCloud(quadSample_, rtabmap::Transform(pt.x, pt.y, pt.z, q.x(), q.y(), q.z(), q.w())),
+                                            quadSamplePolygons_,
+                                            false,
+                                            dTmp,
+                                            nTmp,
+                                            indexTmp))
+                    {
+                        measures_.erase(iter);
+                        removed = true;
+                        break;
+                    }
+                }
+            }
+
+            measuresUpdated_ |= removed;
+        }
+    }
+
+    float distance =0.0f;
+    Eigen::Vector3f n;
+    int index;
+    if(rtabmap::util3d::intersectRayMesh(
+                    rtabmapOrigin,
+                    rtabmapDir,
+                    *optMesh_.cloud,
+                    optMesh_.polygons,
+                    true,
+                    distance,
+                    n,
+                    index))
+    {
+        
+        Eigen::Vector3f intersectionPt = origin + dir*distance;
+        cv::Point3f pt(intersectionPt[0], intersectionPt[1], intersectionPt[2]);
+        cv::Point3f normal(n[0], n[1], n[2]); // rtabmap world
+        cv::Point3f normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world); // opengl world
+        tango_gl::Color quadColor = color;
+        float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+        float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+        float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+        if(fabs(normal.x) > fabs(normal.z) && fabs(normal.y) > fabs(normal.z))
+        {
+            if(fabs(normalProdX) > snapAxisThr_)
+            {
+                normal.x = snapAxes_[0][0] * (normalProdX>0?1:-1);
+                normal.y = snapAxes_[0][1] * (normalProdX>0?1:-1);
+                normal.z = snapAxes_[0][2] * (normalProdX>0?1:-1);
+                quadColor = xColor;
+            }
+            else if(fabs(normalProdY) > snapAxisThr_)
+            {
+                normal.x = snapAxes_[1][0] * (normalProdY>0?1:-1);
+                normal.y = snapAxes_[1][1] * (normalProdY>0?1:-1);
+                normal.z = snapAxes_[1][2] * (normalProdY>0?1:-1);
+                quadColor = yColor;
+            }
+            else if(measuringMode_ == 0)
+            {
+                // We force to be aligned with xy plane
+                normal.z = 0;
+                float n = cv::norm(normal);
+                normal.x/=n;
+                normal.y/=n;
+                normal.z/=n;
+            }
+        }
+        else if(fabs(normalProdZ) > snapAxisThr_)
+        {
+            normal.x = snapAxes_[2][0] * (normalProdZ>0?1:-1);
+            normal.y = snapAxes_[2][1] * (normalProdZ>0?1:-1);
+            normal.z = snapAxes_[2][2] * (normalProdZ>0?1:-1);
+            quadColor = zColor;
+        }
+        normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
+
+        if(teleportClicked_)
+        {
+            camera_->resetOrigin(rtabmap::Transform(-pt.z, -pt.x, pt.y-Scene::kHeightOffset.y,0,0,0));
+        }
+        else if(addMeasureClicked_) // Add measure
+        {
+            if((measuringMode_ == 1 || measuringTmpPts_.size()==1))
+            {
+                if(measuringMode_ == 1) // Height single click
+                {
+                    measuringTmpPts_.clear();
+                    measuringTmpNormals_.clear();
+
+                    normal.x=0;
+                    normal.y=0;
+                    normal.z=1;
+                    normalGl = rtabmap::util3d::transformPoint(normal, rtabmap::opengl_world_T_rtabmap_world);
+
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                    pt.y = 0;
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                }
+                else if(measuringMode_ == 0 || measuringMode_ == 2) // Plane to Plane or point to point
+                {
+                    if(measuringMode_ == 0)
+                    {
+                        normalGl = measuringTmpNormals_.front();
+
+                        // project point on line
+                        float n = (pt-measuringTmpPts_.front()).dot(measuringTmpNormals_.front());
+                        pt = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
+                    }
+                    measuringTmpPts_.push_back(pt);
+                    measuringTmpNormals_.push_back(normalGl);
+                }
+
+                // Add measure!
+                const cv::Point3f & pt1 = measuringTmpPts_.at(0);
+                const cv::Point3f & pt2 = measuringTmpPts_.at(1);
+                const cv::Vec3f & n1 = measuringTmpNormals_.at(0);
+                const cv::Vec3f & n2 = measuringTmpNormals_.at(1);
+
+                Measure measure(
+                        rtabmap::util3d::transformPoint(pt1, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(pt2, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(n1, rtabmap::rtabmap_world_T_opengl_world),
+                        rtabmap::util3d::transformPoint(n2, rtabmap::rtabmap_world_T_opengl_world));
+
+                if(measure.length()>=0.01f)
+                {
+                    measures_.push_back(measure);
+
+                    measuringTmpPts_.clear();
+                    measuringTmpNormals_.clear();
+
+                    measuresUpdated_ = true;
+                }
+            }
+            else
+            {
+                measuringTmpPts_.clear();
+                measuringTmpNormals_.clear();
+
+                // init the first point
+                measuringTmpPts_.push_back(pt);
+                measuringTmpNormals_.push_back(normalGl);
+            }
+        }
+        else if(measuringMode_ >= 0)
+        {
+            // move action
+            if(measuringMode_ == 0 && measuringTmpNormals_.size()==1)
+            {
+                normalGl = measuringTmpNormals_.front();
+                normal = rtabmap::util3d::transformPoint(normalGl, rtabmap::rtabmap_world_T_opengl_world);
+                n = Eigen::Vector3f(normal.x, normal.y, normal.z);
+                float normalProdX = n.dot(Eigen::Vector3f(snapAxes_[0][0], snapAxes_[0][1], snapAxes_[0][2]));
+                float normalProdY = n.dot(Eigen::Vector3f(snapAxes_[1][0], snapAxes_[1][1], snapAxes_[1][2]));
+                float normalProdZ = n.dot(Eigen::Vector3f(snapAxes_[2][0], snapAxes_[2][1], snapAxes_[2][2]));
+                if(fabs(normalProdX) > snapAxisThr_)
+                {
+                    quadColor = xColor;
+                }
+                else if(fabs(normalProdY) > snapAxisThr_)
+                {
+                    quadColor = yColor;
+                }
+                else if(fabs(normalProdZ) > snapAxisThr_)
+                {
+                    quadColor = zColor;
+                }
+                else
+                {
+                    quadColor = color;
+                }
+            }
+
+            float quadWidthLeft = 0.05;
+            float quadWidthRight = quadWidthLeft;
+            float quadHeightBottom = quadWidthLeft;
+            float quadHeightTop = quadWidthLeft;
+            cv::Point3f pt2 = pt;
+            float lineLength = 0.0f;
+            // project point on line
+            if(measuringTmpPts_.size() == 1)
+            {
+                cv::Point3f v = pt2-measuringTmpPts_.front();
+                if(measuringMode_ == 0)
+                {
+                    float n = v.dot(measuringTmpNormals_.front());
+                    lineLength = fabs(n);
+                    pt2 = measuringTmpPts_.front() + (measuringTmpNormals_.front() * n);
+                    v = rtabmap::util3d::transformPoint(v, rtabmap::rtabmap_world_T_opengl_world);
+                    if(!(fabs(normal.z) > fabs(normal.x) && fabs(normal.z) > fabs(normal.y)))
+                    {
+                        quadHeightTop = v.z>quadSize?v.z:quadSize;
+                        quadHeightBottom = -v.z>quadSize?-v.z:quadSize;
+                        if(fabs(normal.x) > fabs(normal.y))
+                        {
+                            cv::Point3f y = cv::Point3f(0,0,1).cross(normal);
+                            float n = v.dot(y);
+                            quadWidthRight = n>quadSize?n:quadSize;
+                            quadWidthLeft = n<-quadSize?fabs(n):quadSize;
+                            if(normal.x>0)
+                            {
+                                quadWidthLeft = n>quadSize?n:quadSize;
+                                quadWidthRight = n<-quadSize?fabs(n):quadSize;
+                                float tmp  =quadHeightTop;
+                                quadHeightTop= quadHeightBottom;
+                                quadHeightBottom = tmp;
+                            }
+                        }
+                        else
+                        {
+                            cv::Point3f x = normal.cross(cv::Point3f(0,0,1));
+                            float n = v.dot(x);
+                            quadWidthRight = n<-quadSize?fabs(n):quadSize;
+                            quadWidthLeft = n>quadSize?n:quadSize;
+                        }
+                    }
+                    else
+                    {
+                        if(normal.z>0)
+                        {
+                            quadHeightBottom = v.x<-quadSize?fabs(v.x):quadSize;
+                            quadHeightTop = v.x>quadSize?v.x:quadSize;
+                            quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
+                            quadWidthLeft = v.y>quadSize?v.y:quadSize;
+                        }
+                        else
+                        {
+                            quadHeightTop = v.x<-quadSize?fabs(v.x):quadSize;
+                            quadHeightBottom = v.x>quadSize?v.x:quadSize;
+                            quadWidthRight = v.y<-quadSize?fabs(v.y):quadSize;
+                            quadWidthLeft = v.y>quadSize?v.y:quadSize;
+                        }
+                    }
+                }
+                else
+                {
+                    lineLength = cv::norm(v);
+                }
+                
+                std::string text = uFormat("%0.2f m", lineLength);
+                if(!metricSystem_)
+                {
+                    static const double METERS_PER_FOOT = 0.3048;
+                    static double INCHES_PER_FOOT = 12.0;
+                    double lengthInFeet = lineLength / METERS_PER_FOOT;
+                    int feet = (int)lengthInFeet;
+                    float inches = (lengthInFeet - feet) * INCHES_PER_FOOT;
+                    if(feet > 0)
+                    {
+                        text = uFormat("%d' %0.1f\"", feet, inches);
+                    }
+                    else
+                    {
+                        text = uFormat("%0.1f\"", inches);
+                    }
+                }
+                main_scene_.addText(55555, text, rtabmap::Transform(pt.x, pt.y, pt.z, 0,0,0), 0.05f, measuringMode_ == 0?quadColor:color);
+            }
+
+            Eigen::Quaternionf q;
+            q.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGl.x,normalGl.y,normalGl.z));
+
+            if(measuringTmpPts_.size() == 1 && measuringMode_ != 1)
+            {
+                const cv::Point3f & pt1 = measuringTmpPts_.at(0);
+                main_scene_.addLine(55555, pt1, pt2, measuringMode_ == 0?quadColor:color);
+
+                if(measuringMode_ == 2)
+                {
+                    // Use respective orientation for each circle
+                    Eigen::Quaternionf q1;
+                	cv::Vec3f normalGL1 = measuringTmpNormals_.front();
+                    q1.setFromTwoVectors(Eigen::Vector3f(0,0,1), Eigen::Vector3f(normalGL1[0],normalGL1[0],normalGL1[0]));
+                    main_scene_.addCircle(55555,
+                                        circleRadius,
+                                        rtabmap::Transform(
+                                                           measuringTmpPts_.front().x,
+                                                           measuringTmpPts_.front().y,
+                                                           measuringTmpPts_.front().z,
+                                                           q1.x(),q1.y(), q1.z(), q1.w()), color, quadAlpha);
+                    main_scene_.addCircle(55556,
+                                        circleRadius,
+                                        rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
+                }
+                else
+                {
+                    // Use same orientation for both quads
+                    main_scene_.addQuad(55555,
+                                        quadSize,
+                                        rtabmap::Transform(
+                                                           measuringTmpPts_.front().x,
+                                                           measuringTmpPts_.front().y,
+                                                           measuringTmpPts_.front().z,
+                                                           q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+                    main_scene_.addQuad(55556,
+                                quadWidthLeft,
+                                quadWidthRight,
+                                quadHeightBottom,
+                                quadHeightTop,
+                                rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+                }
+            }
+            else if(measuringMode_ == 2)
+            {
+                main_scene_.addCircle(55555,
+                            circleRadius,
+                            rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), color, quadAlpha);
+            }
+            else
+            {
+                main_scene_.addQuad(55555,
+                            quadSize,
+                            rtabmap::Transform(pt2.x, pt2.y, pt2.z, q.x(),q.y(), q.z(), q.w()), quadColor, quadAlpha);
+            }
+        }
+    }
+
+    Eigen::Vector3f target = origin+dir*(distance<100.0f?distance:0.5f);
+    rtabmap::Transform pose(target[0], target[1], target[2], 0,0,0);
+    if(main_scene_.hasCloud(-99999))
+    {
+        main_scene_.setCloudPose(-99999, pose);
+    }
+    else
+    {
+        main_scene_.addCloud(-99999, targetPoint_, pcl::IndicesPtr(), pose);
+    }
+
+    // reset states
+    removeMeasureClicked_ = false;
+    addMeasureClicked_ = false;
+    teleportClicked_ = false;
+}
+
 void RTABMapApp::SetCameraType(
     tango_gl::GestureCamera::CameraType camera_type) {
   main_scene_.SetCameraType(camera_type);
@@ -2337,6 +2903,15 @@ void RTABMapApp::setOrthoCropFactor(float value)
 }
 void RTABMapApp::setGridRotation(float value)
 {
+    // Update measuring snap axes
+    rtabmap::Transform rotation(0,0, value * DEGREE_2_RADIANS);
+    renderingMutex_.lock();
+    snapAxes_[0] = rtabmap::util3d::transformPoint(cv::Vec3f(1,0,0), rotation);
+    snapAxes_[1] = rtabmap::util3d::transformPoint(cv::Vec3f(0,1,0), rotation);
+    measuresUpdated_ = true;
+    renderingMutex_.unlock();
+    
+    // Update grid
 	main_scene_.setGridRotation(value);
 }
 void RTABMapApp::setLighting(bool enabled)
@@ -2351,6 +2926,10 @@ void RTABMapApp::setWireframe(bool enabled)
 {
 	main_scene_.setWireframe(enabled);
 }
+void RTABMapApp::setTextureColorSeamsHidden(bool hidden)
+{
+    main_scene_.setTextureColorSeamsHidden(hidden);
+}
 
 void RTABMapApp::setLocalizationMode(bool enabled)
 {
@@ -2538,7 +3117,7 @@ void RTABMapApp::setDepthConfidence(int value)
 void RTABMapApp::setExportPointCloudFormat(const std::string & format)
 {
 #if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
-    if(format == "las") {
+    if(format == "las" || format == "laz") {
         exportPointCloudFormat_ = format;
     }
     else
@@ -2710,6 +3289,7 @@ bool RTABMapApp::exportMesh(
 		int optimizedMinClusterSize,
 		float optimizedMaxTextureDistance,
 		int optimizedMinTextureClusterSize,
+        int textureVertexColorPolicy,
 		bool blockRendering)
 {
 	// make sure createdMeshes_ is not modified while exporting! We don't
@@ -2998,7 +3578,7 @@ bool RTABMapApp::exportMesh(
 									0,
 									mergedClouds,
 									optimizedColorRadius,
-									textureSize == 0,
+                                    !(textureSize > 0 && textureVertexColorPolicy == 0),
 									optimizedCleanWhitePolygons,
 									optimizedMinClusterSize);
 
@@ -3240,7 +3820,9 @@ bool RTABMapApp::exportMesh(
 							textureCount,
 							vertexToPixels,
 							true, 10.0f, true ,true, 0, 0, 0, false,
-							&progressionStatus_);
+							&progressionStatus_,
+                        	255,
+                            textureVertexColorPolicy == 1);
                     LOGI("Merging %d textures... globalTextures=%dx%d", (int)textureMesh->tex_materials.size(),
                          globalTextures.cols, globalTextures.rows);
 				}
@@ -3281,9 +3863,28 @@ bool RTABMapApp::exportMesh(
 				}
 				else if(textureMesh->tex_materials.size())
 				{
-					pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
-					pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
-					cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    bool hasColors = false;
+                    for(unsigned int i=0; i<textureMesh->cloud.fields.size(); ++i)
+                    {
+                        if(textureMesh->cloud.fields[i].name.compare("rgb") == 0)
+                        {
+                            hasColors = true;
+                            break;
+                        }
+                    }
+                    cv::Mat cloudMat;
+                    if(hasColors)
+                    {
+                        pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
+                    else
+                    {
+                        pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
+                        pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
+                        cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
+                    }
 
 					// save in database
 					std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
@@ -3492,7 +4093,8 @@ bool RTABMapApp::exportMesh(
 bool RTABMapApp::postExportation(bool visualize)
 {
 	LOGI("postExportation(visualize=%d)", visualize?1:0);
-	optMesh_.reset(new pcl::TextureMesh);
+	optTextureMesh_.reset(new pcl::TextureMesh);
+    optMesh_= rtabmap::Mesh();
 	optTexture_ = cv::Mat();
 	exportedMeshUpdated_ = false;
 
@@ -3513,8 +4115,9 @@ bool RTABMapApp::postExportation(bool visualize)
 			if(!cloudMat.empty())
 			{
 				LOGI("postExportation: Found optimized mesh! Visualizing it.");
-				optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
-				optTexture_ = textures;
+				optTextureMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
+                optMesh_ = rtabmap::Mesh();
+                optTexture_ = textures;
 
 				boost::mutex::scoped_lock  lock(renderingMutex_);
 				visualizingMesh_ = true;
@@ -3606,9 +4209,9 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 	if(polygonMesh->cloud.data.size())
 	{
 #if defined(RTABMAP_PDAL) || defined(RTABMAP_LIBLAS)
-        if(polygonMesh->polygons.empty() && exportPointCloudFormat_ == "las") {
+        if(polygonMesh->polygons.empty() && (exportPointCloudFormat_ == "las" || exportPointCloudFormat_ == "laz")) {
             // Point cloud LAS
-            std::string filePath = directory + UDirectory::separator() + name + ".las";
+            std::string filePath = directory + UDirectory::separator() + name + (exportPointCloudFormat_ == "las"? ".las" : ".laz");
             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);
@@ -3684,7 +4287,7 @@ bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::str
 				totalPolygons += textureMesh->tex_polygons[i].size();
 			}
 			LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
-			success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
+			success = rtabmap::util3d::saveOBJFile(filePath, *textureMesh) == 0;
 
 			if(success)
 			{
@@ -3799,6 +4402,66 @@ int RTABMapApp::postProcessing(int approach)
 	return returnedValue;
 }
 
+void RTABMapApp::clearMeasures()
+{
+    boost::mutex::scoped_lock  lockRender(renderingMutex_);
+    measures_.clear();
+    measuresUpdated_ = true;
+}
+
+void RTABMapApp::setMeasuringMode(int mode)
+{
+    if(visualizingMesh_)
+    {
+        measuringMode_ = mode;
+    }
+}
+
+void RTABMapApp::addMeasureButtonClicked()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        addMeasureClicked_ = true;
+    }
+}
+
+void RTABMapApp::teleportButtonClicked()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        teleportClicked_ = true;
+    }
+}
+
+void RTABMapApp::removeMeasure()
+{
+    if(visualizingMesh_)
+    {
+        boost::mutex::scoped_lock lock(renderingMutex_);
+        removeMeasureClicked_ = true;
+    }
+}
+
+void RTABMapApp::setMetricSystem(bool enabled)
+{
+    metricSystem_ = enabled;
+    if(measures_.size())
+    {
+        measuresUpdated_ = true;
+    }
+}
+
+void RTABMapApp::setMeasuringTextSize(float size)
+{
+    measuringTextSize_ = size;
+    if(measures_.size())
+    {
+        measuresUpdated_ = true;
+    }
+}
+
 void RTABMapApp::postOdometryEvent(
 		rtabmap::Transform pose,
 		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,