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

Tango: Added Level of detail rendering depending on the distance from the camera

Browse Source
This commit is contained in:
matlabbe
2017-03-05 21:48:18 -05:00
parent fed9d4ef3b
commit 9f65ef199b
10 changed files with 192 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -56,6 +56,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/surface/poisson.h>
#include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
#define LOW_RES_PIX 1
const int g_exportedMeshId = -100;
@@ -169,6 +170,7 @@ RTABMapApp::RTABMapApp() :
totalPolygons_(0),
lastDrawnCloudsCount_(0),
renderingTime_(0.0f),
previousRenderingTime_(0.0f),
processMemoryUsedBytes(0),
processGPUMemoryUsedBytes(0),
visualizingMesh_(false),
@@ -209,6 +211,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingTime_ = 0.0f;
previousRenderingTime_ = 0.0f;
processMemoryUsedBytes = 0;
processGPUMemoryUsedBytes = 0;
progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
@@ -622,6 +625,7 @@ int RTABMapApp::Render()
totalPolygons_ = 0;
lastDrawnCloudsCount_ = 0;
renderingTime_ = 0.0f;
previousRenderingTime_ = 0.0f;
processMemoryUsedBytes = 0;
processGPUMemoryUsedBytes = 0;
}
@@ -638,15 +642,17 @@ int RTABMapApp::Render()
}
if(added.size() != meshes)
{
LOGD("added (%d) != meshes (%d)", (int)added.size(), meshes);
processGPUMemoryUsedBytes = 0;
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(!main_scene_.hasCloud(iter->first))
if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
{
LOGI("Re-add mesh %d to OpenGL context", iter->first);
if(main_scene_.isMeshRendering() && iter->second.polygons.size() == 0)
{
iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
iter->second.polygonsLowRes = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
}
if(main_scene_.isMeshTexturing())
@@ -667,6 +673,7 @@ int RTABMapApp::Render()
estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb
estimateGPUMem += iter->second.indices->size()*4; // int
estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
estimateGPUMem += iter->second.polygonsLowRes.size()*4*3; // 3 indices per polygon
processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total());
@@ -821,10 +828,13 @@ int RTABMapApp::Render()
{
UTimer time;
std::vector<pcl::Vertices> polygons;
std::vector<pcl::Vertices> polygonsLowRes;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
@@ -834,6 +844,7 @@ int RTABMapApp::Render()
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
@@ -911,10 +922,10 @@ int RTABMapApp::Render()
}
else
{
main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_);
main_scene_.setCloudVisible(-1, !(renderingTime_ > 0.05 || previousRenderingTime_>0.05) && odomCloudShown_ && !trajectoryMode_ && !paused_);
//just process the last one
if(!odomEvent.pose().isNull())
if(!odomEvent.pose().isNull() && !(renderingTime_ > 0.05 || previousRenderingTime_>0.05))
{
if(odomCloudShown_ && !trajectoryMode_)
{
@@ -1076,6 +1087,11 @@ int RTABMapApp::Render()
return notifyDataLoaded||notifyCameraStarted?1:0;
}
}
catch(const UException & e)
{
UERROR("Exception! msg=\"%s\"", e.what());
return -2;
}
catch(const std::exception & e)
{
UERROR("Exception! msg=\"%s\"", e.what());
@@ -1382,7 +1398,7 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string &
resetMapping();
}
uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value));
UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
UEventsManager::post(new rtabmap::ParamEvent(this->getRtabmapParameters()));
return 0;
}
else
@@ -2647,7 +2663,7 @@ void RTABMapApp::handleEvent(UEvent * event)
if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
event->getClassName().compare("RtabmapEvent") == 0)
{
LOGI("Received RtabmapEvent initialized event!");
LOGI("Received RtabmapEvent event!");
if(camera_->isRunning())
{
rtabmap::RtabmapEvent * rtabmapEvent = (rtabmap::RtabmapEvent*)event;
@@ -2660,7 +2676,8 @@ void RTABMapApp::handleEvent(UEvent * event)
smallMovement == 0 &&
rehearsalMerged == 0 &&
!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().imageRaw().empty() &&
!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty())
!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty() &&
rtabmapEvent->getStats().poses().find(rtabmapEvent->getStats().getSignatures().rbegin()->first) != rtabmapEvent->getStats().poses().end())
{
int id = rtabmapEvent->getStats().getSignatures().rbegin()->first;
const rtabmap::SensorData & data = rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData();
@@ -2675,10 +2692,13 @@ void RTABMapApp::handleEvent(UEvent * event)
{
UTimer time;
std::vector<pcl::Vertices> polygons;
std::vector<pcl::Vertices> polygonsLowRes;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("(EVENT) Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
LOGI("(EVENT) Creating mesh low res, %d polygons (%fs)", (int)polygonsLowRes.size(), time.ticks());
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
@@ -2697,6 +2717,7 @@ void RTABMapApp::handleEvent(UEvent * event)
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.polygonsLowRes = polygonsLowRes;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
@@ -2853,6 +2874,7 @@ void RTABMapApp::handleEvent(UEvent * event)
{
UERROR("Failed to call RTABMapActivity::updateStatsCallback");
}
previousRenderingTime_ = renderingTime_;
renderingTime_ = 0.0f;
}
}

1
app/android/jni/RTABMapApp.h
View File

@@ -208,6 +208,7 @@ class RTABMapApp : public UEventsHandler {
int totalPolygons_;
int lastDrawnCloudsCount_;
float renderingTime_;
float previousRenderingTime_;
long processMemoryUsedBytes;
long processGPUMemoryUsedBytes;

126
app/android/jni/point_cloud_drawable.cpp
View File

@@ -35,6 +35,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <GLES2/gl2.h>
#define LOW_DEC 2
#define LOWLOW_DEC 4
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
@@ -89,7 +92,7 @@ PointCloudDrawable::~PointCloudDrawable()
}
}
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes)
{
LOGD("Update polygons");
polygons_.clear();
@@ -107,6 +110,22 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
}
}
if(polygonsLowRes.size())
{
unsigned int polygonSize = polygonsLowRes[0].vertices.size();
UASSERT(polygonSize == 3);
polygonsLowRes_.resize(polygonsLowRes.size() * polygonSize);
int oi = 0;
for(unsigned int i=0; i<polygonsLowRes.size(); ++i)
{
UASSERT(polygonsLowRes[i].vertices.size() == polygonSize);
for(unsigned int j=0; j<polygonSize; ++j)
{
polygonsLowRes_[oi++] = organizedToDenseIndices_.at(polygonsLowRes[i].vertices[j]);
}
}
}
}
}
@@ -115,7 +134,10 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
UASSERT(cloud.get() && !cloud->empty());
nPoints_ = 0;
polygons_.clear();
polygonsLowRes_.clear();
gain_ = gain;
verticesLowRes_.clear();
verticesLowLowRes_.clear();
if (vertex_buffers_)
{
@@ -145,25 +167,61 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
{
totalPoints = indices->size();
vertices.resize(indices->size()*4);
verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
int oi_low = 0;
int oi_lowlow = 0;
for(unsigned int i=0; i<indices->size(); ++i)
{
vertices[i*4] = cloud->at(indices->at(i)).x;
vertices[i*4+1] = cloud->at(indices->at(i)).y;
vertices[i*4+2] = cloud->at(indices->at(i)).z;
vertices[i*4+3] = cloud->at(indices->at(i)).rgb;
if(cloud->isOrganized())
{
if(indices->at(i)%LOW_DEC == 0 && (indices->at(i)/cloud->width) % LOW_DEC == 0)
{
verticesLowRes_[oi_low++] = i;
}
if(indices->at(i)%LOWLOW_DEC == 0 && (indices->at(i)/cloud->width) % LOWLOW_DEC == 0)
{
verticesLowLowRes_[oi_lowlow++] = i;
}
}
}
verticesLowRes_.resize(oi_low);
verticesLowLowRes_.resize(oi_lowlow);
}
else
{
totalPoints = cloud->size();
vertices.resize(cloud->size()*4);
verticesLowRes_.resize(cloud->isOrganized()?totalPoints:0);
verticesLowLowRes_.resize(cloud->isOrganized()?totalPoints:0);
int oi_low = 0;
int oi_lowlow = 0;
for(unsigned int i=0; i<cloud->size(); ++i)
{
vertices[i*4] = cloud->at(i).x;
vertices[i*4+1] = cloud->at(i).y;
vertices[i*4+2] = cloud->at(i).z;
vertices[i*4+3] = cloud->at(i).rgb;
if(cloud->isOrganized())
{
if(i%LOW_DEC == 0 && (i/cloud->width) % LOW_DEC == 0)
{
verticesLowRes_[oi_low++] = i;
}
if(i%LOWLOW_DEC == 0 && (i/cloud->width) % LOWLOW_DEC == 0)
{
verticesLowLowRes_[oi_lowlow++] = i;
}
}
}
verticesLowRes_.resize(oi_low);
verticesLowLowRes_.resize(oi_lowlow);
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
@@ -229,12 +287,18 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
std::vector<float> vertices;
int totalPoints = 0;
std::vector<pcl::Vertices> polygons = mesh.polygons;
std::vector<pcl::Vertices> polygonsLowRes;
hasNormals_ = mesh.normals.get() && mesh.normals->size() == mesh.cloud->size();
UASSERT(!hasNormals_ || mesh.cloud->size() == mesh.normals->size());
if(mesh.cloud->isOrganized()) // assume organized mesh
{
polygonsLowRes = mesh.polygonsLowRes; // only in organized we keep the low res
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
totalPoints = mesh.indices->size();
verticesLowRes_.resize(totalPoints);
verticesLowLowRes_.resize(totalPoints);
int oi_low = 0;
int oi_lowlow = 0;
if(textures_ && polygons.size())
{
//LOGD("Organized mesh with texture");
@@ -263,6 +327,15 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
}
organizedToDenseIndices_[mesh.indices->at(i)] = i;
if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
{
verticesLowRes_[oi_low++] = i;
}
if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
{
verticesLowLowRes_[oi_lowlow++] = i;
}
}
}
else
@@ -286,8 +359,19 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
}
organizedToDenseIndices_[mesh.indices->at(i)] = i;
if(mesh.indices->at(i)%LOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOW_DEC == 0)
{
verticesLowRes_[oi_low++] = i;
}
if(mesh.indices->at(i)%LOWLOW_DEC == 0 && (mesh.indices->at(i)/mesh.cloud->width) % LOWLOW_DEC == 0)
{
verticesLowLowRes_[oi_lowlow++] = i;
}
}
}
verticesLowRes_.resize(oi_low);
verticesLowLowRes_.resize(oi_lowlow);
}
else // assume dense mesh with texCoords set to polygons
{
@@ -427,7 +511,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
if(polygons_.size() != polygons.size())
{
updatePolygons(polygons);
updatePolygons(polygons, polygonsLowRes);
}
}
@@ -443,11 +527,12 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
bool meshRendering,
float pointSize,
bool textureRendering,
bool lighting) {
bool lighting,
float distanceToCameraSqr) {
if(vertex_buffers_ && nPoints_ && visible_)
{
if(meshRendering && textureRendering && textures_)
if(meshRendering && textureRendering && textures_ && (verticesLowRes_.empty() || distanceToCameraSqr<50.0f))
{
glUseProgram(texture_shader_program_);
@@ -512,7 +597,14 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
}
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
else
{
glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
}
}
else // point cloud or colored mesh
{
@@ -588,7 +680,29 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
}
if(meshRendering && polygons_.size())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
if(distanceToCameraSqr<150.0f || polygonsLowRes_.empty())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
else
{
glDrawElements(GL_TRIANGLES, polygonsLowRes_.size(), GL_UNSIGNED_INT, polygonsLowRes_.data());
}
}
else if(!verticesLowRes_.empty())
{
if(distanceToCameraSqr>600.0f)
{
glDrawElements(GL_POINTS, verticesLowLowRes_.size(), GL_UNSIGNED_INT, verticesLowLowRes_.data());
}
else if(distanceToCameraSqr>150.0f)
{
glDrawElements(GL_POINTS, verticesLowRes_.size(), GL_UNSIGNED_INT, verticesLowRes_.data());
}
else
{
glDrawArrays(GL_POINTS, 0, nPoints_);
}
}
else
{

8
app/android/jni/point_cloud_drawable.h
View File

@@ -53,7 +53,7 @@ class PointCloudDrawable {
const Mesh & mesh);
virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons);
void updatePolygons(const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygonsLowRes = std::vector<pcl::Vertices>());
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
void updateMesh(const Mesh & mesh);
void setPose(const rtabmap::Transform & pose);
@@ -75,13 +75,17 @@ class PointCloudDrawable {
bool meshRendering = true,
float pointSize = 3.0f,
bool textureRendering = false,
bool lighting = true);
bool lighting = true,
float distanceToCamSqr = 0.0f);
private:
// Vertex buffer of the point cloud geometry.
GLuint vertex_buffers_;
GLuint textures_;
std::vector<GLuint> polygons_;
std::vector<GLuint> polygonsLowRes_;
std::vector<GLuint> verticesLowRes_;
std::vector<GLuint> verticesLowLowRes_;
int nPoints_;
glm::mat4 pose_;
bool visible_;

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

