Hello, I would like to display the localizationCovariance</c

Hi <a class="user-mention notranslate" data-hovercard-type="user" data-hovercard-url="

Displaying Covariance in the Render loop about rtabmap HOT 3 OPEN

Genozen commented on May 23, 2024

Displaying Covariance in the Render loop

from rtabmap.

Comments (3)

matlabbe commented on May 23, 2024

If the pose is null, it means ARKit cannot output a pose, so the mapping node will likely ignore updating and the rtabmapEvents will be empty till the pose can be recomputed.

Do you mean that if ArKit tracking state is in any other state than "normal", you would still want a covariance value?

rtabmap/app/ios/RTABMapApp/ViewController.swift

Lines 979 to 1014 in a11ea29

 switch frame.camera.trackingState { 

 case .normal: 

 accept = true 

 case .notAvailable: 

 status = "Tracking not available" 

 case .limited(.excessiveMotion): 

 accept = true 

 status = "Please Slow Your Movement" 

 case .limited(.insufficientFeatures): 

 accept = true 

 status = "Avoid Featureless Surfaces" 

 case .limited(.initializing): 

 status = "Initializing" 

 case .limited(.relocalizing): 

 status = "Relocalizing" 

 default: 

 status = "Unknown tracking state" 

 } 

 mLastLightEstimate = frame.lightEstimate?.ambientIntensity 

 if !status.isEmpty && mLastLightEstimate != nil && mLastLightEstimate! < 100 && accept { 

 status = "Camera Is Occluded Or Lighting Is Too Dark" 

 } 

 if accept 

 { 

 if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation 

 { 

 rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size) 

 } 

 } 

 else 

 { 

 rtabmap?.notifyLost(); 

 }

In that code, we won't forward any pose to rtabmap if the tracking is not normal.

from rtabmap.

Genozen commented on May 23, 2024

Hi @matlabbe, yes that's the hope to continue computing covariances based on updated pose from ARKit.

I've verified in the ViewController.Swift that the pose still gets computed by the ARKit (there's values) even when the camera status are excessiveMotion insufficientFeatures since the accept = true is still valid and therefore executes the rtabmap?.postOdometryEvent(...)

I've narrowed down to the RtabMap.Swift where the update is managed in this if statement:

rtabmap/app/ios/RTABMapApp/RTABMap.swift

Line 248 in 81e249b

if points != nil && (depthMap != nil || points!.count>0)

I then create an else case to try to continue perform postOdometryEvent with some dummyData:

            else
            {
                var dummyDepthDataPtr: UnsafeMutableRawPointer? = nil // Dummy depth pointer
                var dummyDepthSize: Int32 = 0                         // Dummy depth size
                var dummyDepthWidth: Int32 = 0                        // Dummy depth width
                var dummyDepthHeight: Int32 = 0                       // Dummy depth height
                var dummyDepthFormat: Int32 = 0                       // Dummy depth format

                var dummyConfDataPtr: UnsafeMutableRawPointer? = nil  // Dummy conf pointer
                var dummyConfSize: Int32 = 0                          // Dummy conf size
                var dummyConfWidth: Int32 = 0                         // Dummy conf width
                var dummyConfHeight: Int32 = 0                        // Dummy conf height
                var dummyConfFormat: Int32 = 0                        // Dummy conf format

                var dummyBufferPoints: UnsafeMutableRawPointer? = nil // Dummy buffer points
                var dummyBufferPointsCount: Int32 = 0                 // Dummy buffer points count
                
                print("----------error log----------")
                print(pose)
                print(frame.camera.intrinsics)
                print(frame.timestamp)
                postOdometryEventNative(native_rtabmap,
                                        pose[3,0], pose[3,1], pose[3,2], quat.x, quat.y, quat.z, quat.w,
                                        frame.camera.intrinsics[0,0], // fx
                                        frame.camera.intrinsics[1,1], // fy
                                        frame.camera.intrinsics[2,0], // cx
                                        frame.camera.intrinsics[2,1], // cy
                                        frame.timestamp,
                                        nil, //CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 0),  // y plane pointer
                                        nil,                                                         // u plane pointer
                                        nil, //CVPixelBufferGetBaseAddressOfPlane(frame.capturedImage, 1),  // v plane pointer
                                        1, //Int32(CVPixelBufferGetBytesPerRowOfPlane(frame.capturedImage, 0)) * Int32(CVPixelBufferGetHeightOfPlane(frame.capturedImage, 0)),  // yPlaneLen
                                        1, //Int32(CVPixelBufferGetWidth(frame.capturedImage)),           // rgb width
                                        1, //Int32(CVPixelBufferGetHeight(frame.capturedImage)),          // rgb height
                                        1, //Int32(CVPixelBufferGetPixelFormatType(frame.capturedImage)), // rgb format
                                        dummyDepthDataPtr, // depth pointer
                                        dummyDepthSize,    // depth size
                                        dummyDepthWidth,   // depth width
                                        dummyDepthHeight,  // depth height
                                        dummyDepthFormat,  // depth format
                                        dummyConfDataPtr, // conf pointer
                                        dummyConfSize,    // conf size
                                        dummyConfWidth,   // conf width
                                        dummyConfHeight,  // conf height
                                        dummyConfFormat,  // conf format
                                        dummyBufferPoints, dummyBufferPointsCount, 4,
                                        v[3,0], v[3,1], v[3,2], quatv.x, quatv.y, quatv.z, quatv.w,
                                        p[0,0], p[1,1], p[2,0], p[2,1], p[2,2], p[2,3], p[3,2],
                                        texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7])
            }

Trying to bypass this portion correctly without compromising any pose estimation embedded inside:

rtabmap/app/android/jni/RTABMapApp.cpp

Line 3748 in 81e249b

 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) 

The problem with this is yPlaneLen == rgbWidth*rgbHeight does not match since I'm sending dummy data of 1. Is there a way to bypass these values correctly?

from rtabmap.

matlabbe commented on May 23, 2024

Can you keep the rgb data as usual? The rgb frame I think should be always available.

from rtabmap.

Displaying Covariance in the Render loop about rtabmap HOT 3 OPEN

Comments (3)

Related Issues (20)

Recommend Projects

React

Vue.js

Typescript

TensorFlow

Django

Laravel

D3

Recommend Topics

javascript

web

server

Machine learning

Visualization

Game

Recommend Org

Facebook

Microsoft

Google

Alibaba

D3

Tencent

	switch frame.camera.trackingState {
	case .normal:
	accept = true
	case .notAvailable:
	status = "Tracking not available"
	case .limited(.excessiveMotion):
	accept = true
	status = "Please Slow Your Movement"
	case .limited(.insufficientFeatures):
	accept = true
	status = "Avoid Featureless Surfaces"
	case .limited(.initializing):
	status = "Initializing"
	case .limited(.relocalizing):
	status = "Relocalizing"
	default:
	status = "Unknown tracking state"
	}

	mLastLightEstimate = frame.lightEstimate?.ambientIntensity

	if !status.isEmpty && mLastLightEstimate != nil && mLastLightEstimate! < 100 && accept {
	status = "Camera Is Occluded Or Lighting Is Too Dark"
	}

	if accept
	{
	if let rotation = UIApplication.shared.windows.first?.windowScene?.interfaceOrientation
	{
	rtabmap?.postOdometryEvent(frame: frame, orientation: rotation, viewport: self.view.frame.size)
	}
	}
	else
	{
	rtabmap?.notifyLost();
	}