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 Rendering Options... -> Mesh Rendering -> Mesh Decimation

Browse Source
This commit is contained in:
matlabbe
2016-12-03 21:49:40 -05:00
parent d2287ab9f4
commit 3c17049084
15 changed files with 365 additions and 180 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
app/android/AndroidManifest.xml.in
View File

@@ -2,7 +2,7 @@
<!-- BEGIN_INCLUDE(manifest) -->
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
package="com.introlab.rtabmap"
android:versionCode="26"
android:versionCode="27"
android:versionName="@RTABMAP_VERSION@">
<uses-permission android:name="android.permission.CAMERA" />

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

@@ -77,6 +77,7 @@ public:
void close(); // close Tango connection
virtual bool isCalibrated() const;
virtual std::string getSerial() const;
const CameraModel & getCameraModel() const {return model_;}
rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const;
void setDecimation(int value) {decimation_ = value;}
void setAutoExposure(bool enabled) {autoExposure_ = enabled;}

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

@@ -133,6 +133,7 @@ RTABMapApp::RTABMapApp() :
fullResolution_(false),
appendMode_(true),
maxCloudDepth_(0.0),
meshDecimation_(1),
meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0),
paused_(false),
@@ -333,35 +334,58 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
{
UTimer t;
// reconstruct depth image
cv::Mat depth = cv::Mat::zeros(mesh.height, mesh.width, CV_32FC1);
UASSERT(mesh.indices.get() && mesh.indices->size());
cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1);
rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
for(unsigned int i=0; i<mesh.denseToOrganizedIndices.size(); ++i)
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
int index = mesh.indices->at(i);
// FastBilateralFilter works in camera frame
pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(i), localTransformInv);
depth.at<float>(mesh.denseToOrganizedIndices[i]) = pt.z;
if(mesh.cloud->at(index).x > 0)
{
pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
depth.at<float>(index) = pt.z;
}
}
depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudOrganized = rtabmap::util3d::cloudFromDepthRGB(mesh.texture.rows>1?mesh.texture:rtabmap::uncompressImage(mesh.texture), depth, mesh.cameraModel, 1, maxCloudDepth_);
cloudOrganized = rtabmap::util3d::transformPointCloud(cloudOrganized, mesh.cameraModel.localTransform());
//reconstruct the mesh with smoothed surfaces
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloudOrganized, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
// filter NaN points
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloudOrganized, polygons, *outputCloud, outputPolygons);
LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
if(outputPolygons.size())
if(!depth.empty() && mesh.indices->size())
{
mesh.cloud = outputCloud;
mesh.polygons = outputPolygons;
mesh.denseToOrganizedIndices = denseToOrganizedIndices;
pcl::IndicesPtr newIndices(new std::vector<int>(mesh.indices->size()));
int oi = 0;
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
int index = mesh.indices->at(i);
pcl::PointXYZRGB & pt = mesh.cloud->at(index);
pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
if(depth.at<float>(index) > 0)
{
newPt.z = depth.at<float>(index);
newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform());
newIndices->at(oi++) = index;
}
else
{
newPt.x = newPt.y = newPt.z = std::numeric_limits<float>::quiet_NaN();
}
pt.x = newPt.x;
pt.y = newPt.y;
pt.z = newPt.z;
}
newIndices->resize(oi);
mesh.indices = newIndices;
//reconstruct the mesh with smoothed surfaces
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
}
LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
mesh.polygons = polygons;
}
else
{
@@ -414,11 +438,13 @@ int RTABMapApp::Render()
{
if(!main_scene_.hasCloud(iter->first))
{
cv::Mat compressed = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(iter->second.texture);
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
texture = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
}
main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
iter->second.texture = compressed;
}
}
}
@@ -554,56 +580,33 @@ int RTABMapApp::Render()
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
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, meshDecimation_, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size())
{
UTimer time;
// pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::ExtractIndices<pcl::PointXYZRGB> filter;
filter.setIndices(indices);
filter.setKeepOrganized(true);
filter.setInputCloud(cloud);
filter.filter(*output);
std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*output, polygons, *outputCloud, outputPolygons);
LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
if(outputCloud->size() && outputPolygons.size())
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
totalPolygons_ += outputPolygons.size();
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
{
totalPolygons_ += polygons.size();
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = outputCloud;
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.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
inserted.first->second.visible = true;
inserted.first->second.texture = data.imageRaw();
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
if(notifyDataLoaded)
{
// gain compensation is done, so don't compress the texture yet
inserted.first->second.texture = data.imageRaw(); // keep raw
// mesh will be added in gain compensation below
}
else
{
main_scene_.addMesh(id, inserted.first->second, iter->second);
inserted.first->second.texture = data.imageCompressed(); // keep compressed
}
main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second);
}
else
{
@@ -646,7 +649,7 @@ int RTABMapApp::Render()
main_scene_.setCloudVisible(*iter, false);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.visible = true;
meshIter->second.visible = false;
}
}
}
@@ -663,14 +666,15 @@ int RTABMapApp::Render()
if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), 1, maxCloudDepth_);
pcl::IndicesPtr indices(new std::vector<int>);
cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get());
if(cloud->size())
{
LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
(int)cloud->width, (int)cloud->height);
main_scene_.addCloud(-1, cloud, opengl_world_T_rtabmap_world*odomEvent.pose());
main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*odomEvent.pose());
main_scene_.setCloudVisible(-1, true);
}
else
@@ -692,11 +696,12 @@ int RTABMapApp::Render()
LOGI("Gain compensation...");
boost::mutex::scoped_lock lock(meshesMutex_);
rtabmap::GainCompensator compensator;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
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));
indices.insert(std::make_pair(iter->first, iter->second.indices));
}
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
@@ -718,38 +723,24 @@ int RTABMapApp::Render()
}
}
rtabmap::GainCompensator compensator;
if(clouds.size() > 1 && links.size())
{
compensator.feed(clouds, links);
compensator.feed(clouds, indices, links);
LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
}
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
cv::Mat compressedImage = iter->second.texture;
iter->second.texture = compressedImage.rows == 1 ? rtabmap::uncompressImage(compressedImage) : compressedImage;
if(!iter->second.cloud->empty())
{
if(clouds.size() > 1 && links.size())
{
compensator.apply(iter->first, iter->second.cloud);
if(!iter->second.texture.empty())
{
compensator.apply(iter->first, iter->second.texture);
}
iter->second.gain = compensator.getGain(iter->first);
}
}
if(notifyDataLoaded)
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world* poses.at(iter->first));
}
else
{
main_scene_.updateMesh(iter->first, iter->second);
}
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
}
LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
@@ -764,11 +755,11 @@ int RTABMapApp::Render()
boost::mutex::scoped_lock lock(meshesMutex_);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(iter->second.cloud->size())
if(iter->second.cloud->size() && iter->second.indices->size())
{
if(smoothMesh(iter->first, iter->second))
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
}
}
}
@@ -980,6 +971,45 @@ void RTABMapApp::setMaxCloudDepth(float value)
maxCloudDepth_ = value;
}
void RTABMapApp::setMeshDecimation(int value)
{
LOGE("Set decimation to level %d", value);
meshDecimation_ = 1;
if(camera_)
{
// Google Tango Tablet 160x90
// Phab2Pro 240x135
int width = camera_->getCameraModel().imageWidth()/8;
if(value == 2) // high
{
if(width % 10 == 0)
{
meshDecimation_ = 10;
}
else if(width % 15 == 0)
{
meshDecimation_ = 15;
}
else
{
LOGE("Could not set decimation to high (width=%d)", width);
}
}
else if(value == 1) // medium
{
if(width % 5 == 0)
{
meshDecimation_ = 5;
}
else
{
LOGE("Could not set decimation to medium (width=%d)", width);
}
}
}
LOGE("Set decimation to %d", meshDecimation_);
}
void RTABMapApp::setMeshAngleTolerance(float value)
{
meshAngleToleranceDeg_ = value;
@@ -1049,13 +1079,26 @@ void RTABMapApp::save(const std::string & databasePath)
{
rtabmapThread_->join(true);
// save mapping parameters in the database
bool appendModeBackup = appendMode_;
if(appendMode_)
{
appendMode_ = false;
}
bool dataRecorderModeBackup = dataRecorderMode_;
if(dataRecorderMode_)
{
// to save mapping parameters in the database
dataRecorderMode_ = false;
}
if(appendModeBackup || dataRecorderModeBackup)
{
rtabmap::ParametersMap parameters = getRtabmapParameters();
rtabmap_->parseParameters(parameters);
dataRecorderMode_ = true;
appendMode_ = appendModeBackup;
dataRecorderMode_ = dataRecorderModeBackup;
}
rtabmap_->close(true, databasePath);
@@ -1076,7 +1119,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
if(UFile::getExtension(filePath).compare("obj") == 0)
{
pcl::TextureMesh textureMesh;
std::vector<cv::Mat> textures;
std::vector<int> textures;
int totalPolygons = 0;
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
{
@@ -1093,42 +1136,57 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end();
++iter)
{
if(!iter->second.texture.empty() &&
if(!rtabmap_->getMemory()->getImageCompressed(iter->first).empty() &&
iter->second.cloud->size() &&
iter->second.polygons.size() &&
(!iter->second.cloud->is_dense || (iter->second.cloud->is_dense && iter->second.denseToOrganizedIndices.size() == iter->second.cloud->size())))
iter->second.polygons.size())
{
// Convert organized to dense cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
if(iter->second.gain != 1.0f)
{
for(unsigned int i=0; i<outputCloud->size(); ++i)
{
pcl::PointXYZRGB & pt = outputCloud->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
}
}
// OBJ format requires normals
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(iter->second.cloud, 6);
pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, 6);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*iter->second.cloud, *normals, *cloudWithNormals);
pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals);
// polygons
UASSERT(iter->second.polygons.size());
unsigned int polygonSize = iter->second.polygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(iter->second.polygons.size());
textureMesh.tex_coordinates[oi].resize(iter->second.polygons.size() * polygonSize);
for(unsigned int j=0; j<iter->second.polygons.size(); ++j)
UASSERT(outputPolygons.size());
unsigned int polygonSize = outputPolygons.front().vertices.size();
textureMesh.tex_polygons[oi].resize(outputPolygons.size());
textureMesh.tex_coordinates[oi].resize(outputPolygons.size() * polygonSize);
for(unsigned int j=0; j<outputPolygons.size(); ++j)
{
pcl::Vertices vertices = iter->second.polygons[j];
pcl::Vertices vertices = outputPolygons[j];
UASSERT(polygonSize == vertices.vertices.size());
for(unsigned int k=0; k<vertices.vertices.size(); ++k)
{
//uv
UASSERT(vertices.vertices[k] < iter->second.denseToOrganizedIndices.size());
int originalVertex = iter->second.denseToOrganizedIndices[vertices.vertices[k]];
UASSERT(vertices.vertices[k] < denseToOrganizedIndices.size());
int originalVertex = denseToOrganizedIndices[vertices.vertices[k]];
textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
float(originalVertex % iter->second.width) / float(iter->second.width), // u
float(iter->second.height - originalVertex / iter->second.width) / float(iter->second.height)); // v
float(originalVertex % iter->second.cloud->width) / float(iter->second.cloud->width), // u
float(iter->second.cloud->height - originalVertex / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
vertices.vertices[k] += polygonsStep;
}
textureMesh.tex_polygons[oi][j] = vertices;
}
totalPolygons += iter->second.polygons.size();
polygonsStep += iter->second.cloud->size();
totalPolygons += outputPolygons.size();
polygonsStep += outputCloud->size();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second.pose);
if(mergedClouds->size() == 0)
@@ -1140,7 +1198,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
*mergedClouds += *transformedCloud;
}
textures[oi] = iter->second.texture;
textures[oi] = iter->first;
textureMesh.tex_materials[oi].tex_illum = 1;
textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
++oi;
@@ -1163,7 +1221,11 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
UDirectory::makeDir(textureDirectory);
for(unsigned int i=0;i<textures.size(); ++i)
{
cv::Mat rawImage = textures[i].rows>1?textures[i]:rtabmap::uncompressImage(textures[i]);
cv::Mat rawImage = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(textures[i]));
if(createdMeshes_.at(textures[i]).gain != 1.0f)
{
cv::multiply(rawImage, createdMeshes_.at(textures[i]).gain, rawImage);
}
std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
cv::imwrite(texFile, rawImage);
@@ -1198,20 +1260,36 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
iter!= createdMeshes_.end();
++iter)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(iter->second.cloud, iter->second.pose);
// Convert organized to dense cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
std::vector<pcl::Vertices> outputPolygons;
rtabmap::util3d::filterNaNPointsFromMesh(*iter->second.cloud, iter->second.polygons, *outputCloud, outputPolygons);
if(iter->second.gain != 1.0f)
{
for(unsigned int i=0; i<outputCloud->size(); ++i)
{
pcl::PointXYZRGB & pt = outputCloud->at(i);
pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * iter->second.gain)));
pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * iter->second.gain)));
pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * iter->second.gain)));
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(outputCloud, iter->second.pose);
if(mergedClouds->size() == 0)
{
*mergedClouds = *transformedCloud;
mergedPolygons = iter->second.polygons;
mergedPolygons = outputPolygons;
}
else
{
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, iter->second.polygons);
rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, outputPolygons);
}
}
}
if(mergedClouds->size() && mergedPolygons.size())
if(mergedClouds->size())
{
pcl::PolygonMesh mesh;
pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);

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

