android: added occlusion detection using arcore depth api (for FPS view). Added depth from motion option in Mapping options.

2026-09-02 09:30:25 +08:00 · 2020-06-29 20:25:58 -04:00
parent 2ee3445728
commit b7ada1992e
11 changed files with 167 additions and 51 deletions
--- a/app/android/jni/CameraARCore.cpp
+++ b/app/android/jni/CameraARCore.cpp
@@ -37,14 +37,16 @@ namespace rtabmap {
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
-CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smoothing):
+CameraARCore::CameraARCore(void* env, void* context, void* activity, bool depthFromMotion, bool smoothing):
 	CameraMobile(smoothing),
 	env_(env),
 	context_(context),
 	activity_(activity),
 	arInstallRequested_(false),
 	textureId_(9999),
-	uvs_initialized_(false)
+	uvs_initialized_(false),
+	updateOcclusionImage_(false),
+	depthFromMotion_(depthFromMotion)
 {
 }

@@ -162,8 +164,8 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 	UASSERT(ArSession_create(env_, context_, &arSession_) == AR_SUCCESS);
 	UASSERT(arSession_);

-	int32_t is_depth_supported = 0; // Disabled by default, depth is not super accurate for mapping
-	//ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+	int32_t is_depth_supported = 0;
+	ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);

 	ArConfig_create(arSession_, &arConfig_);
 	UASSERT(arConfig_);
@@ -277,6 +279,7 @@ void CameraARCore::close()
 	arPose_ = nullptr;

 	CameraMobile::close();
+	occlusionImage_ = cv::Mat();
 }

 LaserScan CameraARCore::scanFromPointCloudData(
@@ -429,8 +432,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
-				cv::Mat outputDepth;
-				if(is_depth_supported)
+				if(is_depth_supported && (updateOcclusionImage_||depthFromMotion_))
 				{
 					LOGD("Acquire depth image!");
 					ArImage * depthImage = nullptr;
@@ -444,36 +446,24 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 						int planeCount;
 						ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
 						LOGD("planeCount=%d", planeCount);
-						UASSERT_MSG(planeCount == 1,
-								uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+						UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
 						const uint8_t *data = nullptr;
 						int len = 0;
 						int stride;
-						int width;
-						int height;
-						ArImage_getWidth(arSession_, depthImage, &width);
-						ArImage_getHeight(arSession_, depthImage, &height);
+						int depth_width;
+						int depth_height;
+						ArImage_getWidth(arSession_, depthImage, &depth_width);
+						ArImage_getHeight(arSession_, depthImage, &depth_height);
 						ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
 						ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);

-						LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+						LOGD("width=%d, height=%d, bytes=%d stride=%d", depth_width, depth_height, len, stride);

-						outputDepth = cv::Mat(height, width, CV_16UC1);
-						uint16_t *dataShort = (uint16_t *)data;
-						uint16_t max=0x0;
-						for (int y = 0; y < outputDepth.rows; ++y)
-						{
-							for (int x = 0; x < outputDepth.cols; ++x)
-							{
-								uint16_t depthSample = dataShort[y*outputDepth.cols + x];
-								uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
-								outputDepth.at<uint16_t>(y,x) = depthRange;
-								if(depthRange > max)
-								{
-									max = depthRange;
-								}
-							}
-						}
+						occlusionImage_ = cv::Mat(depth_height, depth_width, CV_16UC1, (void*)data).clone();
+
+						float scaleX = (float)depth_width / (float)width;
+						float scaleY = (float)depth_height / (float)height;
+						occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(depth_width, depth_height));
 					}
 					ArImage_release(depthImage);
 				}
@@ -545,7 +535,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}

-						data = SensorData(scan, rgb, outputDepth, model, 0, stamp);
+						data = SensorData(scan, rgb, depthFromMotion_?occlusionImage_:cv::Mat(), model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -619,11 +609,6 @@ void CameraARCore::capturePoseOnly()
 		uvs_initialized_ = true;
 	}

-	/*ArImage* ar_image = nullptr;
-		  ArStatus status =
-		      ArFrame_acquireCameraImage(arSession_, arFrame_, &ar_image);
-		  ArImage_release(ar_image);
-*/
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);

@@ -649,6 +634,52 @@ void CameraARCore::capturePoseOnly()
 			pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 			this->poseReceived(pose);
 		}
