i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-04 00:37:46 +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
app/android/jni/point_cloud_drawable.cpp
+120 -6
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
{