@@ -125,6 +125,7 @@ class RTABMapApp : public UEventsHandler {
void setAppendMode(bool enabled);
void setDataRecorderMode(bool enabled);
void setMaxCloudDepth(float value);
void setMeshDecimation(int value);
void setMeshAngleTolerance(float value);
void setMeshTriangleSize(int value);
int setMappingParameter(const std::string & key, const std::string & value);
@@ -157,6 +158,7 @@ class RTABMapApp : public UEventsHandler {
bool fullResolution_;
bool appendMode_;
float maxCloudDepth_;
int meshDecimation_;
int meshTrianglePix_;
float meshAngleToleranceDeg_;

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

@@ -217,6 +217,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setMaxCloudDepth(
return app.setMaxCloudDepth(value);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setMeshDecimation(
JNIEnv*, jobject, int value)
{
return app.setMeshDecimation(value);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setMeshAngleTolerance(
JNIEnv*, jobject, float value)
{

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

@@ -38,31 +38,36 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud) :
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float gain) :
vertex_buffers_(0),
textures_(0),
nPoints_(0),
pose_(1.0f),
visible_(true),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram)
texture_shader_program_(textureShaderProgram),
gain_(1.0f)
{
updateCloud(cloud);
updateCloud(cloud, indices, gain);
}
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh) :
const Mesh & mesh,
const cv::Mat & texture) :
vertex_buffers_(0),
textures_(0),
nPoints_(0),
pose_(1.0f),
visible_(true),
cloud_shader_program_(cloudShaderProgram),
texture_shader_program_(textureShaderProgram)
texture_shader_program_(textureShaderProgram),
gain_(1.0f)
{
updateMesh(mesh);
updateMesh(mesh, texture);
}
PointCloudDrawable::~PointCloudDrawable()
@@ -86,7 +91,7 @@ PointCloudDrawable::~PointCloudDrawable()
void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polygons)
{
polygons_.clear();
if(polygons.size())
if(polygons.size() && organizedToDenseIndices_.size())
{
int polygonSize = polygons[0].vertices.size();
UASSERT(polygonSize == 3);
@@ -97,21 +102,18 @@ void PointCloudDrawable::updatePolygons(const std::vector<pcl::Vertices> & polyg
UASSERT((int)polygons[i].vertices.size() == polygonSize);
for(int j=0; j<polygonSize; ++j)
{
polygons_[oi++] = (unsigned short)polygons[i].vertices[j];
polygons_[oi++] = organizedToDenseIndices_.at((unsigned short)polygons[i].vertices[j]);
}
}
}
}
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud)
void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain)
{
UASSERT(!cloud->empty());
if(nPoints_)
{
UASSERT((int)cloud->size() == nPoints_);
}
UASSERT(cloud.get() && !cloud->empty() && indices.get() && !indices->empty());
nPoints_ = 0;
polygons_.clear();
gain_ = gain;
if (vertex_buffers_)
{
@@ -135,13 +137,13 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
}
LOGI("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices(cloud->size()*4);
for(unsigned int i=0; i<cloud->size(); ++i)
std::vector<float> vertices(indices->size()*4);
for(unsigned int i=0; i<indices->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;
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;
}
glBindBuffer(GL_ARRAY_BUFFER, vertex_buffers_);
@@ -156,16 +158,12 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
return;
}
nPoints_ = cloud->size();
nPoints_ = indices->size();
}
void PointCloudDrawable::updateMesh(const Mesh & mesh)
void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
{
UASSERT(!mesh.cloud->empty());
if(nPoints_)
{
UASSERT((int)mesh.cloud->size() == nPoints_);
}
UASSERT(mesh.cloud.get() && !mesh.cloud->empty() && mesh.indices.get() && !mesh.indices->empty());
nPoints_ = 0;
if (vertex_buffers_)
@@ -175,8 +173,10 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
vertex_buffers_ = 0;
}
gain_ = mesh.gain;
bool textureUpdate = false;
if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
if(!texture.empty() && texture.type() == CV_8UC3)
{
if (textures_)
{
@@ -196,8 +196,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
if(textureUpdate)
{
UASSERT((!mesh.cloud->is_dense && mesh.cloud->width==mesh.width && mesh.cloud->height==mesh.height) ||
(mesh.cloud->is_dense && mesh.width>1 && mesh.height>1 && mesh.denseToOrganizedIndices.size() == mesh.cloud->size()));
UASSERT(!mesh.cloud->is_dense);
glGenTextures(1, &textures_);
if(!textures_)
{
@@ -209,37 +208,37 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
LOGI("Creating cloud buffer %d", vertex_buffers_);
std::vector<float> vertices;
organizedToDenseIndices_ = std::vector<int>(mesh.cloud->width*mesh.cloud->height, -1);
if(textures_)
{
vertices = std::vector<float>(mesh.cloud->size()*6);
for(unsigned int i=0; i<mesh.cloud->size(); ++i)
vertices = std::vector<float>(mesh.indices->size()*6);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
vertices[i*6] = mesh.cloud->at(i).x;
vertices[i*6+1] = mesh.cloud->at(i).y;
vertices[i*6+2] = mesh.cloud->at(i).z;
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;
// rgb
vertices[i*6+3] = mesh.cloud->at(i).rgb;
vertices[i*6+3] = mesh.cloud->at(mesh.indices->at(i)).rgb;
// texture uv
int index = i;
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
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
organizedToDenseIndices_[mesh.indices->at(i)] = i;
}
}
else
{
vertices = std::vector<float>(mesh.cloud->size()*4);
for(unsigned int i=0; i<mesh.cloud->size(); ++i)
vertices = std::vector<float>(mesh.indices->size()*4);
for(unsigned int i=0; i<mesh.indices->size(); ++i)
{
vertices[i*4] = mesh.cloud->at(i).x;
vertices[i*4+1] = mesh.cloud->at(i).y;
vertices[i*4+2] = mesh.cloud->at(i).z;
vertices[i*4+3] = mesh.cloud->at(i).rgb;
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;
}
}
@@ -262,7 +261,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
cv::Mat rgbImage;
cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB);
cv::cvtColor(texture, rgbImage, CV_BGR2RGB);
glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, rgbImage.cols, rgbImage.rows, 0, GL_RGB, GL_UNSIGNED_BYTE, rgbImage.data);
GLint error = glGetError();
@@ -277,7 +276,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh)
}
}
nPoints_ = mesh.cloud->size();
nPoints_ = mesh.indices->size();
if(polygons_.size() != mesh.polygons.size())
{
@@ -312,6 +311,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
GLuint texture_handle = glGetUniformLocation(texture_shader_program_, "u_Texture");
glUniform1i(texture_handle, 0);
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
glUniform1f(gain_handle, gain_);
GLint attribute_vertex = glGetAttribLocation(texture_shader_program_, "vertex");
GLint attribute_texture = glGetAttribLocation(texture_shader_program_, "a_TexCoordinate");
@@ -334,6 +336,9 @@ void PointCloudDrawable::Render(const glm::mat4 & projectionMatrix, const glm::m
GLuint point_size_handle_ = glGetUniformLocation(cloud_shader_program_, "point_size");
glUniform1f(point_size_handle_, pointSize);
GLuint gain_handle = glGetUniformLocation(texture_shader_program_, "u_gain");
glUniform1f(gain_handle, gain_);
GLint attribute_vertex = glGetAttribLocation(cloud_shader_program_, "vertex");
GLint attribute_color = glGetAttribLocation(cloud_shader_program_, "color");

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

@@ -44,20 +44,24 @@ class PointCloudDrawable {
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
float gain);
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh);
const Mesh & mesh,
const cv::Mat & texture);
virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons);
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud);
void updateMesh(const Mesh & mesh);
void updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud, const pcl::IndicesPtr & indices, float gain);
void updateMesh(const Mesh & mesh, const cv::Mat & texture);
void setPose(const rtabmap::Transform & pose);
void setVisible(bool visible) {visible_=visible;}
rtabmap::Transform getPose() const {return glmToTransform(pose_);}
bool isVisible() const {return visible_;}
bool hasTexture() const {return textures_ != 0;}
// Update current point cloud data.
//
@@ -75,9 +79,12 @@ class PointCloudDrawable {
int nPoints_;
glm::mat4 pose_;
bool visible_;
std::vector<int> organizedToDenseIndices_;
GLuint cloud_shader_program_;
GLuint texture_shader_program_;
float gain_;
};
#endif // TANGO_POINT_CLOUD_POINT_CLOUD_DRAWABLE_H_

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

