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 progression bar when exporting, averaging colors in color radius when exporting without texture

Browse Source
This commit is contained in:
matlabbe
2017-02-28 21:50:50 -05:00
parent b76952f016
commit c9f6dabfc6
20 changed files with 776 additions and 268 deletions
Download Patch File Download Diff File Expand all files Collapse all files

134
app/android/jni/ProgressionStatus.h Normal file
View File

@@ -0,0 +1,134 @@
/*
* ProgressionStatus.h
*
* Created on: Feb 28, 2017
* Author: mathieu
*/
#ifndef APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
#define APP_ANDROID_JNI_PROGRESSIONSTATUS_H_
#include <rtabmap/core/ProgressState.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UEventsManager.h>
#include <jni.h>
namespace rtabmap {
class ProgressEvent : public UEvent
{
public:
ProgressEvent(int count = 1) : count_(count){}
virtual std::string getClassName() const {return "ProgressEvent";}
int count_;
};
class ProgressionStatus: public ProgressState, public UEventsHandler
{
public:
ProgressionStatus() : count_(0), max_(100), canceled_(false), jvm_(0), rtabmap_(0)
{
registerToEventsManager();
}
void setJavaObjects(JavaVM * jvm, jobject rtabmap)
{
jvm_ = jvm;
rtabmap_ = rtabmap;
}
void reset(int max)
{
count_=-1;
max_ = max;
canceled_ = false;
increment();
}
void setMax(int max)
{
max_ = max;
}
int getMax() const {return max_;}
void increment(int count = 1) const
{
UEventsManager::post(new ProgressEvent(count));
}
void finish()
{
}
virtual bool callback(const std::string & msg) const
{
if(!canceled_)
{
increment();
}
return !canceled_;
}
virtual ~ProgressionStatus(){}
void cancel()
{
canceled_ = true;
}
bool isCanceled() const
{
return canceled_;
}
protected:
virtual void handleEvent(UEvent * event)
{
if(event->getClassName().compare("ProgressEvent") == 0)
{
count_ += ((ProgressEvent*)event)->count_;
// Call JAVA callback
bool success = false;
if(jvm_ && rtabmap_)
{
JNIEnv *env = 0;
jint rs = jvm_->AttachCurrentThread(&env, NULL);
if(rs == JNI_OK && env)
{
jclass clazz = env->GetObjectClass(rtabmap_);
if(clazz)
{
jmethodID methodID = env->GetMethodID(clazz, "updateProgressionCallback", "(II)V" );
if(methodID)
{
env->CallVoidMethod(rtabmap_, methodID,
count_,
max_);
success = true;
}
}
}
jvm_->DetachCurrentThread();
}
if(!success)
{
UERROR("Failed to call rtabmap::updateProgressionCallback");
}
}
}
private:
int count_;
int max_;
bool canceled_;
JavaVM *jvm_;
jobject rtabmap_;
};
}
#endif /* APP_ANDROID_JNI_PROGRESSIONSTATUS_H_ */

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

