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

Tango: Optimized memory for created meshes (keeping dense clouds instead of organized clouds)

Browse Source
This commit is contained in:
matlabbe
2016-09-07 19:59:25 -04:00
parent 4a072b3dfc
commit d3173d8533
13 changed files with 255 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -439,7 +439,7 @@ int RTABMapApp::Render()
// Voxelize and filter depending on the previous cloud? // Voxelize and filter depending on the previous cloud?
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows); LOGI("Creating node cloud %d (depth=%dx%d rgb=%dx%d)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows);
cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get()); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size()) if(cloud->size() && indices->size())
@@ -458,8 +458,7 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons; std::vector<pcl::Vertices> outputPolygons;
outputCloud = output; std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputCloud, outputPolygons);
outputPolygons = polygons;
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks()); LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
@@ -467,16 +466,19 @@ int RTABMapApp::Render()
{ {
totalPolygons_ += outputPolygons.size(); totalPolygons_ += outputPolygons.size();
main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
// protect createdMeshes_ used also by exportMesh() method
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh())); std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second); UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud; inserted.first->second.cloud = outputCloud;
inserted.first->second.indices = indices; inserted.first->second.denseToOrganizedIndices = denseToOrganizedIndices;
inserted.first->second.width = cloud->width;
inserted.first->second.height = cloud->height;
inserted.first->second.polygons = outputPolygons; inserted.first->second.polygons = outputPolygons;
inserted.first->second.pose = iter->second; inserted.first->second.pose = iter->second;
inserted.first->second.texture = data.imageCompressed(); inserted.first->second.texture = data.imageRaw();
main_scene_.addMesh(id, inserted.first->second, iter->second);
inserted.first->second.texture = data.imageCompressed(); // keep comrpessed
} }
else else
{ {
@@ -550,7 +552,7 @@ int RTABMapApp::Render()
event.data().imageRaw().cols, event.data().imageRaw().rows, event.data().imageRaw().cols, event.data().imageRaw().rows,
event.data().depthRaw().cols, event.data().depthRaw().rows, event.data().depthRaw().cols, event.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height); (int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, std::vector<pcl::Vertices>(), opengl_world_T_rtabmap_world*event.pose()); main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*event.pose());
main_scene_.setCloudVisible(-1, true); main_scene_.setCloudVisible(-1, true);
} }
else else
@@ -566,41 +568,45 @@ int RTABMapApp::Render()
} }
} }
if(gainCompensationOnNextRender_) if(notifyDataLoaded || gainCompensationOnNextRender_)
{ {
LOGI("Gain compensation...");
gainCompensationOnNextRender_ = false; gainCompensationOnNextRender_ = false;
rtabmap::GainCompensator compensator; boost::mutex::scoped_lock lock(meshesMutex_);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds; if(createdMeshes_.size() > 1)
std::map<int, pcl::IndicesPtr> indices;
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{ {
clouds.insert(std::make_pair(iter->first, iter->second.cloud)); rtabmap::GainCompensator compensator;
indices.insert(std::make_pair(iter->first, iter->second.indices)); std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
} for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, false, true);
compensator.feed(clouds, indices, links);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudCpy(new pcl::PointCloud<pcl::PointXYZRGB>);
*cloudCpy = *iter->second.cloud;
cv::Mat imageCpy = rtabmap::uncompressImage(iter->second.texture);
if(!cloudCpy->empty())
{ {
compensator.apply(iter->first, cloudCpy, iter->second.indices); clouds.insert(std::make_pair(iter->first, iter->second.cloud));
if(!imageCpy.empty()) }
{ std::map<int, rtabmap::Transform> poses;
compensator.apply(iter->first, imageCpy); std::multimap<int, rtabmap::Link> links;
} rtabmap_->getGraph(poses, links, false, true);
compensator.feed(clouds, links);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
cv::Mat compressedImage = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(compressedImage);
if(!iter->second.cloud->empty())
{
compensator.apply(iter->first, iter->second.cloud);
if(!iter->second.texture.empty())
{
compensator.apply(iter->first, iter->second.texture);
}
}
main_scene_.updateMesh(iter->first, iter->second);
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
} }
main_scene_.updateCloudColors(iter->first, cloudCpy, imageCpy);
} }
notifyDataLoaded = true; notifyDataLoaded = true;
} }
if(filterPolygonsOnNextRender_) if(filterPolygonsOnNextRender_)
{ {
LOGI("Polygon filtering...");
filterPolygonsOnNextRender_ = false; filterPolygonsOnNextRender_ = false;
boost::mutex::scoped_lock lock(meshesMutex_); boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
@@ -846,54 +852,44 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end(); iter!= createdMeshes_.end();
++iter) ++iter)
{ {
UASSERT(!iter->second.cloud->is_dense);
if(!iter->second.texture.empty() && if(!iter->second.texture.empty() &&
iter->second.cloud->size() && iter->second.cloud->size() &&
iter->second.polygons.size()) iter->second.polygons.size() &&
(!iter->second.cloud->is_dense || (iter->second.cloud->is_dense && iter->second.denseToOrganizedIndices.size() == iter->second.cloud->size())))
{ {
// OBJ format requires normals // OBJ format requires normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 20); pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 6);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals); pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals);
// create dense cloud
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<pcl::Vertices> densePolygons;
std::map<int, int> newToOldIndices;
newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
*cloudWithNormals,
iter->second.polygons,
*denseCloud,
densePolygons);
// polygons // polygons
UASSERT(densePolygons.size()); UASSERT(iter->second.polygons.size());
unsigned int polygonSize = densePolygons.front().vertices.size(); unsigned int polygonSize = iter->second.polygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(densePolygons.size()); textureMesh.tex_polygons[oi].resize(iter->second.polygons.size());
textureMesh.tex_coordinates[oi].resize(densePolygons.size() * polygonSize); textureMesh.tex_coordinates[oi].resize(iter->second.polygons.size() * polygonSize);
for(unsigned int j=0; j<densePolygons.size(); ++j) for(unsigned int j=0; j<iter->second.polygons.size(); ++j)
{ {
pcl::Vertices vertices = densePolygons[j]; pcl::Vertices vertices = iter->second.polygons[j];
UASSERT(polygonSize == vertices.vertices.size()); UASSERT(polygonSize == vertices.vertices.size());
for(unsigned int k=0; k<vertices.vertices.size(); ++k) for(unsigned int k=0; k<vertices.vertices.size(); ++k)
{ {
//uv //uv
std::map<int, int>::iterator jter = newToOldIndices.find(vertices.vertices[k]); UASSERT(vertices.vertices[k] < iter->second.denseToOrganizedIndices.size());
int originalVertex = iter->second.denseToOrganizedIndices[vertices.vertices[k]];
textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f( textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
float(jter->second % iter->second.cloud->width) / float(iter->second.cloud->width), // u float(originalVertex % iter->second.width) / float(iter->second.width), // u
float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height)); // v float(iter->second.height - originalVertex / iter->second.width) / float(iter->second.height)); // v
vertices.vertices[k] += polygonsStep; vertices.vertices[k] += polygonsStep;
} }
textureMesh.tex_polygons[oi][j] = vertices; textureMesh.tex_polygons[oi][j] = vertices;
} }
totalPolygons += densePolygons.size(); totalPolygons += iter->second.polygons.size();
polygonsStep += denseCloud->size(); polygonsStep += iter->second.cloud->size();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second.pose);
if(mergedClouds->size() == 0) if(mergedClouds->size() == 0)
{ {
*mergedClouds = *transformedCloud; *mergedClouds = *transformedCloud;
@@ -961,24 +957,15 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end(); iter!= createdMeshes_.end();
++iter) ++iter)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.cloud, iter->second.pose);
std::vector<pcl::Vertices> densePolygons;
rtabmap::util3d::filterNotUsedVerticesFromMesh(
*iter->second.cloud,
iter->second.polygons,
*denseCloud,
densePolygons);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
if(mergedClouds->size() == 0) if(mergedClouds->size() == 0)
{ {
*mergedClouds = *transformedCloud; *mergedClouds = *transformedCloud;
mergedPolygons = densePolygons; mergedPolygons = iter->second.polygons;
} }
else else
{ {
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons); rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.polygons);
} }
} }
} }

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