@@ -55,9 +55,10 @@ const std::string kPointCloudVertexShader =
const std::string kPointCloudFragmentShader =
"precision mediump float;\n"
"precision mediump int;\n"
"uniform float u_gain;\n"
"varying vec3 v_color;\n"
"void main() {\n"
" gl_FragColor = vec4(v_color.z, v_color.y, v_color.x, 1.0);\n"
" gl_FragColor = vec4(v_color.z*u_gain, v_color.y*u_gain, v_color.x*u_gain, 1.0);\n"
"}\n";
const std::string kTextureMeshVertexShader =
@@ -75,9 +76,13 @@ 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"
"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"
"}\n";
const std::string kGraphVertexShader =
@@ -418,6 +423,7 @@ void Scene::setTraceVisible(bool visible)
void Scene::addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const rtabmap::Transform & pose)
{
LOGI("add cloud %d", id);
@@ -433,7 +439,9 @@ void Scene::addCloud(
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
cloud);
cloud,
indices,
1.0f);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}
@@ -441,6 +449,7 @@ void Scene::addCloud(
void Scene::addMesh(
int id,
const Mesh & mesh,
const cv::Mat & texture,
const rtabmap::Transform & pose)
{
LOGI("add mesh %d", id);
@@ -456,7 +465,8 @@ void Scene::addMesh(
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
mesh);
mesh,
texture);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}
@@ -486,6 +496,11 @@ bool Scene::hasCloud(int id) const
return pointClouds_.find(id) != pointClouds_.end();
}
bool Scene::hasTexture(int id) const
{
return pointClouds_.find(id) != pointClouds_.end() && pointClouds_.at(id)->hasTexture();
}
std::set<int> Scene::getAddedClouds() const
{
return uKeysSet(pointClouds_);
@@ -500,11 +515,11 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
}
}
void Scene::updateMesh(int id, const Mesh & mesh)
void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture)
{
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
iter->second->updateMesh(mesh);
iter->second->updateMesh(mesh, texture);
}
}

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

