Added AR mapping mode (select ARCore ndk driver and set first person view)

app/android/jni/CMakeLists.txt

@@ -22,6 +22,7 @@ set(sources
   scene.cpp
   point_cloud_drawable.cpp
   graph_drawable.cpp
+  background_renderer.cc
   tango-gl/axis.cpp 
   tango-gl/camera.cpp 
   tango-gl/conversions.cpp 

app/android/jni/CameraARCore.cpp

@@ -34,29 +34,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 
 namespace rtabmap {
 
-#ifdef DEPTH_TEST
-// Camera Callbacks
-static void CameraDeviceOnDisconnected(void* context, ACameraDevice* device) {
-  LOGE("Camera(id: %s) is disconnected.\n", ACameraDevice_getId(device));
-}
-static void CameraDeviceOnError(void* context, ACameraDevice* device,
-                                int error) {
-  LOGE("Error(code: %d) on Camera(id: %s).\n", error,
-       ACameraDevice_getId(device));
-}
-// Capture Callbacks
-bool g_captureSessionReady = false;
-static void CaptureSessionOnReady(void* context,
-                                  ACameraCaptureSession* session) {
-  LOGI("Session is ready.\n");
-  g_captureSessionReady = true;
-}
-static void CaptureSessionOnActive(void* context,
-                                   ACameraCaptureSession* session) {
-  LOGI("Session is activated.\n");
-}
-#endif // DEPTH_TEST
-
 //////////////////////////////
 // CameraARCore
 //////////////////////////////
@@ -65,16 +42,21 @@ CameraARCore::CameraARCore(void* env, void* context, void* activity, bool smooth
 	env_(env),
 	context_(context),
 	activity_(activity),
-	arInstallRequested_(false)
+	arInstallRequested_(false),
+	textureId_(9999),
+	uvs_initialized_(false)
 {
-	glGenTextures(1, &textureId_);
 }
 
 CameraARCore::~CameraARCore() {
 	// Disconnect ARCore service
 	close();
 
-	glDeleteTextures(1, &textureId_);
+	if(textureId_ != 9999)
+	{
+		glDeleteTextures(1, &textureId_);
+		textureId_ = 9999;
+	}
 }
 
 
@@ -146,132 +128,10 @@ std::string CameraARCore::getSerial() const
 	return "ARCore";
 }
 
-#ifdef DEPTH_TEST
-void OnImageCallback(void *ctx, AImageReader *reader) {
-  reinterpret_cast<CameraARCore *>(ctx)->imageCallback(reader);
-}
-void CameraARCore::imageCallback(AImageReader *reader) {
-	int32_t format;
-	media_status_t status = AImageReader_getFormat(reader, &format);
-	UWARN("format=%d", format);
-	UASSERT_MSG(status == AMEDIA_OK, "Failed to get the media format");
-
-	if (format == AIMAGE_FORMAT_DEPTH16) {
-		// Create a thread and write out the jpeg files
-		AImage *image = nullptr;
-		media_status_t status = AImageReader_acquireNextImage(reader, &image);
-		UASSERT_MSG(status == AMEDIA_OK && image, "Image is not available");
-
-		int planeCount;
-		status = AImage_getNumberOfPlanes(image, &planeCount);
-		UASSERT_MSG(status == AMEDIA_OK && planeCount == 1,
-				uFormat("Error: getNumberOfPlanes() planceCount = %d", planeCount).c_str());
-		uint8_t *data = nullptr;
-		int len = 0;
-		int stride;
-		int width;
-		int height;
-		AImage_getWidth(image, &width);
-		AImage_getHeight(image, &height);
-		AImage_getPlaneRowStride(image, 0, &stride);
-		AImage_getPlaneData(image, 0, &data, &len);
-
-		cv::Mat output(height, width, CV_16UC1);
-		uint16_t *dataShort = (uint16_t *)data;
-		uint16_t max=0x0;
-		for (int y = 0; y < output.rows; ++y)
-		{
-			for (int x = 0; x < output.cols; ++x)
-			{
-				uint16_t depthSample = dataShort[y*output.cols + x];
-				uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
-				output.at<uint16_t>(y,x) = depthRange;
-				if(depthRange > max)
-				{
-					max = depthRange;
-				}
-			}
-		}
-		UWARN("width=%d, height=%d, bytes=%d stride=%d max=%dmm",
-				width, height, len, stride, (int)max);
-
-		std::string path = "/storage/emulated/0/RTAB-Map/depth.png";
-		cv::imwrite(path, output);
-		UWARN("depth image saved to %s", path.c_str());
-
-		AImage_delete(image);
-	}
-}
-#endif // DEPTH_TEST
-
 bool CameraARCore::init(const std::string & calibrationFolder, const std::string & cameraName)
 {
 	close();
 
-#ifdef DEPTH_TEST
-	///////////////////////////
-	// Depth image using camera2 API
-	/////////////////////////////
-	camera_status_t cameraStatus = ACAMERA_OK;
-
-	cameraManager_ = ACameraManager_create();
-
-	deviceStateCallbacks_.onDisconnected = CameraDeviceOnDisconnected;
-	deviceStateCallbacks_.onError = CameraDeviceOnError;
-
-	const char * cameraId = "0";
-	cameraStatus = ACameraManager_openCamera(cameraManager_, cameraId, &deviceStateCallbacks_, &cameraDevice_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK, uFormat("Failed to open camera device (id: %s)",
-			cameraId).c_str());
-
-	// Currently only working resolution on Huawei P30 Pro
-	cv::Size size(240, 180);
-	int format = AIMAGE_FORMAT_DEPTH16;
-
-	media_status_t mediaStatus = AImageReader_new(size.width, size.height, format, 2, &imageReader_);
-	UASSERT_MSG(imageReader_ && mediaStatus == AMEDIA_OK, uFormat("Failed to create AImageReader %dx%d format=%d",
-			size.width, size.height, format).c_str());
-
-	AImageReader_ImageListener listener{
-	  .context = this,
-	  .onImageAvailable = OnImageCallback,
-	};
-	AImageReader_setImageListener(imageReader_, &listener);
-
-	//
-	ANativeWindow *nativeWindow;
-	mediaStatus = AImageReader_getWindow(imageReader_, &nativeWindow);
-	UASSERT_MSG(mediaStatus == AMEDIA_OK, "Could not get ANativeWindow");
-
-	outputNativeWindow_ = nativeWindow;
-	ACaptureSessionOutputContainer_create(&captureSessionOutputContainer_);
-	ANativeWindow_acquire(outputNativeWindow_);
-	ACaptureSessionOutput_create(outputNativeWindow_, &sessionOutput_);
-	ACaptureSessionOutputContainer_add(captureSessionOutputContainer_, sessionOutput_);
-	ACameraOutputTarget_create(outputNativeWindow_, &cameraOutputTarget_);
-
-	cameraStatus = ACameraDevice_createCaptureRequest(cameraDevice_, TEMPLATE_RECORD, &captureRequest_);
-	UASSERT_MSG(cameraStatus == ACAMERA_OK,
-			uFormat("Failed to create preview capture request (id: %s, status=%d)",
-					cameraId, cameraStatus).c_str());
-
-	ACaptureRequest_addTarget(captureRequest_, cameraOutputTarget_);
-
-	captureSessionStateCallbacks_.onReady = CaptureSessionOnReady;
-	captureSessionStateCallbacks_.onActive = CaptureSessionOnActive;
-	ACameraDevice_createCaptureSession(
-			cameraDevice_,
-			captureSessionOutputContainer_, // outputs
-			&captureSessionStateCallbacks_, // callbacks
-			&captureSession_);
-
-	ACameraCaptureSession_setRepeatingRequest(captureSession_, nullptr, 1,
-			&captureRequest_, nullptr);
-
-	// Don't start ARCore as we cannot use both at the same time
-	return true;
-#endif // DEPTH_TEST
-
 	UScopeMutex lock(arSessionMutex_);
 
 	ArInstallStatus install_status;
@@ -302,10 +162,19 @@ 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);
+
 	ArConfig_create(arSession_, &arConfig_);
 	UASSERT(arConfig_);
 
-	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_FIXED);
+	if (is_depth_supported!=0) {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_AUTOMATIC);
+	} else {
+	  ArConfig_setDepthMode(arSession_, arConfig_, AR_DEPTH_MODE_DISABLED);
+	}
+
+	ArConfig_setFocusMode(arSession_, arConfig_, AR_FOCUS_MODE_AUTO);
 	UASSERT(ArSession_configure(arSession_, arConfig_) == AR_SUCCESS);
 
 	ArFrame_create(arSession_, &arFrame_);
