diff --git a/app/android/jni/CameraARCore.cpp b/app/android/jni/CameraARCore.cpp index 978fffc9..6e0a8cb1 100644 --- a/app/android/jni/CameraARCore.cpp +++ b/app/android/jni/CameraARCore.cpp @@ -393,6 +393,12 @@ SensorData CameraARCore::captureImage(CameraInfo * info) /*near=*/0.1f, /*far=*/100.f, glm::value_ptr(projectionMatrix_)); + // adjust origin + if(!getOriginOffset().isNull()) + { + viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); + } + ArTrackingState camera_tracking_state; ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); @@ -407,6 +413,22 @@ SensorData CameraARCore::captureImage(CameraInfo * info) pose = Transform(pose_raw[4], pose_raw[5], pose_raw[6], pose_raw[0], pose_raw[1], pose_raw[2], pose_raw[3]); pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; + Transform poseArCore = pose; + if(pose.isNull()) + { + LOGE("CameraARCore: Pose is null"); + } + else + { + this->poseReceived(pose); + // adjust origin + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } + info->odomPose = pose; + } + // Get calibration parameters float fx,fy, cx, cy; int32_t width, height; @@ -527,7 +549,7 @@ SensorData CameraARCore::captureImage(CameraInfo * info) #endif if(pointCloudData && points>0) { - scan = scanFromPointCloudData(pointCloudData, points, pose, model, rgb, &kpts, &kpts3); + scan = scanFromPointCloudData(pointCloudData, points, poseArCore, model, rgb, &kpts, &kpts3); } } else @@ -556,20 +578,6 @@ SensorData CameraARCore::captureImage(CameraInfo * info) ArCamera_release(ar_camera); - if(pose.isNull()) - { - LOGE("CameraARCore: Pose is null"); - } - else - { - this->poseReceived(pose); - // adjust origin - if(!getOriginOffset().isNull()) - { - pose = getOriginOffset() * pose; - } - info->odomPose = pose; - } return data; } @@ -622,6 +630,12 @@ void CameraARCore::capturePoseOnly() /*near=*/0.1f, /*far=*/100.f, glm::value_ptr(projectionMatrix_)); + // adjust origin + if(!getOriginOffset().isNull()) + { + viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * getOriginOffset() *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_)); + } + ArTrackingState camera_tracking_state; ArCamera_getTrackingState(arSession_, ar_camera, &camera_tracking_state); @@ -638,6 +652,11 @@ void CameraARCore::capturePoseOnly() { pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world; this->poseReceived(pose); + + if(!getOriginOffset().isNull()) + { + pose = getOriginOffset() * pose; + } } int32_t is_depth_supported = 0; diff --git a/app/android/jni/CameraMobile.cpp b/app/android/jni/CameraMobile.cpp index 32eaf1ca..ec2cac36 100644 --- a/app/android/jni/CameraMobile.cpp +++ b/app/android/jni/CameraMobile.cpp @@ -154,7 +154,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons { glGenTextures(1, &textureId_); } - + if(texCoord) { memcpy(transformed_uvs_, texCoord, 8*sizeof(float)); @@ -162,7 +162,7 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons } LOGD("CameraMobile::setData textureId_=%d", (int)textureId_); - + if(textureId_ != 0 && texCoord != 0) { cv::Mat rgbImage; @@ -200,16 +200,16 @@ void CameraMobile::spinOnce() if(!this->isRunning()) { bool ignoreFrame = false; - float rate = 10.0f; // maximum 10 FPS for image data + //float rate = 10.0f; // maximum 10 FPS for image data double now = UTimer::now(); - if(rate>0.0f) + /*if(rate>0.0f) { if((spinOncePreviousStamp_>=0.0 && now>spinOncePreviousStamp_ && now - spinOncePreviousStamp_ < 1.0f/rate) || ((spinOncePreviousStamp_<=0.0 || now<=spinOncePreviousStamp_) && spinOnceFrameRateTimer_.getElapsedTime() < 1.0f/rate)) { ignoreFrame = true; } - } + }*/ if(!ignoreFrame) { diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 7ef4fc2f..1eadd507 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -504,7 +504,7 @@ int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMe else { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -1241,7 +1241,9 @@ int RTABMapApp::Render() #ifdef RTABMAP_ARCORE if(cameraDriver_ == 1) { - if(main_scene_.background_renderer_ == 0) + if(main_scene_.background_renderer_ == 0 && + ((rtabmap::CameraARCore*)camera_)->getTextureId() != 9999 && + ((rtabmap::CameraARCore*)camera_)->getTextureId() != 0) { main_scene_.background_renderer_ = new BackgroundRenderer(); main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraARCore*)camera_)->getTextureId(), true); @@ -1250,6 +1252,11 @@ int RTABMapApp::Render() { uvsTransformed = ((rtabmap::CameraARCore*)camera_)->uvsTransformed(); ((rtabmap::CameraARCore*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix); + if(graphOptimization_ && !mapToOdom_.isIdentity()) + { + rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world; + arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix)); + } } if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson) { @@ -1261,7 +1268,7 @@ int RTABMapApp::Render() pcl::IndicesPtr indices(new std::vector); int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); - cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform()); + cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); occlusionMesh.cloud.reset(new pcl::PointCloud()); pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); occlusionMesh.indices = indices; @@ -1276,7 +1283,9 @@ int RTABMapApp::Render() #endif if(cameraDriver_ == 3) { - if(main_scene_.background_renderer_ == 0) + if(main_scene_.background_renderer_ == 0 && + ((rtabmap::CameraMobile*)camera_)->getTextureId() != 9999 && + ((rtabmap::CameraMobile*)camera_)->getTextureId() != 0) { main_scene_.background_renderer_ = new BackgroundRenderer(); main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), false); @@ -1301,7 +1310,7 @@ int RTABMapApp::Render() pcl::IndicesPtr indices(new std::vector); int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows); pcl::PointCloud::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get()); - cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*occlusionModel.localTransform()); + cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform()); occlusionMesh.cloud.reset(new pcl::PointCloud()); pcl::copyPointCloud(*cloud, *occlusionMesh.cloud); occlusionMesh.indices = indices; @@ -1794,7 +1803,7 @@ int RTABMapApp::Render() else { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -2009,7 +2018,7 @@ int RTABMapApp::Render() else { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(odomEvent.data().laserScanRaw(), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(odomEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), odomEvent.data().laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -3241,7 +3250,7 @@ bool RTABMapApp::exportMesh( else if(!data.laserScanRaw().empty()) { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -3271,7 +3280,7 @@ bool RTABMapApp::exportMesh( else if(!data.laserScanRaw().empty()) { //scan - cloud = rtabmap::util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanRaw().localTransform(), 255, 255, 255); + cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255); indices->resize(cloud->size()); for(unsigned int i=0; isize(); ++i) { @@ -3910,8 +3919,13 @@ void RTABMapApp::postOdometryEvent( if(!outputDepth.empty()) { + rtabmap::Transform poseWithOriginOffset = pose; + if(!camera_->getOriginOffset().isNull()) + { + poseWithOriginOffset = camera_->getOriginOffset() * pose; + } rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth())); - depthModel.setLocalTransform(mapToOdom_*pose*model.localTransform()); + depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform()); camera_->setOcclusionImage(outputDepth, depthModel); } diff --git a/app/android/jni/background_renderer.cc b/app/android/jni/background_renderer.cc index fb19a9f7..9908c002 100644 --- a/app/android/jni/background_renderer.cc +++ b/app/android/jni/background_renderer.cc @@ -51,6 +51,8 @@ const std::string kFragmentShaderBlendingOES = "precision mediump float;\n" "varying vec2 v_TexCoord;\n" "uniform samplerExternalOES sTexture;\n" + "uniform sampler2D uDepthTexture;\n" + "uniform vec2 uScreenScale;\n" "uniform bool uRedUnknown;\n" "void main() {\n" " vec4 sample = texture2D(sTexture, v_TexCoord);\n" @@ -120,8 +122,19 @@ const std::string kFragmentShaderBlending = std::vector BackgroundRenderer::shaderPrograms_; +BackgroundRenderer::~BackgroundRenderer() +{ + for(unsigned int i=0; ipref_key_rendering_texture_decimation 4 pref_key_blending - false + true pref_key_background_color 0.2 pref_key_nodes_filtering diff --git a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java index b6206140..7563d2c1 100644 --- a/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java +++ b/app/android/src/com/introlab/rtabmap/ARCoreSharedCamera.java @@ -307,8 +307,6 @@ public class ARCoreSharedCamera { } mTOFImageReader.stopBackgroundThread(); } - - private long mPreviousTime = 0; /** * Return true if the given array contains the given integer. @@ -335,8 +333,6 @@ public class ARCoreSharedCamera { close(); startBackgroundThread(); - - mPreviousTime = System.currentTimeMillis(); if(cameraTextureId == -1) { @@ -641,13 +637,6 @@ public class ARCoreSharedCamera { 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 (to save battery) - if(System. currentTimeMillis() - mPreviousTime < rateMs) - { - return; - } - mPreviousTime = System. currentTimeMillis(); - CameraIntrinsics intrinsics = camera.getImageIntrinsics(); try{ Image image = frame.acquireCameraImage(); @@ -683,7 +672,7 @@ public class ARCoreSharedCamera { frame.transformCoordinates2d( Coordinates2d.OPENGL_NORMALIZED_DEVICE_COORDINATES, QUAD_COORDS, - Coordinates2d.TEXTURE_NORMALIZED, + Coordinates2d.IMAGE_NORMALIZED, texCoord); float[] p = new float[16]; diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 3cc1b7c0..f4cc3a67 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -1075,6 +1075,7 @@ public class RTABMapActivity extends FragmentActivity implements OnClickListener setAndroidOrientation(); + updateCameraDriverSettings(); updatePreferences(); if(mState == State.STATE_MAPPING || mState == State.STATE_CAMERA)