Added measuring tool

2026-09-03 10:00:23 +08:00 · 2025-02-17 20:20:19 -08:00
parent 994fad982f
commit 26d1238c8a
20 changed files with 1878 additions and 114 deletions
--- a/app/ios/RTABMapApp/ViewController.swift
+++ b/app/ios/RTABMapApp/ViewController.swift
@@ -55,8 +55,9 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        STATE_IDLE,            // Camera/Motion off
        STATE_PROCESSING,      // Camera/Motion off - post processing
        STATE_VISUALIZING,     // Camera/Motion off - Showing optimized mesh
-        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh
-        STATE_VISUALIZING_WHILE_LOADING // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_CAMERA,     // Camera/Motion on  - Showing optimized mesh and localizing
+        STATE_VISUALIZING_WHILE_LOADING, // Camera/Motion off - Loading data while showing optimized mesh
+        STATE_VISUALIZING_AND_MEASURING    // Camera/Motion on  - Showing optimized mesh without localizing and measuring tools enabled
    }
    private var mState: State = State.STATE_WELCOME;
    private func getStateString(state: State) -> String {
@@ -75,6 +76,8 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            return "Visualizing with Camera"
        case .STATE_VISUALIZING_WHILE_LOADING:
            return "Visualizing while Loading"
+        case .STATE_VISUALIZING_AND_MEASURING:
+            return "Measuring"
        default: // IDLE
            return "Idle"
        }
@@ -100,6 +103,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
    private var mMenuOpened: Bool = false
    private var lowMemoryWarningShown: Bool = false
    static var previewImages: [String: UIImage] = [:]
+    private var measuringMode: Int = 0
    
    @IBOutlet weak var stopButton: UIButton!
    @IBOutlet weak var recordButton: UIButton!
@@ -110,6 +114,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
    @IBOutlet weak var statusLabel: UILabel!
    @IBOutlet weak var closeVisualizationButton: UIButton!
    @IBOutlet weak var stopCameraButton: UIButton!