@@ -361,9 +230,6 @@ bool CameraARCore::init(const std::string & calibrationFolder, const std::string
 
 	deviceTColorCamera_ = opticalRotation;
 
-	// Required as ArSession_update does some off-screen OpenGL stuff...
-	ArSession_setCameraTextureName(arSession_, textureId_);
-
 	if (ArSession_resume(arSession_) != ArStatus::AR_SUCCESS)
 	{
 		UERROR("Cannot resume camera!");
@@ -410,44 +276,6 @@ void CameraARCore::close()
 	}
 	arPose_ = nullptr;
 
-#ifdef DEPTH_TEST
-
-	if(captureSession_!=nullptr)
-	{
-		g_captureSessionReady = false;
-		ACameraCaptureSession_stopRepeating(captureSession_);
-		double start = UTimer::now();
-		while(g_captureSessionReady != true && UTimer::now()-start < 2.0){
-			uSleep(100);
-			UWARN("Waiting session to close.... max 2 seconds");
-		}
-		//ACameraCaptureSession_close(captureSession_); // FIXME: this crashes?!
-		captureSession_ = nullptr;
-
-		ACaptureRequest_removeTarget(captureRequest_, cameraOutputTarget_);
-		ACaptureRequest_free(captureRequest_);
-		ACameraOutputTarget_free(cameraOutputTarget_);
-		captureRequest_ = nullptr;
-		cameraOutputTarget_ = nullptr;
-
-		ACaptureSessionOutputContainer_remove(captureSessionOutputContainer_, sessionOutput_);
-		ANativeWindow_release(outputNativeWindow_);
-		ACaptureSessionOutputContainer_free(captureSessionOutputContainer_);
-		ACaptureSessionOutput_free(sessionOutput_);
-		captureSessionOutputContainer_ = nullptr;
-		sessionOutput_ = nullptr;
-
-		ACameraDevice_close(cameraDevice_);
-		cameraDevice_ = nullptr;
-
-		ACameraManager_delete(cameraManager_);
-		cameraManager_ = nullptr;
-
-		AImageReader_delete(imageReader_);
-		imageReader_ = nullptr;
-	}
-#endif
-
 	CameraMobile::close();
 }
 