@@ -181,15 +181,6 @@ class RTABMapApp : public UEventsHandler {
boost::mutex poseMutex_; boost::mutex poseMutex_;
boost::mutex renderingMutex_; boost::mutex renderingMutex_;
struct Mesh
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices;
std::vector<pcl::Vertices> polygons;
rtabmap::Transform pose;
cv::Mat texture;
};
std::map<int, Mesh> createdMeshes_; std::map<int, Mesh> createdMeshes_;
std::map<int, rtabmap::Transform> rawPoses_; std::map<int, rtabmap::Transform> rawPoses_;

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

@@ -38,9 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PointCloudDrawable::PointCloudDrawable( PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram, GLuint cloudShaderProgram,
GLuint textureShaderProgram, GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) :
const std::vector<pcl::Vertices> & polygons,
const cv::Mat & image) :
vertex_buffers_(0), vertex_buffers_(0),
textures_(0), textures_(0),
nPoints_(0), nPoints_(0),
@@ -49,8 +47,22 @@ PointCloudDrawable::PointCloudDrawable(
cloud_shader_program_(cloudShaderProgram), cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram) texture_shader_program_(textureShaderProgram)
{ {
updateCloud(cloud, image); updateCloud(cloud);
updatePolygons(polygons); }
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh) :
vertex_buffers_(0),
textures_(0),
nPoints_(0),
pose_(1.0f),
visible_(true),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram)
{
updateMesh(mesh);
} }
PointCloudDrawable::~PointCloudDrawable() PointCloudDrawable::~PointCloudDrawable()
@@ -91,7 +103,7 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
} }
} }
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image) void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
{ {
UASSERT(!cloud->empty()); UASSERT(!cloud->empty());
if(nPoints_) if(nPoints_)
@@ -99,6 +111,62 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
UASSERT((int)cloud->size() == nPoints_); UASSERT((int)cloud->size() == nPoints_);
} }
nPoints_ = 0; nPoints_ = 0;
polygons_.clear();
if (vertex_buffers_)
{
glDeleteBuffers(1, &vertex_buffers_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
vertex_buffers_ = 0;
}
if (textures_)
{
glDeleteTextures(1, &textures_);
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
textures_ = 0;
}
glGenBuffers(1, &vertex_buffers_);
if(!vertex_buffers_)
{
LOGE("OpenGL: could not generate vertex buffers\n");
return;
}
LOGI("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices(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_);
glBufferData(GL_ARRAY_BUFFER, sizeof(GLfloat) * (int)vertices.size(), (const void *)vertices.data(), GL_STATIC_DRAW);
glBindBuffer(GL_ARRAY_BUFFER, 0);
GLint error = glGetError();
if(error != GL_NO_ERROR)
{
LOGE("OpenGL: Could not allocate point cloud (0x%x)\n", error);
vertex_buffers_ = 0;
return;
}
nPoints_ = cloud->size();
}
void PointCloudDrawable::updateMesh(const Mesh & mesh)
{
UASSERT(!mesh.cloud->empty());
if(nPoints_)
{
UASSERT((int)mesh.cloud->size() == nPoints_);
}
nPoints_ = 0;
if (vertex_buffers_) if (vertex_buffers_)
{ {
@@ -107,7 +175,8 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
vertex_buffers_ = 0; vertex_buffers_ = 0;
} }
if(!image.empty()) bool textureUpdate = false;
if(!mesh.texture.empty() && mesh.texture.type() == CV_32FC3)
{ {
if (textures_) if (textures_)
{ {
@@ -115,6 +184,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()"); tango_gl::util::CheckGlError("PointCloudDrawable::~PointCloudDrawable()");
textures_ = 0; textures_ = 0;
} }
textureUpdate = true;
} }
glGenBuffers(1, &vertex_buffers_); glGenBuffers(1, &vertex_buffers_);
@@ -124,10 +194,10 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
return; return;
} }
if(!cloud->is_dense && !image.empty()) if(textureUpdate)
{ {
LOGI("cloud=%dx%d image=%dx%d\n", (int)cloud->width, (int)cloud->height, image.cols, image.rows); UASSERT((!mesh.cloud->is_dense && mesh.cloud->width==mesh.width && mesh.cloud->height==mesh.height) ||
UASSERT(!cloud->is_dense && !image.empty() && image.type() == CV_8UC3); (mesh.cloud->is_dense && mesh.width>1 && mesh.height>1 && mesh.denseToOrganizedIndices.size() == mesh.cloud->size()));
glGenTextures(1, &textures_); glGenTextures(1, &textures_);
if(!textures_) if(!textures_)
{ {
@@ -141,30 +211,35 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
std::vector<float> vertices; std::vector<float> vertices;
if(textures_) if(textures_)
{ {
vertices = std::vector<float>(cloud->size()*6); vertices = std::vector<float>(mesh.cloud->size()*6);
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<mesh.cloud->size(); ++i)
{ {
vertices[i*6] = cloud->at(i).x; vertices[i*6] = mesh.cloud->at(i).x;
vertices[i*6+1] = cloud->at(i).y; vertices[i*6+1] = mesh.cloud->at(i).y;
vertices[i*6+2] = cloud->at(i).z; vertices[i*6+2] = mesh.cloud->at(i).z;
// rgb // rgb
vertices[i*6+3] = cloud->at(i).rgb; vertices[i*6+3] = mesh.cloud->at(i).rgb;
// texture uv // texture uv
vertices[i*6+4] = float(i % cloud->width)/float(cloud->width); //u int index = i;
vertices[i*6+5] = float(i/cloud->width)/float(cloud->height); //v if(mesh.cloud->is_dense)
{
index = mesh.denseToOrganizedIndices[i];
}
vertices[i*6+4] = float(index % mesh.width)/float(mesh.width); //u
vertices[i*6+5] = float(index / mesh.width)/float(mesh.height); //v
} }
} }
else else
{ {
vertices = std::vector<float>(cloud->size()*4); vertices = std::vector<float>(mesh.cloud->size()*4);
for(unsigned int i=0; i<cloud->size(); ++i) for(unsigned int i=0; i<mesh.cloud->size(); ++i)
{ {
vertices[i*4] = cloud->at(i).x; vertices[i*4] = mesh.cloud->at(i).x;
vertices[i*4+1] = cloud->at(i).y; vertices[i*4+1] = mesh.cloud->at(i).y;
vertices[i*4+2] = cloud->at(i).z; vertices[i*4+2] = mesh.cloud->at(i).z;
vertices[i*4+3] = cloud->at(i).rgb; vertices[i*4+3] = mesh.cloud->at(i).rgb;
} }
} }
@@ -180,14 +255,14 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
return; return;
} }
if(textures_ && !image.empty()) if(textures_ && textureUpdate)
{ {
// gen texture from image // gen texture from image
glBindTexture(GL_TEXTURE_2D, textures_); glBindTexture(GL_TEXTURE_2D, textures_);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
cv::Mat rgbImage; cv::Mat rgbImage;
cv::cvtColor(image, rgbImage, CV_BGR2RGB); cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data); glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
GLint error = glGetError(); GLint error = glGetError();
@@ -202,7 +277,12 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
} }
} }
nPoints_ = cloud->size(); nPoints_ = mesh.cloud->size();
if(polygons_.size() != mesh.polygons.size())
{
updatePolygons(mesh.polygons);
}
} }
void PointCloudDrawable::setPose(const rtabmap::Transform & pose) void PointCloudDrawable::setPose(const rtabmap::Transform & pose)

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