@@ -211,6 +211,7 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
renderingTime_ = 0.0f;
processMemoryUsedBytes = 0;
processGPUMemoryUsedBytes = 0;
progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
if(camera_)
{
@@ -467,7 +468,7 @@ int RTABMapApp::Render()
bool notifyCameraStarted = false;
// process only pose events in vsualization mode
// process only pose events in visualization mode
rtabmap::Transform pose;
{
boost::mutex::scoped_lock lock(poseMutex_);
@@ -531,14 +532,13 @@ int RTABMapApp::Render()
pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.cloud);
pcl::fromPCLPointCloud2(exportedMesh_->cloud, *mesh.normals);
mesh.polygons = exportedMesh_->tex_polygons[0];
cv::Mat texture;
if(exportedMesh_->tex_coordinates.size())
{
mesh.texCoords = exportedMesh_->tex_coordinates[0];
texture = exportedTexture_;
mesh.texture = exportedTexture_;
}
main_scene_.addMesh(g_exportedMeshId, mesh, texture, opengl_world_T_rtabmap_world);
main_scene_.addMesh(g_exportedMeshId, mesh, opengl_world_T_rtabmap_world);
}
else
{
@@ -583,17 +583,21 @@ int RTABMapApp::Render()
// should be before clearSceneOnNextRender_ in case openDatabase is called
std::list<rtabmap::Statistics> rtabmapEvents;
{
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmapMutex_.lock();
rtabmapEvents = rtabmapEvents_;
rtabmapEvents_.clear();
rtabmapMutex_.unlock();
boost::mutex::scoped_lock lockMesh(meshesMutex_);
if(!clearSceneOnNextRender_ && rtabmapEvents.size() && createdMeshes_.size())
if(!clearSceneOnNextRender_ && rtabmapEvents.size())
{
if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first)
boost::mutex::scoped_lock lockMesh(meshesMutex_);
if(createdMeshes_.size())
{
LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front().refImageId(), createdMeshes_.rbegin()->first);
clearSceneOnNextRender_ = true;
if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first)
{
LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front().refImageId(), createdMeshes_.rbegin()->first);
clearSceneOnNextRender_ = true;
}
}
}
}
@@ -627,18 +631,24 @@ int RTABMapApp::Render()
added.erase(-1);
{
boost::mutex::scoped_lock lock(meshesMutex_);
if(added.size() != createdMeshes_.size())
unsigned int meshes = createdMeshes_.size();
if(meshes && createdMeshes_.rbegin()->second.pose.isNull())
{
meshes -= 1; // buffered mesh
}
if(added.size() != meshes)
{
processGPUMemoryUsedBytes = 0;
for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
if(!main_scene_.hasCloud(iter->first))
{
LOGI("Re-add mesh %d to OpenGL context", iter->first);
if(main_scene_.isMeshRendering() && iter->second.polygons.size() == 0)
{
iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
}
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
cv::Mat textureRaw;
@@ -647,10 +657,10 @@ int RTABMapApp::Render()
{
cv::Size reducedSize(textureRaw.cols/(textureRaw.cols>1000?4:2), textureRaw.rows/(textureRaw.cols>1000?4:2));
LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
cv::resize(textureRaw, texture, reducedSize, 0, 0, CV_INTER_AREA);
cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_AREA);
}
}
main_scene_.addMesh(iter->first, iter->second, texture, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
main_scene_.setCloudVisible(iter->first, iter->second.visible);
long estimateGPUMem = 0;
@@ -658,7 +668,9 @@ int RTABMapApp::Render()
estimateGPUMem += iter->second.indices->size()*4; // int
estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon
processGPUMemoryUsedBytes += estimateGPUMem + (texture.empty()?0:iter->second.polygons.size()*3*8+texture.total());
processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total());
iter->second.texture = cv::Mat(); // don't keep textures in memory
}
}
}
@@ -788,67 +800,74 @@ int RTABMapApp::Render()
meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
meshIter->second.visible = true;
}
else if(uContains(bufferedSensorData, id))
else if(uContains(bufferedSensorData, id) || createdMeshes_.find(id) != createdMeshes_.end())
{
rtabmap::SensorData data = bufferedSensorData.at(id);
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
if(createdMeshes_.find(id) == createdMeshes_.end())
{
// Voxelize and filter depending on the previous cloud?
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, meshDecimation_, maxCloudDepth_, 0, indices.get());
rtabmap::SensorData data = bufferedSensorData.at(id);
if(cloud->size() && indices->size())
cv::Mat tmpA, depth;
data.uncompressData(&tmpA, &depth);
if(!data.imageRaw().empty() && !data.depthRaw().empty())
{
UTimer time;
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
// Voxelize and filter depending on the previous cloud?
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, meshDecimation_, maxCloudDepth_, 0, indices.get());
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
if(cloud->size() && indices->size())
{
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 = 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.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
cv::Mat texture;
if(main_scene_.isMeshTexturing())
UTimer time;
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
main_scene_.addMesh(id, inserted.first->second, texture, iter->second);
long estimateCPUMem = 0;
estimateCPUMem += inserted.first->second.cloud->size()*16; // 3*float + 1 float rgb
estimateCPUMem += inserted.first->second.indices->size()*4; // int
estimateCPUMem += inserted.first->second.polygons.size()*4*3; // 3 indices per polygon
processMemoryUsedBytes += estimateCPUMem;
processGPUMemoryUsedBytes += estimateCPUMem + (texture.empty()?0:inserted.first->second.polygons.size()*3*8+texture.total());
}
else
{
UERROR("No mesh could be created for node %d", id);
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
{
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
if(main_scene_.isMeshTexturing())
{
cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
LOGD("resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_AREA);
}
}
else
{
UERROR("No mesh could be created for node %d", id);
}
}
}
totalPoints_+=indices->size();
}
if(createdMeshes_.find(id) != createdMeshes_.end())
{
Mesh & mesh = createdMeshes_.at(id);
totalPoints_+=mesh.indices->size();
totalPolygons_ += mesh.polygons.size();
mesh.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
main_scene_.addMesh(id, mesh, iter->second);
long estimateCPUMem = 0;
estimateCPUMem += mesh.cloud->size()*16; // 3*float + 1 float rgb
estimateCPUMem += mesh.indices->size()*4; // int
estimateCPUMem += mesh.polygons.size()*4*3; // 3 indices per polygon
processMemoryUsedBytes += estimateCPUMem;
processGPUMemoryUsedBytes += estimateCPUMem + (mesh.texture.empty()?0:mesh.polygons.size()*3*8+mesh.texture.total());
mesh.texture = cv::Mat(); // don't keep textures in memory
}
}
}
@@ -997,7 +1016,7 @@ int RTABMapApp::Render()
{
if(smoothMesh(iter->first, iter->second))
{
main_scene_.updateMesh(iter->first, iter->second, cv::Mat());
main_scene_.updateMesh(iter->first, iter->second);
}
}
}
@@ -1500,6 +1519,13 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
}
++oi;
}
if(progressionStatus_.isCanceled())
{
return cv::Mat();
}
progressionStatus_.increment();
}
}
else
@@ -1519,6 +1545,12 @@ cv::Mat RTABMapApp::mergeTextures(pcl::TextureMesh & mesh, int textureSize) cons
return globalTexture;
}
void RTABMapApp::cancelProcessing()
{
UWARN("Processing canceled!");
progressionStatus_.cancel();
}
bool RTABMapApp::exportMesh(
const std::string & filePath,
float cloudVoxelSize,
@@ -1530,7 +1562,7 @@ bool RTABMapApp::exportMesh(
bool optimized,
float optimizedVoxelSize,
int optimizedDepth,
float optimizedDecimationFactor,
int optimizedMaxPolygons,
float optimizedColorRadius,
bool optimizedCleanWhitePolygons,
bool optimizedColorWhitePolygons, // not yet used
@@ -1553,6 +1585,34 @@ bool RTABMapApp::exportMesh(
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, true, true);
int totalSteps = 0;
totalSteps+=poses.size(); // assemble
if(meshing)
{
if(optimized)
{
totalSteps += poses.size(); // meshing
if(textureSize > 0 && optimizedMaxPolygons > 0)
{
totalSteps += 1; // decimation
}
totalSteps += 1; // texture/coloring
if(textureSize > 0)
{
totalSteps+=poses.size()+1; // texture cameras + apply polygons
}
}
if(textureSize>0)
{
totalSteps += poses.size()+1; // uncompress and merge textures
}
}
totalSteps += 1; // save file
progressionStatus_.reset(totalSteps);
//Assemble the meshes
if(meshing) // Mesh or Texture Mesh
{
@@ -1647,6 +1707,16 @@ bool RTABMapApp::exportMesh(
{
UERROR("Cloud %d not found or empty", iter->first);
}
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment();
}
LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size());
@@ -1661,17 +1731,31 @@ bool RTABMapApp::exportMesh(
poisson.reconstruct(*mesh);
LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size());
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment(poses.size());
if(mesh->polygons.size())
{
if(textureSize > 0 && optimizedDecimationFactor > 0.0f)
if(textureSize > 0 && optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
{
#ifndef DISABLE_VTK
unsigned int count = mesh->polygons.size();
LOGI("Mesh decimation (factor=%f) from %d polygons...", optimizedDecimationFactor, (int)count);
float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
progressionStatus_.setMax(progressionStatus_.getMax() + optimizedMaxPolygons/10000);
pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
pcl::MeshQuadricDecimationVTK mqd;
mqd.setTargetReductionFactor(optimizedDecimationFactor);
mqd.setTargetReductionFactor(factor);
mqd.setInputMesh(mesh);
mqd.process (*output);
mesh = output;
@@ -1681,7 +1765,7 @@ bool RTABMapApp::exportMesh(
// pcl::MeshQuadricDecimationVTK::performProcessing(pcl::PolygonMesh&): error: undefined reference to 'vtkQuadricDecimation::New()'
// pcl::VTKUtils::mesh2vtk(pcl::PolygonMesh const&, vtkSmartPointer<vtkPolyData>&): error: undefined reference to 'vtkFloatArray::New()'
LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", optimizedDecimationFactor, count, (int)mesh->polygons.size(), timer.ticks());
LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", factor, count, (int)mesh->polygons.size(), timer.ticks());
if(count < mesh->polygons.size())
{
UWARN("Decimated mesh has more polygons than before!");
@@ -1691,6 +1775,17 @@ bool RTABMapApp::exportMesh(
#endif
}
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment();
if(textureSize == 0)
{
// colored polygon mesh
@@ -1721,10 +1816,22 @@ bool RTABMapApp::exportMesh(
}
if(kIndices.size())
{
coloredCloud->at(i).r = mergedClouds->at(kIndices[0]).r;
coloredCloud->at(i).g = mergedClouds->at(kIndices[0]).g;
coloredCloud->at(i).b = mergedClouds->at(kIndices[0]).b;
coloredCloud->at(i).a = mergedClouds->at(kIndices[0]).a;
//compute average color
int r=0;
int g=0;
int b=0;
int a=0;
for(unsigned int j=0; j<kIndices.size(); ++j)
{
r+=(int)mergedClouds->at(kIndices[j]).r;
g+=(int)mergedClouds->at(kIndices[j]).g;
b+=(int)mergedClouds->at(kIndices[j]).b;
a+=(int)mergedClouds->at(kIndices[j]).a;
}
coloredCloud->at(i).r = r/kIndices.size();
coloredCloud->at(i).g = g/kIndices.size();
coloredCloud->at(i).b = b/kIndices.size();
coloredCloud->at(i).a = a/kIndices.size();
coloredPts.at(i) = true;
}
else
@@ -1814,6 +1921,9 @@ bool RTABMapApp::exportMesh(
cloud->at(v.vertices[j]).normal_x = normal[0];
cloud->at(v.vertices[j]).normal_y = normal[1];
cloud->at(v.vertices[j]).normal_z = normal[2];
cloud->at(v.vertices[j]).r = 255;
cloud->at(v.vertices[j]).g = 255;
cloud->at(v.vertices[j]).b = 255;
}
}
pcl::toPCLPointCloud2 (*cloud, mesh->cloud);
@@ -1881,9 +1991,19 @@ bool RTABMapApp::exportMesh(
mesh,
cameraPoses,
cameraModels,
maxTextureDistance);
maxTextureDistance,
&progressionStatus_);
LOGI("Texturing... done! %fs", timer.ticks());
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
// Remove occluded polygons (polygons with no texture)
if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
{
@@ -2125,6 +2245,16 @@ bool RTABMapApp::exportMesh(
{
UERROR("Mesh not found for mesh %d", iter->first);
}
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment();
}
if(textureSize == 0)
{
@@ -2156,6 +2286,15 @@ bool RTABMapApp::exportMesh(
LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size());
globalTexture = mergeTextures(*textureMesh, textureSize);
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
std::string baseName = uSplit(UFile::getName(filePath), '.').front();
std::string textureDirectory = UDirectory::getDir(filePath);
std::string fullPath = textureDirectory+UDirectory::separator()+baseName+".jpg";
@@ -2170,6 +2309,16 @@ bool RTABMapApp::exportMesh(
LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTexture.total()*globalTexture.channels());
}
}
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment();
}
if(totalPolygons)
{
@@ -2300,6 +2449,16 @@ bool RTABMapApp::exportMesh(
*mergedClouds += *transformedCloud;
}
}
if(progressionStatus_.isCanceled())
{
if(blockRendering)
{
renderingMutex_.unlock();
}
return false;
}
progressionStatus_.increment();
}
if(mergedClouds->size())
@@ -2328,6 +2487,8 @@ bool RTABMapApp::exportMesh(
}
}
progressionStatus_.finish();
if(blockRendering)
{
renderingMutex_.unlock();
@@ -2380,7 +2541,15 @@ int RTABMapApp::postProcessing(int approach)
// detect more loop closures
if(approach == -1 || approach == 2)
{
returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
if(approach == -1)
{
progressionStatus_.reset(6);
}
returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, approach==-1?&progressionStatus_:0);
if(approach == -1 && progressionStatus_.isCanceled())
{
return -1;
}
}
// graph optimization
@@ -2479,6 +2648,61 @@ void RTABMapApp::handleEvent(UEvent * event)
LOGI("Received RtabmapEvent initialized event!");
if(camera_->isRunning())
{
rtabmap::RtabmapEvent * rtabmapEvent = (rtabmap::RtabmapEvent*)event;
int smallMovement = (int)uValue(rtabmapEvent->getStats().data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
int rehearsalMerged = (int)uValue(rtabmapEvent->getStats().data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
if(rtabmapEvent->getStats().getSignatures().size() &&
!trajectoryMode_ &&
!dataRecorderMode_ &&
!localizationMode_ &&
smallMovement == 0 &&
rehearsalMerged == 0 &&
!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().imageRaw().empty() &&
!rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData().depthRaw().empty())
{
int id = rtabmapEvent->getStats().getSignatures().rbegin()->first;
const rtabmap::SensorData & data = rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
pcl::IndicesPtr indices(new std::vector<int>);
LOGI("(EVENT) 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(
rtabmapEvent->getStats().getSignatures().rbegin()->second.sensorData(),
meshDecimation_, maxCloudDepth_, 0, indices.get());
if(cloud->size() && indices->size())
{
UTimer time;
std::vector<pcl::Vertices> polygons;
if(main_scene_.isMeshRendering())
{
polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
LOGI("(EVENT) Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
}
if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering())
{
cv::Mat texture;
if(main_scene_.isMeshTexturing())
{
cv::Size reducedSize(data.imageRaw().cols/(data.imageRaw().cols>1000?4:2), data.imageRaw().rows/(data.imageRaw().cols>1000?4:2));
LOGD("(EVENT) resize image from %dx%d to %dx%d", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height);
cv::resize(data.imageRaw(), texture, reducedSize, 0, 0, CV_INTER_AREA);
}
boost::mutex::scoped_lock lockMesh(meshesMutex_);
std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
UASSERT(inserted.second);
inserted.first->second.cloud = cloud;
inserted.first->second.indices = indices;
inserted.first->second.polygons = polygons;
inserted.first->second.visible = true;
inserted.first->second.cameraModel = data.cameraModels()[0];
inserted.first->second.gain = 1.0f;
inserted.first->second.texture = texture;
}
}
}
boost::mutex::scoped_lock lock(rtabmapMutex_);
rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
}

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

@@ -37,6 +37,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "scene.h"
#include "CameraTango.h"
#include "util.h"
#include "ProgressionStatus.h"
#include <rtabmap/core/RtabmapThread.h>
#include <rtabmap/utilite/UEventsHandler.h>
@@ -142,6 +143,7 @@ class RTABMapApp : public UEventsHandler {
void resetMapping();
void save(const std::string & databasePath);
cv::Mat mergeTextures(pcl::TextureMesh & mesh, int textureSize) const;
void cancelProcessing();
bool exportMesh(
const std::string & filePath,
float cloudVoxelSize,
@@ -153,7 +155,7 @@ class RTABMapApp : public UEventsHandler {
bool optimized,
float optimizedVoxelSize,
int optimizedDepth,
float optimizedDecimationFactor,
int optimizedMaxPolygons,
float optimizedColorRadius,
bool optimizedCleanWhitePolygons,
bool optimizedColorWhitePolygons,
@@ -233,6 +235,8 @@ class RTABMapApp : public UEventsHandler {
std::map<int, rtabmap::Transform> rawPoses_;
std::pair<rtabmap::RtabmapEventInit::Status, std::string> status_;
rtabmap::ProgressionStatus progressionStatus_;
};
#endif // TANGO_POINT_CLOUD_POINT_CLOUD_APP_H_

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

@@ -296,6 +296,13 @@ Java_com_introlab_rtabmap_RTABMapLib_save(
return app.save(databasePathC);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_cancelProcessing(
JNIEnv* env, jobject)
{
return app.cancelProcessing();
}
JNIEXPORT bool JNICALL
Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
JNIEnv* env, jobject,
@@ -309,7 +316,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
bool optimized,
float optimizedVoxelSize,
int optimizedDepth,
float optimizedDecimationFactor,
int optimizedMaxPolygons,
float optimizedColorRadius,
bool optimizedCleanWhitePolygons,
bool optimizedColorWhitePolygons,
@@ -328,7 +335,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh(
optimized,
optimizedVoxelSize,
optimizedDepth,
optimizedDecimationFactor,
optimizedMaxPolygons,
optimizedColorRadius,
optimizedCleanWhitePolygons,
optimizedColorWhitePolygons,

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

@@ -57,8 +57,7 @@ PointCloudDrawable::PointCloudDrawable(
PointCloudDrawable::PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh,
const cv::Mat & texture) :
const Mesh & mesh) :
vertex_buffers_(0),
textures_(0),
nPoints_(0),
@@ -69,7 +68,7 @@ PointCloudDrawable::PointCloudDrawable(
texture_shader_program_(textureShaderProgram),
gain_(1.0f)
{
updateMesh(mesh, texture);
updateMesh(mesh);
}
PointCloudDrawable::~PointCloudDrawable()
@@ -182,7 +181,7 @@ void PointCloudDrawable::updateCloud(const pcl::PointCloud<pcl::PointXYZRGB>::Pt
nPoints_ = totalPoints;
}
void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
void PointCloudDrawable::updateMesh(const Mesh & mesh)
{
UASSERT(mesh.cloud.get() && !mesh.cloud->empty());
nPoints_ = 0;
@@ -197,7 +196,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
gain_ = mesh.gain;
bool textureUpdate = false;
if(!texture.empty() && texture.type() == CV_8UC3)
if(!mesh.texture.empty() && mesh.texture.type() == CV_8UC3)
{
if (textures_)
{
@@ -404,7 +403,7 @@ void PointCloudDrawable::updateMesh(const Mesh & mesh, const cv::Mat & texture)
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(texture, rgbImage, CV_BGR2RGB);
cv::cvtColor(mesh.texture, rgbImage, CV_BGR2RGB);
glPixelStorei(GL_UNPACK_ALIGNMENT, 1);
//glPixelStorei(GL_UNPACK_ROW_LENGTH, 0);

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

@@ -50,13 +50,12 @@ class PointCloudDrawable {
PointCloudDrawable(
GLuint cloudShaderProgram,
GLuint textureShaderProgram,
const Mesh & mesh,
const cv::Mat & texture);
const Mesh & mesh);
virtual ~PointCloudDrawable();
void updatePolygons(const std::vector<pcl::Vertices> & polygons);
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 updateMesh(const Mesh & mesh);
void setPose(const rtabmap::Transform & pose);
void setVisible(bool visible) {visible_=visible;}
void setGain(float gain) {gain_ = gain;}

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

@@ -512,7 +512,6 @@ void Scene::addCloud(
void Scene::addMesh(
int id,
const Mesh & mesh,
const cv::Mat & texture,
const rtabmap::Transform & pose)
{
LOGI("add mesh %d", id);
@@ -528,8 +527,7 @@ void Scene::addMesh(
PointCloudDrawable * drawable = new PointCloudDrawable(
cloud_shader_program_,
texture_mesh_shader_program_,
mesh,
texture);
mesh);
drawable->setPose(pose);
pointClouds_.insert(std::make_pair(id, drawable));
}
@@ -583,12 +581,12 @@ void Scene::updateCloudPolygons(int id, const std::vector<pcl::Vertices> & polyg
}
}
void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture)
void Scene::updateMesh(int id, const Mesh & mesh)
{
std::map<int, PointCloudDrawable*>::iterator iter=pointClouds_.find(id);
if(iter != pointClouds_.end())
{
iter->second->updateMesh(mesh, texture);
iter->second->updateMesh(mesh);
}
}

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

@@ -107,7 +107,6 @@ class Scene {
void addMesh(
int id,
const Mesh & mesh,
const cv::Mat & texture,
const rtabmap::Transform & pose);
void setCloudPose(int id, const rtabmap::Transform & pose);
@@ -117,7 +116,7 @@ class Scene {
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, const cv::Mat & texture);
void updateMesh(int id, const Mesh & mesh);
void updateGain(int id, float gain);
void setMapRendering(bool enabled) {mapRendering_ = enabled;}

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

@@ -163,6 +163,7 @@ public:
rtabmap::CameraModel cameraModel;
float gain;
std::vector<Eigen::Vector2f> texCoords;
cv::Mat texture;
};
#endif /* UTIL_H_ */