@@ -499,6 +327,20 @@ LaserScan CameraARCore::scanFromPointCloudData(
 	return LaserScan();
 }
 
+void CameraARCore::setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
+{
+	CameraMobile::setScreenRotationAndSize(colorCameraToDisplayRotation, width, height);
+	if(arSession_)
+	{
+		int ret = static_cast<int>(colorCameraToDisplayRotation) + 1; // remove 90deg camera rotation
+		  if (ret > 3) {
+		    ret -= 4;
+		  }
+
+		ArSession_setDisplayGeometry(arSession_, ret, width, height);
+	}
+}
+
 SensorData CameraARCore::captureImage(CameraInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
@@ -510,15 +352,44 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		return data;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
 		LOGE("CameraARCore::captureImage() ArSession_update error");
 		return data;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		uvs_initialized_ = true;
+	}
+
 	ArCamera* ar_camera;
 	ArFrame_acquireCamera(arSession_, arFrame_, &ar_camera);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 
@@ -551,17 +422,68 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 			ArPointCloud * pointCloud = nullptr;
 			ArFrame_acquirePointCloud(arSession_, arFrame_, &pointCloud);
 
+			int32_t is_depth_supported = 0;
+			ArSession_isDepthModeSupported(arSession_, AR_DEPTH_MODE_AUTOMATIC, &is_depth_supported);
+
 			ArImage * image = nullptr;
 			ArStatus status = ArFrame_acquireCameraImage(arSession_, arFrame_, &image);
 			if(status == AR_SUCCESS)
 			{
+				cv::Mat outputDepth;
+				if(is_depth_supported)
+				{
+					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);
+
+						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;
+								}
+							}
+						}
+					}
+					ArImage_release(depthImage);
+				}
+
 				int64_t timestamp_ns;
 				ArImageFormat format;
 				ArImage_getTimestamp(arSession_, image, &timestamp_ns);
 				ArImage_getFormat(arSession_, image, &format);
 				if(format == AR_IMAGE_FORMAT_YUV_420_888)
 				{
-
 #ifndef DISABLE_LOG
 					int32_t num_planes;
 					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
@@ -623,7 +545,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							LOGI("pointCloud empty");
 						}
 
