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 Settings/Optimized Mesh/Sketchfab Upload

Browse Source
This commit is contained in:
matlabbe
2017-01-26 16:46:24 -05:00
parent 66ee792e8c
commit 3a73414972
27 changed files with 4441 additions and 2029 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because it is too large Load Diff

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

@@ -42,6 +42,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEventsHandler.h>
#include <boost/thread/mutex.hpp>
#include <pcl/pcl_base.h>
#include <pcl/TextureMesh.h>
// RTABMapApp handles the application lifecycle and resources.
class RTABMapApp : public UEventsHandler {
@@ -113,6 +114,8 @@ class RTABMapApp : public UEventsHandler {
void setMapCloudShown(bool shown);
void setOdomCloudShown(bool shown);
void setMeshRendering(bool enabled, bool withTexture);
void setPointSize(float value);
void setLighting(bool enabled);
void setLocalizationMode(bool enabled);
void setTrajectoryMode(bool enabled);
void setGraphOptimization(bool enabled);
@@ -132,7 +135,22 @@ class RTABMapApp : public UEventsHandler {
void resetMapping();
void save(const std::string & databasePath);
bool exportMesh(const std::string & filePath);
cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const;
bool exportMesh(
const std::string & filePath,
float cloudVoxelSize,
bool meshing,
int textureSize,
int normalK,
bool optimized,
float optimizedVoxelSize,
int optimizedDepth,
float optimizedDecimationFactor,
float optimizedColorRadius,
bool optimizedCleanWhitePolygons,
bool optimizedColorWhitePolygons,
bool blockRendering);
bool postExportation(bool visualize);
int postProcessing(int approach);
protected:
@@ -176,6 +194,11 @@ class RTABMapApp : public UEventsHandler {
int lastDrawnCloudsCount_;
float renderingTime_;
bool visualizingMesh_;
bool exportedMeshUpdated_;
pcl::TextureMesh::Ptr exportedMesh_;
cv::Mat exportedTexture_;
// main_scene_ includes all drawable object for visualizing Tango device's
// movement and point cloud.
Scene main_scene_;

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

@@ -145,6 +145,18 @@ Java_com_introlab_rtabmap_RTABMapLib_setMeshRendering(
return app.setMeshRendering(enabled, withTexture);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setPointSize(
JNIEnv*, jobject, float value)
{
return app.setPointSize(value);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setLighting(
JNIEnv*, jobject, bool enabled)
{
return app.setLighting(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setLocalizationMode(
JNIEnv*, jobject, bool enabled)
{
@@ -262,11 +274,44 @@ Java_com_introlab_rtabmap_RTABMapLib_save(
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
JNIEnv* env, jobject, jstring filePath)
JNIEnv* env, jobject,
jstring filePath,
float cloudVoxelSize,
bool meshing,
int textureSize,
int normalK,
bool optimized,
float optimizedVoxelSize,
int optimizedDepth,
float optimizedDecimationFactor,
float optimizedColorRadius,
bool optimizedCleanWhitePolygons,
bool optimizedColorWhitePolygons,
bool blockRendering)
{
std::string filePathC;
GetJStringContent(env,filePath,filePathC);
return app.exportMesh(filePathC);
return app.exportMesh(
filePathC,
cloudVoxelSize,
meshing,
textureSize,
normalK,
optimized,
optimizedVoxelSize,
optimizedDepth,
optimizedDecimationFactor,
optimizedColorRadius,
optimizedCleanWhitePolygons,
optimizedColorWhitePolygons,
blockRendering);
}
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_postExportation(
JNIEnv* env, jobject, bool visualize)
{
return app.postExportation(visualize);
}
JNIEXPORT int JNICALL

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

@@ -46,6 +46,7 @@ PointCloudDrawable::PointCloudDrawable(
nPoints_(0),
pose_(1.0f),
visible_(true),
hasNormals_(false),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram),
gain_(1.0f)
@@ -63,6 +64,7 @@ PointCloudDrawable::PointCloudDrawable(
nPoints_(0),
pose_(1.0f),
visible_(true),
hasNormals_(false),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram),
gain_(1.0f)
@@ -90,19 +92,20 @@ PointCloudDrawable::~PointCloudDrawable()
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
{
LOGD("Update polygons");
polygons_.clear();
if(polygons.size() && organizedToDenseIndices_.size())
{
int polygonSize = polygons[0].vertices.size();
unsigned int polygonSize = polygons[0].vertices.size();
UASSERT(polygonSize == 3);
polygons_.resize(polygons.size() * polygonSize);
int oi = 0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
UASSERT((int)polygons[i].vertices.size() == polygonSize);
for(int j=0; j<polygonSize; ++j)
UASSERT(polygons[i].vertices.size() == polygonSize);
for(unsigned int j=0; j<polygonSize; ++j)
{
polygons_[oi++] = organizedToDenseIndices_.at((unsigned short)polygons[i].vertices[j]);
polygons_[oi++] = organizedToDenseIndices_.at(polygons[i].vertices[j]);
}
}
}
@@ -110,7 +113,7 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain)
{
UASSERT(cloud.get() && !cloud->empty() && indices.get() && !indices->empty());
UASSERT(cloud.get() && !cloud->empty());
nPoints_ = 0;
polygons_.clear();
gain_ = gain;
@@ -137,13 +140,31 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
}
LOGI("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices(indices->size()*4);
for(unsigned int i=0; i<indices->size(); ++i)
std::vector<float> vertices;
int totalPoints = 0;
if(indices.get() && indices->size())
{
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;
totalPoints = indices->size();
vertices.resize(indices->size()*4);
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;
}
}
else
{
totalPoints = cloud->size();
vertices.resize(cloud->size()*4);
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;
}
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
@@ -158,12 +179,12 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
return;
}
nPoints_ = indices->size();
nPoints_ = totalPoints;
}
void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
{
UASSERT(mesh.cloud.get() && !mesh.cloud->empty() && mesh.indices.get() && !mesh.indices->empty());
UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
nPoints_ = 0;
if (vertex_buffers_)
@@ -196,7 +217,6 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
if(textureUpdate)
{
UASSERT(!mesh.cloud->is_dense);
glGenTextures(1, &textures_);
if(!textures_)
{
@@ -206,39 +226,155 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
}
}
LOGI("Creating cloud buffer %d", vertex_buffers_);
LOGD("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices;
organizedToDenseIndices_ = std::vector<int>(mesh.cloud->width*mesh.cloud->height, -1);
if(textures_)
int totalPoints = 0;
std::vector<pcl::Vertices> polygons = mesh.polygons;
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
{
vertices = std::vector<float>(mesh.indices->size()*6);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->width*mesh.cloud->height, -1);
totalPoints = mesh.indices->size();
if(textures_ && polygons.size())
{
vertices[i*6] = mesh.cloud->at(mesh.indices->at(i)).x;
vertices[i*6+1] = mesh.cloud->at(mesh.indices->at(i)).y;
vertices[i*6+2] = mesh.cloud->at(mesh.indices->at(i)).z;
LOGD("Organized mesh with texture");
int items = hasNormals_?9:6;
vertices = std::vector<float>(mesh.indices->size()*9);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
// rgb
vertices[i*6+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
// rgb
vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
// texture uv
int index = mesh.indices->at(i);
vertices[i*6+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
vertices[i*6+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height); //v
// texture uv
int index = mesh.indices->at(i);
vertices[i*items+4] = float(index % mesh.cloud->width)/float(mesh.cloud->width); //u
vertices[i*items+5] = float(index / mesh.cloud->width)/float(mesh.cloud->height); //v
organizedToDenseIndices_[mesh.indices->at(i)] = i;
if(hasNormals_)
{
// normal
vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_x;
vertices[i*items+7] = mesh.normals->at(mesh.indices->at(i)).normal_y;
vertices[i*items+8] = mesh.normals->at(mesh.indices->at(i)).normal_z;
}
organizedToDenseIndices_[mesh.indices->at(i)] = i;
}
}
else
{
LOGD("Organized mesh");
int items = hasNormals_?7:4;
vertices = std::vector<float>(mesh.indices->size()*items);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
vertices[i*items] = mesh.cloud->at(mesh.indices->at(i)).x;
vertices[i*items+1] = mesh.cloud->at(mesh.indices->at(i)).y;
vertices[i*items+2] = mesh.cloud->at(mesh.indices->at(i)).z;
vertices[i*items+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
if(hasNormals_)
{
// normal
vertices[i*items+4] = mesh.normals->at(mesh.indices->at(i)).normal_x;
vertices[i*items+5] = mesh.normals->at(mesh.indices->at(i)).normal_y;
vertices[i*items+6] = mesh.normals->at(mesh.indices->at(i)).normal_z;
}
organizedToDenseIndices_[mesh.indices->at(i)] = i;
}
}
}
else
else // assume dense mesh with texCoords set to polygons
{
vertices = std::vector<float>(mesh.indices->size()*4);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
totalPoints = mesh.cloud->size();
if(textures_ && polygons.size() && mesh.normals->size())
{
vertices[i*4] = mesh.cloud->at(mesh.indices->at(i)).x;
vertices[i*4+1] = mesh.cloud->at(mesh.indices->at(i)).y;
vertices[i*4+2] = mesh.cloud->at(mesh.indices->at(i)).z;
vertices[i*4+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
organizedToDenseIndices_[mesh.indices->at(i)] = i;
LOGD("Dense mesh with texture (%d texCoords %d points %d polygons %dx%d)",
(int)mesh.texCoords.size(), (int)mesh.cloud->size(), (int)mesh.polygons.size(), texture.cols, texture.rows);
// Texturing issue:
// tex_coordinates should be linked to points, not
// polygon vertices. Points linked to multiple different texCoords (different textures) should
// be duplicated.
vertices = std::vector<float>(mesh.texCoords.size()*9);
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.texCoords.size(), -1);
UASSERT_MSG(mesh.texCoords.size() == polygons[0].vertices.size()*polygons.size(),
uFormat("%d vs %d x %d", (int)mesh.texCoords.size(), (int)polygons[0].vertices.size(), (int)polygons.size()).c_str());
int items = hasNormals_?9:6;
unsigned int oi=0;
for(unsigned int i=0; i<polygons.size(); ++i)
{
pcl::Vertices & v = polygons[i];
for(unsigned int j=0; j<v.vertices.size(); ++j)
{
UASSERT(oi < mesh.texCoords.size());
UASSERT(v.vertices[j] < mesh.cloud->size());
vertices[oi*items] = mesh.cloud->at(v.vertices[j]).x;
vertices[oi*items+1] = mesh.cloud->at(v.vertices[j]).y;
vertices[oi*items+2] = mesh.cloud->at(v.vertices[j]).z;
// rgb
vertices[oi*items+3] = mesh.cloud->at(v.vertices[j]).rgb;
// texture uv
if(mesh.texCoords[oi][0]>=0.0f)
{
vertices[oi*items+4] = mesh.texCoords[oi][0]; //u
vertices[oi*items+5] = 1.0f-mesh.texCoords[oi][1]; //v
}
else
{
vertices[oi*items+4] = vertices[oi*items+5] = -1.0f;
}
if(hasNormals_)
{
// normal
vertices[oi*items+6] = mesh.normals->at(v.vertices[j]).normal_x;
vertices[oi*items+7] = mesh.normals->at(v.vertices[j]).normal_y;
vertices[oi*items+8] = mesh.normals->at(v.vertices[j]).normal_z;
}
v.vertices[j] = (int)oi; // new vertex index
UASSERT(oi < organizedToDenseIndices_.size());
organizedToDenseIndices_[oi] = oi;
++oi;
}
}
}
else
{
LOGD("Dense mesh");
int items = hasNormals_?7:4;
organizedToDenseIndices_ = std::vector<unsigned int>(mesh.cloud->size(), -1);
vertices = std::vector<float>(mesh.cloud->size()*items);
for(unsigned int i=0; i<mesh.cloud->size(); ++i)
{
vertices[i*items] = mesh.cloud->at(i).x;
vertices[i*items+1] = mesh.cloud->at(i).y;
vertices[i*items+2] = mesh.cloud->at(i).z;
vertices[i*items+3] = mesh.cloud->at(i).rgb;
if(hasNormals_)
{
vertices[i*items+4] = mesh.normals->at(i).normal_x;
vertices[i*items+5] = mesh.normals->at(i).normal_y;
vertices[i*items+6] = mesh.normals->at(i).normal_z;
}
organizedToDenseIndices_[i] = i;
}
}
}
@@ -256,6 +392,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
if(textures_ && textureUpdate)
{
GLint maxTextureSize = 0;
glGetIntegerv(GL_MAX_TEXTURE_SIZE, &maxTextureSize);
LOGI("maxTextureSize=%d", maxTextureSize);
// gen texture from image
glBindTexture(GL_TEXTURE_2D, textures_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
@@ -276,11 +416,11 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
}
}
nPoints_ = mesh.indices->size();
nPoints_ = totalPoints;
if(polygons_.size() != mesh.polygons.size())
if(polygons_.size() != polygons.size())
{
updatePolygons(mesh.polygons);
updatePolygons(polygons);
}
}
@@ -291,7 +431,12 @@ void PointCloudDrawable::setPose(const rtabmap::Transform & pose)
pose_ = glmFromTransform(pose);
}
void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering, float pointSize, bool textureRendering) {
void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
bool meshRendering,
float pointSize,
bool textureRendering,
bool lighting) {
if(vertex_buffers_ && nPoints_ && visible_)
{
@@ -299,65 +444,144 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
{
glUseProgram(texture_shader_program_);
GLuint mvp_handle_ = glGetUniformLocation(texture_shader_program_, "mvp");
glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint mvp_handle = glGetUniformLocation(texture_shader_program_, "uMVP");
glm::mat4 mv_mat = viewMatrix * pose_;
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
glUniformMatrix4fv(mvp_handle, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
glm::mat3 normalMatrix(mv_mat);
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
if(!hasNormals_)
{
lighting = false;
}
//lighting
GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
glUniform1i(lighting_handle, lighting?1:0);
if(lighting)
{
GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
glUniform3f(ambiant_handle,0.6,0.6,0.6);
GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
}
// Texture activate unit 0
glActiveTexture(GL_TEXTURE0);
// Bind the texture to this unit.
glBindTexture(GL_TEXTURE_2D, textures_);
// Tell the texture uniform sampler to use this texture in the shader by binding to texture unit 0.
GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "uTexture");
glUniform1i(texture_handle, 0);
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "uGain");
glUniform1f(gain_handle, gain_);
GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "aVertex");
GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "aTexCoord");
GLint attribute_normal=0;
if(hasNormals_)
{
attribute_normal = glGetAttribLocation(texture_shader_program_, "aNormal");
}
glEnableVertexAttribArray(attribute_vertex);
glEnableVertexAttribArray(attribute_texture);
if(hasNormals_)
{
glEnableVertexAttribArray(attribute_normal);
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_texture, 2, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
if(hasNormals_)
{
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());
}
else // point cloud or colored mesh
{
glUseProgram(cloud_shader_program_);
GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "mvp");
glm::mat4 mvp_mat = projectionMatrix * viewMatrix * pose_;
GLuint mvp_handle_ = glGetUniformLocation(cloud_shader_program_, "uMVP");
glm::mat4 mv_mat = viewMatrix * pose_;
glm::mat4 mvp_mat = projectionMatrix * mv_mat;
glUniformMatrix4fv(mvp_handle_, 1, GL_FALSE, glm::value_ptr(mvp_mat));
GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
GLuint n_handle = glGetUniformLocation(texture_shader_program_, "uN");
glm::mat3 normalMatrix(mv_mat);
normalMatrix = glm::inverse(normalMatrix);
normalMatrix = glm::transpose(normalMatrix);
glUniformMatrix3fv(n_handle, 1, GL_FALSE, glm::value_ptr(normalMatrix));
if(!hasNormals_)
{
lighting = false;
}
//lighting
GLuint lighting_handle = glGetUniformLocation(texture_shader_program_, "uUseLighting");
glUniform1i(lighting_handle, lighting?1:0);
if(lighting)
{
GLuint ambiant_handle = glGetUniformLocation(texture_shader_program_, "uAmbientColor");
glUniform3f(ambiant_handle,0.6,0.6,0.6);
GLuint lightingDirection_handle = glGetUniformLocation(texture_shader_program_, "uLightingDirection");
glUniform3f(lightingDirection_handle, 0.0, 0.0, 1.0); // from the camera
}
GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "uPointSize");
glUniform1f(point_size_handle_, pointSize);
GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "u_gain");
GLuint gain_handle = glGetUniformLocation(cloud_shader_program_, "uGain");
glUniform1f(gain_handle, gain_);
GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");
GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "aVertex");
GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "aColor");
GLint attribute_normal=0;
if(hasNormals_)
{
attribute_normal = glGetAttribLocation(cloud_shader_program_, "aNormal");
}
glEnableVertexAttribArray(attribute_vertex);
glEnableVertexAttribArray(attribute_color);
if(hasNormals_)
{
glEnableVertexAttribArray(attribute_normal);
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
if(textures_)
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 6*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 6*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?9:6)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?9:6)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
if(hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 9*sizeof(GLfloat), (GLvoid*) (6 * sizeof(GLfloat)));
}
}
else
{
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, 4*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, 4*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
glVertexAttribPointer(attribute_vertex, 3, GL_FLOAT, GL_FALSE, (hasNormals_?7:4)*sizeof(GLfloat), 0);
glVertexAttribPointer(attribute_color, 3, GL_UNSIGNED_BYTE, GL_TRUE, (hasNormals_?7:4)*sizeof(GLfloat), (GLvoid*) (3 * sizeof(GLfloat)));
if(hasNormals_)
{
glVertexAttribPointer(attribute_normal, 3, GL_FLOAT, GL_FALSE, 7*sizeof(GLfloat), (GLvoid*) (4 * sizeof(GLfloat)));
}
}
if(meshRendering && polygons_.size())
{
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_SHORT, polygons_.data());
glDrawElements(GL_TRIANGLES, polygons_.size(), GL_UNSIGNED_INT, polygons_.data());
}
else
{

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

@@ -61,6 +61,7 @@ class PointCloudDrawable {
void setVisible(bool visible) {visible_=visible;}
rtabmap::Transform getPose() const {return glmToTransform(pose_);}
bool isVisible() const {return visible_;}
bool hasMesh() const {return polygons_.size()!=0;}
bool hasTexture() const {return textures_ != 0;}
// Update current point cloud data.
@@ -69,17 +70,23 @@ class PointCloudDrawable {
// @param view_mat: view matrix from current render camera.
// @param model_mat: model matrix for this point cloud frame.
// @param vertices: all vertices in this point cloud frame.
void Render(const glm::mat4 & projectionMatrix, const glm::mat4 & viewMatrix, bool meshRendering = true, float pointSize = 3.0f, bool textureRendering = false);
void Render(const glm::mat4 & projectionMatrix,
const glm::mat4 & viewMatrix,
bool meshRendering = true,
float pointSize = 3.0f,
bool textureRendering = false,
bool lighting = true);
private:
// Vertex buffer of the point cloud geometry.
GLuint vertex_buffers_;
GLuint textures_;
std::vector<GLushort> polygons_;
std::vector<GLuint> polygons_;
int nPoints_;
glm::mat4 pose_;
bool visible_;
std::vector<int> organizedToDenseIndices_;
bool hasNormals_;
std::vector<unsigned int> organizedToDenseIndices_;
GLuint cloud_shader_program_;
GLuint texture_shader_program_;

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

@@ -42,47 +42,87 @@ const glm::vec3 kFrustumScale = glm::vec3(0.4f, 0.3f, 0.5f);
const std::string kPointCloudVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 vertex;\n"
"attribute vec3 color;\n"
"uniform mat4 mvp;\n"
"uniform float point_size;\n"
"varying vec3 v_color;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec3 aColor;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uAmbientColor;\n"
"uniform vec3 uLightingDirection;\n"
"uniform bool uUseLighting;\n"
"uniform float uPointSize;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
" gl_PointSize = point_size;\n"
" v_color = color;\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" gl_PointSize = uPointSize;\n"
" if (!uUseLighting) {\n"
" vLightWeighting = vec3(1.0, 1.0, 1.0);\n"
" } else {\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection), 0.0);\n"
" if(vLightWeighting<0.1) vLightWeighting=0.1;\n"
" }\n"
" vColor = aColor;\n"
"}\n";
const std::string kPointCloudFragmentShader =
"precision mediump float;\n"
"precision mediump int;\n"
"uniform float u_gain;\n"
"varying vec3 v_color;\n"
"uniform float uGain;\n"
"varying vec3 vColor;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_FragColor = vec4(v_color.z*u_gain, v_color.y*u_gain, v_color.x*u_gain, 1.0);\n"
" vec4 textureColor = vec4(vColor.z*uGain, vColor.y*uGain, vColor.x*uGain, 1.0);\n"
" gl_FragColor = vec4(textureColor.rgb * uGain * vLightWeighting, textureColor.a);\n"
"}\n";
const std::string kTextureMeshVertexShader =
"precision mediump float;\n"
"precision mediump int;\n"
"attribute vec3 vertex;\n"
"attribute vec2 a_TexCoordinate;\n"
"uniform mat4 mvp;\n"
"varying vec2 v_TexCoordinate;\n"
"attribute vec3 aVertex;\n"
"attribute vec3 aNormal;\n"
"attribute vec2 aTexCoord;\n"
"uniform mat4 uMVP;\n"
"uniform mat3 uN;\n"
"uniform vec3 uAmbientColor;\n"
"uniform vec3 uLightingDirection;\n"
"uniform bool uUseLighting;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_Position = mvp*vec4(vertex.x, vertex.y, vertex.z, 1.0);\n"
" v_TexCoordinate = a_TexCoordinate;\n"
" gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n"
" if(aTexCoord.x < 0.0) {\n"
" vTexCoord.x = 1.0;\n"
" vTexCoord.y = 1.0;\n" // bottom right corner
" } else {\n"
" vTexCoord = aTexCoord;\n"
" }\n"
" if (!uUseLighting) {\n"
" vLightWeighting = vec3(1.0, 1.0, 1.0);\n"
" } else {\n"
" vec3 transformedNormal = uN * aNormal;\n"
" vLightWeighting = max(dot(transformedNormal, uLightingDirection), 0.0);\n"
" if(vLightWeighting<0.1) vLightWeighting=0.1;\n"
" }\n"
"}\n";
const std::string kTextureMeshFragmentShader =
"precision mediump float;\n"
"precision mediump int;\n"
"uniform sampler2D u_Texture;\n"
"uniform float u_gain;\n"
"varying vec2 v_TexCoordinate;\n"
"uniform sampler2D uTexture;\n"
"uniform float uGain;\n"
"varying vec2 vTexCoord;\n"
"varying float vLightWeighting;\n"
"void main() {\n"
" gl_FragColor = texture2D(u_Texture, v_TexCoordinate);\n"
" gl_FragColor.x *= u_gain;\n"
" gl_FragColor.y *= u_gain;\n"
" gl_FragColor.z *= u_gain;\n"
" vec4 textureColor = texture2D(uTexture, vTexCoord);\n"
" gl_FragColor = vec4(textureColor.rgb * uGain * vLightWeighting, textureColor.a);\n"
"}\n";
const std::string kGraphVertexShader =
@@ -123,21 +163,31 @@ Scene::Scene() :
mapRendering_(true),
meshRendering_(true),
meshRenderingTexture_(true),
pointSize_(3.0f) {}
pointSize_(5.0f),
frustumCulling_(true),
lighting_(true)
{
gesture_camera_ = new tango_gl::GestureCamera();
gesture_camera_->SetCameraType(
tango_gl::GestureCamera::kFirstPerson);
}
Scene::~Scene() {DeleteResources();}
Scene::~Scene() {
DeleteResources();
delete gesture_camera_;
}
//Should only be called in OpenGL thread!
void Scene::InitGLContent()
{
if(gesture_camera_ != 0)
if(axis_ != 0)
{
DeleteResources();
}
UASSERT(gesture_camera_ == 0);
UASSERT(axis_ == 0);
gesture_camera_ = new tango_gl::GestureCamera();
axis_ = new tango_gl::Axis();
frustum_ = new tango_gl::Frustum();
trace_ = new tango_gl::Trace();
@@ -151,8 +201,6 @@ void Scene::InitGLContent()
trace_->SetColor(kTraceColor);
grid_->SetColor(kGridColor);
grid_->SetPosition(-kHeightOffset);
gesture_camera_->SetCameraType(
tango_gl::GestureCamera::kFirstPerson);
if(cloud_shader_program_ == 0)
{
@@ -175,15 +223,14 @@ void Scene::InitGLContent()
void Scene::DeleteResources() {
LOGI("Scene::DeleteResources()");
if(gesture_camera_)
if(axis_)
{
delete gesture_camera_;
delete axis_;
axis_ = 0;
delete frustum_;
delete trace_;
delete grid_;
delete currentPose_;
gesture_camera_ = 0;
}
if (cloud_shader_program_) {
@@ -288,9 +335,8 @@ int Scene::Render() {
gesture_camera_->GetViewMatrix());
}
bool frustumCulling = true;
int cloudDrawn=0;
if(mapRendering_ && frustumCulling)
if(mapRendering_ && frustumCulling_)
{
//Use camera frustum to cull nodes that don't need to be drawn
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
@@ -336,7 +382,7 @@ 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_);
pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
}
}
}
@@ -347,7 +393,7 @@ int Scene::Render() {
if((mapRendering_ || iter->first < 0) && iter->second->isVisible())
{
++cloudDrawn;
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_);
iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_);
}
}
}
@@ -426,7 +472,7 @@ void Scene::addCloud(
const pcl::IndicesPtr & indices,
const rtabmap::Transform & pose)
{
LOGI("add cloud %d", id);
LOGI("add cloud %d (%d points %d indices)", id, (int)cloud->size(), indices.get()?(int)indices->size():0);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
@@ -496,6 +542,11 @@ bool Scene::hasCloud(int id) const
return pointClouds_.find(id) != pointClouds_.end();
}
bool Scene::hasMesh(int id) const
{
return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasMesh();
}
bool Scene::hasTexture(int id) const
{
return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();

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

@@ -111,6 +111,7 @@ class Scene {
void setCloudPose(int id, const rtabmap::Transform & pose);
void setCloudVisible(int id, bool visible);
bool hasCloud(int id) const;
bool hasMesh(int id) const;
bool hasTexture(int id) const;
std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
@@ -119,9 +120,14 @@ class Scene {
void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
void setPointSize(float size) {pointSize_ = size;}
void setFrustumCulling(bool enabled) {frustumCulling_ = enabled;}
void setLighting(bool enabled) {lighting_ = enabled;}
bool isMeshRendering() const {return meshRendering_;}
bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
float getPointSize() const {return pointSize_;}
bool isFrustumCulling() const {return frustumCulling_;}
bool isLighting() const {return lighting_;}
private:
// Camera object that allows user to use touch input to interact with.
@@ -156,6 +162,8 @@ class Scene {
bool meshRendering_;
bool meshRenderingTexture_;
float pointSize_;
bool frustumCulling_;
bool lighting_;
};
#endif // TANGO_POINT_CLOUD_SCENE_H_

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

@@ -143,15 +143,26 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
return transform;
}
struct Mesh
class Mesh
{
public:
Mesh() :
cloud(new pcl::PointCloud<pcl::PointXYZRGB>),
normals(new pcl::PointCloud<pcl::Normal>),
indices(new std::vector<int>),
visible(true),
gain(1.0f)
{}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
pcl::PointCloud<pcl::Normal>::Ptr normals;
pcl::IndicesPtr indices;
std::vector<pcl::Vertices> polygons;
rtabmap::Transform pose; // in rtabmap coordinates
bool visible;
rtabmap::CameraModel cameraModel;
float gain;
std::vector<Eigen::Vector2f> texCoords;
};
#endif /* UTIL_H_ */