i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-03 01:50:24 +08:00
Code Issues Packages Projects Releases Wiki Activity

Fixed occlusion image origin offset (iOS and android). Android: disabled maximum odom image rate for faster feedback in First-Person. Min/Max range parameters also filter scans. Fixed background renderer not showing image (ARCore Java driver).

Browse Source
This commit is contained in:
matlabbe
2021-06-13 22:07:47 -04:00
parent 6342658ec5
commit 696216b35b
8 changed files with 89 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files

49
app/android/jni/CameraARCore.cpp
View File

@@ -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;

10
app/android/jni/CameraMobile.cpp
View File

@@ -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)
{

34
app/android/jni/RTABMapApp.cpp
View File

@@ -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; i<cloud->size(); ++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>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::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::PointXYZRGB>());
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>);
int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
pcl::PointCloud<pcl::PointXYZ>::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::PointXYZRGB>());
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; i<cloud->size(); ++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; i<cloud->size(); ++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; i<cloud->size(); ++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; i<cloud->size(); ++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);
}

31
app/android/jni/background_renderer.cc
View File

@@ -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<GLuint> BackgroundRenderer::shaderPrograms_;
BackgroundRenderer::~BackgroundRenderer()
{
for(unsigned int i=0; i<shaderPrograms_.size(); ++i)
{
glDeleteShader(shaderPrograms_[i]);
}
shaderPrograms_.clear();
}
void BackgroundRenderer::InitializeGlContent(GLuint textureId, bool oes)
{
LOGI("textureId=%d", textureId);
texture_id_ = textureId;
#ifdef __ANDROID__
oes_ = oes;
@@ -175,19 +188,19 @@ void BackgroundRenderer::Draw(const float * transformed_uvs, const GLuint & dept
GLuint screenScale_handle = glGetUniformLocation(program, "uRedUnknown");
glUniform1i(screenScale_handle, redUnknown);
GLuint attribute_vertices_ = glGetAttribLocation(program, "a_Position");
GLuint attribute_uvs_ = glGetAttribLocation(program, "a_TexCoord");
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);
GLuint attributeVertices = glGetAttribLocation(program, "a_Position");
GLuint attributeUvs = glGetAttribLocation(program, "a_TexCoord");
glEnableVertexAttribArray(attribute_vertices_);
glEnableVertexAttribArray(attribute_uvs_);
glVertexAttribPointer(attributeVertices, 2, GL_FLOAT, GL_FALSE, 0, BackgroundRenderer_kVertices);
glVertexAttribPointer(attributeUvs, 2, GL_FLOAT, GL_FALSE, 0, transformed_uvs?transformed_uvs:BackgroundRenderer_kTexCoord);
glEnableVertexAttribArray(attributeVertices);
glEnableVertexAttribArray(attributeUvs);
glDrawArrays(GL_TRIANGLE_STRIP, 0, 4);
glDisableVertexAttribArray(attribute_vertices_);
glDisableVertexAttribArray(attribute_uvs_);
glDisableVertexAttribArray(attributeVertices);
glDisableVertexAttribArray(attributeUvs);
glUseProgram(0);
glDepthMask(GL_TRUE);

2
app/android/jni/background_renderer.h
View File

@@ -51,7 +51,7 @@ public:
public:
BackgroundRenderer() = default;
~BackgroundRenderer() = default;
~BackgroundRenderer();
// Sets up OpenGL state. Must be called on the OpenGL thread and before any
// other methods below.