-						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data = SensorData(scan, rgb, outputDepth, model, 0, stamp);
 						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
@@ -662,21 +584,54 @@ void CameraARCore::capturePoseOnly()
 	UScopeMutex lock(arSessionMutex_);
 	//LOGI("Capturing image...");
 
-	SensorData data;
 	if(!arSession_)
 	{
 		return;
 	}
 
+	if(textureId_ == 9999)
+	{
+		glGenTextures(1, &textureId_);
+		glBindTexture(GL_TEXTURE_EXTERNAL_OES, textureId_);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
+		glTexParameteri(GL_TEXTURE_EXTERNAL_OES, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
+	}
+	ArSession_setCameraTextureName(arSession_, textureId_);
+
 	// Update session to get current frame and render camera background.
 	if (ArSession_update(arSession_, arFrame_) != AR_SUCCESS) {
-		LOGE("CameraARCore::captureImage() ArSession_update error");
+		LOGE("CameraARCore::capturePoseOnly() ArSession_update error");
 		return;
 	}
 
+	// If display rotation changed (also includes view size change), we need to
+	// re-query the uv coordinates for the on-screen portion of the camera image.
+	int32_t geometry_changed = 0;
+	ArFrame_getDisplayGeometryChanged(arSession_, arFrame_, &geometry_changed);
+	if (geometry_changed != 0 || !uvs_initialized_) {
+		ArFrame_transformCoordinates2d(
+				arSession_, arFrame_, AR_COORDINATES_2D_OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				BackgroundRenderer::kNumVertices, BackgroundRenderer_kVertices, AR_COORDINATES_2D_TEXTURE_NORMALIZED,
+				transformed_uvs_);
+		UASSERT(transformed_uvs_);
+		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);
 
+	ArCamera_getViewMatrix(arSession_, ar_camera, glm::value_ptr(viewMatrix_));
+	ArCamera_getProjectionMatrix(arSession_, ar_camera,
+							   /*near=*/0.1f, /*far=*/100.f,
+							   glm::value_ptr(projectionMatrix_));
+
 	ArTrackingState camera_tracking_state;
 	ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state);
 

app/android/jni/CameraARCore.h

@@ -38,14 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/utilite/UEvent.h>
 #include <rtabmap/utilite/UTimer.h>
 #include <boost/thread/mutex.hpp>
+#include <background_renderer.h>
 
 #include <arcore_c_api.h>
-#ifdef DEPTH_TEST
 #include <camera/NdkCameraDevice.h>
 #include <camera/NdkCameraManager.h>
 #include <media/NdkImageReader.h>
 #include <android/native_window.h>
-#endif
 
 namespace rtabmap {
 
@@ -64,16 +63,22 @@ public:
 	CameraARCore(void* env, void* context, void* activity, 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_;}
+
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height);
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
+	void setupGL();
 	virtual void close(); // close Tango connection
 	virtual std::string getSerial() const;
+	GLuint getTextureId() const {return textureId_;}
 
-#ifdef DEPTH_TEST
 	void imageCallback(AImageReader *reader);
-#endif // DEPTH_TEST
 
 protected:
-	virtual SensorData captureImage(CameraInfo * info = 0);
+	virtual SensorData captureImage(CameraInfo * info = 0); // should be called in opengl thread
 	virtual void capturePoseOnly();
 
 private:
@@ -92,22 +97,10 @@ private:
 	GLuint textureId_;
 	UMutex arSessionMutex_;
 
-#ifdef DEPTH_TEST
+	float transformed_uvs_[BackgroundRenderer::kNumVertices*2];
-	// Camera variables
+	bool uvs_initialized_ = false;
-	ACameraDevice* cameraDevice_ = nullptr;
+	glm::mat4 viewMatrix_;
-	ACaptureRequest* captureRequest_ = nullptr;
+	glm::mat4 projectionMatrix_;
-	ACameraOutputTarget* cameraOutputTarget_ = nullptr;
-	ACaptureSessionOutput* sessionOutput_ = nullptr;
-	ACaptureSessionOutputContainer* captureSessionOutputContainer_ = nullptr;
-	ACameraCaptureSession* captureSession_ = nullptr;
-	ANativeWindow *outputNativeWindow_ = nullptr;
-
-	ACameraDevice_StateCallbacks deviceStateCallbacks_;
-	ACameraCaptureSession_stateCallbacks captureSessionStateCallbacks_;
-
-	ACameraManager* cameraManager_ = nullptr;
-	AImageReader* imageReader_ = nullptr;
-#endif // DEPTH_TEST
 };
 
 } /* namespace rtabmap */

