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

iOS: fixed origin when we do New Scan while we were already mapping, improved FPV feedback in point cloud mode, showing camera overlay in visualization mode (pinch out to disable). Localization mode: Updated how marker detection transform are modified by gravity constraints.

Browse Source
This commit is contained in:
matlabbe
2021-06-12 12:42:56 -04:00
parent 3f633081cb
commit 6342658ec5
11 changed files with 92 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -563,6 +563,11 @@ SensorData CameraARCore::captureImage(CameraInfo * info)
else
{
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
return data;

5
app/android/jni/CameraAREngine.cpp
View File

@@ -300,6 +300,11 @@ SensorData CameraAREngine::captureImage(CameraInfo * info)
{
pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
this->poseReceived(pose);
// adjust origin
if(!getOriginOffset().isNull())
{
pose = getOriginOffset() * pose;
}
info->odomPose = pose;
}
return data;

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

@@ -138,11 +138,18 @@ void CameraMobile::setData(const SensorData & data, const Transform & pose, cons
{
LOGD("CameraMobile::setData pose=%s stamp=%f", pose.prettyPrint().c_str(), data.stamp());
data_ = data;
pose_ = pose;
pose_ = pose;
viewMatrix_ = viewMatrix;
projectionMatrix_ = projectionMatrix;
// adjust origin
if(!originOffset_.isNull())
{
pose_ = originOffset_ * pose_;
viewMatrix_ = glm::inverse(rtabmap::glmFromTransform(rtabmap::opengl_world_T_rtabmap_world * originOffset_ *rtabmap::rtabmap_world_T_opengl_world)*glm::inverse(viewMatrix_));
}
if(textureId_ == 9999)
{
glGenTextures(1, &textureId_);

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

@@ -1259,7 +1259,8 @@ int RTABMapApp::Render()
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
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());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
@@ -1298,7 +1299,8 @@ int RTABMapApp::Render()
if(occlusionModel.isValidForProjection())
{
pcl::IndicesPtr indices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, 1, 0, 0, indices.get());
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());
occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
@@ -1451,6 +1453,23 @@ int RTABMapApp::Render()
{
main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
}
// Update markers
for(std::map<int, rtabmap::Transform>::const_iterator iter=stats.poses().begin();
iter!=stats.poses().end() && iter->first<0;
++iter)
{
int id = iter->first;
if(main_scene_.hasMarker(id))
{
//just update pose
main_scene_.setMarkerPose(id, rtabmap::opengl_world_T_rtabmap_world*iter->second);
}
else
{
main_scene_.addMarker(id, rtabmap::opengl_world_T_rtabmap_world*iter->second);
}
}
}
}
@@ -3892,7 +3911,7 @@ void RTABMapApp::postOdometryEvent(
if(!outputDepth.empty())
{
rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
depthModel.setLocalTransform(pose*model.localTransform());
depthModel.setLocalTransform(mapToOdom_*pose*model.localTransform());
camera_->setOcclusionImage(outputDepth, depthModel);
}

72
app/android/jni/scene.cpp
View File

@@ -98,11 +98,11 @@ Scene::Scene() :
b_(0.0f),
fboId_(0),
rboId_(0),
depthTexture_(0),
screenWidth_(0),
screenHeight_(0),
doubleTapOn_(false)
{
depthTextures_[0] = depthTextures_[1] = 0;
gesture_camera_ = new tango_gl::GestureCamera();
gesture_camera_->SetCameraType(
tango_gl::GestureCamera::kThirdPersonFollow);
@@ -179,8 +179,8 @@ void Scene::DeleteResources() {
fboId_ = 0;
glDeleteRenderbuffers(1, &rboId_);
rboId_ = 0;
glDeleteTextures(1, &depthTexture_);
depthTexture_ = 0;
glDeleteTextures(2, depthTextures_);
depthTextures_[0] = depthTextures_[1] = 0;
}
clear();
@@ -234,8 +234,8 @@ void Scene::SetupViewPort(int w, int h) {
fboId_ = 0;
glDeleteRenderbuffers(1, &rboId_);
rboId_ = 0;
glDeleteTextures(1, &depthTexture_);
depthTexture_ = 0;
glDeleteTextures(2, depthTextures_);
depthTextures_[0] = depthTextures_[1] = 0;
}
GLint originid = 0;
@@ -247,14 +247,22 @@ void Scene::SetupViewPort(int w, int h) {
glBindFramebuffer(GL_FRAMEBUFFER, fboId_);
// Create depth texture
glGenTextures(1, &depthTexture_);
glBindTexture(GL_TEXTURE_2D, depthTexture_);
glGenTextures(2, depthTextures_);
glBindTexture(GL_TEXTURE_2D, depthTextures_[0]);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
glBindTexture(GL_TEXTURE_2D, 0);
glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, w, h, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
glBindTexture(GL_TEXTURE_2D, 0);
glGenRenderbuffers(1, &rboId_);
glBindRenderbuffer(GL_RENDERBUFFER, rboId_);
@@ -262,7 +270,7 @@ void Scene::SetupViewPort(int w, int h) {
glBindRenderbuffer(GL_RENDERBUFFER, 0);
// Set the texture to be at the color attachment point of the FBO (we pack depth 32 bits in color)
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTexture_, 0);
glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, depthTextures_[0], 0);
glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rboId_);
GLuint status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
@@ -480,7 +488,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
UTimer timer;
bool onlineBlending = (renderBackgroundCamera && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_ && cloudsToDraw.size()>1);
bool onlineBlending = (!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size()) || (blending_ && gesture_camera_->GetCameraType()!=tango_gl::GestureCamera::kTopOrtho && mapRendering_ && meshRendering_);
if(onlineBlending && fboId_)
{
GLint originid = 0;
@@ -492,24 +500,26 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
glClearColor(0, 0, 0, 0);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
if(renderBackgroundCamera && !meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
{
PointCloudDrawable drawable(occlusionMesh);
drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 999.0f, 0, 0, 0, 0, 0, true);
}
else
{
// Draw scene
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
Eigen::Vector3f cloudToCamera(
(*iter)->getPose().x() - openglCamera.x(),
(*iter)->getPose().y() - openglCamera.y(),
(*iter)->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
}
}
// Draw scene
for(std::vector<PointCloudDrawable*>::const_iterator iter=cloudsToDraw.begin(); iter!=cloudsToDraw.end(); ++iter)
{
Eigen::Vector3f cloudToCamera(
(*iter)->getPose().x() - openglCamera.x(),
(*iter)->getPose().y() - openglCamera.y(),
(*iter)->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
(*iter)->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, false, false, distanceToCameraSqr, 0, 0, 0, 0, 0, true);
}
glBindTexture(GL_TEXTURE_2D, depthTextures_[1]);
glCopyTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, 0, 0, screenWidth_, screenHeight_);
glBindTexture(GL_TEXTURE_2D, 0);
if(!meshRendering_ && occlusionMesh.cloud.get() && occlusionMesh.cloud->size())
{
PointCloudDrawable drawable(occlusionMesh);
drawable.Render(projectionMatrix, viewMatrix, true, pointSize_, false, false, 0, 0, 0, 0, 0, 0, true);
}
// back to normal window-system-provided framebuffer
glBindFramebuffer(GL_FRAMEBUFFER, originid); // unbind
@@ -549,7 +559,7 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
glClearColor(r_, g_, b_, 1.0f);
glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
if(renderBackgroundCamera && (!onlineBlending || !meshRendering_))
if(renderBackgroundCamera && !onlineBlending)
{
background_renderer_->Draw(uvsTransformed, 0, screenWidth_, screenHeight_, false);
@@ -619,14 +629,14 @@ int Scene::Render(const float * uvsTransformed, glm::mat4 arViewMatrix, glm::mat
cloud->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTexture_:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
cloud->Render(projectionMatrix, viewMatrix, meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr, onlineBlending?depthTextures_[0]:0, screenWidth_, screenHeight_, gesture_camera_->getNearClipPlane(), gesture_camera_->getFarClipPlane(), false, wireFrame_);
}
if(onlineBlending)
{
if(renderBackgroundCamera && meshRendering_)
if(renderBackgroundCamera)
{
background_renderer_->Draw(uvsTransformed, depthTexture_, screenWidth_, screenHeight_, mapping);
background_renderer_->Draw(uvsTransformed, depthTextures_[1], screenWidth_, screenHeight_, meshRendering_?mapping:false);
}
glDisable (GL_BLEND);

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

@@ -208,7 +208,7 @@ class Scene {
float b_;
GLuint fboId_;
GLuint rboId_;
GLuint depthTexture_;
GLuint depthTextures_[2]; // 0=objects+occlusion 1=objects only
GLsizei screenWidth_;
GLsizei screenHeight_;
bool doubleTapOn_;

2
app/android/jni/tango-gl/gesture_camera.cpp
View File

@@ -183,7 +183,7 @@ void GestureCamera::SetCameraType(CameraType camera_index) {
case kFirstPerson:
SetOrthoMode(false);
SetFieldOfView(kLowestFov);
SetNearFarClipPlanes(0.3, 50);
SetNearFarClipPlanes(0.1, 50);
SetPosition(glm::vec3(0.0f, 0.0f, 0.0f));
SetRotation(glm::quat(1.0f, 0.0f, 0.0f, 0.0f));
cam_cur_dist_ = 0.0f;