@@ -44,13 +44,16 @@ class PointCloudDrawable {
PointCloudDrawable( PointCloudDrawable(
GLuint cloudShaderProgram, GLuint cloudShaderProgram,
GLuint textureShaderProgram, GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
const std::vector<pcl::Vertices> & polygons = std::vector<pcl::Vertices>(), PointCloudDrawable(
const cv::Mat & image = cv::Mat()); GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh);
virtual ~PointCloudDrawable(); virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons); void updatePolygons(const std::vector<pcl::Vertices> & polygons);
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image = cv::Mat()); void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
void updateMesh(const Mesh & mesh);
void setPose(const rtabmap::Transform & pose); void setPose(const rtabmap::Transform & pose);
void setVisible(bool visible) {visible_=visible;} void setVisible(bool visible) {visible_=visible;}
rtabmap::Transform getPose() const {return glmToTransform(pose_);} rtabmap::Transform getPose() const {return glmToTransform(pose_);}

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

@@ -418,11 +418,9 @@ void Scene::setTraceVisible(bool visible)
void Scene::addCloud( void Scene::addCloud(
int id, int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons, const rtabmap::Transform & pose)
const rtabmap::Transform & pose,
const cv::Mat & image)
{ {
LOGI("addOrUpdateCloud cloud %d", id); LOGI("add cloud %d", id);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id); std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end()) if(iter != pointClouds_.end())
{ {
@@ -435,9 +433,30 @@ void Scene::addCloud(
PointCloudDrawable * drawable = new PointCloudDrawable( PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_, cloud_shader_program_,
texture_mesh_shader_program_, texture_mesh_shader_program_,
cloud, cloud);
polygons, drawable->setPose(pose);
image); pointClouds_.insert(std::make_pair(id, drawable));
}
void Scene::addMesh(
int id,
const Mesh & mesh,
const rtabmap::Transform & pose)
{
LOGI("add mesh %d", id);
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
delete iter->second;
pointClouds_.erase(iter);
}
//create
UASSERT(cloud_shader_program_ != 0 && texture_mesh_shader_program_!=0);
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
mesh);
drawable->setPose(pose); drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable)); pointClouds_.insert(std::make_pair(id, drawable));
} }
@@ -481,11 +500,11 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
} }
} }
void Scene::updateCloudColors(int id, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image) void Scene::updateMesh(int id, const Mesh & mesh)
{ {
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id); std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end()) if(iter != pointClouds_.end())
{ {
iter->second->updateCloud(cloud, image); iter->second->updateMesh(mesh);
} }
} }

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