app/android/jni/CameraMobile.h

@@ -94,7 +94,7 @@ public:
 	const CameraModel & getCameraModel() const {return model_;}
 	const Transform & getDeviceTColorCamera() const {return deviceTColorCamera_;}
 	void setSmoothing(bool enabled) {smoothing_ = enabled;}
-	virtual void setScreenRotation(ScreenRotation colorCameraToDisplayRotation) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
+	virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height) {colorCameraToDisplayRotation_ = colorCameraToDisplayRotation;}
 	void setGPS(const GPS & gps);
 	void addEnvSensor(int type, float value);
 	void setData(const SensorData & data, const Transform & pose);

app/android/jni/RTABMapApp.cpp

@@ -69,6 +69,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/poisson.h>
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
+
 #define LOW_RES_PIX 2
 //#define DEBUG_RENDERING_PERFORMANCE
 
@@ -263,7 +264,7 @@ void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(camera_)
 	{
-		camera_->setScreenRotation(rotation);
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 	}
 }
 
@@ -657,6 +658,10 @@ bool RTABMapApp::isBuiltWith(int cameraDriver) const
 
 bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
 {
+	//ccapp = new computer_vision::ComputerVisionApplication();
+	//ccapp->OnResume(env, context, activity);
+	//return true;
+
 	cameraDriver_ = driver;
 	LOGW("startCamera() camera driver=%d", cameraDriver_);
 	boost::mutex::scoped_lock  lock(cameraMutex_);
@@ -686,7 +691,6 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 	{
 #ifdef RTABMAP_ARCORE
 		camera_ = new rtabmap::CameraARCore(env, context, activity, smoothing_);
-
 #else
 		UERROR("RTAB-Map is not built with ARCore support!");
 #endif
@@ -712,7 +716,7 @@ bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobj
 
 	if(camera_->init())
 	{
-		camera_->setScreenRotation(main_scene_.getScreenRotation());
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
 
 		//update mesh decimation based on camera calibration
 		LOGI("Cloud density level %d", cloudDensityLevel_);
@@ -937,6 +941,11 @@ void RTABMapApp::SetViewPort(int width, int height)
 {
 	UINFO("");
 	main_scene_.SetupViewPort(width, height);
+	boost::mutex::scoped_lock  lock(cameraMutex_);
+	if(camera_)
+	{
+		camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
+	}
 }
 
 class PostRenderEvent : public UEvent
@@ -1101,12 +1110,31 @@ int RTABMapApp::Render()
 		}
 
 		// ARCore and AREngine capture should be done in opengl thread!
+		const float* uvsTransformed = 0;
+		glm::mat4 arProjectionMatrix(0);
+		glm::mat4 arViewMatrix(0);
 		if((cameraDriver_ == 1 || cameraDriver_ == 2) && camera_!=0)
 		{
 			boost::mutex::scoped_lock  lock(cameraMutex_);
 			if(camera_!=0)
 			{
 				camera_->spinOnce();
+				if(cameraDriver_ == 1)
+				{
+#ifdef RTABMAP_ARCORE
+					if(main_scene_.background_renderer_ == 0)
+					{
+						main_scene_.background_renderer_ = new BackgroundRenderer();
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
+					}
+					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
+					{
+						uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed();
+						((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
+						//main_scene_.background_renderer_->Draw(uvsTransformed);
+					}
+#endif
+				}
 			}
 		}
 
@@ -1792,7 +1820,7 @@ int RTABMapApp::Render()
 
 			fpsTime.restart();
 			main_scene_.setFrustumVisible(camera_!=0);
-			lastDrawnCloudsCount_ = main_scene_.Render();
+			lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
 			if(renderingTime_ < fpsTime.elapsed())
 			{
 				renderingTime_ = fpsTime.elapsed();

app/android/jni/RTABMapApp.h

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/pcl_base.h>
 #include <pcl/TextureMesh.h>
 
+
 // RTABMapApp handles the application lifecycle and resources.
 class RTABMapApp : public UEventsHandler {
  public:

app/android/jni/background_renderer.cc

@@ -0,0 +1,92 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+// This modules handles drawing the passthrough camera image into the OpenGL
+// scene.
+
+#include "background_renderer.h"
+
+#include <type_traits>
+
+namespace {
+
+const std::string kVertexShader =
+		"attribute vec4 a_Position;\n"
+		"attribute vec2 a_TexCoord;\n"
+
+		"varying vec2 v_TexCoord;\n"
+
+		"void main() {\n"
+		"   gl_Position = a_Position;\n"
+		"   v_TexCoord = a_TexCoord;\n"
+		"}\n";
+
+const std::string kFragmentShader =
+	"#extension GL_OES_EGL_image_external : require\n"
+
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
+	"uniform samplerExternalOES sTexture;\n"
+
+	"void main() {\n"
+	"    vec4 sample = texture2D(sTexture, v_TexCoord);\n"
+	"    float grey = 0.21 * sample.r + 0.71 * sample.g + 0.07 * sample.b;\n"
+	"    gl_FragColor = vec4(grey, grey, grey, 0.5);\n"
+	"}\n";
+
+}  // namespace
+
+void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+{
+  texture_id_ = textureId;
+
+  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  if (!shader_program_) {
+    LOGE("Could not create program.");
+  }
+  glUseProgram(shader_program_);
+  attribute_vertices_ = glGetAttribLocation(shader_program_, "a_Position");
+  attribute_uvs_ = glGetAttribLocation(shader_program_, "a_TexCoord");
+  glUseProgram(0);
+}
+
+void BackgroundRenderer::Draw(const float * transformed_uvs) {
+  static_assert(std::extent<decltype(BackgroundRenderer_kVertices)>::value == kNumVertices * 2, "Incorrect kVertices length");
+
+  glUseProgram(shader_program_);
+  glDepthMask(GL_FALSE);
+  glEnable (GL_BLEND);
+
+  glActiveTexture(GL_TEXTURE0);
+  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+
+  glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
+  glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs);
+
+  glEnableVertexAttribArray(attribute_vertices_);
+  glEnableVertexAttribArray(attribute_uvs_);
+
+  glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
+
+  glDisableVertexAttribArray(attribute_vertices_);
+  glDisableVertexAttribArray(attribute_uvs_);
+
+  glUseProgram(0);
+  glDepthMask(GL_TRUE);
+  glDisable (GL_BLEND);
+  tango_gl::util::CheckGlError("BackgroundRenderer::Draw() error");
+}
+

app/android/jni/background_renderer.h

@@ -0,0 +1,59 @@
+/*
+ * Copyright 2018 Google Inc. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *      http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+#define C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_
+
+#include <GLES2/gl2.h>
+#include <GLES2/gl2ext.h>
+#include <cstdlib>
+
+#include <arcore_c_api.h>
+#include "util.h"
+
+static const GLfloat BackgroundRenderer_kVertices[] = {
+	-1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+};
+
+// This class renders the passthrough camera image into the OpenGL frame.
+class BackgroundRenderer {
+public:
+	// Positions of the quad vertices in clip space (X, Y).
+
+	static constexpr int kNumVertices = 4;
+
+ public:
+  BackgroundRenderer() = default;
+  ~BackgroundRenderer() = default;
+
+  // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
+  // other methods below.
+  void InitializeGlContent(GLuint textureId);
+
+  // Draws the background image.  This methods must be called for every ArFrame
+  // returned by ArSession_update() to catch display geometry change events.
+  void Draw(const float * transformed_uvs);
+
+ private:
+
+  GLuint shader_program_;
+  GLuint texture_id_;
+
+  GLuint attribute_vertices_;
+  GLuint attribute_uvs_;
+};
+
+#endif  // C_ARCORE_AUGMENTED_IMAGE_BACKGROUND_RENDERER_H_

app/android/jni/scene.cpp

@@ -69,6 +69,7 @@ const std::string kGraphFragmentShader =
 
 
 Scene::Scene() :
+		background_renderer_(0),
 		gesture_camera_(0),
 		axis_(0),
 		frustum_(0),
@@ -160,6 +161,8 @@ void Scene::DeleteResources() {
 		delete trace_;
 		delete grid_;
 		delete box_;
+		delete background_renderer_;
+		background_renderer_ = 0;
 	}
 
 	PointCloudDrawable::releaseShaderPrograms();
@@ -364,7 +367,7 @@ bool intersectFrustumAABB(
 }
 
 //Should only be called in OpenGL thread!
-int Scene::Render() {
+int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat4 arProjectionMatrix) {
 	UASSERT(gesture_camera_ != 0);
 
 	if(currentPose_ == 0)
@@ -395,6 +398,17 @@ int Scene::Render() {
 	glm::mat4 projectionMatrix = gesture_camera_->GetProjectionMatrix();
 	glm::mat4 viewMatrix = gesture_camera_->GetViewMatrix();
 
+	bool renderBackgroundCamera =
+			background_renderer_ &&
+			gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson &&
+			!rtabmap::glmToTransform(arProjectionMatrix).isNull() &&
+			uvsTransformed;
+	if(renderBackgroundCamera)
+	{
+		projectionMatrix = arProjectionMatrix;
+		viewMatrix = arViewMatrix;
+	}
+
 	rtabmap::Transform openglCamera = GetOpenGLCameraPose();//*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
 	// transform in same coordinate as frustum filtering
 	openglCamera *= rtabmap::Transform(
@@ -495,6 +509,11 @@ int Scene::Render() {
 	glClearColor(r_, g_, b_, 1.0f);
 	glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
 
+	if(renderBackgroundCamera)
+	{
+		background_renderer_->Draw(uvsTransformed);
+	}
+
 	if(!currentPose_->isNull())
 	{
 		if (frustumVisible_ && gesture_camera_->GetCameraType() != tango_gl::GestureCamera::kFirstPerson)
@@ -523,7 +542,7 @@ int Scene::Render() {
 		}
 	}
 
-	if(gridVisible_)
+	if(gridVisible_ && !renderBackgroundCamera)
 	{
 		grid_->Render(projectionMatrix, viewMatrix);
 	}

app/android/jni/scene.h

@@ -38,6 +38,7 @@
 #include <point_cloud_drawable.h>
 #include <graph_drawable.h>
 #include <bounding_box_drawable.h>
+#include <background_renderer.h>
 
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
@@ -71,7 +72,7 @@ class Scene {
   //         frame's timestamp.
   // @param: point_cloud_vertices, point cloud's vertices of the current point
   //         frame.
-  int Render();
+  int Render(const float * uvsTransformed = 0, glm::mat4 arViewMatrix = glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0));
 
   // Set render camera's viewing angle, first person, third person or top down.
   //
@@ -152,6 +153,8 @@ class Scene {
   bool isLighting() const {return lighting_;}
   bool isBackfaceCulling() const {return backfaceCulling_;}
 
+  BackgroundRenderer * background_renderer_;
+
  private:
   // Camera object that allows user to use touch input to interact with.
   tango_gl::GestureCamera* gesture_camera_;

app/android/jni/util.h

@@ -252,7 +252,7 @@ inline ScreenRotation GetAndroidRotationFromColorCameraToDisplay(
 // @param display: integer value of display orientation, values available
 // are 0, 1, 2 ,3. Followed by Android display orientation standard:
 // https://developer.android.com/reference/android/view/Display.html#getRotation()
-// @param color_camera: integer value of color camera oreintation, values
+// @param color_camera: integer value of color camera orientation, values
 // available are 0, 90, 180, 270. Followed by Android camera orientation
 // standard:
 // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation

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

@@ -1265,7 +1265,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 							{
 								if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && mCameraDriver == 1)
 								{
-									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses and RGB images can be recorded.", mToast.LENGTH_LONG).show();
+									mToast.makeText(getApplicationContext(), "Currently ARCore NDK driver doesn't support depth, only poses, RGB images and 3d features can be recorded.", mToast.LENGTH_LONG).show();
 								}
 								updateState(mState==State.STATE_VISUALIZING?State.STATE_VISUALIZING_CAMERA:State.STATE_CAMERA);	
 								if(mState==State.STATE_VISUALIZING_CAMERA && mItemLocalizationMode.isChecked())