Added depth/rgb registration option

app/android/jni/CameraMobile.cpp

@@ -56,13 +56,13 @@ CameraMobile::CameraMobile(bool smoothing) :
 		Camera(10),
 		deviceTColorCamera_(Transform::getIdentity()),
 		spinOncePreviousStamp_(0.0),
+		textureId_(9999),
+		uvs_initialized_(false),
 		previousStamp_(0.0),
 		stampEpochOffset_(0.0),
 		smoothing_(smoothing),
 		colorCameraToDisplayRotation_(ROTATION_0),
-		originUpdate_(false),
+		originUpdate_(false)
-        textureId_(9999),
-        uvs_initialized_(false)
 {
     glGenTextures(1, &textureId_);
 }
@@ -156,7 +156,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
     
     LOGD("CameraMobile::setData textureId_=%d", (int)textureId_);
     
-    if(textureId_ != 0)
+    if(textureId_ != 0 && texCoord != 0)
     {
         cv::Mat rgbImage;
         cv::cvtColor(data.imageRaw(), rgbImage, CV_BGR2RGBA);

app/android/jni/RTABMapApp.cpp

@@ -916,6 +916,7 @@ void RTABMapApp::stopCamera()
 			camera_->close();
 			delete camera_;
 			camera_ = 0;
+			poseBuffer_.clear();
 		}
 	}
     {
@@ -1243,7 +1244,7 @@ int RTABMapApp::Render()
 					if(main_scene_.background_renderer_ == 0)
 					{
 						main_scene_.background_renderer_ = new BackgroundRenderer();
-						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId());
+						main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId(), true);
 					}
 					if(((rtabmap::CameraARCore*)camera_)->uvsInitialized())
 					{
@@ -1277,7 +1278,7 @@ int RTABMapApp::Render()
                     if(main_scene_.background_renderer_ == 0)
                     {
                         main_scene_.background_renderer_ = new BackgroundRenderer();
-                        main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId());
+                        main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), false);
                     }
                     if(((rtabmap::CameraMobile*)camera_)->uvsInitialized())
                     {
@@ -3657,7 +3658,7 @@ int RTABMapApp::postProcessing(int approach)
 }
 
 void RTABMapApp::postCameraPoseEvent(
-		float x, float y, float z, float qx, float qy, float qz, float qw)
+		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(cameraDriver_ == 3 && camera_)
@@ -3665,18 +3666,28 @@ void RTABMapApp::postCameraPoseEvent(
 		rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 		pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 		camera_->poseReceived(pose);
+
+		poseBuffer_.insert(std::make_pair(stamp, pose));
+		if(poseBuffer_.size() > 1000)
+		{
+			poseBuffer_.erase(poseBuffer_.begin());
+		}
 	}
 }
 
 void RTABMapApp::postOdometryEvent(
-		float x, float y, float z, float qx, float qy, float qz, float qw,
+		rtabmap::Transform pose,
-		float fx, float fy, float cx, float cy,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		const rtabmap::Transform & rgbFrame,
+		const rtabmap::Transform & depthFrame,
 		double stamp,
+		double depthStamp,
         const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
         const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
         const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
         const float * points, int pointsLen, int pointsChannels,
-        float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw,
+        const rtabmap::Transform & viewMatrix,
         float p00, float p11, float p02, float p12, float p22, float p32, float p23,
         float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
 {
@@ -3684,7 +3695,7 @@ void RTABMapApp::postOdometryEvent(
 	boost::mutex::scoped_lock  lock(cameraMutex_);
 	if(cameraDriver_ == 3 && camera_)
 	{
-		if(fx > 0.0f && fy > 0.0f && cx > 0.0f && cy > 0.0f && stamp > 0.0f && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight)
+		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)
 		{
 #ifndef DISABLE_LOG
             //LOGD("rgb format = %d depth format =%d ", rgbFormat, depthFormat);
@@ -3770,10 +3781,87 @@ void RTABMapApp::postOdometryEvent(
 
 				if(!outputRGB.empty())
 				{
-					rtabmap::CameraModel model = rtabmap::CameraModel(fx, fy, cx, cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
-					rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
 					pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
 
+					// Registration depth to rgb
+					if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
+					{
+						UTimer time;
+						rtabmap::Transform motion = rtabmap::Transform::getIdentity();
+						if(depthStamp != stamp && !poseBuffer_.empty())
+						{
+							// Interpolate pose
+							if(!poseBuffer_.empty())
+							{
+								if(poseBuffer_.rbegin()->first < depthStamp)
+								{
+									UWARN("Could not find poses to interpolate at time %f (last is %f)...", depthStamp, poseBuffer_.rbegin()->first);
+								}
+								else
+								{
+									std::map<double, rtabmap::Transform >::const_iterator iterB = poseBuffer_.lower_bound(depthStamp);
+									std::map<double, rtabmap::Transform >::const_iterator iterA = iterB;
+									rtabmap::Transform poseDepth;
+									if(iterA != poseBuffer_.begin())
+									{
+										iterA = --iterA;
+									}
+									if(iterB == poseBuffer_.end())
+									{
+										iterB = --iterB;
+									}
+									if(iterA == iterB && depthStamp == iterA->first)
+									{
+										poseDepth = iterA->second;
+									}
+									else if(depthStamp >= iterA->first && depthStamp <= iterB->first)
+									{
+										poseDepth = iterA->second.interpolate((depthStamp-iterA->first) / (iterB->first-iterA->first), iterB->second);
+									}
+									else if(depthStamp < iterA->first)
+									{
+										UERROR("Could not find poses to interpolate at image time %f (earliest is %f). Are sensors synchronized?", depthStamp, iterA->first);
+									}
+									else
+									{
+										UERROR("Could not find poses to interpolate at image time %f (between %f and %f), Are sensors synchronized?", depthStamp, iterA->first, iterB->first);
+									}
+									if(!poseDepth.isNull())
+									{
+#ifndef DISABLE_LOG
+										UDEBUG("poseRGB  =%s (stamp=%f)", pose.prettyPrint().c_str(), depthStamp);
+										UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
+#endif
+										motion = pose.inverse()*poseDepth;
+										// transform in camera frame
+#ifndef DISABLE_LOG
+										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+#endif
+										motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
+#ifndef DISABLE_LOG
+										UDEBUG("motion=%s", motion.prettyPrint().c_str());
+#endif
+									}
+								}
+							}
+						}
+						rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
+						float scale = (float)outputDepth.cols/(float)outputRGB.cols;
+						cv::Mat colorK = (cv::Mat_<double>(3,3) <<
+								rgb_fx*scale, 0, rgb_cx*scale,
+								0, rgb_fy*scale, rgb_cy*scale,
+								0, 0, 1);
+						cv::Mat depthK = (cv::Mat_<double>(3,3) <<
+								depth_fx, 0, depth_cx,
+								0, depth_fy, depth_cy,
+								0, 0, 1);
+						outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
+#ifndef DISABLE_LOG
+						UDEBUG("Depth registration time: %fs", time.elapsed());
+#endif
+					}
+
+					rtabmap::CameraModel model = rtabmap::CameraModel(rgb_fx, rgb_fy, rgb_cx, rgb_cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
 #ifndef DISABLE_LOG
 					//LOGI("pointCloudData size=%d", pointsLen);
 #endif
@@ -3818,7 +3906,7 @@ void RTABMapApp::postOdometryEvent(
                     projectionMatrix[2][2] = p22;
                     projectionMatrix[2][3] = p32;
                     projectionMatrix[3][2] = p23;
-                    glm::mat4 viewMatrix = rtabmap::glmFromTransform(rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw));
+                    glm::mat4 viewMatrixMat = rtabmap::glmFromTransform(viewMatrix);
                     float texCoords[8];
                     texCoords[0] = t0;
                     texCoords[1] = t1;
@@ -3828,7 +3916,7 @@ void RTABMapApp::postOdometryEvent(
                     texCoords[5] = t5;
                     texCoords[6] = t6;
                     texCoords[7] = t7;
-					camera_->setData(data, pose, viewMatrix, projectionMatrix, texCoords);
+					camera_->setData(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
 					camera_->spinOnce();
 				}
 			}
@@ -3836,7 +3924,7 @@ void RTABMapApp::postOdometryEvent(
 		else
 		{
 			UERROR("Missing image information! fx=%f fy=%f cx=%f cy=%f stamp=%f yPlane=%d vPlane=%d yPlaneLen=%d rgbWidth=%d rgbHeight=%d",
-                   fx, fy, cx, cy, stamp, yPlane?1:0, vPlane?1:0, yPlaneLen, rgbWidth, rgbHeight);
+                   rgb_fx, rgb_fy, rgb_cx, rgb_cy, stamp, yPlane?1:0, vPlane?1:0, yPlaneLen, rgbWidth, rgbHeight);
 		}
 	}
 #else

app/android/jni/RTABMapApp.h

@@ -176,17 +176,21 @@ class RTABMapApp : public UEventsHandler {
   int postProcessing(int approach);
 
   void postCameraPoseEvent(
-  		float x, float y, float z, float qx, float qy, float qz, float qw);
+  		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
 
   void postOdometryEvent(
-  		float x, float y, float z, float qx, float qy, float qz, float qw,
+		rtabmap::Transform pose,
-  		float fx, float fy, float cx, float cy,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
-  		double stamp,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		const rtabmap::Transform & rgbFrame,
+		const rtabmap::Transform & depthFrame,
+		double stamp,
+		double depthStamp,
         const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
         const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
         const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
         const float * points, int pointsLen, int pointsChannels,
-        float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		const rtabmap::Transform & viewMatrix, //view matrix
         float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
         float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
 
@@ -265,6 +269,7 @@ class RTABMapApp : public UEventsHandler {
 	std::list<rtabmap::RtabmapEvent*> rtabmapEvents_;
 	std::list<rtabmap::OdometryEvent> odomEvents_;
 	std::list<rtabmap::Transform> poseEvents_;
+	std::map<double, rtabmap::Transform> poseBuffer_;
 
 	rtabmap::Transform mapToOdom_;
 

app/android/jni/background_renderer.cc

@@ -34,17 +34,21 @@ const std::string kVertexShader =
 		"   v_TexCoord = a_TexCoord;\n"
 		"}\n";
 
-const std::string kFragmentShader =
+const std::string kFragmentShaderOES =
-#ifdef __ANDROID__
 	"#extension GL_OES_EGL_image_external : require\n"
-#endif
 	"precision mediump float;\n"
 	"varying vec2 v_TexCoord;\n"
-#ifdef __ANDROID__
 	"uniform samplerExternalOES sTexture;\n"
-#else
+	"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";
+
+const std::string kFragmentShader =
+	"precision mediump float;\n"
+	"varying vec2 v_TexCoord;\n"
     "uniform sampler2D sTexture;\n"
-#endif
 	"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"
@@ -73,11 +77,14 @@ const std::string kFragmentShader =
 
 }  // namespace
 
-void BackgroundRenderer::InitializeGlContent(GLuint textureId)
+void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
 {
   texture_id_ = textureId;
+  oes_ = oes;
 
-  shader_program_ = tango_gl::util::CreateProgram(kVertexShader.c_str(), kFragmentShader.c_str());
+  shader_program_ = tango_gl::util::CreateProgram(
+		  kVertexShader.c_str(),
+		  oes_?kFragmentShaderOES.c_str():kFragmentShader.c_str());
   if (!shader_program_) {
     LOGE("Could not create program.");
   }
@@ -95,11 +102,10 @@ void BackgroundRenderer::Draw(const float * transformed_uvs) {
   glEnable (GL_BLEND);
 
   glActiveTexture(GL_TEXTURE0);
-#ifdef __ANDROID__
+  if(oes_)
-  glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
+    glBindTexture(GL_TEXTURE_EXTERNAL_OES, texture_id_);
-#else
+  else
-  glBindTexture(GL_TEXTURE_2D, texture_id_);
+    glBindTexture(GL_TEXTURE_2D, texture_id_);
-#endif
 
   glVertexAttribPointer(attribute_vertices_, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
   glVertexAttribPointer(attribute_uvs_, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);

app/android/jni/background_renderer.h

@@ -55,7 +55,7 @@ public:
 
   // Sets up OpenGL state.  Must be called on the OpenGL thread and before any
   // other methods below.
-  void InitializeGlContent(GLuint textureId);
+  void InitializeGlContent(GLuint textureId, bool oes);
 
   // Draws the background image.  This methods must be called for every ArFrame
   // returned by ArSession_update() to catch display geometry change events.
@@ -65,6 +65,7 @@ public:
 
   GLuint shader_program_;
   GLuint texture_id_;
+  bool oes_ = false;
 
   GLuint attribute_vertices_;
   GLuint attribute_uvs_;

app/android/jni/jni_interface.cpp

@@ -869,11 +869,11 @@ Java_com_introlab_rtabmap_RTABMapLib_postProcessing(
 JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postCameraPoseEvent(
 		JNIEnv* env, jclass, jlong native_application,
-		float x, float y, float z, float qx, float qy, float qz, float qw)
+		float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
 {
 	if(native_application)
 	{
-		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw);
+		native(native_application)->postCameraPoseEvent(x,y,z,qx,qy,qz,qw, stamp);
 	}
 	else
 	{
@@ -886,30 +886,36 @@ JNIEXPORT void JNICALL
 Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 		JNIEnv* env, jclass, jlong native_application,
 		float x, float y, float z, float qx, float qy, float qz, float qw,
-		float fx, float fy, float cx, float cy,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
 		double stamp,
 		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
-		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+		jobject points, int pointsLen,
-		jobject points, int pointsLen)
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord
 {
 	if(native_application)
 	{
 		void *yPtr = env->GetDirectBufferAddress(yPlane);
 		void *uPtr = env->GetDirectBufferAddress(uPlane);
 		void *vPtr = env->GetDirectBufferAddress(vPlane);
-		void *depthPtr = env->GetDirectBufferAddress(depth);
 		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
 		native(native_application)->postOdometryEvent(
-				x,y,z,qx,qy,qz,qw,
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
-				fx,fy,cx,cy,
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				0,0,0,0,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(),
 				stamp,
+				0,
 				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
-				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				0,0,0,0,0, //depth
-				0,0,0,0,0,
+				0,0,0,0,0, //conf
 				pointsPtr, pointsLen, 4,
-				0,0,0,0,0,0,0,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
-				0,0,0,0,0,0,0,
+				p00, p11, p02, p12, p22, p32, p23,
-				0,0,0,0,0,0,0,0);
+				t0, t1, t2, t3, t4, t5, t6, t7);
 	}
 	else
 	{
@@ -918,6 +924,44 @@ Java_com_introlab_rtabmap_RTABMapLib_postOdometryEvent(
 	}
 }
 
+JNIEXPORT void JNICALL
+Java_com_introlab_rtabmap_RTABMapLib_postOdometryEventDepth(
+		JNIEnv* env, jclass, jlong native_application,
+		float x, float y, float z, float qx, float qy, float qz, float qw,
+		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+		double rgbStamp,
+		double depthStamp,
+		jobject yPlane, jobject uPlane, jobject vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+ 		jobject depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
+ 		jobject points, int pointsLen,
+		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+		float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+		float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7) // tex coord)
+ {
+		void *yPtr = env->GetDirectBufferAddress(yPlane);
+		void *uPtr = env->GetDirectBufferAddress(uPlane);
+		void *vPtr = env->GetDirectBufferAddress(vPlane);
+ 		void *depthPtr = env->GetDirectBufferAddress(depth);
+ 		float *pointsPtr = (float *)env->GetDirectBufferAddress(points);
+ 		native(native_application)->postOdometryEvent(
+				rtabmap::Transform(x,y,z,qx,qy,qz,qw),
+				rgb_fx,rgb_fy,rgb_cx,rgb_cy,
+				depth_fx,depth_fy,depth_cx,depth_cy,
+				rtabmap::Transform(rgbFrameX, rgbFrameY, rgbFrameZ, rgbFrameQX, rgbFrameQY, rgbFrameQZ, rgbFrameQW),
+				rtabmap::Transform(depthFrameX, depthFrameY, depthFrameZ, depthFrameQX, depthFrameQY, depthFrameQZ, depthFrameQW),
+				rgbStamp,
+				depthStamp,
+ 				yPtr, uPtr, vPtr, yPlaneLen, rgbWidth, rgbHeight, rgbFormat,
+ 				depthPtr, depthLen, depthWidth, depthHeight, depthFormat,
+				0,0,0,0,0, // conf
+ 				pointsPtr, pointsLen, 4,
+				rtabmap::Transform(vx, vy, vz, vqx, vqy, vqz, vqw),
+				p00, p11, p02, p12, p22, p32, p23,
+				t0, t1, t2, t3, t4, t5, t6, t7);
+ }
 
 
 #ifdef __cplusplus

app/android/res/values/strings.xml

@@ -51,7 +51,7 @@
     <string name="pref_key_remove_button">pref_key_remove_button</string>
     <string name="pref_key_reset_button">pref_key_reset_button</string>
     <string name="pref_key_density">pref_key_density</string>
-    <string name="pref_default_density">1</string>
+    <string name="pref_default_density">2</string>
     <string name="pref_key_min_depth">pref_key_min_depth</string>
     <string name="pref_default_min_depth">0</string>
     <string name="pref_key_depth">pref_key_depth</string>

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

@@ -11,8 +11,8 @@ import java.util.concurrent.atomic.AtomicBoolean;
 import com.google.ar.core.Camera;
 import com.google.ar.core.CameraIntrinsics;
 import com.google.ar.core.Config;
+import com.google.ar.core.Coordinates2d;
 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;
@@ -29,6 +29,7 @@ import android.hardware.camera2.CameraCaptureSession;
 import android.hardware.camera2.CameraCharacteristics;
 import android.hardware.camera2.CameraDevice;
 import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CameraMetadata;
 import android.hardware.camera2.CaptureFailure;
 import android.hardware.camera2.CaptureRequest;
 import android.hardware.camera2.TotalCaptureResult;
@@ -45,7 +46,11 @@ public class ARCoreSharedCamera {
 
 	public static final String TAG = ARCoreSharedCamera.class.getSimpleName();
 
-
+	private static final float[] QUAD_COORDS =
+      new float[] {
+        -1.0f, -1.0f, +1.0f, -1.0f, -1.0f, +1.0f, +1.0f, +1.0f,
+      };
+	
 	private static RTABMapActivity mActivity;
 	public ARCoreSharedCamera(RTABMapActivity c) {
 		mActivity = c;
@@ -53,8 +58,8 @@ public class ARCoreSharedCamera {
 
 	// Depth TOF Image.
 	// Use 240 * 180 for now, hardcoded for Huawei P30 Pro
-	private static final int DEPTH_WIDTH = 240;
+	private int depthWidth = 640;
-	private static final int DEPTH_HEIGHT = 180;
+	private int depthHeight = 480;
 
 	// GL Surface used to draw camera preview image.
 	public GLSurfaceView surfaceView;
@@ -81,6 +86,11 @@ public class ARCoreSharedCamera {
 
 	// Camera ID for the camera used by ARCore.
 	private String cameraId;
+	private String depthCameraId;
+	
+	private Pose rgbExtrinsics;
+	private Pose depthExtrinsics;
+	private float[] depthIntrinsics = null;
 	
 	private AtomicBoolean mReady = new AtomicBoolean(false);
 
@@ -95,6 +105,7 @@ public class ARCoreSharedCamera {
 	
 	public boolean isDepthSupported() {return mTOFAvailable;}
 
+	
 	// Camera device state callback.
 	private final CameraDevice.StateCallback cameraDeviceCallback =
 			new CameraDevice.StateCallback() {
@@ -176,7 +187,7 @@ public class ARCoreSharedCamera {
 				@NonNull CameraCaptureSession session,
 				@NonNull CaptureRequest request,
 				@NonNull TotalCaptureResult result) {
-			Log.i(TAG, "onCaptureCompleted");
+			//Log.i(TAG, "onCaptureCompleted");
 		}
 
 		//@Override // android 23 
@@ -251,7 +262,7 @@ public class ARCoreSharedCamera {
 
 			// Add a CPU image reader surface. On devices that don't support CPU image access, the image
 			// may arrive significantly later, or not arrive at all.
-			if (mTOFAvailable) surfaceList.add(mTOFImageReader.imageReader.getSurface());
+			if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) surfaceList.add(mTOFImageReader.imageReader.getSurface());
 			// Surface list should now contain three surfacemReadymReadys:
 			// 0. sharedCamera.getSurfaceTexture()
 			// 1. …
@@ -272,8 +283,6 @@ public class ARCoreSharedCamera {
 		} catch (CameraAccessException e) {
 			Log.e(TAG, "CameraAccessException", e);
 		}
-
-
 	}
 
 	// Start background handler thread, used to run callbacks without blocking UI thread.
@@ -301,6 +310,25 @@ public class ARCoreSharedCamera {
 	
 	private long mPreviousTime = 0;
 
+    /**
+     * Return true if the given array contains the given integer.
+     *
+     * @param modes array to check.
+     * @param mode  integer to get for.
+     * @return true if the array contains the given integer, otherwise false.
+     */
+    private static boolean contains(int[] modes, int mode) {
+        if (modes == null) {
+            return false;
+        }
+        for (int i : modes) {
+            if (i == mode) {
+                return true;
+            }
+        }
+        return false;
+    }
+    
 	// Perform various checks, then open camera device and create CPU image reader.
 	public boolean openCamera() {
 
@@ -317,7 +345,7 @@ public class ARCoreSharedCamera {
 			cameraTextureId = textures[0];
 		}
 
-		Log.v(TAG + " opencamera: ", "Perform various checks, then open camera device and create CPU image reader.");
+		Log.v(TAG, "Perform various checks, then open camera device and create CPU image reader.");
 		// Don't open camera if already opened.
 		if (cameraDevice != null) {
 			return false;
@@ -335,10 +363,10 @@ public class ARCoreSharedCamera {
 			// Enable auto focus mode while ARCore is running.
 			Config config = sharedSession.getConfig();
 			config.setFocusMode(Config.FocusMode.FIXED);
-			config.setUpdateMode(Config.UpdateMode.LATEST_CAMERA_IMAGE);
+			config.setUpdateMode(Config.UpdateMode.BLOCKING);
 			config.setPlaneFindingMode(Config.PlaneFindingMode.DISABLED);
 			config.setLightEstimationMode(Config.LightEstimationMode.DISABLED);
-			//config.setCloudAnchorMode(Config.CloudAnchorMode.ENABLED);
+			config.setCloudAnchorMode(Config.CloudAnchorMode.DISABLED);
 			sharedSession.configure(config);
 
 		}
@@ -347,27 +375,128 @@ public class ARCoreSharedCamera {
 		sharedCamera = sharedSession.getSharedCamera();
 		// Store the ID of the camera used by ARCore.
 		cameraId = sharedSession.getCameraConfig().getCameraId();
-		initCamera(mActivity, cameraId, 1);
+	
-		ArrayList<String> resolutions;
+		Log.d(TAG, "Shared camera ID: " + cameraId);
 
 		mTOFAvailable = false;
+		
+		// Store a reference to the camera system service.
+		cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
+		
+		depthCameraId = null;
+		// show all cameras
+		try {
+            // Find a CameraDevice that supports DEPTH16 captures, and configure state.
+            for (String tmpCameraId : cameraManager.getCameraIdList()) {
+                CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(tmpCameraId);
 
-		resolutions = getResolutions(mActivity, cameraId, ImageFormat.DEPTH16);
+                Log.i(TAG, "Camera " + tmpCameraId + " extrinsics:");
-		if (resolutions != null) {
+                
-			for( String temp : resolutions) {
+            	float[] translation = characteristics.get(CameraCharacteristics.LENS_POSE_TRANSLATION);
-				Log.e(TAG + "DEPTH16 resolution: ", temp);
+                if(translation != null)
-			};
+                {
-			if (resolutions.size()>0) mTOFAvailable = true;
+                	 Log.i(TAG, String.format("Translation (x,y,z): %f,%f,%f", translation[0], translation[1], translation[2]));
+                }
+                float[] rotation = characteristics.get(CameraCharacteristics.LENS_POSE_ROTATION);
+                if(rotation != null)
+                {
+                	 Log.i(TAG, String.format("Rotation (qx,qy,qz,qw): %f,%f,%f,%f",  rotation[0], rotation[1], rotation[2], rotation[3]));
+                }
+                
+                if(tmpCameraId.compareTo(cameraId)==0 && translation!=null && rotation!=null)
+                {
+                	rgbExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set rgb extrinsics!");
+                }
+                
+                if (!contains(characteristics.get(CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES), CameraCharacteristics.REQUEST_AVAILABLE_CAPABILITIES_DEPTH_OUTPUT) ||
+                		characteristics.get(CameraCharacteristics.LENS_FACING) == CameraMetadata.LENS_FACING_FRONT) {
+                    continue;
+                }
+                Log.i(TAG, "Camera " + tmpCameraId + " has depth output available");
+                
+                depthCameraId = tmpCameraId;
+                if(translation!=null && rotation != null)
+                {
+                	depthExtrinsics = new Pose(translation, rotation);
+                	Log.i(TAG,"Set depth extrinsics!");
+                }
+                depthIntrinsics = characteristics.get(CameraCharacteristics.LENS_INTRINSIC_CALIBRATION);
+                Log.i(TAG, String.format("Intrinsics (fx,fy,cx,cy,s): %f,%f,%f,%f,%f", 
+                		depthIntrinsics[0],
+                		depthIntrinsics[1],
+                		depthIntrinsics[2],
+                		depthIntrinsics[3],
+                		depthIntrinsics[4]));
+            }
+        } catch (CameraAccessException e) {
+            e.printStackTrace();
+        }
+		if(rgbExtrinsics == null)
+		{
+			float[] translation = {0,0,0};
+			float[] rotation = {0,0,0,1};
+			rgbExtrinsics = new Pose(translation, rotation);
 		}
+		if(depthExtrinsics == null)
+		{
+			depthExtrinsics = rgbExtrinsics;
+		}
+
+		if(depthCameraId != null)
+		{
+			ArrayList<String> resolutions = getResolutions(mActivity, depthCameraId, ImageFormat.DEPTH16);
+			if (resolutions != null) {
+				float[] newDepthIntrinsics = null;
+				int largestWidth = 0;
+				for( String temp : resolutions) {
+					Log.i(TAG, "DEPTH16 resolution: " + temp);
+					depthWidth = Integer.parseInt(temp.split("x")[0]);
+					depthHeight = Integer.parseInt(temp.split("x")[1]);
+					if(depthIntrinsics != null)
+					{
+						if(depthIntrinsics[0] != 0)
+						{
+							if(largestWidth == 0 && depthWidth>largestWidth)
+							{
+								largestWidth = depthWidth; // intrinsics should match this resolution
+							}
+							// Samsung Galaxy Note10+: take smallest resolution and match the intrinsics
+							if(depthWidth < largestWidth)
+							{
+								float scale = (float)depthWidth/(float)largestWidth;
+								newDepthIntrinsics = depthIntrinsics.clone();
+								newDepthIntrinsics[0] *= scale;
+								newDepthIntrinsics[1] *= scale;
+								newDepthIntrinsics[2] *= scale;
+								newDepthIntrinsics[3] *= scale;
+							}
+						}
+						else if(depthWidth ==240 && depthHeight==180)
+						{
+							// Huawei P30 Pro: only 240x180 is working
+							break;
+						}
+					}
+				}
+				if (resolutions.size()>0) {
+					mTOFAvailable = true;
+					if(newDepthIntrinsics!=null) {
+						depthIntrinsics = newDepthIntrinsics;
+					}
+				}
+			}
+		}
+		Log.i(TAG, "TOF_available: " + mTOFAvailable);
 
 		// Color CPU Image.
 		// Use the currently configured CPU image size.
 		//Size desiredCPUImageSize = sharedSession.getCameraConfig().getImageSize();
 
-		if (mTOFAvailable) mTOFImageReader.createImageReader(DEPTH_WIDTH, DEPTH_HEIGHT);
+		if (mTOFAvailable) mTOFImageReader.createImageReader(depthWidth, depthHeight);
 
 		// When ARCore is running, make sure it also updates our CPU image surface.
-		if (mTOFAvailable) {
+		if (mTOFAvailable && cameraId.compareTo(depthCameraId) == 0) {
 			sharedCamera.setAppSurfaces(this.cameraId, Arrays.asList(mTOFImageReader.imageReader.getSurface()));
 		}
 
@@ -376,15 +505,14 @@ public class ARCoreSharedCamera {
 			// Wrap our callback in a shared camera callback.
 			CameraDevice.StateCallback wrappedCallback = sharedCamera.createARDeviceStateCallback(cameraDeviceCallback, backgroundHandler);
 
-			// Store a reference to the camera system service.
-			cameraManager = (CameraManager) mActivity.getSystemService(Context.CAMERA_SERVICE);
-
-			// Get the characteristics for the ARCore camera.
-			//CameraCharacteristics characteristics = cameraManager.getCameraCharacteristics(this.cameraId);
-
 			// Open the camera device using the ARCore wrapped callback.
 			cameraManager.openCamera(cameraId, wrappedCallback, backgroundHandler);
 			
+			if(mTOFAvailable && cameraId.compareTo(depthCameraId) != 0)
+			{
+				cameraManager.openCamera(depthCameraId, mTOFImageReader.cameraDeviceCallback, mTOFImageReader.backgroundHandler);
+			}
+			
 		} catch (CameraAccessException e) {
 			Log.e(TAG, "Failed to open camera", e);
 			return false;
@@ -395,14 +523,58 @@ public class ARCoreSharedCamera {
 			Log.e(TAG, "Failed to open camera", e);
 			return false;
 		} 
-
+		
-		Log.i(TAG, " opencamera: TOF_available: " + mTOFAvailable);
 		return true;
 	}
+	
+	public static void rotationMatrixToQuaternion(float[] R, float[] q) {
+        final float m00 = R[0];
+        final float m10 = R[1];
+        final float m20 = R[2];
+        final float m01 = R[4];
+        final float m11 = R[5];
+        final float m21 = R[6];
+        final float m02 = R[8];
+        final float m12 = R[9];
+        final float m22 = R[10];
+
+        float tr = m00 + m11 + m22;
+
+        if (tr > 0) {
+            float S = (float) Math.sqrt(tr + 1.0) * 2; // S=4*qw 
+            q[0] = 0.25f * S;/* w  w  w.j ava  2s.co m*/
+            q[1] = (m21 - m12) / S;
+            q[2] = (m02 - m20) / S;
+            q[3] = (m10 - m01) / S;
+        } else if ((m00 > m11) & (m00 > m22)) {
+            float S = (float) Math.sqrt(1.0 + m00 - m11 - m22) * 2; //
+            // S=4*q[1]
+            q[0] = (m21 - m12) / S;
+            q[1] = 0.25f * S;
+            q[2] = (m01 + m10) / S;
+            q[3] = (m02 + m20) / S;
+        } else if (m11 > m22) {
+            float S = (float) Math.sqrt(1.0 + m11 - m00 - m22) * 2; //
+            // S=4*q[2]
+            q[0] = (m02 - m20) / S;
+            q[1] = (m01 + m10) / S;
+            q[2] = 0.25f * S;
+            q[3] = (m12 + m21) / S;
+        } else {
+            float S = (float) Math.sqrt(1.0 + m22 - m00 - m11) * 2; //
+            // S=4*q[3]
+            q[0] = (m10 - m01) / S;
+            q[1] = (m02 + m20) / S;
+            q[2] = (m12 + m21) / S;
+            q[3] = 0.25f * S;
+        }
+
+    }
 
 
 	// Close the camera device.
 	public void close() {
+		Log.w(TAG, "close()");
 		
 		if (sharedSession != null)  {
 			sharedSession.pause();
@@ -415,11 +587,8 @@ public class ARCoreSharedCamera {
 		if (cameraDevice != null) {
 			cameraDevice.close();
 		}
-
+		
-		if (mTOFImageReader.imageReader != null) {
+		mTOFImageReader.close();
-			mTOFImageReader.imageReader.close();
-			mTOFImageReader.imageReader = null;
-		}
 
 		if(cameraTextureId>=0)
 		{
@@ -429,8 +598,12 @@ public class ARCoreSharedCamera {
 		stopBackgroundThread();
 	}
 
-	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */
+	public void setDisplayGeometry(int rotation, int width, int height)
-
+	{
+		sharedSession.setDisplayGeometry(rotation, width, height);
+	}
+	
+	/*************************************************** ONDRAWFRAME ARCORE ************************************************************* */	
 	// Draw frame when in AR mode. Called on the GL thread.
 	public void updateGL() throws CameraNotAvailableException {
 
@@ -463,12 +636,12 @@ public class ARCoreSharedCamera {
 		if (camera.getTrackingState() == TrackingState.PAUSED) return;
 
 		if (frame.getTimestamp() != 0) {
-			
 			Pose pose = camera.getPose();
-			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()));
+			double stamp = (double)frame.getTimestamp()/10e8;
-			RTABMapLib.postCameraPoseEvent(RTABMapActivity.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 stamp=%f", pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp));
+			RTABMapLib.postCameraPoseEvent(RTABMapActivity.nativeApplication, pose.tx(), pose.ty(), pose.tz(), pose.qx(), pose.qy(), pose.qz(), pose.qw(), stamp);
 			
-			int rateMs = 100; // send images at most 10 Hz
+			int rateMs = 100; // send images at most 10 Hz (to save battery)
 			if(System. currentTimeMillis() - mPreviousTime < rateMs)
 			{
 				return;
@@ -478,6 +651,7 @@ public class ARCoreSharedCamera {
 			CameraIntrinsics intrinsics = camera.getImageIntrinsics();
 			try{
 				Image image = frame.acquireCameraImage();
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("frame=%d vs image=%d", frame.getTimestamp(), image.getTimestamp()));
 				PointCloud cloud = frame.acquirePointCloud();
 				FloatBuffer points = cloud.getPoints();
 
@@ -502,33 +676,66 @@ public class ARCoreSharedCamera {
 				ByteBuffer u = image.getPlanes()[1].getBuffer().asReadOnlyBuffer();
 				ByteBuffer v = image.getPlanes()[2].getBuffer().asReadOnlyBuffer();
 
-				double stamp = (double)image.getTimestamp()/10e8;
+				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d stamp=%f", 
-				if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("RGB %dx%d len=%dbytes format=%d =%f",
 						image.getWidth(), image.getHeight(), y.limit(), image.getFormat(), stamp));
+				
+				float[] texCoord = new float[8];
+				frame.transformCoordinates2d(
+				        Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES,
+				        QUAD_COORDS,
+				        Coordinates2d.TEXTURE_NORMALIZED,
+				        texCoord);
+				
+				float[] p = new float[16];
+		        camera.getProjectionMatrix(p, 0, 0.1f, 100.0f);
+   
+		        float[] viewMatrix = new float[16];
+		        camera.getViewMatrix(viewMatrix, 0);
+		        float[] quat = new float[4];
+		        rotationMatrixToQuaternion(viewMatrix, quat);
 
+		        
 				if(mTOFAvailable)
 				{
-					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+					ByteBuffer depth;
-							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, mTOFImageReader.depth16_raw.limit(), ImageFormat.DEPTH16, (double)mTOFImageReader.timestamp/10e9));
+					double depthStamp;
+					synchronized (mTOFImageReader) {
+						depth = mTOFImageReader.depth16_raw;
+						depthStamp = (double)mTOFImageReader.timestamp/10e8;
+					}
 					
-					RTABMapLib.postOdometryEvent(
+					if(!RTABMapActivity.DISABLE_LOG) Log.d(TAG, String.format("Depth %dx%d len=%dbytes format=%d stamp=%f",
+							mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, depth.limit(), ImageFormat.DEPTH16, depthStamp));
+
+					RTABMapLib.postOdometryEventDepth(
 							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, 
+							fl[0], fl[1], pp[0], pp[1], 
+							depthIntrinsics[0], depthIntrinsics[1], depthIntrinsics[2], depthIntrinsics[3],
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							depthExtrinsics.tx(), depthExtrinsics.ty(), depthExtrinsics.tz(), depthExtrinsics.qx(), depthExtrinsics.qy(), depthExtrinsics.qz(), depthExtrinsics.qw(),
+							stamp,
+							depthStamp,
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							mTOFImageReader.depth16_raw, mTOFImageReader.depth16_raw.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
+							depth, depth.limit(), mTOFImageReader.WIDTH, mTOFImageReader.HEIGHT, ImageFormat.DEPTH16,
-							points, points.limit()/4);
+							points, points.limit()/4,
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
 				}
 				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, 
+							fl[0], fl[1], pp[0], pp[1], 
+							rgbExtrinsics.tx(), rgbExtrinsics.ty(), rgbExtrinsics.tz(), rgbExtrinsics.qx(), rgbExtrinsics.qy(), rgbExtrinsics.qz(), rgbExtrinsics.qw(),
+							stamp, 
 							y, u, v, y.limit(), image.getWidth(), image.getHeight(), image.getFormat(), 
-							bb, 0, 0, 0, ImageFormat.DEPTH16,
+							points, points.limit()/4,
-							points, points.limit()/4);
+							viewMatrix[12], viewMatrix[13], viewMatrix[14], quat[1], quat[2], quat[3], quat[0],
+							p[0], p[5], p[8], p[9], p[10], p[11], p[14],
+                            texCoord[0],texCoord[1],texCoord[2],texCoord[3],texCoord[4],texCoord[5],texCoord[6],texCoord[7]);
 				}
 				
 				image.close();
@@ -545,8 +752,8 @@ public class ARCoreSharedCamera {
 	/********************************************************************************************************************* */
 
 
-	public ArrayList<String> getResolutions (Context context, String cameraId,int imageFormat){
+	public ArrayList<String> getResolutions (Context context, String cameraId, int imageFormat){
-		Log.v(TAG + "getResolutions:", " cameraId:" + cameraId + " imageFormat: " + imageFormat);
+		Log.v(TAG, "getResolutions: cameraId:" + cameraId + " imageFormat: " + imageFormat);
 
 		ArrayList<String> output = new ArrayList<String>();
 		try {
@@ -562,27 +769,4 @@ public class ARCoreSharedCamera {
 		return output;
 	}
 
-
-	public void initCamera (Context context, String cameraId,int index){
-		boolean ok = false;
-		try {
-			int current = 0;
-			CameraManager manager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
-			CameraCharacteristics characteristics = manager.getCameraCharacteristics(cameraId);
-			for (android.util.Size s : characteristics.get(CameraCharacteristics.SCALER_STREAM_CONFIGURATION_MAP).getOutputSizes(ImageFormat.DEPTH16)) {
-				ok = true;
-				if (current == index)
-					break;
-				else ;
-				current++;
-			}
-		} catch (Exception e) {
-			e.printStackTrace();
-		}
-		if (!ok) {
-			Log.e(TAG + " initCamera", "Depth sensor not found!");
-		}
-	}
-
-
 }

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

@@ -0,0 +1,166 @@
+package com.introlab.rtabmap;
+
+/*
+ * Copyright 2017 Google LLC
+ *
+ * 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.
+ */
+
+import android.app.Activity;
+import android.content.Context;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCharacteristics;
+import android.hardware.camera2.CameraManager;
+import android.hardware.display.DisplayManager;
+import android.hardware.display.DisplayManager.DisplayListener;
+import android.view.Display;
+import android.view.Surface;
+import android.view.WindowManager;
+import com.google.ar.core.Session;
+
+/**
+ * Helper to track the display rotations. In particular, the 180 degree rotations are not notified
+ * by the onSurfaceChanged() callback, and thus they require listening to the android display
+ * events.
+ */
+public final class DisplayRotationHelper implements DisplayListener {
+  private boolean viewportChanged;
+  private int viewportWidth;
+  private int viewportHeight;
+  private final Display display;
+  private final DisplayManager displayManager;
+  private final CameraManager cameraManager;
+
+  /**
+   * Constructs the DisplayRotationHelper but does not register the listener yet.
+   *
+   * @param context the Android {@link Context}.
+   */
+  public DisplayRotationHelper(Context context) {
+    displayManager = (DisplayManager) context.getSystemService(Context.DISPLAY_SERVICE);
+    cameraManager = (CameraManager) context.getSystemService(Context.CAMERA_SERVICE);
+    WindowManager windowManager = (WindowManager) context.getSystemService(Context.WINDOW_SERVICE);
+    display = windowManager.getDefaultDisplay();
+  }
+
+  /** Registers the display listener. Should be called from {@link Activity#onResume()}. */
+  public void onResume() {
+    displayManager.registerDisplayListener(this, null);
+  }
+
+  /** Unregisters the display listener. Should be called from {@link Activity#onPause()}. */
+  public void onPause() {
+    displayManager.unregisterDisplayListener(this);
+  }
+
+  /**
+   * Records a change in surface dimensions. This will be later used by {@link
+   * #updateSessionIfNeeded(Session)}. Should be called from {@link
+   * android.opengl.GLSurfaceView.Renderer
+   * #onSurfaceChanged(javax.microedition.khronos.opengles.GL10, int, int)}.
+   *
+   * @param width the updated width of the surface.
+   * @param height the updated height of the surface.
+   */
+  public void onSurfaceChanged(int width, int height) {
+    viewportWidth = width;
+    viewportHeight = height;
+    viewportChanged = true;
+  }
+
+  /**
+   * Updates the session display geometry if a change was posted either by {@link
+   * #onSurfaceChanged(int, int)} call or by {@link #onDisplayChanged(int)} system callback. This
+   * function should be called explicitly before each call to {@link Session#update()}. This
+   * function will also clear the 'pending update' (viewportChanged) flag.
+   *
+   * @param session the {@link Session} object to update if display geometry changed.
+   */
+  public void updateSessionIfNeeded(ARCoreSharedCamera session) {
+    if (viewportChanged) {
+      int displayRotation = display.getRotation();
+      session.setDisplayGeometry(displayRotation, viewportWidth, viewportHeight);
+      viewportChanged = false;
+    }
+  }
+
+  /**
+   *  Returns the aspect ratio of the GL surface viewport while accounting for the display rotation
+   *  relative to the device camera sensor orientation.
+   */
+  public float getCameraSensorRelativeViewportAspectRatio(String cameraId) {
+    float aspectRatio;
+    int cameraSensorToDisplayRotation = getCameraSensorToDisplayRotation(cameraId);
+    switch (cameraSensorToDisplayRotation) {
+      case 90:
+      case 270:
+        aspectRatio = (float) viewportHeight / (float) viewportWidth;
+        break;
+      case 0:
+      case 180:
+        aspectRatio = (float) viewportWidth / (float) viewportHeight;
+        break;
+      default:
+        throw new RuntimeException("Unhandled rotation: " + cameraSensorToDisplayRotation);
+    }
+    return aspectRatio;
+  }
+
+  /**
+   * Returns the rotation of the back-facing camera with respect to the display. The value is one of
+   * 0, 90, 180, 270.
+   */
+  public int getCameraSensorToDisplayRotation(String cameraId) {
+    CameraCharacteristics characteristics;
+    try {
+      characteristics = cameraManager.getCameraCharacteristics(cameraId);
+    } catch (CameraAccessException e) {
+      throw new RuntimeException("Unable to determine display orientation", e);
+    }
+
+    // Camera sensor orientation.
+    int sensorOrientation = characteristics.get(CameraCharacteristics.SENSOR_ORIENTATION);
+
+    // Current display orientation.
+    int displayOrientation = toDegrees(display.getRotation());
+
+    // Make sure we return 0, 90, 180, or 270 degrees.
+    return (sensorOrientation - displayOrientation + 360) % 360;
+  }
+
+  private int toDegrees(int rotation) {
+    switch (rotation) {
+      case Surface.ROTATION_0:
+        return 0;
+      case Surface.ROTATION_90:
+        return 90;
+      case Surface.ROTATION_180:
+        return 180;
+      case Surface.ROTATION_270:
+        return 270;
+      default:
+        throw new RuntimeException("Unknown rotation " + rotation);
+    }
+  }
+
+  @Override
+  public void onDisplayAdded(int displayId) {}
+
+  @Override
+  public void onDisplayRemoved(int displayId) {}
+
+  @Override
+  public void onDisplayChanged(int displayId) {
+    viewportChanged = true;
+  }
+}

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

@@ -104,7 +104,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener
 
 	// Tag for debug logging.
 	public static final String TAG = RTABMapActivity.class.getSimpleName();
-	public static boolean DISABLE_LOG = true;
+	public static boolean DISABLE_LOG = false;
 
 	// The minimum Tango Core version required from this application.
 	private static final int  MIN_TANGO_CORE_VERSION = 9377;

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

@@ -134,13 +134,30 @@ public class RTABMapLib
     public static native float getUpdateTime(long nativeApplication);
     public static native int getLoopClosureId(long nativeApplication);
     
-    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw);
+    public static native void postCameraPoseEvent(long nativeApplication, float x, float y, float z, float qx, float qy, float qz, float qw, double stamp);
     public static native void postOdometryEvent(long nativeApplication,
     		float x, float y, float z, float qx, float qy, float qz, float qw,
-    		float fx, float fy, float cx, float cy,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
     		double stamp, 
     		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
+    		FloatBuffer points, int pointsLen,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
+    public static native void postOdometryEventDepth(long nativeApplication,
+    		float x, float y, float z, float qx, float qy, float qz, float qw,
+    		float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
+    		float depth_fx, float depth_fy, float depth_cx, float depth_cy,
+    		float rgbFrameX, float rgbFrameY, float rgbFrameZ, float rgbFrameQX, float rgbFrameQY, float rgbFrameQZ, float rgbFrameQW,
+    		float depthFrameX, float depthFrameY, float depthFrameZ, float depthFrameQX, float depthFrameQY, float depthFrameQZ, float depthFrameQW,
+    		double rgbStamp,
+    		double depthStamp,
+    		ByteBuffer yPlane, ByteBuffer uPlane, ByteBuffer vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
     		ByteBuffer depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
-    		FloatBuffer points, int pointsLen);
+    		FloatBuffer points, int pointsLen,
+    		float vx, float vy, float vz, float vqx, float vqy, float vqz, float vqw, //view matrix
+            float p00, float p11, float p02, float p12, float p22, float p32, float p23, // projection matrix
+            float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7); // tex coord
     
 }

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

@@ -44,12 +44,14 @@ public class Renderer implements GLSurfaceView.Renderer {
 	private float mTextColor = 1.0f;
 	private int mOffset = 0;
 	private ARCoreSharedCamera mCamera = null;
+	private DisplayRotationHelper mDisplayRotationHelper = null;
 	
 	private Vector<TextObject> mTexts;
 
 	private static RTABMapActivity mActivity;
 	public Renderer(RTABMapActivity c) {
 		mActivity = c;
+		mDisplayRotationHelper = new DisplayRotationHelper(/*context=*/ c);
 	}
 
 	private ProgressDialog mProgressDialog = null;
@@ -76,6 +78,11 @@ public class Renderer implements GLSurfaceView.Renderer {
 	public void setCamera(ARCoreSharedCamera camera)
 	{
 		mCamera = camera;
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.onDisplayChanged(0);
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
 	}
 
 	// Render loop of the Gl context.
@@ -176,6 +183,12 @@ public class Renderer implements GLSurfaceView.Renderer {
 			RTABMapLib.setupGraphic(mActivity.nativeApplication, width, height);
 		}
 		
+		mDisplayRotationHelper.onSurfaceChanged(width, height);
+		if(mCamera!=null)
+		{
+			mDisplayRotationHelper.updateSessionIfNeeded(mCamera);
+		}
+		
 		mSurfaceHeight = (float)height;
 
 		// Clear our matrices

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

@@ -2,18 +2,33 @@ package com.introlab.rtabmap;
 
 
 import android.graphics.ImageFormat;
+import android.hardware.camera2.CameraAccessException;
+import android.hardware.camera2.CameraCaptureSession;
+import android.hardware.camera2.CameraDevice;
+import android.hardware.camera2.CameraManager;
+import android.hardware.camera2.CaptureFailure;
+import android.hardware.camera2.CaptureRequest;
+import android.hardware.camera2.TotalCaptureResult;
 import android.media.Image;
 import android.media.ImageReader;
 import android.os.Handler;
 import android.os.HandlerThread;
+import android.support.annotation.NonNull;
 import android.util.Log;
+import android.view.Surface;
 
 import java.nio.ByteBuffer;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
 
 public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
 
-    public int WIDTH;
+	public static final String TAG = TOF_ImageReader.class.getSimpleName();
-    public int HEIGHT;
+	
+	public int WIDTH;
+	public int HEIGHT;
+	
     public ImageReader imageReader;
     public int frameCount = 0;
     public long timestamp;
@@ -21,16 +36,177 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
     // Looper handler thread.
     private HandlerThread backgroundThread;
     // Looper handler.
-    private Handler backgroundHandler;
+    public Handler backgroundHandler;
 
     public ByteBuffer depth16_raw;
+    
+    // Camera capture session. 
+ 	private CameraCaptureSession captureSession = null;
+ 	// Camera device. 
+ 	private CameraDevice cameraDevice = null;
+ 	
+ // Camera preview capture request builder
+ 	private CaptureRequest.Builder previewCaptureRequestBuilder;
+
 
     TOF_ImageReader(){
     }
+    
+    public void close()
+    {
+    	Log.i(TAG, "close()");
+    	if (captureSession != null) {
+			captureSession.close();
+			captureSession = null;
+		}
+		if (cameraDevice != null) {
+			cameraDevice.close();
+			cameraDevice = null;
+		}
+		
+		if (imageReader != null) {
+			imageReader.close();
+			imageReader = null;
+		}
+    }
+    
+ // Camera device state callback.
+ 	public final CameraDevice.StateCallback cameraDeviceCallback =
+ 			new CameraDevice.StateCallback() {
+ 		@Override
+ 		public void onOpened(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera depth ID " + cameraDevice.getId() + " opened.");
+ 			TOF_ImageReader.this.cameraDevice = cameraDevice;
+ 			createCameraPreviewSession();
+ 		}
+
+ 		@Override
+ 		public void onClosed(@NonNull CameraDevice cameraDevice) {
+ 			Log.d(TAG, "Camera device ID " + cameraDevice.getId() + " closed.");
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onDisconnected(@NonNull CameraDevice cameraDevice) {
+ 			Log.w(TAG, "Camera depth ID " + cameraDevice.getId() + " disconnected.");
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+
+ 		@Override
+ 		public void onError(@NonNull CameraDevice cameraDevice, int error) {
+ 			Log.e(TAG, "Camera depth ID " + cameraDevice.getId() + " error " + error);
+ 			cameraDevice.close();
+ 			TOF_ImageReader.this.cameraDevice = null;
+ 		}
+ 	};
+ 	
+ 	private CameraCaptureSession.StateCallback captureStateCallback = new CameraCaptureSession.StateCallback() {
+ 		// Called when the camera capture session is first configured after the app
+		// is initialized, and again each time the activity is resumed.
+		@Override
+		public void onConfigured(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session configured.");
+			captureSession = session;
+			setRepeatingCaptureRequest();
+		}
+
+		@Override
+		public void onSurfacePrepared(
+				@NonNull CameraCaptureSession session, @NonNull Surface surface) {
+			Log.d(TAG, "Camera capture surface prepared.");
+		}
+
+		@Override
+		public void onReady(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session ready.");
+		}
+
+		@Override
+		public void onActive(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session active.");
+		}
+
+		@Override
+		public void onClosed(@NonNull CameraCaptureSession session) {
+			Log.d(TAG, "Camera capture session closed.");
+		}
+
+		@Override
+		public void onConfigureFailed(@NonNull CameraCaptureSession session) {
+			Log.e(TAG, "Failed to configure camera capture session.");
+		}
+ 	};
+ 	
+ // Repeating camera capture session capture callback.
+ 	private final CameraCaptureSession.CaptureCallback captureSessionCallback =
+ 			new CameraCaptureSession.CaptureCallback() {
+
+ 		@Override
+ 		public void onCaptureCompleted(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull TotalCaptureResult result) {
+ 			//Log.i(TAG, "onCaptureCompleted");
+ 		}
+
+ 		//@Override // android 23 
+ 		public void onCaptureBufferLost(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull Surface target,
+ 				long frameNumber) {
+ 			Log.e(TAG, "onCaptureBufferLost: " + frameNumber);
+ 		}
+
+ 		@Override
+ 		public void onCaptureFailed(
+ 				@NonNull CameraCaptureSession session,
+ 				@NonNull CaptureRequest request,
+ 				@NonNull CaptureFailure failure) {
+ 			Log.e(TAG, "onCaptureFailed: " + failure.getFrameNumber() + " " + failure.getReason());
+ 		}
+
+ 		@Override
+ 		public void onCaptureSequenceAborted(
+ 				@NonNull CameraCaptureSession session, int sequenceId) {
+ 			Log.e(TAG, "onCaptureSequenceAborted: " + sequenceId + " " + session);
+ 		}
+ 	};
+ 	
+ 	// Called when starting non-AR mode or switching to non-AR mode.
+ 	// Also called when app starts in AR mode, or resumes in AR mode.
+ 	private void setRepeatingCaptureRequest() {
+ 		try {
+ 			captureSession.setRepeatingRequest(
+ 					previewCaptureRequestBuilder.build(), captureSessionCallback, backgroundHandler);
+ 		} catch (CameraAccessException e) {
+ 			Log.e(TAG, "Failed to set repeating request", e);
+ 		}
+ 	}
+ 	
+ 	private void createCameraPreviewSession() {
+		Log.e(TAG, "createCameraPreviewSession: " + "starting camera preview session.");
+		frameCount = 0;
+		try {
+			
+			// Create an ARCore compatible capture request using `TEMPLATE_RECORD`.
+			previewCaptureRequestBuilder = cameraDevice.createCaptureRequest(CameraDevice.TEMPLATE_RECORD);
+
+			previewCaptureRequestBuilder.addTarget(imageReader.getSurface());
+
+			// Create camera capture session for camera preview using callback.
+			cameraDevice.createCaptureSession(Arrays.asList(imageReader.getSurface()), captureStateCallback, backgroundHandler);
+
+		} catch (CameraAccessException e) {
+			Log.e(TAG, "CameraAccessException", e);
+		}
+	}
 
     public void createImageReader(int width, int height){
-        this.WIDTH = width;
+    	Log.w(TAG, String.valueOf(width) + "x" + String.valueOf(height));
-        this.HEIGHT = height;
+    	WIDTH = width;
+    	HEIGHT = height;
         this.imageReader =
                 ImageReader.newInstance(
                         width,
@@ -45,18 +221,20 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
     public void onImageAvailable(ImageReader imageReader) {
         Image image  = imageReader.acquireLatestImage();
         if (image == null) {
-            Log.w("RTABMapActivity", "onImageAvailable: Skipping null image.");
+            Log.w(TAG, "onImageAvailable: Skipping null image.");
             return;
         }
         else{
             if(image.getFormat() == ImageFormat.DEPTH16){
-                this.timestamp = image.getTimestamp();
+                synchronized (this) {
-                depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                	 depth16_raw = image.getPlanes()[0].getBuffer().asReadOnlyBuffer();
+                	 timestamp = image.getTimestamp();
+				}
                 // copy raw undecoded DEPTH16 format depth data to NativeBuffer
                 frameCount++;
             }
             else{
-                Log.w("RTABMapActivity", "onImageAvailable: depth image not in DEPTH16 format, skipping image");
+                Log.w(TAG, "onImageAvailable: depth image not in DEPTH16 format, skipping image");
             }
         }
         image.close();
@@ -78,7 +256,7 @@ public class TOF_ImageReader implements ImageReader.OnImageAvailableListener {
                 this.backgroundThread = null;
                 this.backgroundHandler = null;
             } catch (InterruptedException e) {
-                Log.e("RTABMapActivity", "Interrupted while trying to join depth background handler thread", e);
+                Log.e(TAG, "Interrupted while trying to join depth background handler thread", e);
             }
         }
     }