+
+		int32_t is_depth_supported = 0;
+		ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
+		if(is_depth_supported && updateOcclusionImage_)
+		{
+			LOGD("Acquire depth image!");
+			ArImage * depthImage = nullptr;
+			ArFrame_acquireDepthImage(arSession_, arFrame_, &depthImage);
+
+			ArImageFormat format;
+			ArImage_getFormat(arSession_, depthImage, &format);
+			if(format == AR_IMAGE_FORMAT_DEPTH16)
+			{
+				LOGD("Depth format detected!");
+				int planeCount;
+				ArImage_getNumberOfPlanes(arSession_, depthImage, &planeCount);
+				LOGD("planeCount=%d", planeCount);
+				UASSERT_MSG(planeCount == 1, uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
+				const uint8_t *data = nullptr;
+				int len = 0;
+				int stride;
+				int width;
+				int height;
+				ArImage_getWidth(arSession_, depthImage, &width);
+				ArImage_getHeight(arSession_, depthImage, &height);
+				ArImage_getPlaneRowStride(arSession_, depthImage, 0, &stride);
+				ArImage_getPlaneData(arSession_, depthImage, 0, &data, &len);
+
+				LOGD("width=%d, height=%d, bytes=%d stride=%d", width, height, len, stride);
+
+				occlusionImage_ = cv::Mat(height, width, CV_16UC1, (void*)data).clone();
+
+				float fx,fy, cx, cy;
+				int32_t rgb_width, rgb_height;
+				ArCamera_getImageIntrinsics(arSession_, ar_camera, arCameraIntrinsics_);
+				ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
+				ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
+				ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &rgb_width, &rgb_height);
+
+				float scaleX = (float)width / (float)rgb_width;
+				float scaleY = (float)height / (float)rgb_height;
+				occlusionModel_ = CameraModel(fx*scaleX, fy*scaleY, cx*scaleX, cy*scaleY, pose*deviceTColorCamera_, 0, cv::Size(width, height));
+			}
+			ArImage_release(depthImage);
+		}
 	}

 	ArCamera_release(ar_camera);
--- a/app/android/jni/CameraARCore.h
+++ b/app/android/jni/CameraARCore.h
@@ -60,13 +60,16 @@ public:
 			std::vector<cv::Point3f> * kpts3D = 0);

 public:
-	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
+	CameraARCore(void* env, void* context, void* activity, bool depthFromMotion = false, bool smoothing = false);
 	virtual ~CameraARCore();

 	bool uvsInitialized() const {return uvs_initialized_;}
 	const float* uvsTransformed() const {return transformed_uvs_;}
 	void getVPMatrices(glm::mat4 & view, glm::mat4 & projection) const {view=viewMatrix_; projection=projectionMatrix_;}

+	void updateOcclusionImage(bool enabled) {updateOcclusionImage_ = enabled;}
+	const cv::Mat & getOcclusionImage(CameraModel * model=0) const {if(model)*model=occlusionModel_; return occlusionImage_; }
+
 	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);

 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
@@ -101,6 +104,11 @@ private:
 	bool uvs_initialized_ = false;
 	glm::mat4 viewMatrix_;
 	glm::mat4 projectionMatrix_;
+
+	bool updateOcclusionImage_;
+	cv::Mat occlusionImage_;
+	CameraModel occlusionModel_;
+	bool depthFromMotion_;
 };

 } /* namespace rtabmap */
--- a/app/android/jni/RTABMapApp.cpp
+++ b/app/android/jni/RTABMapApp.cpp
@@ -690,7 +690,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	else if(cameraDriver_ == 1)
 	{
 #ifdef RTABMAP_ARCORE
-		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
+		camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -1113,15 +1113,24 @@ int RTABMapApp::Render()
 		const float* uvsTransformed = 0;
 		glm::mat4 arProjectionMatrix(0);
 		glm::mat4 arViewMatrix(0);
+		rtabmap::Mesh occlusionMesh;
 		if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
-				camera_->spinOnce();
+#ifdef RTABMAP_ARCORE
 				if(cameraDriver_ == 1)
 				{
+					((rtabmap::CameraARCore*)camera_)->updateOcclusionImage(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson);
+				}
+#endif
+
+				camera_->spinOnce();
+
 #ifdef RTABMAP_ARCORE
+				if(cameraDriver_ == 1)
+				{
 					if(main_scene_.background_renderer_ == 0)
 					{
 						main_scene_.background_renderer_ = new BackgroundRenderer();
@@ -1131,10 +1140,29 @@ int RTABMapApp::Render()
 					{
 						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
 						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
-						//main_scene_.background_renderer_->Draw(uvsTransformed);
 					}
-#endif
+					if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
+					{
+						rtabmap::CameraModel occlusionModel;
+						cv::Mat occlusionImage = ((rtabmap::CameraARCore*)camera_)->getOcclusionImage(&occlusionModel);
+
+						if(occlusionModel.isValidForProjection())
+						{
+							pcl::IndicesPtr indices(new std::vector<int>);
+							pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
+							cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform());
+							occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
+							pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
+							occlusionMesh.indices = indices;
+							occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
+						}
+						else
+						{
+							UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
+						}
+					}
 				}
+#endif
 			}
 		}