+    @IBOutlet weak var stopMeasuringButton: UIButton!
+    @IBOutlet weak var teleportButton: UIButton!
+    @IBOutlet weak var addMeasureButton: UIButton!
+    @IBOutlet weak var removeMeasureButton: UIButton!
+    @IBOutlet weak var measuringModeButton: UIButton!
    @IBOutlet weak var exportOBJPLYButton: UIButton!
    @IBOutlet weak var orthoDistanceSlider: UISlider!{
        didSet{
@@ -206,6 +215,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        addShadow(libraryButton)
        addShadow(closeVisualizationButton)
        addShadow(stopCameraButton)
+        addShadow(stopMeasuringButton)
+        addShadow(teleportButton)
+        addShadow(addMeasureButton)
+        addShadow(removeMeasureButton)
+        addShadow(measuringModeButton)
        addShadow(exportOBJPLYButton)
        
        addShadow(statusLabel, 0)
@@ -529,7 +543,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
    }
    
    @objc func appMovedToBackground() {
-        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
+        if(mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING || mState == .STATE_MAPPING || mState == .STATE_CAMERA)
        {
            stopMapping(ignoreSaving: true)
        }
@@ -580,29 +594,33 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        switch AVCaptureDevice.authorizationStatus(for: .video) {
            case .authorized: // The user has previously granted access to the camera.
                print("Start Camera")
-                rtabmap!.startCamera()
+            	rtabmap!.startCamera()
                let configuration = ARWorldTrackingConfiguration()
                var message = ""
-                if(!UserDefaults.standard.bool(forKey: "LidarMode"))
-                {
-                    message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
-                    self.setMeshRendering(viewMode: 0)
-                }
-                else if !depthSupported
-                {
-                    message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
-                    self.setMeshRendering(viewMode: 0)
-                }
-                else
-                {
-                    configuration.frameSemantics = .sceneDepth
-                }
+            	if(mState != .STATE_VISUALIZING_AND_MEASURING)
+            	{
+                	if(!UserDefaults.standard.bool(forKey: "LidarMode"))
+                	{
+       		        	message = "LiDAR is disabled (Settings->Mapping->LiDAR Mode = OFF), only tracked features will be mapped."
+       		        	self.setMeshRendering(viewMode: 0)
+        	    	}
+                	else if !depthSupported
+                	{
+                    	message = "The device does not have a LiDAR, only tracked features will be mapped. A LiDAR is required for accurate 3D reconstruction."
+                    	self.setMeshRendering(viewMode: 0)
+                	}
+                	else
+                	{
+                    	configuration.frameSemantics = .sceneDepth
+                	}
+            	}
                
                session.run(configuration, options: [.resetSceneReconstruction, .resetTracking, .removeExistingAnchors])
                
                switch mState {
-                case .STATE_VISUALIZING:
-                    updateState(state: .STATE_VISUALIZING_CAMERA)
+                case .STATE_VISUALIZING_AND_MEASURING,
+                    .STATE_VISUALIZING_CAMERA:
+                    break // State should be already set
                default:
                    locationManager?.startUpdatingLocation()
                    updateState(state: .STATE_CAMERA)
@@ -669,6 +687,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        var actionExportEnabled: Bool
        var actionOptimizeEnabled: Bool
        var actionSettingsEnabled: Bool
+        var actionMeasuringEnabled: Bool
        
        switch mState {
        case .STATE_CAMERA:
@@ -681,9 +700,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopButton.isHidden = true
            closeVisualizationButton.isHidden = true
            stopCameraButton.isHidden = false
+            stopMeasuringButton.isHidden = true
            exportOBJPLYButton.isHidden = true
            orthoDistanceSlider.isHidden = cameraMode != 3
            orthoGridSlider.isHidden = cameraMode != 3
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
            actionNewScanEnabled = !mDataRecording
            actionNewDataRecording = mDataRecording
            actionSaveEnabled = false
@@ -691,6 +715,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            actionExportEnabled = false
            actionOptimizeEnabled = false
            actionSettingsEnabled = false
+            actionMeasuringEnabled = false
        case .STATE_MAPPING:
            libraryButton.isEnabled = false
            libraryButton.isHidden = !mHudVisible
@@ -701,9 +726,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopButton.isHidden = false
            closeVisualizationButton.isHidden = true
            stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
            exportOBJPLYButton.isHidden = true
            orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
            orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
            actionNewScanEnabled = !mDataRecording
            actionNewDataRecording = mDataRecording
            actionSaveEnabled = false
@@ -711,9 +741,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            actionExportEnabled = false
            actionOptimizeEnabled = false
            actionSettingsEnabled = false
+            actionMeasuringEnabled = false
        case .STATE_PROCESSING,
             .STATE_VISUALIZING_WHILE_LOADING,
-             .STATE_VISUALIZING_CAMERA:
+             .STATE_VISUALIZING_CAMERA,
+             .STATE_VISUALIZING_AND_MEASURING:
            libraryButton.isEnabled = false
            libraryButton.isHidden = !mHudVisible
            menuButton.isHidden = !mHudVisible
@@ -723,9 +755,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopButton.isHidden = true
            closeVisualizationButton.isHidden = true
            stopCameraButton.isHidden = mState != .STATE_VISUALIZING_CAMERA
+            stopMeasuringButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING
            exportOBJPLYButton.isHidden = true
-            orthoDistanceSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
-            orthoGridSlider.isHidden = cameraMode != 3 || mState != .STATE_VISUALIZING_WHILE_LOADING
+            orthoDistanceSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            orthoGridSlider.isHidden = cameraMode != 3 || mState == .STATE_PROCESSING
+            teleportButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode != 0
+            addMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            removeMeasureButton.isHidden = mState != .STATE_VISUALIZING_AND_MEASURING || cameraMode == 3
+            measuringModeButton.isHidden = true
            actionNewScanEnabled = false
            actionNewDataRecording = false
            actionSaveEnabled = false
@@ -733,6 +770,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            actionExportEnabled = false
            actionOptimizeEnabled = false
            actionSettingsEnabled = false
+            actionMeasuringEnabled = mState == .STATE_VISUALIZING_AND_MEASURING
        case .STATE_VISUALIZING:
            libraryButton.isEnabled = !databases.isEmpty
            libraryButton.isHidden = !mHudVisible
@@ -743,9 +781,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopButton.isHidden = true
            closeVisualizationButton.isHidden = !mHudVisible
            stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
            exportOBJPLYButton.isHidden = !mHudVisible
            orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
            orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = !mHudVisible
            actionNewScanEnabled = true
            actionNewDataRecording = true
            actionSaveEnabled = mMapNodes>0
@@ -753,6 +796,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            actionExportEnabled = mMapNodes>0
            actionOptimizeEnabled = mMapNodes>0
            actionSettingsEnabled = true
+            actionMeasuringEnabled = true
        default: // IDLE // WELCOME
            libraryButton.isEnabled = !databases.isEmpty
            libraryButton.isHidden = mState != .STATE_WELCOME && !mHudVisible
@@ -763,9 +807,14 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            stopButton.isHidden = true
            closeVisualizationButton.isHidden = true
            stopCameraButton.isHidden = true
+            stopMeasuringButton.isHidden = true
            exportOBJPLYButton.isHidden = true
            orthoDistanceSlider.isHidden = cameraMode != 3 || !mHudVisible
            orthoGridSlider.isHidden = cameraMode != 3 || !mHudVisible
+            teleportButton.isHidden = true
+            addMeasureButton.isHidden = true
+            removeMeasureButton.isHidden = true
+            measuringModeButton.isHidden = true
            actionNewScanEnabled = true
            actionNewDataRecording = true
            actionSaveEnabled = mState != .STATE_WELCOME && mMapNodes>0
@@ -773,10 +822,11 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            actionExportEnabled = mState != .STATE_WELCOME && mMapNodes>0
            actionOptimizeEnabled = mState != .STATE_WELCOME && mMapNodes>0
            actionSettingsEnabled = true
+            actionMeasuringEnabled = false
        }

        let view = self.view as? GLKView
-        if(mState != .STATE_MAPPING && mState != .STATE_CAMERA && mState != .STATE_VISUALIZING_CAMERA)
+        if(mState != .STATE_MAPPING && mState != .STATE_CAMERA && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_VISUALIZING_AND_MEASURING)
        {
            self.isPaused = true
            view?.enableSetNeedsDisplay = true
@@ -856,6 +906,25 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            	self.newScan(dataRecordingMode: true)
        	})
        ])
+        
+        // Measuring menu
+        print("measuringMode = \(measuringMode)")
+        let measuringMenu = UIMenu(title: "Measuring...", image: UIImage(systemName: "ruler"), children: [
+            UIAction(title: "Plane to Plane Mode", image: measuringMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+                self.measuringMode = 0
+                self.rtabmap!.setMeasuringMode(self.measuringMode)
+                self.resetNoTouchTimer(true)
+            }),
+            UIAction(title: "Point to Point Mode", image: measuringMode == 2 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+                self.measuringMode = 2
+                self.rtabmap!.setMeasuringMode(self.measuringMode)
+                self.resetNoTouchTimer(true)
+            }),
+            UIAction(title: "Clear All Measures", image: UIImage(systemName: "trash"), state: .off, handler: { _ in
+                self.clearMeasures();
+                self.resetNoTouchTimer(true)
+            })
+        ])
                
        var fileMenuChildren: [UIMenuElement] = []
        fileMenuChildren.append(UIAction(title: "New Mapping Session", image: UIImage(systemName: "plus.app"), attributes: actionNewScanEnabled ? [] : .disabled, state: .off, handler: { _ in
@@ -881,6 +950,13 @@ 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()
        }))