@@ -100,16 +100,18 @@ class Scene {
void addCloud( void addCloud(
int id, int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const std::vector<pcl::Vertices> & polygons, const rtabmap::Transform & pose);
const rtabmap::Transform & pose, void addMesh(
const cv::Mat & image = cv::Mat()); int id,
const Mesh & mesh,
const rtabmap::Transform & pose);
void setCloudPose(int id, const rtabmap::Transform & pose); void setCloudPose(int id, const rtabmap::Transform & pose);
void setCloudVisible(int id, bool visible); void setCloudVisible(int id, bool visible);
bool hasCloud(int id) const; bool hasCloud(int id) const;
std::set<int> getAddedClouds() const; std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons); void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
void updateCloudColors(int id, const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const cv::Mat & image = cv::Mat()); void updateMesh(int id, const Mesh & mesh);
void setMapRendering(bool enabled) {mapRendering_ = enabled;} void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;} void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}

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

@@ -33,6 +33,9 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UEventsHandler.h> #include <rtabmap/utilite/UEventsHandler.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <tango-gl/util.h> #include <tango-gl/util.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/Vertices.h>
class LogHandler : public UEventsHandler class LogHandler : public UEventsHandler
{ {
@@ -132,4 +135,15 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
return transform; return transform;
} }
struct Mesh
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // dense or organized cloud
std::vector<pcl::Vertices> polygons;
std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
int width; // width of the organized cloud
int height; // height of the organized cloud
rtabmap::Transform pose;
cv::Mat texture;
};
#endif /* UTIL_H_ */ #endif /* UTIL_H_ */