@@ -100,23 +100,29 @@ class Scene {
void addCloud(
int id,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr & cloud,
const pcl::IndicesPtr & indices,
const rtabmap::Transform & pose);
void addMesh(
int id,
const Mesh & mesh,
const cv::Mat & texture,
const rtabmap::Transform & pose);
void setCloudPose(int id, const rtabmap::Transform & pose);
void setCloudVisible(int id, bool visible);
bool hasCloud(int id) const;
bool hasTexture(int id) const;
std::set<int> getAddedClouds() const;
void updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polygons);
void updateMesh(int id, const Mesh & mesh);
void updateMesh(int id, const Mesh & mesh, const cv::Mat & texture);
void setMapRendering(bool enabled) {mapRendering_ = enabled;}
void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;}
void setPointSize(float size) {pointSize_ = size;}
bool isMeshRendering() const {return meshRendering_;}
bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;}
private:
// Camera object that allows user to use touch input to interact with.
tango_gl::GestureCamera* gesture_camera_;

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

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/Vertices.h>
#include <pcl/pcl_base.h>
class LogHandler : public UEventsHandler
{
@@ -144,15 +145,13 @@ inline rtabmap::Transform glmToTransform(const glm::mat4 & mat)
struct Mesh
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // dense or organized cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; // organized cloud
pcl::IndicesPtr indices;
std::vector<pcl::Vertices> polygons;
std::vector<int> denseToOrganizedIndices; // should be set if cloud is dense, used for texturing
unsigned int width; // width of the organized cloud
unsigned int height; // height of the organized cloud
rtabmap::Transform pose; // in rtabmap coordinates
bool visible;
cv::Mat texture;
rtabmap::CameraModel cameraModel;
float gain;
};
#endif /* UTIL_H_ */

