ARCore: when tof camera is not available, use arcore point cloud as keypoints

app/android/jni/CameraARCore.cpp

@@ -451,6 +451,54 @@ void CameraARCore::close()
 	CameraMobile::close();
 }
 
+LaserScan CameraARCore::scanFromPointCloudData(
+		const float * pointCloudData,
+		int points,
+		const Transform & pose,
+		const CameraModel & model,
+		const cv::Mat & rgb,
+		std::vector<cv::KeyPoint> * kpts,
+		std::vector<cv::Point3f> * kpts3D)
+{
+	if(pointCloudData && points>0)
+	{
+		cv::Mat scanData(1, points, CV_32FC4);
+		float * ptr = scanData.ptr<float>();
+		for(unsigned int i=0;i<points; ++i)
+		{
+			cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
+			pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
+			ptr[i*4] = pt.x;
+			ptr[i*4 + 1] = pt.y;
+			ptr[i*4 + 2] = pt.z;
+
+			//get color from rgb image
+			cv::Point3f org= pt;
+			pt = util3d::transformPoint(pt, opticalRotationInv);
+			int u,v;
+			model.reproject(pt.x, pt.y, pt.z, u, v);
+			unsigned char r=255,g=255,b=255;
+			if(model.inFrame(u, v))
+			{
+				b=rgb.at<cv::Vec3b>(v,u).val[0];
+				g=rgb.at<cv::Vec3b>(v,u).val[1];
+				r=rgb.at<cv::Vec3b>(v,u).val[2];
+				if(kpts)
+					kpts->push_back(cv::KeyPoint(u,v,3));
+				if(kpts3D)
+					kpts3D->push_back(org);
+			}
+			*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
+
+			//confidence
+			//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
+
+		}
+		return LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+	}
+	return LaserScan();
+}
+
 SensorData CameraARCore::captureImage(CameraInfo * info)
 {
 	UScopeMutex lock(arSessionMutex_);
@@ -492,7 +540,9 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 		ArCameraIntrinsics_getFocalLength(arSession_, arCameraIntrinsics_, &fx, &fy);
 		ArCameraIntrinsics_getPrincipalPoint(arSession_, arCameraIntrinsics_, &cx, &cy);
 		ArCameraIntrinsics_getImageDimensions(arSession_, arCameraIntrinsics_, &width, &height);
-		UINFO("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#ifndef DISABLE_LOG
+		LOGI("%f %f %f %f %d %d", fx, fy, cx, cy, width, height);
+#endif
 
 		if(fx > 0 && fy > 0 && width > 0 && height > 0 && cx > 0 && cy > 0)
 		{
@@ -513,13 +563,17 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 				{
 					int32_t num_planes;
 					int32_t pixel_stride;
+					int32_t row_stride;
 					const uint8_t * plane_data;
 					int32_t data_length;
 					ArImage_getNumberOfPlanes(arSession_, image, &num_planes);
 					for(int i=0;i<num_planes; ++i)
 					{
-						ArImage_getPlanePixelStride(arSession_, image, 0, &pixel_stride);
-						//LOGI("Plane %d/%d: w=%d h=%d stride=%d", i+1, num_planes, width, height, pixel_stride);
+						ArImage_getPlanePixelStride(arSession_, image, i, &pixel_stride);
+						ArImage_getPlaneRowStride(arSession_, image, i, &row_stride);
+#ifndef DISABLE_LOG
+						LOGI("Plane %d/%d: pixel stride=%d, row stride=%d", i+1, num_planes, pixel_stride, row_stride);
+#endif
 					}
 
 					ArImage_getPlaneData(arSession_, image, 0, &plane_data, &data_length);
@@ -527,10 +581,14 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 					if(plane_data != nullptr)
 					{
 						double stamp = double(timestamp_ns)/10e8;
-						//LOGI("data_length=%d stamp=%f", data_length, stamp);
+#ifndef DISABLE_LOG
+						LOGI("data_length=%d stamp=%f", data_length, stamp);
+#endif
 						cv::Mat rgb;
 						cv::cvtColor(cv::Mat(height+height/2, width, CV_8UC1, (void*)plane_data), rgb, CV_YUV2BGR_NV21);
 
+						std::vector<cv::KeyPoint> kpts;
+						std::vector<cv::Point3f> kpts3;
 						LaserScan scan;
 						if(pointCloud)
 						{
@@ -538,38 +596,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 							ArPointCloud_getNumberOfPoints(arSession_, pointCloud, &points);
 							const float * pointCloudData = 0;
 							ArPointCloud_getData(arSession_, pointCloud, &pointCloudData);
+#ifndef DISABLE_LOG
 							LOGI("pointCloudData=%d size=%d", pointCloudData?1:0, points);
+#endif
 							if(pointCloudData && points>0)
 							{
-								cv::Mat scanData(1, points, CV_32FC4);
-								float * ptr = scanData.ptr<float>();
-								for(unsigned int i=0;i<points; ++i)
-								{
-									cv::Point3f pt(pointCloudData[i*4], pointCloudData[i*4 + 1], pointCloudData[i*4 + 2]);
-									pt = util3d::transformPoint(pt, pose.inverse()*rtabmap_world_T_opengl_world);
-									ptr[i*4] = pt.x;
-									ptr[i*4 + 1] = pt.y;
-									ptr[i*4 + 2] = pt.z;
-
-									//get color from rgb image
-									cv::Point3f org= pt;
-									pt = util3d::transformPoint(pt, opticalRotationInv);
-									int u,v;
-									model.reproject(pt.x, pt.y, pt.z, u, v);
-									unsigned char r=255,g=255,b=255;
-									if(model.inFrame(u, v))
-									{
-										b=rgb.at<cv::Vec3b>(v,u).val[0];
-										g=rgb.at<cv::Vec3b>(v,u).val[1];
-										r=rgb.at<cv::Vec3b>(v,u).val[2];
-									}
-									*(int*)&ptr[i*4 + 3] = int(b) | (int(g) << 8) | (int(r) << 16);
-
-									//confidence
-									//*(int*)&ptr[i*4 + 3] = (int(pointCloudData[i*4 + 3] * 255.0f) << 8) | (int(255) << 16);
-
-								}
-								scan = LaserScan::backwardCompatibility(scanData, 0, 10, rtabmap::Transform::getIdentity());
+								scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3);
 							}
 						}
 						else
@@ -578,6 +610,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
 						}
 
 						data = SensorData(scan, rgb, cv::Mat(), model, 0, stamp);
+						data.setFeatures(kpts,  kpts3, cv::Mat());
 					}
 				}
 				else

app/android/jni/CameraARCore.h

@@ -50,6 +50,16 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace rtabmap {
 
 class CameraARCore : public CameraMobile {
+public:
+	static LaserScan scanFromPointCloudData(
+			const float * pointCloudData,
+			int points,
+			const Transform & pose,
+			const CameraModel & model,
+			const cv::Mat & rgb,
+			std::vector<cv::KeyPoint> * kpts = 0,
+			std::vector<cv::Point3f> * kpts3D = 0);
+
 public:
 	CameraARCore(void* env, void* context, void* activity, bool smoothing = false);
 	virtual ~CameraARCore();

app/android/jni/RTABMapApp.cpp

@@ -70,7 +70,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
 
 #define LOW_RES_PIX 2
-#define DEBUG_RENDERING_PERFORMANCE
+//#define DEBUG_RENDERING_PERFORMANCE
 
 const int g_optMeshId = -100;
 
@@ -1436,7 +1436,7 @@ int RTABMapApp::Render()
 				}
 
 #ifdef DEBUG_RENDERING_PERFORMANCE
-				LOGW("Looking for data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
+				LOGW("Looking for data to load (%d) %fs", (int)bufferedSensorData.size(), time.ticks());
 #endif
 
 				std::map<int, rtabmap::Transform> posesWithMarkers = rtabmapEvents.back()->getStats().poses();
@@ -3282,12 +3282,13 @@ void RTABMapApp::postOdometryEvent(
 		float fx, float fy, float cx, float cy,
 		double stamp,
 		void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
-		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat)
+		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		float * points, int pointsLen)
 {
 #ifdef RTABMAP_ARCORE
 	if(cameraDriver_ == 3 && camera_)
 	{
-		if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && rgb && depth)
+		if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && rgb)
 		{
 			if(rgbFormat == AR_IMAGE_FORMAT_YUV_420_888 &&
 			   depthFormat == AIMAGE_FORMAT_DEPTH16)
@@ -3295,7 +3296,11 @@ void RTABMapApp::postOdometryEvent(
 				cv::Mat outputRGB;
 				cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, rgb), outputRGB, CV_YUV2BGR_NV21);
 
-				cv::Mat outputDepth(depthHeight, depthWidth, CV_16UC1);
+				cv::Mat outputDepth;
+				if(depthHeight>0 && depthWidth>0)
+				{
+					outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
+				}
 				uint16_t *dataShort = (uint16_t *)depth;
 				for (int y = 0; y < outputDepth.rows; ++y)
 				{
@@ -3307,12 +3312,33 @@ void RTABMapApp::postOdometryEvent(
 					}
 				}
 
-				if(!outputRGB.empty() && !outputDepth.empty())
+				if(!outputRGB.empty())
 				{
 					rtabmap::CameraModel model = rtabmap::CameraModel(fx, fy, cx, cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
-					rtabmap::SensorData data(outputRGB, outputDepth, model, 0, stamp);
 					rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 					pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
+
+#ifndef DISABLE_LOG
+					LOGI("pointCloudData size=%d", pointsLen);
+#endif
+					std::vector<cv::KeyPoint> kpts;
+					std::vector<cv::Point3f> kpts3;
+					rtabmap::LaserScan scan;
+					if(points && pointsLen>0)
+					{
+						if(outputDepth.empty())
+						{
+							scan = rtabmap::CameraARCore::scanFromPointCloudData(points, pointsLen, pose, model, outputRGB, &kpts, &kpts3);
+						}
+						else
+						{
+							// We will recompute features if depth is available
+							scan = rtabmap::CameraARCore::scanFromPointCloudData(points, pointsLen, pose, model, outputRGB);
+						}
+					}
+
+					rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
+					data.setFeatures(kpts,  kpts3, cv::Mat());
 					camera_->setData(data, pose);
 					camera_->spinOnce();
 				}

app/android/jni/RTABMapApp.h

@@ -154,7 +154,8 @@ class RTABMapApp : public UEventsHandler {
   		float fx, float fy, float cx, float cy,
   		double stamp,
   		void * rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
-  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat);
+  		void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		float * points, int pointsLen);
 
  protected:
   virtual bool handleEvent(UEvent * event);

app/android/jni/jni_interface.cpp

@@ -862,18 +862,21 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 		float fx, float fy, float cx, float cy,
 		double stamp,
 		jobject rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
-		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat)
+		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		jobject points, int pointsLen)
 {
 	if(native_application)
 	{
 		void *rgbPtr = env->GetDirectBufferAddress(rgb);
 		void *depthPtr = env->GetDirectBufferAddress(depth);
+		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
 		native(native_application)->postOdometryEvent(
 				x,y,z,qx,qy,qz,qw,
 				fx,fy,cx,cy,
 				stamp,
 				rgbPtr, rgbLen, rgbWidth, rgbHeight, rgbFormat,
-				depthPtr, depthLen, depthWidth, depthHeight, depthFormat);
+				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				pointsPtr, pointsLen);
 	}
 	else
 	{

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

@@ -1,6 +1,7 @@
 package com.introlab.rtabmap;
 
 import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
 import java.util.ArrayList;
 import java.util.Arrays;
 import java.util.EnumSet;
@@ -12,6 +13,7 @@ import com.google.ar.core.CameraIntrinsics;
 import com.google.ar.core.Config;
 import com.google.ar.core.Frame;
 import com.google.ar.core.ImageMetadata;
+import com.google.ar.core.PointCloud;
 import com.google.ar.core.Pose;
 import com.google.ar.core.Session;
 import com.google.ar.core.SharedCamera;
@@ -90,6 +92,8 @@ public class ARCoreSharedCamera {
 	// Image reader that continuously processes CPU images.
 	public TOF_ImageReader mTOFImageReader = new TOF_ImageReader();
 	private boolean mTOFAvailable = false;
+	
+	public boolean isDepthSupported() {return mTOFAvailable;}
 
 	// Camera device state callback.
 	private final CameraDevice.StateCallback cameraDeviceCallback =
@@ -330,11 +334,11 @@ public class ARCoreSharedCamera {
 
 			// Enable auto focus mode while ARCore is running.
 			Config config = sharedSession.getConfig();
-			config.setFocusMode(Config.FocusMode.AUTO);
+			config.setFocusMode(Config.FocusMode.FIXED);
 			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
-			config.setPlaneFindingMode(Config.PlaneFindingMode.HORIZONTAL_AND_VERTICAL);
+			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
 			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
-			config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
+			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
 			sharedSession.configure(config);
 
 		}
@@ -351,9 +355,9 @@ public class ARCoreSharedCamera {
 		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
 		if (resolutions != null) {
 			for( String temp : resolutions) {
-				Log.v(TAG + "DEPTH16 resolution: ", temp);
+				Log.e(TAG + "DEPTH16 resolution: ", temp);
 			};
-			if (resolutions.size() > 0) mTOFAvailable = true;
+			if (resolutions.size()>0) mTOFAvailable = true;
 		}
 
 		// Color CPU Image.
@@ -462,8 +466,8 @@ public class ARCoreSharedCamera {
 		if (frame.getTimestamp() != 0) {
 			
 			Pose pose = camera.getPose();
-			if(!mActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
-			RTABMapLib.postCameraPoseEvent(mActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
+			if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("pose=%f %f %f q=%f %f %f %f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw()));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw());
 			
 			int rateMs = 100; // send images at most 10 Hz
 			if(System. currentTimeMillis() - mPreviousTime < rateMs)
@@ -475,32 +479,58 @@ public class ARCoreSharedCamera {
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
+				PointCloud cloud = frame.acquirePointCloud();
+				FloatBuffer points = cloud.getPoints();
 
 				if (image.getFormat() != ImageFormat.YUV_420_888) {
 					throw new IllegalArgumentException(
 							"Expected image in YUV_420_888 format, got format " + image.getFormat());
 				}
-
+				
+				if(!RTABMapActivity.DISABLE_LOG)
+				{
+					for(int i =0;i<image.getPlanes().length;++i)
+					{
+						Log.d(TAG, String.format("Plane[%d] pixel stride = %d,  row stride = %d", i, image.getPlanes()[i].getPixelStride(), image.getPlanes()[i].getRowStride()));
+					}
+				}
+				
 				float[] fl = intrinsics.getFocalLength();
 				float[] pp = intrinsics.getPrincipalPoint();
-				if(!mActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("fx=%f fy=%f cx=%f cy=%f", fl[0], fl[1], pp[0], pp[1]));
 				ByteBuffer rgb = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
 
 				double stamp = (double)image.getTimestamp()/10e8;
-				if(!mActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
 						image.getWidth(), image.getHeight(), rgb.limit(), image.getFormat(), stamp));
-				if(!mActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
-						mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
 
-				RTABMapLib.postOdometryEvent(
-						mActivity.nativeApplication,
-						pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
-						fl[0], fl[1], pp[0], pp[1], stamp, 
-						rgb, rgb.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-						mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16);
-				
+				if(mTOFAvailable)
+				{
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
+					
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							rgb, rgb.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							points, points.limit());
+				}
+				else
+				{
+					ByteBuffer bb = ByteBuffer.allocate(0);
+					RTABMapLib.postOdometryEvent(
+							RTABMapActivity.nativeApplication,
+							pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), 
+							fl[0], fl[1], pp[0], pp[1], stamp, 
+							rgb, rgb.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
+							bb, 0, 0, 0, ImageFormat.DEPTH16,
+							points, points.limit()/4);
+				}
 				
 				image.close();
+				cloud.close();
 				
 			} catch (NotYetAvailableException e) {
 

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

@@ -1185,6 +1185,12 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 									else
 									{
 										mRenderer.setCamera(mArCoreCamera);
+										if((mState==State.STATE_IDLE || mState==State.STATE_WELCOME) && !mArCoreCamera.isDepthSupported())
+										{
+											mItemRenderingPointCloud.setChecked(true);
+											RTABMapLib.setMeshRendering(nativeApplication, false, false);
+											mToast.makeText(getApplicationContext(), "Depth camera not found, only poses and RGB images can be recorded.", mToast.LENGTH_LONG).show();
+										}
 									}
 								}
 							}

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

@@ -1,6 +1,7 @@
 
 package com.introlab.rtabmap;
 import java.nio.ByteBuffer;
+import java.nio.FloatBuffer;
 
 import android.app.Activity;
 import android.content.Context;
@@ -143,6 +144,7 @@ public class RTABMapLib
     		float fx, float fy, float cx, float cy,
     		double stamp, 
     		ByteBuffer rgb, int rgbLen, int rgbWidth, int rgbHeight, int rgbFormat,
-    		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat);
+    		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+    		FloatBuffer points, int pointsLen);
     
 }