8
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
View File

@@ -66,6 +66,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
// Screen size for normalizing the touch input for orbiting the render camera. // Screen size for normalizing the touch input for orbiting the render camera.
private Point mScreenSize = new Point(); private Point mScreenSize = new Point();
private boolean mPauseFirstTime = true;
private MenuItem mItemPause; private MenuItem mItemPause;
private MenuItem mItemSave; private MenuItem mItemSave;
@@ -652,6 +653,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{ {
RTABMapLib.setPausedMapping(true); RTABMapLib.setPausedMapping(true);
((TextView)findViewById(R.id.status)).setText("Paused"); ((TextView)findViewById(R.id.status)).setText("Paused");
if(mPauseFirstTime)
{
mPauseFirstTime = false;
mToast.makeText(getActivity(), String.format("Try \"Post-Processing...\" to optimize even more the map!"), mToast.LENGTH_LONG).show();
}
} }
else else
{ {
@@ -673,7 +679,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
if(loopDetected >= 0) if(loopDetected >= 0)
{ {
mTotalLoopClosures+=loopDetected; mTotalLoopClosures+=loopDetected;
mToast.makeText(getActivity(), String.format("Optimization done! Adjusting colors..."), mToast.LENGTH_SHORT).show(); mProgressDialog.setMessage(String.format("Optimization done! Adjusting colors..."));
} }
else if(loopDetected < 0) else if(loopDetected < 0)
{ {

4
corelib/include/rtabmap/core/util3d_surface.h
View File

@@ -89,12 +89,12 @@ void RTABMAP_EXP appendMesh(
const std::vector<pcl::Vertices> & polygonsB); const std::vector<pcl::Vertices> & polygonsB);
// return map from new to old polygon indices // return map from new to old polygon indices
std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh( std::vector<int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud, pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
std::vector<pcl::Vertices> & outputPolygons); std::vector<pcl::Vertices> & outputPolygons);
std::map<int, int> RTABMAP_EXP filterNotUsedVerticesFromMesh( std::vector<int> RTABMAP_EXP filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud, pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,

3
corelib/src/GainCompensator.cpp
View File

@@ -268,8 +268,7 @@ void GainCompensator::apply(
pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
{ {
pcl::IndicesPtr indices(new std::vector<int>); pcl::IndicesPtr indices(new std::vector<int>);
UASSERT_MSG(uContains(idToIndex_, id), uFormat("id=%d idToIndex_.size()=%d", id, (int)idToIndex_.size()).c_str()); apply(id, cloud, indices);
apply(idToIndex_.at(id), cloud, indices);
} }
void GainCompensator::apply( void GainCompensator::apply(
int id, int id,

16
corelib/src/util3d_surface.cpp
View File

@@ -266,7 +266,7 @@ void appendMesh(
} }
} }
std::map<int, int> filterNotUsedVerticesFromMesh( std::vector<int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud, const pcl::PointCloud<pcl::PointXYZRGBNormal> & cloud,
const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud, pcl::PointCloud<pcl::PointXYZRGBNormal> & outputCloud,
@@ -274,7 +274,8 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
{ {
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size()); UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex> std::map<int, int> addedVertices; //<oldIndex, newIndex>
std::map<int, int> output; //<newIndex, oldIndex> std::vector<int> output; //<oldIndex>
output.resize(cloud.size());
outputCloud.resize(cloud.size()); outputCloud.resize(cloud.size());
outputCloud.is_dense = true; outputCloud.is_dense = true;
outputPolygons.resize(polygons.size()); outputPolygons.resize(polygons.size());
@@ -290,7 +291,7 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
{ {
outputCloud[oi] = cloud.at(polygons[i].vertices[j]); outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi)); addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
output.insert(std::make_pair(oi, polygons[i].vertices[j])); output[oi] = polygons[i].vertices[j];
v.vertices[j] = oi++; v.vertices[j] = oi++;
} }
else else
@@ -300,11 +301,12 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
} }
} }
outputCloud.resize(oi); outputCloud.resize(oi);
output.resize(oi);
return output; return output;
} }
std::map<int, int> filterNotUsedVerticesFromMesh( std::vector<int> filterNotUsedVerticesFromMesh(
const pcl::PointCloud<pcl::PointXYZRGB> & cloud, const pcl::PointCloud<pcl::PointXYZRGB> & cloud,
const std::vector<pcl::Vertices> & polygons, const std::vector<pcl::Vertices> & polygons,
pcl::PointCloud<pcl::PointXYZRGB> & outputCloud, pcl::PointCloud<pcl::PointXYZRGB> & outputCloud,
@@ -312,7 +314,8 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
{ {
UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size()); UDEBUG("size=%d polygons=%d", (int)cloud.size(), (int)polygons.size());
std::map<int, int> addedVertices; //<oldIndex, newIndex> std::map<int, int> addedVertices; //<oldIndex, newIndex>
std::map<int, int> output; //<newIndex, oldIndex> std::vector<int> output; //<oldIndex>
output.resize(cloud.size());
outputCloud.resize(cloud.size()); outputCloud.resize(cloud.size());
outputCloud.is_dense = true; outputCloud.is_dense = true;
outputPolygons.resize(polygons.size()); outputPolygons.resize(polygons.size());
@@ -328,7 +331,7 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
{ {
outputCloud[oi] = cloud.at(polygons[i].vertices[j]); outputCloud[oi] = cloud.at(polygons[i].vertices[j]);
addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi)); addedVertices.insert(std::make_pair(polygons[i].vertices[j], oi));
output.insert(std::make_pair(oi, polygons[i].vertices[j])); output[oi] = polygons[i].vertices[j];
v.vertices[j] = oi++; v.vertices[j] = oi++;
} }
else else
@@ -338,6 +341,7 @@ std::map<int, int> filterNotUsedVerticesFromMesh(
} }
} }
outputCloud.resize(oi); outputCloud.resize(oi);
output.resize(oi);
return output; return output;
} }