+        if(actionMeasuringEnabled) {
+            fileMenuChildren.append(measuringMenu)
+        }
+        else {
+            fileMenuChildren.append(UIAction(title: "Measuring...", image: UIImage(systemName: "ruler"), attributes: .disabled, state: .off, handler: { _ in
+            }))
+        }
        fileMenuChildren.append(advancedMenu)
        
        // File menu
@@ -896,7 +972,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                self.debugShown = !self.debugShown
                self.resetNoTouchTimer(true)
            }),
-            UIAction(title: "Odom Visible", image: odomShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_MAPPING || self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING_CAMERA) ? [] : .disabled, handler: { _ in
+            UIAction(title: "Odom Visible", image: odomShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_MAPPING || self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                self.odomShown = !self.odomShown
                self.rtabmap!.setOdomCloudShown(shown: self.odomShown)
                self.resetNoTouchTimer(true)
@@ -956,7 +1032,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
                self.rtabmap!.setWireframe(enabled: self.wireframeShown)
                self.resetNoTouchTimer(true)
            }),
-            UIAction(title: "Lighting", image: self.lightingShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: self.mState == .STATE_VISUALIZING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_WHILE_LOADING ? [] : .disabled, handler: { _ in
+            UIAction(title: "Lighting", image: self.lightingShown ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: self.mState == .STATE_VISUALIZING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING || self.mState == .STATE_VISUALIZING_WHILE_LOADING ? [] : .disabled, handler: { _ in
                self.lightingShown = !self.lightingShown
                self.rtabmap!.setLighting(enabled: self.lightingShown)
                self.resetNoTouchTimer(true)
@@ -969,21 +1045,21 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        ])
        
        let cameraMenu = UIMenu(title: "View", options: .displayInline, children: [
-            UIAction(title: "First-P. View", image: cameraMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING || self.mState == .STATE_MAPPING || self.mState == .STATE_VISUALIZING_CAMERA) ? [] : .disabled, handler: { _ in
+            UIAction(title: "First-P. View", image: cameraMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState == .STATE_CAMERA || self.mState == .STATE_VISUALIZING || self.mState == .STATE_MAPPING || self.mState == .STATE_VISUALIZING_CAMERA || self.mState == .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                self.setGLCamera(type: 0)
                if(self.mState == .STATE_VISUALIZING)
                {
                    self.rtabmap?.setLocalizationMode(enabled: true)
                    self.rtabmap?.setPausedMapping(paused: false);
-                    self.startCamera()
                    self.updateState(state: .STATE_VISUALIZING_CAMERA)
+                    self.startCamera()
                }
                else
                {
                    self.resetNoTouchTimer(true)
                }
            }),
-            UIAction(title: "Third-P. View", image: cameraMode == 1 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), handler: { _ in
+            UIAction(title: "Third-P. View", image: cameraMode == 1 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: (self.mState != .STATE_VISUALIZING_AND_MEASURING) ? [] : .disabled, handler: { _ in
                self.setGLCamera(type: 1)
                self.resetNoTouchTimer(true)
            }),
@@ -997,7 +1073,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
            })
        ])
        
-        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_PROCESSING && mState != .STATE_VISUALIZING_WHILE_LOADING
+        let showCloudMeshActions = mState != .STATE_VISUALIZING && mState != .STATE_VISUALIZING_CAMERA && mState != .STATE_VISUALIZING_AND_MEASURING && mState != .STATE_PROCESSING && mState != .STATE_VISUALIZING_WHILE_LOADING
        let cloudMeshMenu = UIMenu(title: "CloudMesh", options: .displayInline, children: [
            UIAction(title: "Point Cloud", image: viewMode == 0 ? UIImage(systemName: "checkmark.circle") : UIImage(systemName: "circle"), attributes: showCloudMeshActions ? [] : .disabled, handler: { _ in
                self.setMeshRendering(viewMode: 0)
@@ -1987,7 +2063,7 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        {
            self.rtabmap?.setLocalizationMode(enabled: false)
        }
-        updateState(state: mState == .STATE_VISUALIZING_CAMERA ? .STATE_VISUALIZING : .STATE_IDLE);
+        updateState(state: mState == .STATE_VISUALIZING_CAMERA || mState == .STATE_VISUALIZING_AND_MEASURING ? .STATE_VISUALIZING : .STATE_IDLE);
        
        if !ignoreSaving
        {
@@ -2419,6 +2495,26 @@ class ViewController: GLKViewController, ARSessionDelegate, RTABMapObserver, UIP
        rtabmap!.setOrthoCropFactor(Float(120-sender.value)/20.0 - 3.0)
        self.view.setNeedsDisplay()
    }
+    
+    @IBAction func removeMeasureAction(_ sender: UIButton) {
+        self.rtabmap!.removeMeasure()
+    }
+    @IBAction func addMeasureAction(_ sender: UIButton) {
+        self.rtabmap!.addMeasureButtonClicked()
+    }
+    @IBAction func teleportButtonAction(_ sender: UIButton) {
+        self.rtabmap!.teleportButtonClicked()
+    }
+    func clearMeasures()
+    {
+        self.rtabmap!.clearMeasures()
+        self.resetNoTouchTimer(true)
+    }
+    @IBAction func startMeasuring(_ sender: UIButton) {
+        self.setGLCamera(type: 0)
+        self.updateState(state: .STATE_VISUALIZING_AND_MEASURING)
+        self.startCamera()
+    }
 }

 func clearBackgroundColor(of view: UIView) {