1
app/android/res/menu/optionmenu.xml
View File

@@ -46,6 +46,7 @@
<item android:id="@+id/mesh" android:title="Mesh" />
<item android:id="@+id/texture_mesh" android:checked="true" android:title="Texture Mesh" />
</group>
<item android:id="@+id/mesh_decimation" android:checkable="false" android:title="Mesh Decimation..." />
<item android:id="@+id/mesh_angle_tolerance" android:checkable="false" android:title="Mesh Angle Tolerance..." />
<item android:id="@+id/mesh_triangle_size" android:checkable="false" android:title="Mesh Triangle Size..." />
<item android:id="@+id/max_depth" android:checkable="false" android:title="Max Depth..." />

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

@@ -89,6 +89,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private String mWorkingDirectory = "";
private int mMaxDepthIndex = 5;
private int mMeshDecimationIndex = 0;
private int mMeshAngleToleranceIndex = 2;
private int mMeshTriangleSizeIndex = 0;
@@ -558,7 +559,7 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mItemOpen.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
mItemPostProcessing.setEnabled(item.isChecked() && !mItemDataRecorderMode.isChecked());
mItemDataRecorderMode.setEnabled(item.isChecked());
// mItemSave.setEnabled(item.isChecked() && !mWorkingDirectory.isEmpty());
if(item.isChecked())
{
RTABMapLib.setPausedMapping(true);
@@ -849,6 +850,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
});
builder.show();
}
else if(itemId == R.id.mesh_decimation)
{
// get double
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("Mesh Decimation");
final String[] values = {"Disabled", "Medium", "High"};
builder.setSingleChoiceItems(values, mMeshDecimationIndex, new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
if(which >=0 && which <= 2)
{
mMeshDecimationIndex = which;
RTABMapLib.setMeshDecimation(which);
}
}
});
builder.show();
}
else if(itemId == R.id.mesh_angle_tolerance)
{
// get double

1
app/android/src/com/introlab/rtabmap/RTABMapLib.java
View File

@@ -72,6 +72,7 @@ public class RTABMapLib
public static native void setAppendMode(boolean enabled);
public static native void setDataRecorderMode(boolean enabled);
public static native void setMaxCloudDepth(float value);
public static native void setMeshDecimation(int value);
public static native void setMeshAngleTolerance(float value);
public static native void setMeshTriangleSize(int value);
public static native int setMappingParameter(String key, String value);