18
guilib/src/ExportCloudsDialog.cpp
View File

@@ -835,7 +835,7 @@ bool ExportCloudsDialog::getExportedClouds(
} }
//used for organized texturing below //used for organized texturing below
std::map<int, std::map<int, int> > organizedIndices; std::map<int, std::vector<int> > organizedIndices;
std::map<int, cv::Size> organizedCloudSizes; std::map<int, cv::Size> organizedCloudSizes;
//mesh //mesh
@@ -913,7 +913,7 @@ bool ExportCloudsDialog::getExportedClouds(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>); pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
std::vector<pcl::Vertices> densePolygons; std::vector<pcl::Vertices> densePolygons;
std::map<int, int> newToOldIndices = util3d::filterNotUsedVerticesFromMesh(*iter->second, polygons, *denseCloud, densePolygons); std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*iter->second, polygons, *denseCloud, densePolygons);
if(!_ui->checkBox_assemble->isChecked() || if(!_ui->checkBox_assemble->isChecked() ||
(_ui->checkBox_textureMapping->isEnabled() && (_ui->checkBox_textureMapping->isEnabled() &&
@@ -937,7 +937,7 @@ bool ExportCloudsDialog::getExportedClouds(
} }
else else
{ {
organizedIndices.insert(std::make_pair(iter->first, newToOldIndices)); organizedIndices.insert(std::make_pair(iter->first, denseToOrganizedIndices));
organizedCloudSizes.insert(std::make_pair(iter->first, cv::Size(iter->second->width, iter->second->height))); organizedCloudSizes.insert(std::make_pair(iter->first, cv::Size(iter->second->width, iter->second->height)));
} }
meshes.insert(std::make_pair(iter->first, mesh)); meshes.insert(std::make_pair(iter->first, mesh));
@@ -1136,7 +1136,7 @@ bool ExportCloudsDialog::getExportedClouds(
if(cameraPoses.size()) if(cameraPoses.size())
{ {
pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
std::map<int, std::map<int, int> >::iterator oter = organizedIndices.find(iter->first); std::map<int, std::vector<int> >::iterator oter = organizedIndices.find(iter->first);
std::map<int, cv::Size>::iterator ster = organizedCloudSizes.find(iter->first); std::map<int, cv::Size>::iterator ster = organizedCloudSizes.find(iter->first);
if(iter->first != 0 && oter != organizedIndices.end()) if(iter->first != 0 && oter != organizedIndices.end())
{ {
@@ -1162,9 +1162,8 @@ bool ExportCloudsDialog::getExportedClouds(
for(int k=0; k<polygonSize; ++k) for(int k=0; k<polygonSize; ++k)
{ {
//uv //uv
std::map<int, int>::iterator vter = oter->second.find(vertices.vertices[k]); UASSERT(vertices.vertices[k] < oter->second.size());
UASSERT(vter != oter->second.end()); int originalVertex = oter->second[vertices.vertices[k]];
int originalVertex = vter->second;
textureMesh->tex_coordinates[0][i*polygonSize+k] = Eigen::Vector2f( textureMesh->tex_coordinates[0][i*polygonSize+k] = Eigen::Vector2f(
float(originalVertex % w) / float(w), // u float(originalVertex % w) / float(w), // u
float(h - originalVertex / w) / float(h)); // v float(h - originalVertex / w) / float(h)); // v
@@ -1178,9 +1177,8 @@ bool ExportCloudsDialog::getExportedClouds(
for(int i=0; i<nPoints; ++i) for(int i=0; i<nPoints; ++i)
{ {
//uv //uv
std::map<int, int>::iterator vter = oter->second.find(i); UASSERT(i < oter->second.size());
UASSERT(vter != oter->second.end()); int originalVertex = oter->second[i];
int originalVertex = vter->second;
textureMesh->tex_coordinates[0][i] = Eigen::Vector2f( textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
float(originalVertex % w) / float(w), // u float(originalVertex % w) / float(w), // u
float(h - originalVertex / w) / float(h)); // v float(h - originalVertex / w) / float(h)); // v

11
guilib/src/MainWindow.cpp
View File

@@ -2632,7 +2632,7 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
// remove unused vertices to save memory // remove unused vertices to save memory
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons; std::vector<pcl::Vertices> outputPolygons;
std::map<int, int> newToOldIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons); std::vector<int> denseToOrganizedIndices = util3d::filterNotUsedVerticesFromMesh(*output, polygons, *outputFiltered, outputPolygons);
if(_preferencesDialog->isCloudMeshingTexture() && !image.empty()) if(_preferencesDialog->isCloudMeshingTexture() && !image.empty())
{ {
@@ -2643,14 +2643,13 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
int h = cloud->height; int h = cloud->height;
UASSERT(w > 1 && h > 1); UASSERT(w > 1 && h > 1);
textureMesh->tex_coordinates.resize(1); textureMesh->tex_coordinates.resize(1);
int nPoints = textureMesh->cloud.data.size()/textureMesh->cloud.point_step; unsigned int nPoints = outputFiltered->size();
textureMesh->tex_coordinates[0].resize(nPoints); textureMesh->tex_coordinates[0].resize(nPoints);
for(int i=0; i<nPoints; ++i) for(unsigned int i=0; i<nPoints; ++i)
{ {
//uv //uv
std::map<int, int>::iterator vter = newToOldIndices.find(i); UASSERT(i < denseToOrganizedIndices.size());
UASSERT(vter != newToOldIndices.end()); int originalVertex = denseToOrganizedIndices[i];
int originalVertex = vter->second;
textureMesh->tex_coordinates[0][i] = Eigen::Vector2f( textureMesh->tex_coordinates[0][i] = Eigen::Vector2f(
float(originalVertex % w) / float(w), // u float(originalVertex % w) / float(w), // u
float(h - originalVertex / w) / float(h)); // v float(h - originalVertex / w) / float(h)); // v