@@ -352,6 +352,14 @@ int Scene::Render() {
}
}
float fov = 45.0f;
rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
// transform in same coordinate as frustum filtering
openglCamera *= rtabmap::Transform(
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
int cloudDrawn=0;
if(mapRendering_ && frustumCulling_)
{
@@ -375,13 +383,6 @@ int Scene::Render() {
if(oi)
{
float fov = 45.0f;
rtabmap::Transform openglCamera = GetOpenGLCameraPose(&fov)*rtabmap::Transform(0.0f, 0.0f, 3.0f, 0.0f, 0.0f, 0.0f);
// transform in same coordinate as frustum filtering
openglCamera *= rtabmap::Transform(
0.0f, 0.0f, 1.0f, 0.0f,
0.0f, 1.0f, 0.0f, 0.0f,
-1.0f, 0.0f, 0.0f, 0.0f);
pcl::IndicesPtr indices = rtabmap::util3d::frustumFiltering(
cloud,
pcl::IndicesPtr(new std::vector<int>),
@@ -399,7 +400,13 @@ int Scene::Render() {
for(unsigned int i=0; i<indices->size(); ++i)
{
++cloudDrawn;
pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
std::map<int, PointCloudDrawable*>::const_iterator iter = pointClouds_.find(ids[indices->at(i)]);
Eigen::Vector3f cloudToCamera(
iter->second->getPose().x() - openglCamera.x(),
iter->second->getPose().y() - openglCamera.y(),
iter->second->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
}
}
}
@@ -410,7 +417,12 @@ int Scene::Render() {
if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
{
++cloudDrawn;
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
Eigen::Vector3f cloudToCamera(
iter->second->getPose().x() - openglCamera.x(),
iter->second->getPose().y() - openglCamera.y(),
iter->second->getPose().z() - openglCamera.z());
float distanceToCameraSqr = cloudToCamera[0]*cloudToCamera[0] + cloudToCamera[1]*cloudToCamera[1] + cloudToCamera[2]*cloudToCamera[2];
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_, distanceToCameraSqr);
}
}
}

1
app/android/jni/util.h
View File

@@ -158,6 +158,7 @@ public:
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::IndicesPtr indices;
std::vector<pcl::Vertices> polygons;
std::vector<pcl::Vertices> polygonsLowRes;
rtabmap::Transform pose; // in rtabmap coordinates
bool visible;
rtabmap::CameraModel cameraModel;