@@ -1820,7 +1848,7 @@ int RTABMapApp::Render()

 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
+			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh);
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();
@@ -2106,6 +2134,14 @@ void RTABMapApp::setSmoothing(bool enabled)
 	}
 }

+void RTABMapApp::setDepthFromMotion(bool enabled)
+{
+	if(depthFromMotion_ != enabled)
+	{
+		depthFromMotion_ = enabled;
+	}
+}
+
 void RTABMapApp::setAppendMode(bool enabled)
 {
 	if(appendMode_ != enabled)
--- a/app/android/jni/RTABMapApp.h
+++ b/app/android/jni/RTABMapApp.h
@@ -110,6 +110,7 @@ class RTABMapApp : public UEventsHandler {
  void setCameraColor(bool enabled);
  void setFullResolution(bool enabled);
  void setSmoothing(bool enabled);
+  void setDepthFromMotion(bool enabled);
  void setAppendMode(bool enabled);
  void setDataRecorderMode(bool enabled);
  void setMaxCloudDepth(float value);
@@ -183,6 +184,7 @@ class RTABMapApp : public UEventsHandler {
  bool trajectoryMode_;
  bool rawScanSaved_;
  bool smoothing_;
+  bool depthFromMotion_;
  bool cameraColor_;
  bool fullResolution_;
  bool appendMode_;
--- a/app/android/jni/jni_interface.cpp
+++ b/app/android/jni/jni_interface.cpp
@@ -512,6 +512,19 @@ Java_com_introlab_rtabmap_RTABMapLib_setSmoothing(
 	}
 }
 JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_setDepthFromMotion(
+		JNIEnv*, jclass, jlong native_application, bool enabled)
+{
+	if(native_application)
+	{
+		return native(native_application)->setDepthFromMotion(enabled);
+	}
+	else
+	{
+		UERROR("native_application is null!");
+	}
+}
+JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_setCameraColor(
 		JNIEnv*, jclass, jlong native_application, bool enabled)
 {
--- a/app/android/jni/scene.cpp
+++ b/app/android/jni/scene.cpp
@@ -367,7 +367,7 @@ bool intersectFrustumAABB(
 }

 //Should only be called in OpenGL thread!
-int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix) {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix, const rtabmap::Mesh & occlusionMesh) {
 	UASSERT(gesture_camera_ != 0);

 	if(currentPose_ == 0)
@@ -458,7 +458,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat

 	UTimer timer;

-	bool onlineBlending = blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1;
+	bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
 	if(onlineBlending && fboId_)
 	{
 		// set the rendering destination to FBO
@@ -468,11 +468,19 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 		glClearColor(1, 1, 1, 1);
 		glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);

-		// Draw scene
-		for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+		if(renderBackgroundCamera)
 		{
-			// set large distance to cam to use low res polygons for fast processing
-			(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			PointCloudDrawable drawable(occlusionMesh);
+			drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
+		}
+		else
+		{
+			// Draw scene
+			for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
+			{
+				// set large distance to cam to use low res polygons for fast processing
+				(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, 999.0f);
+			}
 		}

 		// back to normal window-system-provided framebuffer
@@ -512,6 +520,10 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
 	if(renderBackgroundCamera)
 	{
 		background_renderer_->Draw(uvsTransformed);
+
+		//To debug occlusion image:
+		//PointCloudDrawable drawable(occlusionMesh);
+		//drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f);
 	}

 	if(!currentPose_->isNull())
--- a/app/android/jni/scene.h
+++ b/app/android/jni/scene.h
@@ -72,13 +72,14 @@ class Scene {
  //         frame's timestamp.
  // @param: point_cloud_vertices, point cloud's vertices of the current point
  //         frame.
-  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0));
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh & occlusionMesh=rtabmap::Mesh());

  // Set render camera's viewing angle, first person, third person or top down.
  //
  // @param: camera_type, camera type includes first person, third person and
  //         top down
  void SetCameraType(tango_gl::GestureCamera::CameraType camera_type);
+  tango_gl::GestureCamera::CameraType GetCameraType() const {return gesture_camera_->GetCameraType();}

  void SetCameraPose(const rtabmap::Transform & pose); // opengl camera
  rtabmap::Transform GetCameraPose() const {return currentPose_!=0?*currentPose_:rtabmap::Transform();}