From f7616f6e941652180b4ab3e98175b8c234d35814 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 5 Feb 2017 17:24:45 -0500 Subject: [PATCH] Tango: update to 0.11.14 --- app/android/AndroidManifest.xml.in | 3 +- app/android/jni/CameraTango.cpp | 17 +- app/android/jni/CameraTango.h | 4 +- app/android/jni/RTABMapApp.cpp | 2283 +++++++++-------- app/android/jni/RTABMapApp.h | 10 +- app/android/jni/jni_interface.cpp | 24 +- app/android/jni/point_cloud_drawable.h | 1 + app/android/jni/scene.cpp | 126 +- app/android/jni/scene.h | 9 + .../jni/tango-gl/include/tango-gl/util.h | 28 + app/android/jni/tango-gl/util.cpp | 40 + app/android/res/layout/activity_rtabmap.xml | 82 +- app/android/res/layout/activity_settings.xml | 351 +-- app/android/res/menu/optionmenu.xml | 3 +- app/android/res/values/strings.xml | 170 +- .../com/introlab/rtabmap/RTABMapActivity.java | 562 ++-- .../src/com/introlab/rtabmap/RTABMapLib.java | 8 +- .../src/com/introlab/rtabmap/Renderer.java | 46 +- .../introlab/rtabmap/SettingsActivity.java | 12 +- corelib/include/rtabmap/core/Parameters.h | 10 +- corelib/src/DBDriverSqlite3.cpp | 18 +- corelib/src/Features2d.cpp | 17 +- corelib/src/FlannIndex.cpp | 4 + corelib/src/GainCompensator.cpp | 4 +- corelib/src/Memory.cpp | 12 +- corelib/src/VWDictionary.cpp | 11 +- corelib/src/util2d.cpp | 4 +- 27 files changed, 2284 insertions(+), 1575 deletions(-) diff --git a/app/android/AndroidManifest.xml.in b/app/android/AndroidManifest.xml.in index e6c454b7..8b655486 100644 --- a/app/android/AndroidManifest.xml.in +++ b/app/android/AndroidManifest.xml.in @@ -29,7 +29,8 @@ + android:screenOrientation="landscape" + android:configChanges="orientation|screenSize|keyboardHidden"> diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index b40915e9..94de5816 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -94,13 +94,14 @@ void onTangoEventAvailableRouter(void* context, const TangoEvent* event) ////////////////////////////// // CameraTango ////////////////////////////// -CameraTango::CameraTango(int decimation, bool autoExposure) : +CameraTango::CameraTango(int decimation, bool autoExposure, bool publishRawScan) : Camera(0), tango_config_(0), firstFrame_(true), stampEpochOffset_(0.0), decimation_(decimation), autoExposure_(autoExposure), + rawScanPublished_(publishRawScan), cloudStamp_(0), tangoColorType_(0), tangoColorStamp_(0) @@ -577,18 +578,19 @@ SensorData CameraTango::captureImage(CameraInfo * info) // The Color Camera frame at timestamp t0 with respect to Depth // Camera frame at timestamp t1. //LOGD("colorToDepth=%s", colorToDepth.prettyPrint().c_str()); + LOGD("rgb=%dx%d cloud size=%d", rgb.cols, rgb.rows, (int)cloud.total()); int pixelsSet = 0; depth = cv::Mat::zeros(model_.imageHeight()/8, model_.imageWidth()/8, CV_16UC1); // mm CameraModel depthModel = model_.scaled(1.0f/8.0f); - std::vector scanData(cloud.total()); + std::vector scanData(rawScanPublished_?cloud.total():0); int oi=0; for(unsigned int i=0; i(0,i); cv::Point3f pt = util3d::transformPoint(cv::Point3f(p[0], p[1], p[2]), colorToDepth); - if(pt.z > 0.0f && i%scanDownsampling == 0) + if(pt.z > 0.0f && i%scanDownsampling == 0 && rawScanPublished_) { scanData.at(oi++) = pt; } @@ -639,7 +641,14 @@ SensorData CameraTango::captureImage(CameraInfo * info) //LOGD("rtabmap = %s", odom.prettyPrint().c_str()); //LOGD("opengl(r)= %s", (opengl_world_T_rtabmap_world * odom * rtabmap_device_T_opengl_device).prettyPrint().c_str()); - data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp); + if(rawScanPublished_) + { + data = SensorData(scan, LaserScanInfo(cloud.total()/scanDownsampling, 0, model.localTransform()), rgb, depth, model, this->getNextSeqID(), rgbStamp); + } + else + { + data = SensorData(rgb, depth, model, this->getNextSeqID(), rgbStamp); + } data.setGroundTruth(odom); } else diff --git a/app/android/jni/CameraTango.h b/app/android/jni/CameraTango.h index 44253e71..9761a283 100644 --- a/app/android/jni/CameraTango.h +++ b/app/android/jni/CameraTango.h @@ -70,7 +70,7 @@ private: class CameraTango : public Camera, public UThread, public UEventsSender { public: - CameraTango(int decimation, bool autoExposure); + CameraTango(int decimation, bool autoExposure, bool publishRawScan); virtual ~CameraTango(); virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); @@ -81,6 +81,7 @@ public: rtabmap::Transform tangoPoseToTransform(const TangoPoseData * tangoPose) const; void setDecimation(int value) {decimation_ = value;} void setAutoExposure(bool enabled) {autoExposure_ = enabled;} + void setRawScanPublished(bool enabled) {rawScanPublished_ = enabled;} void cloudReceived(const cv::Mat & cloud, double timestamp); void rgbReceived(const cv::Mat & tangoImage, int type, double timestamp); @@ -103,6 +104,7 @@ private: double stampEpochOffset_; int decimation_; bool autoExposure_; + bool rawScanPublished_; cv::Mat cloud_; double cloudStamp_; cv::Mat tangoColor_; diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index cd84e6b8..2c1118dd 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -81,7 +81,6 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1")); @@ -95,8 +94,24 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"2":"0"))); + if(parameters.find(rtabmap::Parameters::kKpMaxFeatures())!=parameters.end() && + parameters.find(rtabmap::Parameters::kVisMaxFeatures())!=parameters.end()) + { + int featuresVoc = uStr2Int(parameters.at(rtabmap::Parameters::kKpMaxFeatures())); + int featuresLoop = uStr2Int(parameters.at(rtabmap::Parameters::kVisMaxFeatures())); + if(featuresVoc==0 || featuresLoop < featuresVoc) + { + // re-use already extracted descriptors + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), std::string("true"))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("false"))); + } + else + { + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemRawDescriptorsKept(), std::string("false"))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLoopClosureReextractFeatures(), std::string("true"))); + } + } + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("10"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10"))); @@ -114,6 +129,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1"))); uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0"))); // deactivate rehearsal uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels + uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true"))); } return parameters; @@ -127,10 +143,10 @@ RTABMapApp::RTABMapApp() : odomCloudShown_(true), graphOptimization_(true), nodesFiltering_(false), - driftCorrection_(false), localizationMode_(false), trajectoryMode_(false), autoExposure_(true), + rawScanSaved_(false), fullResolution_(false), appendMode_(true), maxCloudDepth_(0.0), @@ -142,6 +158,7 @@ RTABMapApp::RTABMapApp() : paused_(false), dataRecorderMode_(false), clearSceneOnNextRender_(false), + optimizeOpenedDatabase_(true), filterPolygonsOnNextRender_(false), gainCompensationOnNextRender_(0), bilateralFilteringOnNextRender_(false), @@ -208,10 +225,16 @@ void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity) this->registerToEventsManager(); - camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_); + camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_, rawScanSaved_); } -void RTABMapApp::openDatabase(const std::string & databasePath) +void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation) +{ + LOGI("Set orientation: display=%d camera=%d", displayRotation, cameraRotation); + main_scene_.setScreenRotation(displayRotation, cameraRotation); +} + +void RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize) { this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap status_.first = rtabmap::RtabmapEventInit::kInitializing; @@ -228,6 +251,7 @@ void RTABMapApp::openDatabase(const std::string & databasePath) rtabmap_ = new rtabmap::Rtabmap(); rtabmap::ParametersMap parameters = getRtabmapParameters(); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory))); rtabmap_->init(parameters, databasePath); rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_); if(parameters.find(rtabmap::Parameters::kRtabmapDetectionRate()) != parameters.end()) @@ -246,6 +270,7 @@ void RTABMapApp::openDatabase(const std::string & databasePath) true, true); + optimizeOpenedDatabase_ = optimize; clearSceneOnNextRender_ = true; rtabmap::Statistics stats; stats.setSignatures(signatures); @@ -277,7 +302,7 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder) camera_->join(true); if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) { - LOGE("TangoHandler::ConnectTango, TangoService_setBinder error"); + UERROR("TangoHandler::ConnectTango, TangoService_setBinder error"); return false; } @@ -291,7 +316,7 @@ bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder) cameraJustInitialized_ = true; return true; } - LOGE("Failed camera initialization!"); + UERROR("Failed camera initialization!"); } return false; } @@ -399,7 +424,7 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh) } else { - LOGE("smoothMesh() Failed to smooth surface %d", id); + UERROR("smoothMesh() Failed to smooth surface %d", id); return false; } return true; @@ -408,519 +433,560 @@ bool RTABMapApp::smoothMesh(int id, Mesh & mesh) // OpenGL thread int RTABMapApp::Render() { - UASSERT(camera_!=0 && rtabmap_!=0); - - UTimer fpsTime; - boost::mutex::scoped_lock lock(renderingMutex_); - - bool notifyCameraStarted = false; - - // process only pose events in vsualization mode - rtabmap::Transform pose; + try { - boost::mutex::scoped_lock lock(poseMutex_); - if(poseEvents_.size()) + UASSERT(camera_!=0 && rtabmap_!=0); + + UTimer fpsTime; + boost::mutex::scoped_lock lock(renderingMutex_); + + bool notifyCameraStarted = false; + + // process only pose events in vsualization mode + rtabmap::Transform pose; { - pose = poseEvents_.back(); - poseEvents_.clear(); + boost::mutex::scoped_lock lock(poseMutex_); + if(poseEvents_.size()) + { + pose = poseEvents_.back(); + poseEvents_.clear(); + } } - } - if(!pose.isNull()) - { - // update camera pose? - main_scene_.SetCameraPose(opengl_world_T_tango_world*pose); - if(!camera_->isRunning() && cameraJustInitialized_) + if(!pose.isNull()) { - notifyCameraStarted = true; - cameraJustInitialized_ = false; - } - } - - rtabmap::OdometryEvent odomEvent; - { - boost::mutex::scoped_lock lock(odomMutex_); - if(odomEvents_.size()) - { - LOGI("Process odom events"); - odomEvent = odomEvents_.back(); - odomEvents_.clear(); - if(cameraJustInitialized_) + // update camera pose? + main_scene_.SetCameraPose(opengl_world_T_tango_world*pose); + if(!camera_->isRunning() && cameraJustInitialized_) { notifyCameraStarted = true; cameraJustInitialized_ = false; } } - } - if(clearSceneOnNextRender_) - { - visualizingMesh_ = false; - } - - if(visualizingMesh_) - { - if(exportedMeshUpdated_) + rtabmap::OdometryEvent odomEvent; { - main_scene_.clear(); - exportedMeshUpdated_ = false; - } - if(!main_scene_.hasCloud(g_exportedMeshId)) - { - if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size()) + boost::mutex::scoped_lock lock(odomMutex_); + if(odomEvents_.size()) { - Mesh mesh; - mesh.gain = 1.0f; - mesh.cloud.reset(new pcl::PointCloud); - mesh.normals.reset(new pcl::PointCloud); - 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()) + LOGI("Process odom events"); + odomEvent = odomEvents_.back(); + odomEvents_.clear(); + if(cameraJustInitialized_) { - mesh.texCoords = exportedMesh_->tex_coordinates[0]; - texture = exportedTexture_; - } - - main_scene_.addMesh(g_exportedMeshId, mesh, texture, opengl_world_T_rtabmap_world); - } - else - { - pcl::IndicesPtr indices(new std::vector); // null - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(exportedMesh_->cloud, *cloud); - main_scene_.addCloud(g_exportedMeshId, cloud, indices, opengl_world_T_rtabmap_world); - } - } - - //backup state - bool isMeshRendering = main_scene_.isMeshRendering(); - bool isTextureRendering = main_scene_.isMeshTexturing(); - bool isFrustumCulling = main_scene_.isFrustumCulling(); - - main_scene_.setMeshRendering(main_scene_.hasMesh(g_exportedMeshId), main_scene_.hasTexture(g_exportedMeshId)); - main_scene_.setFrustumCulling(false); - - main_scene_.Render(); - - // revert state - main_scene_.setMeshRendering(isMeshRendering, isTextureRendering); - main_scene_.setFrustumCulling(isFrustumCulling); - - if(renderingTime_ < fpsTime.elapsed()) - { - renderingTime_ = fpsTime.elapsed(); - } - - return notifyCameraStarted; - } - else - { - if(main_scene_.hasCloud(g_exportedMeshId)) - { - main_scene_.clear(); - exportedMesh_.reset(new pcl::TextureMesh); - exportedTexture_ = cv::Mat(); - } - - bool notifyDataLoaded = false; - // should be before clearSceneOnNextRender_ in case openDatabase is called - std::list rtabmapEvents; - { - boost::mutex::scoped_lock lock(rtabmapMutex_); - rtabmapEvents = rtabmapEvents_; - rtabmapEvents_.clear(); - - boost::mutex::scoped_lock lockMesh(meshesMutex_); - if(!clearSceneOnNextRender_ && rtabmapEvents.size() && createdMeshes_.size()) - { - 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; + notifyCameraStarted = true; + cameraJustInitialized_ = false; } } } if(clearSceneOnNextRender_) { - boost::mutex::scoped_lock lock(meshesMutex_); - - odomMutex_.lock(); - odomEvents_.clear(); - odomMutex_.unlock(); - - poseMutex_.lock(); - poseEvents_.clear(); - poseMutex_.unlock(); - - main_scene_.clear(); - clearSceneOnNextRender_ = false; - createdMeshes_.clear(); - rawPoses_.clear(); - totalPoints_ = 0; - totalPolygons_ = 0; - lastDrawnCloudsCount_ = 0; - renderingTime_ = 0.0f; + visualizingMesh_ = false; } - // Did we lose OpenGL context? If so, recreate the context; - std::set added = main_scene_.getAddedClouds(); - added.erase(-1); + if(visualizingMesh_) { - boost::mutex::scoped_lock lock(meshesMutex_); - if(added.size() != createdMeshes_.size()) + if(exportedMeshUpdated_) { - for(std::map::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) - { - if(!main_scene_.hasCloud(iter->first)) - { - 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); - } - } + main_scene_.clear(); + exportedMeshUpdated_ = false; } - } - - if(rtabmapEvents.size()) - { - LOGI("Process rtabmap events"); - - // update buffered signatures - std::map bufferedSensorData; - if(!trajectoryMode_ && !dataRecorderMode_) + if(!main_scene_.hasCloud(g_exportedMeshId)) { - for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) + if(exportedMesh_->tex_polygons.size() && exportedMesh_->tex_polygons[0].size()) { - for(std::map::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter) + Mesh mesh; + mesh.gain = 1.0f; + mesh.cloud.reset(new pcl::PointCloud); + mesh.normals.reset(new pcl::PointCloud); + 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()) { - if(!jter->second.sensorData().imageRaw().empty() && - !jter->second.sensorData().depthRaw().empty()) - { - uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData())); - uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); - } - else if(totalPoints_ == 0 && - !jter->second.sensorData().imageCompressed().empty() && - !jter->second.sensorData().depthOrRightCompressed().empty()) - { - // uncompress - rtabmap::SensorData data = jter->second.sensorData(); - cv::Mat tmpA,depth; - data.uncompressData(&tmpA, &depth); - - // do post-processing bilateral filtering now - UTimer t; - depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f); - data.setDepthOrRightRaw(depth); - LOGI("Bilateral filtering of %d, time=%fs", jter->first, t.ticks()); - - uInsert(bufferedSensorData, std::make_pair(jter->first, data)); - uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); - - LOGI("Detecting that we are loading a database, so do some post-processing..."); - notifyDataLoaded = true; - } + mesh.texCoords = exportedMesh_->tex_coordinates[0]; + texture = exportedTexture_; } - } - } - std::map poses = rtabmapEvents.back().poses(); - - // Transform pose in OpenGL world - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - if(!graphOptimization_) - { - std::map::iterator jter = rawPoses_.find(iter->first); - if(jter != rawPoses_.end()) - { - iter->second = opengl_world_T_rtabmap_world*jter->second; - } + main_scene_.addMesh(g_exportedMeshId, mesh, texture, opengl_world_T_rtabmap_world); } else { - iter->second = opengl_world_T_rtabmap_world*iter->second; + pcl::IndicesPtr indices(new std::vector); // null + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(exportedMesh_->cloud, *cloud); + main_scene_.addCloud(g_exportedMeshId, cloud, indices, opengl_world_T_rtabmap_world); } } - const std::multimap & links = rtabmapEvents.back().constraints(); - if(poses.size()) + //backup state + bool isMeshRendering = main_scene_.isMeshRendering(); + bool isTextureRendering = main_scene_.isMeshTexturing(); + bool isFrustumCulling = main_scene_.isFrustumCulling(); + + main_scene_.setMeshRendering(main_scene_.hasMesh(g_exportedMeshId), main_scene_.hasTexture(g_exportedMeshId)); + main_scene_.setFrustumCulling(false); + + main_scene_.Render(); + + // revert state + main_scene_.setMeshRendering(isMeshRendering, isTextureRendering); + main_scene_.setFrustumCulling(isFrustumCulling); + + if(renderingTime_ < fpsTime.elapsed()) { - //update graph - main_scene_.updateGraph(poses, links); - - // update clouds - boost::mutex::scoped_lock lock(meshesMutex_); - std::set strIds; - for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) - { - int id = iter->first; - if(!iter->second.isNull()) - { - if(main_scene_.hasCloud(id)) - { - //just update pose - main_scene_.setCloudPose(id, iter->second); - main_scene_.setCloudVisible(id, true); - std::map::iterator meshIter = createdMeshes_.find(id); - UASSERT(meshIter!=createdMeshes_.end()); - meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second; - meshIter->second.visible = true; - } - else if(uContains(bufferedSensorData, id)) - { - rtabmap::SensorData & data = bufferedSensorData.at(id); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) - { - // Voxelize and filter depending on the previous cloud? - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr indices(new std::vector); - 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(cloud->size() && indices->size()) - { - UTimer time; - std::vector 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()); - } - - if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering()) - { - totalPolygons_ += polygons.size(); - - std::pair::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; - - main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second); - } - else - { - LOGE("No mesh could be created for node %d", id); - } - } - totalPoints_+=indices->size(); - } - } - } - } + renderingTime_ = fpsTime.elapsed(); } - //filter poses? - if(poses.size() > 2) - { - if(nodesFiltering_) - { - for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) - { - if(iter->second.type() != rtabmap::Link::kNeighbor) - { - int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to(); - poses.erase(oldId); - } - } - } - } - - if(poses.size()) - { - //update cloud visibility - boost::mutex::scoped_lock lock(meshesMutex_); - std::set addedClouds = main_scene_.getAddedClouds(); - for(std::set::const_iterator iter=addedClouds.begin(); - iter!=addedClouds.end(); - ++iter) - { - if(*iter > 0 && poses.find(*iter) == poses.end()) - { - main_scene_.setCloudVisible(*iter, false); - std::map::iterator meshIter = createdMeshes_.find(*iter); - UASSERT(meshIter!=createdMeshes_.end()); - meshIter->second.visible = false; - } - } - } + return notifyCameraStarted; } else { - main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_); - - //just process the last one - if(!odomEvent.pose().isNull()) + if(main_scene_.hasCloud(g_exportedMeshId)) { - if(odomCloudShown_ && !trajectoryMode_) + main_scene_.clear(); + exportedMesh_.reset(new pcl::TextureMesh); + exportedTexture_ = cv::Mat(); + } + + bool notifyDataLoaded = false; + // should be before clearSceneOnNextRender_ in case openDatabase is called + std::list rtabmapEvents; + { + boost::mutex::scoped_lock lock(rtabmapMutex_); + rtabmapEvents = rtabmapEvents_; + rtabmapEvents_.clear(); + + boost::mutex::scoped_lock lockMesh(meshesMutex_); + if(!clearSceneOnNextRender_ && rtabmapEvents.size() && createdMeshes_.size()) { - if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) + if(rtabmapEvents.front().refImageId()>0 && rtabmapEvents.front().refImageId() < createdMeshes_.rbegin()->first) { - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr indices(new std::vector); - cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get()); - if(cloud->size() && indices->size()) + LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front().refImageId(), createdMeshes_.rbegin()->first); + clearSceneOnNextRender_ = true; + } + } + } + + if(clearSceneOnNextRender_) + { + boost::mutex::scoped_lock lock(meshesMutex_); + + odomMutex_.lock(); + odomEvents_.clear(); + odomMutex_.unlock(); + + poseMutex_.lock(); + poseEvents_.clear(); + poseMutex_.unlock(); + + main_scene_.clear(); + clearSceneOnNextRender_ = false; + createdMeshes_.clear(); + rawPoses_.clear(); + totalPoints_ = 0; + totalPolygons_ = 0; + lastDrawnCloudsCount_ = 0; + renderingTime_ = 0.0f; + } + + // Did we lose OpenGL context? If so, recreate the context; + std::set added = main_scene_.getAddedClouds(); + added.erase(-1); + { + boost::mutex::scoped_lock lock(meshesMutex_); + if(added.size() != createdMeshes_.size()) + { + for(std::map::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) + { + if(!main_scene_.hasCloud(iter->first)) { - 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, indices, opengl_world_T_rtabmap_world*odomEvent.pose()); - main_scene_.setCloudVisible(-1, true); + 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()) + { + 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); + } + } + } + } + + if(rtabmapEvents.size()) + { + LOGI("Process rtabmap events"); + + // update buffered signatures + std::map bufferedSensorData; + if(!trajectoryMode_ && !dataRecorderMode_) + { + for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) + { + // Don't create mesh for the last node added if rehearsal happened or if discarded (small movement) + int smallMovement = (int)uValue(iter->data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f); + int rehearsalMerged = (int)uValue(iter->data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f); + if(smallMovement == 0 && rehearsalMerged == 0) + { + for(std::map::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter) + { + if(!jter->second.sensorData().imageRaw().empty() && + !jter->second.sensorData().depthRaw().empty()) + { + uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData())); + uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); + } + else if(totalPoints_ == 0 && + !jter->second.sensorData().imageCompressed().empty() && + !jter->second.sensorData().depthOrRightCompressed().empty()) + { + uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData())); + uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); + if(notifyDataLoaded == false) + { + LOGI("Detecting that we are loading a database"); + } + notifyDataLoaded = true; + } + } + } + + int loopClosure = (int)uValue(iter->data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f); + int rejected = (int)uValue(iter->data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f); + if(!paused_ && loopClosure>0) + { + main_scene_.setBackgroundColor(0, 0.7f, 0); // green + } + else if(!paused_ && rejected>0) + { + main_scene_.setBackgroundColor(0, 0.1f, 0); // dark green + } + else if(!paused_ && rehearsalMerged>0) + { + main_scene_.setBackgroundColor(0, 0, 0.2f); // blue } else { - LOGE("Generated cloud is empty!"); + main_scene_.setBackgroundColor(0, 0, 0); + } + } + } + + std::map poses = rtabmapEvents.back().poses(); + + // Transform pose in OpenGL world + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) + { + if(!graphOptimization_) + { + std::map::iterator jter = rawPoses_.find(iter->first); + if(jter != rawPoses_.end()) + { + iter->second = opengl_world_T_rtabmap_world*jter->second; } } else { - LOGE("Odom data images are empty!"); - } - } - } - } - - if(notifyDataLoaded || gainCompensationOnNextRender_>0) - { - UTimer tGainCompensation; - LOGI("Gain compensation..."); - boost::mutex::scoped_lock lock(meshesMutex_); - - std::map::Ptr > clouds; - std::map indices; - for(std::map::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 poses; - std::multimap links; - rtabmap_->getGraph(poses, links, true, true); - if(gainCompensationOnNextRender_ == 2) - { - // full compensation - links.clear(); - for(std::map::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) - { - int from = iter->first; - std::map::Ptr>::const_iterator jter = iter; - ++jter; - for(;jter!=clouds.end(); ++jter) - { - int to = jter->first; - links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to)))); - } - } - } - - UASSERT(maxGainRadius_>0.0f); - rtabmap::GainCompensator compensator(maxGainRadius_); - if(clouds.size() > 1 && links.size()) - { - compensator.feed(clouds, indices, links); - LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks()); - } - - for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) - { - if(!iter->second.cloud->empty()) - { - if(clouds.size() > 1 && links.size()) - { - iter->second.gain = compensator.getGain(iter->first); + iter->second = opengl_world_T_rtabmap_world*iter->second; } } - main_scene_.updateMesh(iter->first, iter->second, cv::Mat()); - } - LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks()); - - gainCompensationOnNextRender_ = 0; - notifyDataLoaded = true; - } - - if(bilateralFilteringOnNextRender_) - { - LOGI("Bilateral filtering..."); - bilateralFilteringOnNextRender_ = false; - boost::mutex::scoped_lock lock(meshesMutex_); - for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) - { - if(iter->second.cloud->size() && iter->second.indices->size()) + const std::multimap & links = rtabmapEvents.back().constraints(); + if(poses.size()) { - if(smoothMesh(iter->first, iter->second)) - { - main_scene_.updateMesh(iter->first, iter->second, cv::Mat()); - } - } - } - notifyDataLoaded = true; - } + //update graph + main_scene_.updateGraph(poses, links); - if(filterPolygonsOnNextRender_ && minClusterSize_>0) - { - LOGI("Polygon filtering..."); - filterPolygonsOnNextRender_ = false; - boost::mutex::scoped_lock lock(meshesMutex_); - for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) - { - if(iter->second.polygons.size()) - { - // filter polygons - std::vector > neighbors; - std::vector > vertexToPolygons; - rtabmap::util3d::createPolygonIndexes( - iter->second.polygons, - iter->second.cloud->size(), - neighbors, - vertexToPolygons); - std::list > clusters = rtabmap::util3d::clusterPolygons( - neighbors, - minClusterSize_); - std::vector filteredPolygons(iter->second.polygons.size()); - int oi=0; - for(std::list >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter) + // update clouds + boost::mutex::scoped_lock lock(meshesMutex_); + std::set strIds; + for(std::map::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { - for(std::list::iterator kter=jter->begin(); kter!=jter->end(); ++kter) + int id = iter->first; + if(!iter->second.isNull()) { - filteredPolygons[oi++] = iter->second.polygons.at(*kter); + if(main_scene_.hasCloud(id)) + { + //just update pose + main_scene_.setCloudPose(id, iter->second); + main_scene_.setCloudVisible(id, true); + std::map::iterator meshIter = createdMeshes_.find(id); + UASSERT(meshIter!=createdMeshes_.end()); + meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second; + meshIter->second.visible = true; + } + else if(uContains(bufferedSensorData, id)) + { + rtabmap::SensorData data = bufferedSensorData.at(id); + + cv::Mat tmpA, depth; + data.uncompressData(&tmpA, &depth); + + if(notifyDataLoaded && optimizeOpenedDatabase_) + { + // do post-processing bilateral filtering now + UTimer t; + depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f); + data.setDepthOrRightRaw(depth); + LOGI("Bilateral filtering of %d, time=%fs", id, t.ticks()); + } + + if(!data.imageRaw().empty() && !data.depthRaw().empty()) + { + // Voxelize and filter depending on the previous cloud? + pcl::PointCloud::Ptr cloud; + pcl::IndicesPtr indices(new std::vector); + 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(cloud->size() && indices->size()) + { + UTimer time; + std::vector 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()); + } + + if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering()) + { + totalPolygons_ += polygons.size(); + + std::pair::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; + main_scene_.addMesh(id, inserted.first->second, main_scene_.isMeshTexturing()?data.imageRaw():cv::Mat(), iter->second); + } + else + { + UERROR("No mesh could be created for node %d", id); + } + } + totalPoints_+=indices->size(); + } + } + } + } + } + + //filter poses? + if(poses.size() > 2) + { + if(nodesFiltering_) + { + for(std::multimap::const_iterator iter=links.begin(); iter!=links.end(); ++iter) + { + if(iter->second.type() != rtabmap::Link::kNeighbor) + { + int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to(); + poses.erase(oldId); + } + } + } + } + + if(poses.size()) + { + //update cloud visibility + boost::mutex::scoped_lock lock(meshesMutex_); + std::set addedClouds = main_scene_.getAddedClouds(); + for(std::set::const_iterator iter=addedClouds.begin(); + iter!=addedClouds.end(); + ++iter) + { + if(*iter > 0 && poses.find(*iter) == poses.end()) + { + main_scene_.setCloudVisible(*iter, false); + std::map::iterator meshIter = createdMeshes_.find(*iter); + UASSERT(meshIter!=createdMeshes_.end()); + meshIter->second.visible = false; } } - filteredPolygons.resize(oi); - iter->second.polygons = filteredPolygons; - main_scene_.updateCloudPolygons(iter->first, iter->second.polygons); } } - notifyDataLoaded = true; - } + else + { + main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_); - lastDrawnCloudsCount_ = main_scene_.Render(); - if(renderingTime_ < fpsTime.elapsed()) - { - renderingTime_ = fpsTime.elapsed(); - } + //just process the last one + if(!odomEvent.pose().isNull()) + { + if(odomCloudShown_ && !trajectoryMode_) + { + if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty()) + { + pcl::PointCloud::Ptr cloud; + pcl::IndicesPtr indices(new std::vector); + cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, 0.0f, indices.get()); + if(cloud->size() && indices->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, indices, opengl_world_T_rtabmap_world*odomEvent.pose()); + main_scene_.setCloudVisible(-1, true); + } + else + { + UERROR("Generated cloud is empty!"); + } + } + else + { + UERROR("Odom data images are empty!"); + } + } + } + } - if(rtabmapEvents.size()) - { - // send statistics to GUI - LOGI("Posting PostRenderEvent!"); - UEventsManager::post(new PostRenderEvent(rtabmapEvents.back())); - } + if((notifyDataLoaded&&optimizeOpenedDatabase_) || gainCompensationOnNextRender_>0) + { + UTimer tGainCompensation; + LOGI("Gain compensation..."); + boost::mutex::scoped_lock lock(meshesMutex_); - return notifyDataLoaded||notifyCameraStarted?1:0; + std::map::Ptr > clouds; + std::map indices; + for(std::map::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 poses; + std::multimap links; + rtabmap_->getGraph(poses, links, true, true); + if(gainCompensationOnNextRender_ == 2) + { + // full compensation + links.clear(); + for(std::map::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter) + { + int from = iter->first; + std::map::Ptr>::const_iterator jter = iter; + ++jter; + for(;jter!=clouds.end(); ++jter) + { + int to = jter->first; + links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to)))); + } + } + } + + UASSERT(maxGainRadius_>0.0f); + rtabmap::GainCompensator compensator(maxGainRadius_); + if(clouds.size() > 1 && links.size()) + { + compensator.feed(clouds, indices, links); + LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks()); + } + + for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) + { + if(!iter->second.cloud->empty()) + { + if(clouds.size() > 1 && links.size()) + { + iter->second.gain = compensator.getGain(iter->first); + LOGI("%d mesh has gain %f", iter->first, iter->second.gain); + } + } + + main_scene_.updateGain(iter->first, iter->second.gain); + } + LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks()); + + gainCompensationOnNextRender_ = 0; + notifyDataLoaded = true; + } + + if(bilateralFilteringOnNextRender_) + { + LOGI("Bilateral filtering..."); + bilateralFilteringOnNextRender_ = false; + boost::mutex::scoped_lock lock(meshesMutex_); + for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) + { + if(iter->second.cloud->size() && iter->second.indices->size()) + { + if(smoothMesh(iter->first, iter->second)) + { + main_scene_.updateMesh(iter->first, iter->second, cv::Mat()); + } + } + } + notifyDataLoaded = true; + } + + if(filterPolygonsOnNextRender_ && minClusterSize_>0) + { + LOGI("Polygon filtering..."); + filterPolygonsOnNextRender_ = false; + boost::mutex::scoped_lock lock(meshesMutex_); + for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) + { + if(iter->second.polygons.size()) + { + // filter polygons + std::vector > neighbors; + std::vector > vertexToPolygons; + rtabmap::util3d::createPolygonIndexes( + iter->second.polygons, + iter->second.cloud->size(), + neighbors, + vertexToPolygons); + std::list > clusters = rtabmap::util3d::clusterPolygons( + neighbors, + minClusterSize_); + std::vector filteredPolygons(iter->second.polygons.size()); + int oi=0; + for(std::list >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter) + { + for(std::list::iterator kter=jter->begin(); kter!=jter->end(); ++kter) + { + filteredPolygons[oi++] = iter->second.polygons.at(*kter); + } + } + filteredPolygons.resize(oi); + iter->second.polygons = filteredPolygons; + main_scene_.updateCloudPolygons(iter->first, iter->second.polygons); + } + } + notifyDataLoaded = true; + } + + lastDrawnCloudsCount_ = main_scene_.Render(); + if(renderingTime_ < fpsTime.elapsed()) + { + renderingTime_ = fpsTime.elapsed(); + } + + if(rtabmapEvents.size()) + { + // send statistics to GUI + LOGI("Posting PostRenderEvent!"); + UEventsManager::post(new PostRenderEvent(rtabmapEvents.back())); + } + + return notifyDataLoaded||notifyCameraStarted?1:0; + } + } + catch(const std::exception & e) + { + UERROR("Exception! msg=\"%s\"", e.what()); + return -1; } } @@ -940,6 +1006,7 @@ void RTABMapApp::setPausedMapping(bool paused) { boost::mutex::scoped_lock lock(renderingMutex_); visualizingMesh_ = false; + main_scene_.setBackgroundColor(0, 0, 0); } paused_ = paused; if(camera_) @@ -1018,14 +1085,6 @@ void RTABMapApp::setNodesFiltering(bool enabled) nodesFiltering_ = enabled; setGraphOptimization(graphOptimization_); // this will resend the graph if paused } -void RTABMapApp::setDriftCorrection(bool enabled) -{ - driftCorrection_ = enabled; - rtabmap::ParametersMap parameters; - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0"))); - this->post(new rtabmap::ParamEvent(parameters)); -} void RTABMapApp::setGraphVisible(bool visible) { main_scene_.setGraphVisible(visible); @@ -1048,6 +1107,18 @@ void RTABMapApp::setAutoExposure(bool enabled) } } +void RTABMapApp::setRawScanSaved(bool enabled) +{ + if(rawScanSaved_ != enabled) + { + rawScanSaved_ = enabled; + if(camera_) + { + camera_->setRawScanPublished(rawScanSaved_); + } + } +} + void RTABMapApp::setFullResolution(bool enabled) { if(fullResolution_ != enabled) @@ -1096,34 +1167,50 @@ void RTABMapApp::setMeshDecimation(int value) // Google Tango Tablet 160x90 // Phab2Pro 240x135 int width = camera_->getCameraModel().imageWidth()/8; - if(value == 2) // high + int height = camera_->getCameraModel().imageHeight()/8; + if(value == 3) // high { - if(width % 10 == 0) + if(width % 10 == 0 && height % 10 == 0) { meshDecimation_ = 10; } - else if(width % 15 == 0) + else if(width % 15 == 0 && height % 15 == 0) { meshDecimation_ = 15; } else { - LOGE("Could not set decimation to high (width=%d)", width); + UERROR("Could not set decimation to high (size=%dx%d)", width, height); } } - else if(value == 1) // medium + else if(value == 2) // medium { - if(width % 5 == 0) + if(width % 5 == 0 && height % 5 == 0) { meshDecimation_ = 5; } else { - LOGE("Could not set decimation to medium (width=%d)", width); + UERROR("Could not set decimation to medium (size=%dx%d)", width, height); + } + } + else if(value == 1) // low + { + if(width % 3 == 0 && width % 3 == 0) + { + meshDecimation_ = 3; + } + else if(width % 2 == 0 && width % 2 == 0) + { + meshDecimation_ = 2; + } + else + { + UERROR("Could not set decimation to low (size=%dx%d)", width, height); } } } - LOGE("Set decimation to %d", meshDecimation_); + UINFO("Set decimation to %d", meshDecimation_); } void RTABMapApp::setMeshAngleTolerance(float value) @@ -1165,12 +1252,12 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string & { if(iter->second.second.empty()) { - LOGE("Parameter \"%s\" doesn't exist anymore!", + UERROR("Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { - LOGE("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", + UERROR("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } @@ -1191,7 +1278,7 @@ int RTABMapApp::setMappingParameter(const std::string & key, const std::string & } else { - LOGE(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str()); + UERROR(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str()); return -1; } } @@ -1347,6 +1434,7 @@ bool RTABMapApp::exportMesh( bool meshing, int textureSize, int normalK, + float maxTextureDistance, bool optimized, float optimizedVoxelSize, int optimizedDepth, @@ -1362,414 +1450,82 @@ bool RTABMapApp::exportMesh( if(blockRendering) { renderingMutex_.lock(); + main_scene_.clear(); } bool success = false; - std::map poses; - std::multimap links; - rtabmap_->getGraph(poses, links, true, true); - - //Assemble the meshes - if(meshing) // Mesh or Texture Mesh + try { - pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh); - pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); - cv::Mat globalTexture; - int totalPolygons = 0; + std::map poses; + std::multimap links; + rtabmap_->getGraph(poses, links, true, true); + + //Assemble the meshes + if(meshing) // Mesh or Texture Mesh { - if(optimized) + pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh); + pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); + cv::Mat globalTexture; + int totalPolygons = 0; { - std::map cameraPoses; - std::map cameraModels; - - UTimer timer; - LOGI("Assemble clouds (%d)...", (int)poses.size()); - pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); - for(std::map::iterator iter=poses.begin(); - iter!= poses.end(); - ++iter) + if(optimized) { - std::map::iterator jter = createdMeshes_.find(iter->first); - pcl::PointCloud::Ptr cloud; - pcl::IndicesPtr indices(new std::vector); - rtabmap::CameraModel model; - float gain = 1.0f; - if(jter != createdMeshes_.end()) + std::map cameraPoses; + std::map cameraModels; + + UTimer timer; + LOGI("Assemble clouds (%d)...", (int)poses.size()); + int cloudCount=0; + pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); + for(std::map::iterator iter=poses.begin(); + iter!= poses.end(); + ++iter) { - cloud = jter->second.cloud; - indices = jter->second.indices; - model = jter->second.cameraModel; - gain = jter->second.gain; - } - else - { - rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); - if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) + std::map::iterator jter = createdMeshes_.find(iter->first); + pcl::PointCloud::Ptr cloud; + pcl::IndicesPtr indices(new std::vector); + rtabmap::CameraModel model; + float gain = 1.0f; + if(jter != createdMeshes_.end()) { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); - model = data.cameraModels()[0]; - } - } - if(cloud->size() && indices->size() && model.isValidForProjection()) - { - pcl::PointCloud::Ptr transformedCloud(new pcl::PointCloud); - if(optimizedVoxelSize > 0.0f) - { - transformedCloud = rtabmap::util3d::voxelize(cloud, indices, optimizedVoxelSize); - transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); + cloud = jter->second.cloud; + indices = jter->second.indices; + model = jter->second.cameraModel; + gain = jter->second.gain; } else { - // it looks like that using only transformPointCloud with indices - // flushes the colors, so we should extract points before... maybe a too old PCL version - pcl::copyPointCloud(*cloud, *indices, *transformedCloud); - transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); - } - - Eigen::Vector3f viewpoint( iter->second.x(), iter->second.y(), iter->second.z()); - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, viewpoint); - - pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); - pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals); - - if(textureSize == 0 && gain != 1.0f) - { - for(unsigned int i=0; isize(); ++i) + rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); + if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) { - pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); - pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); - pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); - pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + model = data.cameraModels()[0]; } } - - if(mergedClouds->size() == 0) + if(cloud->size() && indices->size() && model.isValidForProjection()) { - *mergedClouds = *cloudWithNormals; - } - else - { - *mergedClouds += *cloudWithNormals; - } - - cameraPoses.insert(std::make_pair(iter->first, iter->second)); - cameraModels.insert(std::make_pair(iter->first, model)); - } - else - { - UERROR("Cloud %d not found or empty", iter->first); - } - } - LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size()); - - if(mergedClouds->size()) - { - int before = mergedClouds->size(); - if(optimizedVoxelSize > 0.0f) - { - mergedClouds = rtabmap::util3d::voxelize(mergedClouds, optimizedVoxelSize); - LOGI("Voxelized from %d points to %d points", before, (int)mergedClouds->size()); - } - - // Mesh reconstruction - LOGI("Mesh reconstruction..."); - pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); - pcl::Poisson poisson; - poisson.setDepth(optimizedDepth); - poisson.setInputCloud(mergedClouds); - poisson.reconstruct(*mesh); - LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size()); - - if(mesh->polygons.size()) - { - if(optimizedDecimationFactor > 0.0f) - { -#ifndef DISABLE_VTK - unsigned int count = mesh->polygons.size(); - LOGI("Mesh decimation (factor=%f) from %d polygons...", optimizedDecimationFactor, (int)count); - - pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh); - pcl::MeshQuadricDecimationVTK mqd; - mqd.setTargetReductionFactor(optimizedDecimationFactor); - mqd.setInputMesh(mesh); - mqd.process (*output); - mesh = output; - - //mesh = rtabmap::util3d::meshDecimation(mesh, decimationFactor); - // use direct instantiation above to this fix some linker errors on android like: - // pcl::MeshQuadricDecimationVTK::performProcessing(pcl::PolygonMesh&): error: undefined reference to 'vtkQuadricDecimation::New()' - // pcl::VTKUtils::mesh2vtk(pcl::PolygonMesh const&, vtkSmartPointer&): 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()); - if(count < mesh->polygons.size()) + pcl::PointCloud::Ptr transformedCloud(new pcl::PointCloud); + if(optimizedVoxelSize > 0.0f) { - UWARN("Decimated mesh has more polygons than before!"); - } -#else - UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!"); -#endif - } - - if(textureSize == 0) - { - // colored polygon mesh - if(optimizedColorRadius >= 0.0f) - { - LOGI("Transferring color from point cloud to mesh..."); - // transfer color from point cloud to mesh - pcl::search::KdTree::Ptr tree (new pcl::search::KdTree(true)); - tree->setInputCloud(mergedClouds); - pcl::PointCloud::Ptr coloredCloud(new pcl::PointCloud); - pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud); - std::vector coloredPts(coloredCloud->size()); - for(unsigned int i=0; isize(); ++i) - { - std::vector kIndices; - std::vector kDistances; - pcl::PointXYZRGBNormal pt; - pt.x = coloredCloud->at(i).x; - pt.y = coloredCloud->at(i).y; - pt.z = coloredCloud->at(i).z; - if(optimizedColorRadius > 0.0f) - { - tree->radiusSearch(pt, optimizedColorRadius, kIndices, kDistances); - } - else - { - tree->nearestKSearch(pt, 1, kIndices, kDistances); - } - 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; - coloredPts.at(i) = true; - } - else - { - //white - coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255; - coloredPts.at(i) = false; - } - } - pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud); - - // remove polygons with no color - if(optimizedCleanWhitePolygons) - { - std::vector filteredPolygons(mesh->polygons.size()); - int oi=0; - for(unsigned int i=0; ipolygons.size(); ++i) - { - bool coloredPolygon = true; - for(unsigned int j=0; jpolygons[i].vertices.size(); ++j) - { - if(!coloredPts.at(mesh->polygons[i].vertices[j])) - { - coloredPolygon = false; - break; - } - } - if(coloredPolygon) - { - filteredPolygons[oi++] = mesh->polygons[i]; - } - } - filteredPolygons.resize(oi); - mesh->polygons = filteredPolygons; - } - LOGI("Transfering color from point cloud to mesh...done! %fs", timer.ticks()); - } - polygonMesh = mesh; - totalPolygons = mesh->polygons.size(); - } - else - { - LOGI("Texturing..."); - textureMesh = rtabmap::util3d::createTextureMesh( - mesh, - cameraPoses, - cameraModels, - normalK); - LOGI("Texturing... done! %fs", timer.ticks()); - - // Remove occluded polygons (polygons with no texture) - if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons) - { - LOGI("Cleanup mesh..."); - - // assume last texture is the occluded texture - textureMesh->tex_coordinates.pop_back(); - textureMesh->tex_polygons.pop_back(); - textureMesh->tex_materials.pop_back(); - - if(minClusterSize_>0) - { - LOGI("Filter small polygon clusters..."); - - // concatenate all polygons - int totalSize = 0; - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - totalSize+=textureMesh->tex_polygons[t].size(); - } - std::vector allPolygons(totalSize); - int oi=0; - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - for(unsigned int i=0; itex_polygons[t].size(); ++i) - { - allPolygons[oi++] = textureMesh->tex_polygons[t][i]; - } - } - - // filter polygons - std::vector > neighbors; - std::vector > vertexToPolygons; - rtabmap::util3d::createPolygonIndexes(allPolygons, - textureMesh->cloud.data.size()/textureMesh->cloud.point_step, - neighbors, - vertexToPolygons); - std::list > clusters = rtabmap::util3d::clusterPolygons( - neighbors, - minClusterSize_); - - std::set validPolygons; - for(std::list >::iterator kter=clusters.begin(); kter!=clusters.end(); ++kter) - { - for(std::list::iterator jter=kter->begin(); jter!=kter->end(); ++jter) - { - validPolygons.insert(*jter); - } - } - - // for each texture - unsigned int allPolygonsIndex = 0; - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - std::vector filteredPolygons(textureMesh->tex_polygons[t].size()); -#if PCL_VERSION_COMPARE(>=, 1, 8, 0) - std::vector > filteredCoordinates(textureMesh->tex_coordinates[t].size()); -#else - std::vector filteredCoordinates(textureMesh->tex_coordinates[t].size()); -#endif - int oi=0; - unsigned int polygonSize = 0; - if(textureMesh->tex_polygons[t].size()) - { - UASSERT(allPolygonsIndex < allPolygons.size()); - - polygonSize = textureMesh->tex_polygons[t][0].vertices.size(); - - UASSERT(filteredCoordinates.size() == textureMesh->tex_polygons[t].size()*polygonSize); - for(unsigned int i=0; itex_polygons[t].size(); ++i) - { - if(validPolygons.find(allPolygonsIndex) != validPolygons.end()) - { - filteredPolygons[oi] = textureMesh->tex_polygons[t].at(i); - for(unsigned int j=0; jtex_coordinates[t][i*polygonSize + j]; - } - ++oi; - } - ++allPolygonsIndex; - } - filteredPolygons.resize(oi); - filteredCoordinates.resize(oi*polygonSize); - textureMesh->tex_polygons[t] = filteredPolygons; - textureMesh->tex_coordinates[t] = filteredCoordinates; - } - } - - LOGI("Filtered %d polygons.", (int)(allPolygons.size()-validPolygons.size())); - } - - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - totalPolygons+=textureMesh->tex_polygons[t].size(); - } - - LOGI("Cleanup mesh... done! %fs (total polygons=%d)", timer.ticks(), totalPolygons); + transformedCloud = rtabmap::util3d::voxelize(cloud, indices, optimizedVoxelSize); + transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); } else { - for(unsigned int t=0; ttex_polygons.size(); ++t) - { - totalPolygons+=textureMesh->tex_polygons[t].size(); - } + // it looks like that using only transformPointCloud with indices + // flushes the colors, so we should extract points before... maybe a too old PCL version + pcl::copyPointCloud(*cloud, *indices, *transformedCloud); + transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); } - } - } - } - } - else // organized meshes - { - pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); - if(textureSize > 0) - { - textureMesh->tex_materials.resize(poses.size()); - textureMesh->tex_polygons.resize(poses.size()); - textureMesh->tex_coordinates.resize(poses.size()); - } + Eigen::Vector3f viewpoint( iter->second.x(), iter->second.y(), iter->second.z()); + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, viewpoint); - int polygonsStep = 0; - int oi = 0; - for(std::map::iterator iter=poses.begin(); - iter!= poses.end(); - ++iter) - { - LOGI("Assembling cloud %d (total=%d)...", iter->first, (int)poses.size()); + pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); + pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals); - std::map::iterator jter = createdMeshes_.find(iter->first); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - std::vector polygons; - float gain = 1.0f; - if(jter != createdMeshes_.end()) - { - cloud = jter->second.cloud; - polygons= jter->second.polygons; - if(cloud->size() && polygons.size() == 0) - { - polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); - } - gain = jter->second.gain; - } - else - { - rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); - if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) - { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0); - polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); - } - } - - if(cloud->size() && polygons.size()) - { - // Convert organized to dense cloud - pcl::PointCloud::Ptr outputCloud(new pcl::PointCloud); - std::vector outputPolygons; - std::vector denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloud, polygons, *outputCloud, outputPolygons); - - pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, normalK); - - pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); - pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals); - - UASSERT(outputPolygons.size()); - - totalPolygons+=outputPolygons.size(); - - if(textureSize == 0) - { - // colored mesh - cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); - - if(gain != 1.0f) + if(textureSize == 0 && gain != 1.0f) { for(unsigned int i=0; isize(); ++i) { @@ -1780,252 +1536,659 @@ bool RTABMapApp::exportMesh( } } + if(mergedClouds->size() == 0) { *mergedClouds = *cloudWithNormals; - polygonMesh->polygons = outputPolygons; } else { - rtabmap::util3d::appendMesh(*mergedClouds, polygonMesh->polygons, *cloudWithNormals, outputPolygons); + *mergedClouds += *cloudWithNormals; + } + + cameraPoses.insert(std::make_pair(iter->first, iter->second)); + cameraModels.insert(std::make_pair(iter->first, model)); + + LOGI("Assembled %d points (%d/%d total=%d)", (int)cloudWithNormals->size(), ++cloudCount, (int)poses.size(), (int)mergedClouds->size()); + } + else + { + UERROR("Cloud %d not found or empty", iter->first); + } + } + LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size()); + + if(mergedClouds->size()) + { + int before = mergedClouds->size(); + if(optimizedVoxelSize > 0.0f) + { + mergedClouds = rtabmap::util3d::voxelize(mergedClouds, optimizedVoxelSize); + LOGI("Voxelized from %d points to %d points", before, (int)mergedClouds->size()); + } + + // Mesh reconstruction + LOGI("Mesh reconstruction..."); + pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh); + pcl::Poisson poisson; + poisson.setDepth(optimizedDepth); + poisson.setInputCloud(mergedClouds); + poisson.reconstruct(*mesh); + LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size()); + + if(mesh->polygons.size()) + { + if(optimizedDecimationFactor > 0.0f) + { + #ifndef DISABLE_VTK + unsigned int count = mesh->polygons.size(); + LOGI("Mesh decimation (factor=%f) from %d polygons...", optimizedDecimationFactor, (int)count); + + pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh); + pcl::MeshQuadricDecimationVTK mqd; + mqd.setTargetReductionFactor(optimizedDecimationFactor); + mqd.setInputMesh(mesh); + mqd.process (*output); + mesh = output; + + //mesh = rtabmap::util3d::meshDecimation(mesh, decimationFactor); + // use direct instantiation above to this fix some linker errors on android like: + // pcl::MeshQuadricDecimationVTK::performProcessing(pcl::PolygonMesh&): error: undefined reference to 'vtkQuadricDecimation::New()' + // pcl::VTKUtils::mesh2vtk(pcl::PolygonMesh const&, vtkSmartPointer&): 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()); + if(count < mesh->polygons.size()) + { + UWARN("Decimated mesh has more polygons than before!"); + } + #else + UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!"); + #endif + } + + if(textureSize == 0) + { + // colored polygon mesh + if(optimizedColorRadius >= 0.0f) + { + LOGI("Transferring color from point cloud to mesh..."); + // transfer color from point cloud to mesh + pcl::search::KdTree::Ptr tree (new pcl::search::KdTree(true)); + tree->setInputCloud(mergedClouds); + pcl::PointCloud::Ptr coloredCloud(new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *coloredCloud); + std::vector coloredPts(coloredCloud->size()); + for(unsigned int i=0; isize(); ++i) + { + std::vector kIndices; + std::vector kDistances; + pcl::PointXYZRGBNormal pt; + pt.x = coloredCloud->at(i).x; + pt.y = coloredCloud->at(i).y; + pt.z = coloredCloud->at(i).z; + if(optimizedColorRadius > 0.0f) + { + tree->radiusSearch(pt, optimizedColorRadius, kIndices, kDistances); + } + else + { + tree->nearestKSearch(pt, 1, kIndices, kDistances); + } + 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; + coloredPts.at(i) = true; + } + else + { + //white + coloredCloud->at(i).r = coloredCloud->at(i).g = coloredCloud->at(i).b = 255; + coloredPts.at(i) = false; + } + } + + // recompute normals and remove polygons with no color + std::vector filteredPolygons(optimizedCleanWhitePolygons?mesh->polygons.size():0); + int oi=0; + for(unsigned int i=0; ipolygons.size(); ++i) + { + // recompute normals + pcl::Vertices & v = mesh->polygons[i]; + UASSERT(v.vertices.size()>2); + Eigen::Vector3f v0( + coloredCloud->at(v.vertices[1]).x - coloredCloud->at(v.vertices[0]).x, + coloredCloud->at(v.vertices[1]).y - coloredCloud->at(v.vertices[0]).y, + coloredCloud->at(v.vertices[1]).z - coloredCloud->at(v.vertices[0]).z); + int last = v.vertices.size()-1; + Eigen::Vector3f v1( + coloredCloud->at(v.vertices[last]).x - coloredCloud->at(v.vertices[0]).x, + coloredCloud->at(v.vertices[last]).y - coloredCloud->at(v.vertices[0]).y, + coloredCloud->at(v.vertices[last]).z - coloredCloud->at(v.vertices[0]).z); + Eigen::Vector3f normal = v0.cross(v1); + normal.normalize(); + // flat normal (per face) + for(unsigned int j=0; jat(v.vertices[j]).normal_x = normal[0]; + coloredCloud->at(v.vertices[j]).normal_y = normal[1]; + coloredCloud->at(v.vertices[j]).normal_z = normal[2]; + } + + if(optimizedCleanWhitePolygons) + { + bool coloredPolygon = true; + for(unsigned int j=0; jpolygons[i].vertices.size(); ++j) + { + if(!coloredPts.at(mesh->polygons[i].vertices[j])) + { + coloredPolygon = false; + break; + } + } + if(coloredPolygon) + { + filteredPolygons[oi++] = mesh->polygons[i]; + } + } + } + if(optimizedCleanWhitePolygons) + { + filteredPolygons.resize(oi); + mesh->polygons = filteredPolygons; + } + + pcl::toPCLPointCloud2(*coloredCloud, mesh->cloud); + LOGI("Transfering color from point cloud to mesh...done! %fs", timer.ticks()); + } + else // recompute normals + { + pcl::PointCloud::Ptr cloud (new pcl::PointCloud); + pcl::fromPCLPointCloud2(mesh->cloud, *cloud); + + for(unsigned int i=0; ipolygons.size(); ++i) + { + pcl::Vertices & v = mesh->polygons[i]; + UASSERT(v.vertices.size()>2); + Eigen::Vector3f v0( + cloud->at(v.vertices[1]).x - cloud->at(v.vertices[0]).x, + cloud->at(v.vertices[1]).y - cloud->at(v.vertices[0]).y, + cloud->at(v.vertices[1]).z - cloud->at(v.vertices[0]).z); + int last = v.vertices.size()-1; + Eigen::Vector3f v1( + cloud->at(v.vertices[last]).x - cloud->at(v.vertices[0]).x, + cloud->at(v.vertices[last]).y - cloud->at(v.vertices[0]).y, + cloud->at(v.vertices[last]).z - cloud->at(v.vertices[0]).z); + Eigen::Vector3f normal = v0.cross(v1); + normal.normalize(); + // flat normal (per face) + for(unsigned int j=0; jat(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]; + } + } + pcl::toPCLPointCloud2 (*cloud, mesh->cloud); + } + polygonMesh = mesh; + totalPolygons = mesh->polygons.size(); + } + else + { + LOGI("Texturing..."); + textureMesh = rtabmap::util3d::createTextureMesh( + mesh, + cameraPoses, + cameraModels, + maxTextureDistance); + LOGI("Texturing... done! %fs", timer.ticks()); + + // Remove occluded polygons (polygons with no texture) + if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons) + { + LOGI("Cleanup mesh..."); + + // assume last texture is the occluded texture + textureMesh->tex_coordinates.pop_back(); + textureMesh->tex_polygons.pop_back(); + textureMesh->tex_materials.pop_back(); + + if(minClusterSize_>0) + { + LOGI("Filter small polygon clusters..."); + + // concatenate all polygons + int totalSize = 0; + for(unsigned int t=0; ttex_polygons.size(); ++t) + { + totalSize+=textureMesh->tex_polygons[t].size(); + } + std::vector allPolygons(totalSize); + int oi=0; + for(unsigned int t=0; ttex_polygons.size(); ++t) + { + for(unsigned int i=0; itex_polygons[t].size(); ++i) + { + allPolygons[oi++] = textureMesh->tex_polygons[t][i]; + } + } + + // filter polygons + std::vector > neighbors; + std::vector > vertexToPolygons; + rtabmap::util3d::createPolygonIndexes(allPolygons, + textureMesh->cloud.data.size()/textureMesh->cloud.point_step, + neighbors, + vertexToPolygons); + std::list > clusters = rtabmap::util3d::clusterPolygons( + neighbors, + minClusterSize_); + + std::set validPolygons; + for(std::list >::iterator kter=clusters.begin(); kter!=clusters.end(); ++kter) + { + for(std::list::iterator jter=kter->begin(); jter!=kter->end(); ++jter) + { + validPolygons.insert(*jter); + } + } + + // for each texture + unsigned int allPolygonsIndex = 0; + for(unsigned int t=0; ttex_polygons.size(); ++t) + { + std::vector filteredPolygons(textureMesh->tex_polygons[t].size()); + #if PCL_VERSION_COMPARE(>=, 1, 8, 0) + std::vector > filteredCoordinates(textureMesh->tex_coordinates[t].size()); + #else + std::vector filteredCoordinates(textureMesh->tex_coordinates[t].size()); + #endif + int oi=0; + unsigned int polygonSize = 0; + if(textureMesh->tex_polygons[t].size()) + { + UASSERT(allPolygonsIndex < allPolygons.size()); + + polygonSize = textureMesh->tex_polygons[t][0].vertices.size(); + + UASSERT(filteredCoordinates.size() == textureMesh->tex_polygons[t].size()*polygonSize); + for(unsigned int i=0; itex_polygons[t].size(); ++i) + { + if(validPolygons.find(allPolygonsIndex) != validPolygons.end()) + { + filteredPolygons[oi] = textureMesh->tex_polygons[t].at(i); + for(unsigned int j=0; jtex_coordinates[t][i*polygonSize + j]; + } + ++oi; + } + ++allPolygonsIndex; + } + filteredPolygons.resize(oi); + filteredCoordinates.resize(oi*polygonSize); + textureMesh->tex_polygons[t] = filteredPolygons; + textureMesh->tex_coordinates[t] = filteredCoordinates; + } + } + + LOGI("Filtered %d polygons.", (int)(allPolygons.size()-validPolygons.size())); + } + + for(unsigned int t=0; ttex_polygons.size(); ++t) + { + totalPolygons+=textureMesh->tex_polygons[t].size(); + } + + LOGI("Cleanup mesh... done! %fs (total polygons=%d)", timer.ticks(), totalPolygons); + } + else + { + for(unsigned int t=0; ttex_polygons.size(); ++t) + { + totalPolygons+=textureMesh->tex_polygons[t].size(); + } + } + } + } + } + } + else // organized meshes + { + pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); + + if(textureSize > 0) + { + textureMesh->tex_materials.resize(poses.size()); + textureMesh->tex_polygons.resize(poses.size()); + textureMesh->tex_coordinates.resize(poses.size()); + } + + int polygonsStep = 0; + int oi = 0; + for(std::map::iterator iter=poses.begin(); + iter!= poses.end(); + ++iter) + { + LOGI("Assembling cloud %d (total=%d)...", iter->first, (int)poses.size()); + + std::map::iterator jter = createdMeshes_.find(iter->first); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + std::vector polygons; + float gain = 1.0f; + if(jter != createdMeshes_.end()) + { + cloud = jter->second.cloud; + polygons= jter->second.polygons; + if(cloud->size() && polygons.size() == 0) + { + polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); + } + gain = jter->second.gain; + } + else + { + rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); + if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1) + { + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0); + polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); + } + } + + if(cloud->size() && polygons.size()) + { + // Convert organized to dense cloud + pcl::PointCloud::Ptr outputCloud(new pcl::PointCloud); + std::vector outputPolygons; + std::vector denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloud, polygons, *outputCloud, outputPolygons); + + pcl::PointCloud::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, normalK); + + pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); + pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals); + + UASSERT(outputPolygons.size()); + + totalPolygons+=outputPolygons.size(); + + if(textureSize == 0) + { + // colored mesh + cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); + + if(gain != 1.0f) + { + for(unsigned int i=0; isize(); ++i) + { + pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i); + pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); + pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); + pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + } + } + + if(mergedClouds->size() == 0) + { + *mergedClouds = *cloudWithNormals; + polygonMesh->polygons = outputPolygons; + } + else + { + rtabmap::util3d::appendMesh(*mergedClouds, polygonMesh->polygons, *cloudWithNormals, outputPolygons); + } + } + else + { + // texture mesh + 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; jtex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f( + float(originalVertex % cloud->width) / float(cloud->width), // u + float(cloud->height - originalVertex / cloud->width) / float(cloud->height)); // v + + vertices.vertices[k] += polygonsStep; + } + textureMesh->tex_polygons[oi][j] = vertices; + + } + polygonsStep += outputCloud->size(); + + pcl::PointCloud::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); + if(mergedClouds->size() == 0) + { + *mergedClouds = *transformedCloud; + } + else + { + *mergedClouds += *transformedCloud; + } + + textureMesh->tex_materials[oi].tex_illum = 1; + textureMesh->tex_materials[oi].tex_name = uFormat("material_%d", iter->first); + textureMesh->tex_materials[oi].tex_file = uNumber2Str(iter->first); + ++oi; } } else { - // texture mesh - 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; jtex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f( - float(originalVertex % cloud->width) / float(cloud->width), // u - float(cloud->height - originalVertex / cloud->width) / float(cloud->height)); // v - - vertices.vertices[k] += polygonsStep; - } - textureMesh->tex_polygons[oi][j] = vertices; - - } - polygonsStep += outputCloud->size(); - - pcl::PointCloud::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second); - if(mergedClouds->size() == 0) - { - *mergedClouds = *transformedCloud; - } - else - { - *mergedClouds += *transformedCloud; - } - - textureMesh->tex_materials[oi].tex_illum = 1; - textureMesh->tex_materials[oi].tex_name = uFormat("material_%d", iter->first); - textureMesh->tex_materials[oi].tex_file = uNumber2Str(iter->first); - ++oi; + UERROR("Mesh not found for mesh %d", iter->first); + } + } + if(textureSize == 0) + { + if(mergedClouds->size()) + { + pcl::toPCLPointCloud2(*mergedClouds, polygonMesh->cloud); + } + else + { + polygonMesh->polygons.clear(); } } else { - UERROR("Mesh not found for mesh %d", iter->first); + textureMesh->tex_materials.resize(oi); + textureMesh->tex_polygons.resize(oi); + + if(mergedClouds->size()) + { + pcl::toPCLPointCloud2(*mergedClouds, textureMesh->cloud); + } } } + + // end optimized or organized + + if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size()) + { + LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size()); + globalTexture = mergeTextures(*textureMesh, textureSize); + + std::string baseName = uSplit(UFile::getName(filePath), '.').front(); + std::string textureDirectory = UDirectory::getDir(filePath); + std::string fullPath = textureDirectory+UDirectory::separator()+baseName+".jpg"; + textureMesh->tex_materials[0].tex_file = baseName+".jpg"; + LOGI("Saving texture to %s.", fullPath.c_str()); + if(!cv::imwrite(fullPath, globalTexture)) + { + LOGI("Failed saving %s!", fullPath.c_str()); + } + else + { + LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTexture.total()*globalTexture.channels()); + } + } + } + if(totalPolygons) + { if(textureSize == 0) { - if(mergedClouds->size()) + UASSERT((int)polygonMesh->polygons.size() == totalPolygons); + if(polygonMesh->polygons.size()) { - pcl::toPCLPointCloud2(*mergedClouds, polygonMesh->cloud); - } - else - { - polygonMesh->polygons.clear(); + LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, totalPolygons, filePath.c_str()); + success = pcl::io::savePLYFile(filePath, *polygonMesh) == 0; + if(success) + { + UINFO("Saved ply to %s!", filePath.c_str()); + exportedMesh_.reset(new pcl::TextureMesh); + exportedMesh_->cloud = polygonMesh->cloud; + exportedMesh_->tex_polygons.push_back(polygonMesh->polygons); + } + else + { + UERROR("Failed saving ply to %s!", filePath.c_str()); + } } } - else + else if(textureMesh->tex_materials.size()) { - textureMesh->tex_materials.resize(oi); - textureMesh->tex_polygons.resize(oi); - - if(mergedClouds->size()) - { - pcl::toPCLPointCloud2(*mergedClouds, textureMesh->cloud); - } - } - } - - // end optimized or organized - - if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size()) - { - LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size()); - globalTexture = mergeTextures(*textureMesh, textureSize); - - std::string baseName = uSplit(UFile::getName(filePath), '.').front(); - std::string textureDirectory = UDirectory::getDir(filePath); - std::string fullPath = textureDirectory+UDirectory::separator()+baseName+".jpg"; - textureMesh->tex_materials[0].tex_file = baseName+".jpg"; - LOGI("Saving texture to %s.", fullPath.c_str()); - if(!cv::imwrite(fullPath, globalTexture)) - { - LOGI("Failed saving %s!", fullPath.c_str()); - } - else - { - LOGI("Saved %s (%d bytes).", fullPath.c_str(), globalTexture.total()*globalTexture.channels()); - } - } - } - if(totalPolygons) - { - if(textureSize == 0) - { - UASSERT((int)polygonMesh->polygons.size() == totalPolygons); - if(polygonMesh->polygons.size()) - { - LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, totalPolygons, filePath.c_str()); - success = pcl::io::savePLYFile(filePath, *polygonMesh) == 0; + UASSERT(textureMesh->tex_polygons.size() && (int)textureMesh->tex_polygons[0].size() == totalPolygons); + LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str()); + success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0; if(success) { - UINFO("Saved ply to %s!", filePath.c_str()); - exportedMesh_.reset(new pcl::TextureMesh); - exportedMesh_->cloud = polygonMesh->cloud; - exportedMesh_->tex_polygons.push_back(polygonMesh->polygons); + LOGI("Saved obj to %s!", filePath.c_str()); + exportedMesh_ = textureMesh; + exportedTexture_ = globalTexture; } else { - UERROR("Failed saving ply to %s!", filePath.c_str()); + UERROR("Failed saving obj to %s!", filePath.c_str()); } } - } - else if(textureMesh->tex_materials.size()) - { - UASSERT(textureMesh->tex_polygons.size() && (int)textureMesh->tex_polygons[0].size() == totalPolygons); - LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str()); - success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0; - if(success) - { - LOGI("Saved obj to %s!", filePath.c_str()); - exportedMesh_ = textureMesh; - exportedTexture_ = globalTexture; - } else { - UERROR("Failed saving obj to %s!", filePath.c_str()); + UERROR("Failed exporting obj to %s! There are no textures!", filePath.c_str()); } } else { - UERROR("Failed exporting obj to %s! There are no textures!", filePath.c_str()); + UERROR("Failed exporting to %s! There are no polygons!", filePath.c_str()); } } - else + else // Point cloud { - UERROR("Failed exporting to %s! There are no polygons!", filePath.c_str()); - } - } - else // Point cloud - { - pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); - for(std::map::iterator iter=poses.begin(); - iter!= poses.end(); - ++iter) - { - std::map::iterator jter=createdMeshes_.find(iter->first); - pcl::PointCloud::Ptr cloud(new pcl::PointCloud); - pcl::IndicesPtr indices(new std::vector); - float gain = 1.0f; - if(jter != createdMeshes_.end()) + pcl::PointCloud::Ptr mergedClouds(new pcl::PointCloud); + for(std::map::iterator iter=poses.begin(); + iter!= poses.end(); + ++iter) { - cloud = jter->second.cloud; - indices = jter->second.indices; - gain = jter->second.gain; - } - else - { - rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); - if(!data.imageRaw().empty() && !data.depthRaw().empty()) + std::map::iterator jter=createdMeshes_.find(iter->first); + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::IndicesPtr indices(new std::vector); + float gain = 1.0f; + if(jter != createdMeshes_.end()) { - cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + cloud = jter->second.cloud; + indices = jter->second.indices; + gain = jter->second.gain; + } + else + { + rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); + if(!data.imageRaw().empty() && !data.depthRaw().empty()) + { + cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, 0, indices.get()); + } + } + if(cloud->size() && indices->size()) + { + // Convert organized to dense cloud + pcl::PointCloud::Ptr transformedCloud(new pcl::PointCloud); + if(cloudVoxelSize > 0.0f) + { + transformedCloud = rtabmap::util3d::voxelize(cloud, indices, cloudVoxelSize); + transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); + } + else + { + // it looks like that using only transformPointCloud with indices + // flushes the colors, so we should extract points before... maybe a too old PCL version + pcl::copyPointCloud(*cloud, *indices, *transformedCloud); + transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); + } + + if(gain != 1.0f) + { + //LOGD("cloud %d, gain=%f", iter->first, gain); + for(unsigned int i=0; isize(); ++i) + { + pcl::PointXYZRGB & pt = transformedCloud->at(i); + //LOGI("color %d = %d %d %d", i, (int)pt.r, (int)pt.g, (int)pt.b); + pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); + pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); + pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); + + } + } + + if(mergedClouds->size() == 0) + { + *mergedClouds = *transformedCloud; + } + else + { + *mergedClouds += *transformedCloud; + } } } - if(cloud->size() && indices->size()) + + if(mergedClouds->size()) { - // Convert organized to dense cloud - pcl::PointCloud::Ptr transformedCloud(new pcl::PointCloud); if(cloudVoxelSize > 0.0f) { - transformedCloud = rtabmap::util3d::voxelize(cloud, indices, cloudVoxelSize); - transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); + mergedClouds = rtabmap::util3d::voxelize(mergedClouds, cloudVoxelSize); + } + + pcl::PolygonMesh mesh; + pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud); + + LOGI("Saving ply (%d points) to %s.", (int)mergedClouds->size(), filePath.c_str()); + success = pcl::io::savePLYFileBinary(filePath, mesh) == 0; + if(success) + { + LOGI("Saved ply to %s!", filePath.c_str()); + mergedClouds->clear(); + exportedMesh_.reset(new pcl::TextureMesh); + exportedMesh_->cloud = mesh.cloud; } else { - // it looks like that using only transformPointCloud with indices - // flushes the colors, so we should extract points before... maybe a too old PCL version - pcl::copyPointCloud(*cloud, *indices, *transformedCloud); - transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second); - } - - if(gain != 1.0f) - { - //LOGD("cloud %d, gain=%f", iter->first, gain); - for(unsigned int i=0; isize(); ++i) - { - pcl::PointXYZRGB & pt = transformedCloud->at(i); - //LOGI("color %d = %d %d %d", i, (int)pt.r, (int)pt.g, (int)pt.b); - pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gain))); - pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gain))); - pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gain))); - - } - } - - if(mergedClouds->size() == 0) - { - *mergedClouds = *transformedCloud; - } - else - { - *mergedClouds += *transformedCloud; + UERROR("Failed saving ply to %s!", filePath.c_str()); } } } - if(mergedClouds->size()) + if(blockRendering) { - if(cloudVoxelSize > 0.0f) - { - mergedClouds = rtabmap::util3d::voxelize(mergedClouds, cloudVoxelSize); - } - - pcl::PolygonMesh mesh; - pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud); - - LOGI("Saving ply (%d points) to %s.", (int)mergedClouds->size(), filePath.c_str()); - success = pcl::io::savePLYFileBinary(filePath, mesh) == 0; - if(success) - { - LOGI("Saved ply to %s!", filePath.c_str()); - mergedClouds->clear(); - exportedMesh_.reset(new pcl::TextureMesh); - exportedMesh_->cloud = mesh.cloud; - } - else - { - UERROR("Failed saving ply to %s!", filePath.c_str()); - } + renderingMutex_.unlock(); } } - - if(blockRendering) + catch (std::exception & e) { - renderingMutex_.unlock(); + UERROR("Out of memory! %s", e.what()); + + if(blockRendering) + { + renderingMutex_.unlock(); + } + + success = false; } return success; @@ -2067,22 +2230,6 @@ int RTABMapApp::postProcessing(int approach) returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1); } - // ICP refining - if(returnedValue >=0 && approach == 3) - { - rtabmap::ParametersMap parameters; - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string("1"))); // ICP - rtabmap_->parseParameters(parameters); - int r = rtabmap_->refineLinks(); - if(approach == 3 ) - { - returnedValue = r; - } - // reset back default registration (visual) - uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0"))); // Visual - rtabmap_->parseParameters(parameters); - } - // graph optimization if(returnedValue >=0) { @@ -2102,7 +2249,7 @@ int RTABMapApp::postProcessing(int approach) } else { - LOGE("g2o not available!"); + UERROR("g2o not available!"); } } else if(approach!=4 && approach!=5 && approach != 7) @@ -2277,6 +2424,7 @@ void RTABMapApp::handleEvent(UEvent * event) int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f); int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f); int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f); + float rehearsalValue = uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f); int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0; float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f); @@ -2292,7 +2440,7 @@ void RTABMapApp::handleEvent(UEvent * event) jclass clazz = env->GetObjectClass(RTABMapActivity); if(clazz) { - jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" ); + jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFIF)V" ); if(methodID) { env->CallVoidMethod(RTABMapActivity, methodID, @@ -2309,7 +2457,8 @@ void RTABMapApp::handleEvent(UEvent * event) hypothesis, lastDrawnCloudsCount_, renderingTime_>0.0f?1.0f/renderingTime_:0.0f, - rejected); + rejected, + rehearsalValue); success = true; } } diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 6a3ca5e3..39fda520 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -53,7 +53,9 @@ class RTABMapApp : public UEventsHandler { void onCreate(JNIEnv* env, jobject caller_activity); - void openDatabase(const std::string & databasePath = ""); + void setScreenRotation(int displayRotation, int cameraRotation); + + void openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize); bool onTangoServiceConnected(JNIEnv* env, jobject iBinder); @@ -120,10 +122,10 @@ class RTABMapApp : public UEventsHandler { void setTrajectoryMode(bool enabled); void setGraphOptimization(bool enabled); void setNodesFiltering(bool enabled); - void setDriftCorrection(bool enabled); void setGraphVisible(bool visible); void setGridVisible(bool visible); void setAutoExposure(bool enabled); + void setRawScanSaved(bool enabled); void setFullResolution(bool enabled); void setAppendMode(bool enabled); void setDataRecorderMode(bool enabled); @@ -144,6 +146,7 @@ class RTABMapApp : public UEventsHandler { bool meshing, int textureSize, int normalK, + float maxTextureDistance, bool optimized, float optimizedVoxelSize, int optimizedDepth, @@ -171,10 +174,10 @@ class RTABMapApp : public UEventsHandler { bool odomCloudShown_; bool graphOptimization_; bool nodesFiltering_; - bool driftCorrection_; bool localizationMode_; bool trajectoryMode_; bool autoExposure_; + bool rawScanSaved_; bool fullResolution_; bool appendMode_; float maxCloudDepth_; @@ -189,6 +192,7 @@ class RTABMapApp : public UEventsHandler { bool paused_; bool dataRecorderMode_; bool clearSceneOnNextRender_; + bool optimizeOpenedDatabase_; bool filterPolygonsOnNextRender_; int gainCompensationOnNextRender_; bool bilateralFilteringOnNextRender_; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index 5ebe87fd..62298c00 100644 --- a/app/android/jni/jni_interface.cpp +++ b/app/android/jni/jni_interface.cpp @@ -56,19 +56,19 @@ Java_com_introlab_rtabmap_RTABMapLib_onCreate( } JNIEXPORT void JNICALL -Java_com_introlab_rtabmap_RTABMapLib_openEmptyDatabase( - JNIEnv* env, jobject) +Java_com_introlab_rtabmap_RTABMapLib_setScreenRotation( + JNIEnv* env, jobject, int displayRotation, int cameraRotation) { - return app.openDatabase(); + return app.setScreenRotation(displayRotation, cameraRotation); } JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_openDatabase( - JNIEnv* env, jobject, jstring databasePath) + JNIEnv* env, jobject, jstring databasePath, bool databaseInMemory, bool optimize) { std::string databasePathC; GetJStringContent(env,databasePath,databasePathC); - return app.openDatabase(databasePathC); + return app.openDatabase(databasePathC, databaseInMemory, optimize); } JNIEXPORT bool JNICALL @@ -181,12 +181,6 @@ Java_com_introlab_rtabmap_RTABMapLib_setNodesFiltering( return app.setNodesFiltering(enabled); } JNIEXPORT void JNICALL -Java_com_introlab_rtabmap_RTABMapLib_setDriftCorrection( - JNIEnv*, jobject, bool enabled) -{ - return app.setDriftCorrection(enabled); -} -JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_setGraphVisible( JNIEnv*, jobject, bool visible) { @@ -205,6 +199,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setAutoExposure( return app.setAutoExposure(enabled); } JNIEXPORT void JNICALL +Java_com_introlab_rtabmap_RTABMapLib_setRawScanSaved( + JNIEnv*, jobject, bool enabled) +{ + return app.setRawScanSaved(enabled); +} +JNIEXPORT void JNICALL Java_com_introlab_rtabmap_RTABMapLib_setFullResolution( JNIEnv*, jobject, bool enabled) { @@ -292,6 +292,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh( bool meshing, int textureSize, int normalK, + float maxTextureDistance, bool optimized, float optimizedVoxelSize, int optimizedDepth, @@ -309,6 +310,7 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh( meshing, textureSize, normalK, + maxTextureDistance, optimized, optimizedVoxelSize, optimizedDepth, diff --git a/app/android/jni/point_cloud_drawable.h b/app/android/jni/point_cloud_drawable.h index 000c9061..71551bb3 100644 --- a/app/android/jni/point_cloud_drawable.h +++ b/app/android/jni/point_cloud_drawable.h @@ -59,6 +59,7 @@ class PointCloudDrawable { void updateMesh(const Mesh & mesh, const cv::Mat & texture); void setPose(const rtabmap::Transform & pose); void setVisible(bool visible) {visible_=visible;} + void setGain(float gain) {gain_ = gain;} rtabmap::Transform getPose() const {return glmToTransform(pose_);} bool isVisible() const {return visible_;} bool hasMesh() const {return polygons_.size()!=0;} diff --git a/app/android/jni/scene.cpp b/app/android/jni/scene.cpp index b1a4216d..666e1911 100644 --- a/app/android/jni/scene.cpp +++ b/app/android/jni/scene.cpp @@ -21,6 +21,8 @@ #include #include +#include + #include "scene.h" #include "util.h" @@ -60,7 +62,7 @@ const std::string kPointCloudVertexShader = " gl_Position = uMVP*vec4(aVertex.x, aVertex.y, aVertex.z, 1.0);\n" " gl_PointSize = uPointSize;\n" " if (!uUseLighting) {\n" - " vLightWeighting = vec3(1.0, 1.0, 1.0);\n" + " vLightWeighting = 1.0;\n" " } else {\n" " vec3 transformedNormal = uN * aNormal;\n" " vLightWeighting = max(dot(transformedNormal, uLightingDirection), 0.0);\n" @@ -106,7 +108,7 @@ const std::string kTextureMeshVertexShader = " }\n" " if (!uUseLighting) {\n" - " vLightWeighting = vec3(1.0, 1.0, 1.0);\n" + " vLightWeighting = 1.0;\n" " } else {\n" " vec3 transformedNormal = uN * aNormal;\n" " vLightWeighting = max(dot(transformedNormal, uLightingDirection), 0.0);\n" @@ -156,6 +158,7 @@ Scene::Scene() : graphVisible_(true), gridVisible_(true), traceVisible_(true), + color_camera_to_display_rotation_(ROTATION_0), currentPose_(0), cloud_shader_program_(0), texture_mesh_shader_program_(0), @@ -165,7 +168,10 @@ Scene::Scene() : meshRenderingTexture_(true), pointSize_(5.0f), frustumCulling_(true), - lighting_(true) + lighting_(true), + r_(0.0f), + g_(0.0f), + b_(0.0f) { gesture_camera_ = new tango_gl::GestureCamera(); gesture_camera_->SetCameraType( @@ -249,6 +255,14 @@ void Scene::DeleteResources() { clear(); } +void Scene::setScreenRotation(int displayOrientation, int cameraOrientation) +{ + color_camera_to_display_rotation_ = + tango_gl::util::GetAndroidRotationFromColorCameraToDisplay( + displayOrientation, cameraOrientation); + LOGI("color_camera_to_display_rotation_=%d", color_camera_to_display_rotation_); +} + //Should only be called in OpenGL thread! void Scene::clear() { @@ -284,56 +298,61 @@ void Scene::SetupViewPort(int w, int h) { int Scene::Render() { UASSERT(gesture_camera_ != 0); - glEnable(GL_DEPTH_TEST); - glEnable(GL_CULL_FACE); + glEnable(GL_DEPTH_TEST); + glEnable(GL_CULL_FACE); - glClearColor(0.0f, 0.0f, 0.0f, 1.0f); - glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT); + glClearColor(r_, g_, b_, 1.0f); + glClear(GL_DEPTH_BUFFER_BIT | GL_COLOR_BUFFER_BIT); - if(!currentPose_->isNull()) - { - glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z()); - Eigen::Quaternionf quat = currentPose_->getQuaternionf(); - glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z()); + glm::mat4 rotateM; + if(gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + { + rotateM = glm::rotate(float(color_camera_to_display_rotation_)*1.57079632679489661923132169163975144, glm::vec3(0.0f, 0.0f, 1.0f)); + } + if(!currentPose_->isNull()) + { + glm::vec3 position(currentPose_->x(), currentPose_->y(), currentPose_->z()); + Eigen::Quaternionf quat = currentPose_->getQuaternionf(); + glm::quat rotation(quat.w(), quat.x(), quat.y(), quat.z()); - if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson) - { - // In first person mode, we directly control camera's motion. - gesture_camera_->SetPosition(position); - gesture_camera_->SetRotation(rotation); - } - else - { - // In third person or top down mode, we follow the camera movement. - gesture_camera_->SetAnchorPosition(position, rotation); + if (gesture_camera_->GetCameraType() == tango_gl::GestureCamera::kFirstPerson) + { + // In first person mode, we directly control camera's motion. + gesture_camera_->SetPosition(position); + gesture_camera_->SetRotation(rotation); + } + else + { + // In third person or top down mode, we follow the camera movement. + gesture_camera_->SetAnchorPosition(position, rotation); - frustum_->SetPosition(position); - frustum_->SetRotation(rotation); - // Set the frustum scale to 4:3, this doesn't necessarily match the physical - // camera's aspect ratio, this is just for visualization purposes. - frustum_->SetScale(kFrustumScale); - frustum_->Render(gesture_camera_->GetProjectionMatrix(), - gesture_camera_->GetViewMatrix()); + frustum_->SetPosition(position); + frustum_->SetRotation(rotation); + // Set the frustum scale to 4:3, this doesn't necessarily match the physical + // camera's aspect ratio, this is just for visualization purposes. + frustum_->SetScale(kFrustumScale); + frustum_->Render(gesture_camera_->GetProjectionMatrix(), + rotateM*gesture_camera_->GetViewMatrix()); - axis_->SetPosition(position); - axis_->SetRotation(rotation); - axis_->Render(gesture_camera_->GetProjectionMatrix(), - gesture_camera_->GetViewMatrix()); - } + axis_->SetPosition(position); + axis_->SetRotation(rotation); + axis_->Render(gesture_camera_->GetProjectionMatrix(), + rotateM*gesture_camera_->GetViewMatrix()); + } - trace_->UpdateVertexArray(position); - if(traceVisible_) - { - trace_->Render(gesture_camera_->GetProjectionMatrix(), - gesture_camera_->GetViewMatrix()); - } - } + trace_->UpdateVertexArray(position); + if(traceVisible_) + { + trace_->Render(gesture_camera_->GetProjectionMatrix(), + rotateM*gesture_camera_->GetViewMatrix()); + } - if(gridVisible_) - { - grid_->Render(gesture_camera_->GetProjectionMatrix(), - gesture_camera_->GetViewMatrix()); - } + if(gridVisible_) + { + grid_->Render(gesture_camera_->GetProjectionMatrix(), + rotateM*gesture_camera_->GetViewMatrix()); + } + } int cloudDrawn=0; if(mapRendering_ && frustumCulling_) @@ -382,7 +401,7 @@ int Scene::Render() { for(unsigned int i=0; isize(); ++i) { ++cloudDrawn; - pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_); + pointClouds_.find(ids[indices->at(i)])->second->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_); } } } @@ -393,14 +412,14 @@ int Scene::Render() { if((mapRendering_ || iter->first < 0) && iter->second->isVisible()) { ++cloudDrawn; - iter->second->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_); + iter->second->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix(), meshRendering_, pointSize_, meshRenderingTexture_, lighting_); } } } if(graphVisible_ && graph_) { - graph_->Render(gesture_camera_->GetProjectionMatrix(), gesture_camera_->GetViewMatrix()); + graph_->Render(gesture_camera_->GetProjectionMatrix(), rotateM*gesture_camera_->GetViewMatrix()); } return cloudDrawn; @@ -574,3 +593,12 @@ void Scene::updateMesh(int id, const Mesh & mesh, const cv::Mat & texture) iter->second->updateMesh(mesh, texture); } } + +void Scene::updateGain(int id, float gain) +{ + std::map::iterator iter=pointClouds_.find(id); + if(iter != pointClouds_.end()) + { + iter->second->setGain(gain); + } +} diff --git a/app/android/jni/scene.h b/app/android/jni/scene.h index 7331270b..1a1c8e15 100644 --- a/app/android/jni/scene.h +++ b/app/android/jni/scene.h @@ -57,6 +57,8 @@ class Scene { // Setup GL view port. void SetupViewPort(int w, int h); + void setScreenRotation(int displayRotation, int cameraRotation); + void clear(); // removed all point clouds // Render loop. @@ -116,12 +118,14 @@ class Scene { std::set getAddedClouds() const; void updateCloudPolygons(int id, const std::vector & polygons); void updateMesh(int id, const Mesh & mesh, const cv::Mat & texture); + void updateGain(int id, float gain); void setMapRendering(bool enabled) {mapRendering_ = enabled;} void setMeshRendering(bool enabled, bool withTexture) {meshRendering_ = enabled; meshRenderingTexture_ = withTexture;} void setPointSize(float size) {pointSize_ = size;} void setFrustumCulling(bool enabled) {frustumCulling_ = enabled;} void setLighting(bool enabled) {lighting_ = enabled;} + void setBackgroundColor(float r, float g, float b) {r_=r; g_=g; b_=b;} // 0.0f <> 1.0f bool isMeshRendering() const {return meshRendering_;} bool isMeshTexturing() const {return meshRendering_ && meshRenderingTexture_;} @@ -149,6 +153,8 @@ class Scene { bool gridVisible_; bool traceVisible_; + TangoSupportDisplayRotation color_camera_to_display_rotation_; + std::map pointClouds_; rtabmap::Transform * currentPose_; @@ -164,6 +170,9 @@ class Scene { float pointSize_; bool frustumCulling_; bool lighting_; + float r_; + float g_; + float b_; }; #endif // TANGO_POINT_CLOUD_SCENE_H_ diff --git a/app/android/jni/tango-gl/include/tango-gl/util.h b/app/android/jni/tango-gl/include/tango-gl/util.h index b0e76058..e96d744f 100644 --- a/app/android/jni/tango-gl/include/tango-gl/util.h +++ b/app/android/jni/tango-gl/include/tango-gl/util.h @@ -25,6 +25,7 @@ #include #include #include +#include #include "glm/glm.hpp" #include "glm/gtc/matrix_transform.hpp" @@ -76,6 +77,33 @@ namespace util { const glm::vec3& start, const glm::vec3& end); glm::vec3 ApplyTransform(const glm::mat4& mat, const glm::vec3& vec); + + // Get the Android rotation integer value from color camera to display. + // This function is used to compute the orientation difference to handle + // the portrait and landscape mode for color camera display. + // + // @param display: integer value of display orientation, values available + // are 0, 1, 2 ,3. Followed by Android display orientation standard: + // https://developer.android.com/reference/android/view/Display.html#getRotation() + // @param color_camera: integer value of color camera oreintation, values + // available are 0, 90, 180, 270. Followed by Android camera orientation + // standard: + // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation + TangoSupportDisplayRotation GetAndroidRotationFromColorCameraToDisplay( + int display_rotation, int color_camera_rotation); + + // Get the Android rotation integer value from color camera to display. + // This function is used to compute the orientation difference to handle + // the portrait and landscape mode for color camera display. + // + // @param display: the device display orientation. + // @param color_camera: integer value of color camera oreintation, values + // available are 0, 90, 180, 270. Followed by Android camera orientation + // standard: + // https://developer.android.com/reference/android/hardware/Camera.CameraInfo.html#orientation + TangoSupportDisplayRotation GetAndroidRotationFromColorCameraToDisplay( + TangoSupportDisplayRotation display_rotation, int color_camera_rotation); + } // namespace util } // namespace tango_gl #endif // TANGO_GL_RENDERER_GL_UTIL diff --git a/app/android/jni/tango-gl/util.cpp b/app/android/jni/tango-gl/util.cpp index 73cc1125..ba5b53c2 100644 --- a/app/android/jni/tango-gl/util.cpp +++ b/app/android/jni/tango-gl/util.cpp @@ -19,6 +19,27 @@ namespace tango_gl { +namespace { +int NormalizedColorCameraRotation(int camera_rotation) { + int camera_n = 0; + switch (camera_rotation) { + case 90: + camera_n = 1; + break; + case 180: + camera_n = 2; + break; + case 270: + camera_n = 3; + break; + default: + camera_n = 0; + break; + } + return camera_n; +} +} // annonymous namespace + void util::CheckGlError(const char* operation) { for (GLint error = glGetError(); error; error = glGetError()) { LOGE("after %s() glError (0x%x)\n", operation, error); @@ -217,4 +238,23 @@ glm::vec3 util::ApplyTransform(const glm::mat4& mat, const glm::vec3& vec) { return glm::vec3(mat * glm::vec4(vec, 1.0f)); } +TangoSupportDisplayRotation util::GetAndroidRotationFromColorCameraToDisplay( + int display_rotation, int color_camera_rotation) { + TangoSupportDisplayRotation r = + static_cast(display_rotation); + return util::GetAndroidRotationFromColorCameraToDisplay( + r, color_camera_rotation); +} + +TangoSupportDisplayRotation util::GetAndroidRotationFromColorCameraToDisplay( + TangoSupportDisplayRotation display_rotation, int color_camera_rotation) { + int color_camera_n = NormalizedColorCameraRotation(color_camera_rotation); + + int ret = static_cast(display_rotation) - color_camera_n; + if (ret < 0) { + ret += 4; + } + return static_cast(ret % 4); +} + } // namespace tango_gl diff --git a/app/android/res/layout/activity_rtabmap.xml b/app/android/res/layout/activity_rtabmap.xml index e39e8887..57cf41e2 100644 --- a/app/android/res/layout/activity_rtabmap.xml +++ b/app/android/res/layout/activity_rtabmap.xml @@ -23,15 +23,15 @@ android:layout_height="fill_parent" android:layout_gravity="top" /> - - + + + + + + + + + + + + + + + + + + + android:text="@string/database_size" /> - - - - - - + + + + + + + android:key="@string/pref_key_density" + android:title="@string/pref_title_density" + android:summary="@string/pref_summary_density" + android:entries="@array/pref_density_keys" + android:entryValues="@array/pref_density_values" + android:defaultValue="@string/pref_default_density"/> - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -15,7 +16,6 @@ - @@ -25,6 +25,7 @@ + diff --git a/app/android/res/values/strings.xml b/app/android/res/values/strings.xml index 21273327..071620cc 100644 --- a/app/android/res/values/strings.xml +++ b/app/android/res/values/strings.xml @@ -2,7 +2,7 @@ RTAB-Map RTAB-Map - Real-Time Appearance-Based Mapping + RTAB-Map Settings Dropbox "Status: " @@ -20,12 +20,14 @@ "Number of points: " "Update time (ms): " "Loop closure ID: " - "Database (MB): " + "Free Memory (MB): " + "Database (MB): " "Loop closures: " "Inliers: " "Features: " + "Rehearsal: " "Polygons: " - "Memory (MB): " + "Used Memory (MB): " "Hypothesis: " "FPS (rendering): " @@ -33,7 +35,7 @@ pref_key_rendering2 pref_key_reset_button - pref_key_decimation 0 + pref_key_density 1 pref_key_depth 0 pref_key_point_size 5 pref_key_angle 15 @@ -41,23 +43,29 @@ pref_key_nodes_filtering false pref_key_append true - pref_key_drift_correction false pref_key_auto_exposure true pref_key_resolution false pref_key_update_rate 1 - pref_key_time_thr 800 + pref_key_time_thr 1000 + pref_key_mem_thr 0 pref_key_loop_thr 0.11 + pref_key_sim_thr 0.3 pref_key_opt_error 0.1 + pref_key_features_voc 200 pref_key_features 400 pref_key_features_type 6 + pref_key_keep_all_db true + pref_key_raw_scan_saved false + pref_key_db_in_memory true - pref_key_cloud_voxel 0 + pref_key_cloud_voxel 0.01 pref_key_texture_size 4096 pref_key_normal_k 6 + pref_key_max_texture_distance 0 pref_key_block_render false - pref_key_opt_depth 8 + pref_key_opt_depth 9 pref_key_opt_decimation_factor80 - pref_key_opt_color_radius 0.0 + pref_key_opt_color_radius 0 pref_key_opt_clean_white true pref_key_gain_max_radius 0.02 @@ -66,8 +74,8 @@ Rendering - Mesh Decimation - Decimate the cloud size to reduce rendering time and memory. + Point Cloud Density + Decrease density to reduce rendering time and memory. Tip: To apply a different density to current map: save the map, change density and re-open the same map to regenerate the point clouds at this density. Mesh Angle Tolerance Minimum polygon angle. Mesh Triangle Size @@ -77,17 +85,19 @@ Point Size Size of the points when rendering only the point cloud. Nodes Filtering - Hide close point clouds from rendering. + Render only the newest point cloud of a loop closure. - + + "Maximum" "High" - "Medium" - "Disabled" + "Low" + "Very Low" - - "2" - "1" + "0" + "1" + "2" + "3" @@ -148,13 +158,14 @@ "2" + Mapping... Mapping Advanced mapping parameters for fine tuning. + Core + Database Append Mode When resuming mapping, wait for a relocalization on the current map before starting a new map. - Drift Correction - Iterative-closest-point (ICP) is done to refine geometrically the links in the map. Use only when environment is highly geometric. Camera should move slowly. Auto Exposure Adjust camera exposure depending on the lighting to get always maximum contrast. This may change texture color between scanned images. Color correction option in Post-Processing can help to uniformize colors. May not work on some devices. HD Mode @@ -164,14 +175,26 @@ Rate at which a new node is added to map. Time Limit Maximum time allowed for map updates. If time to add a new node is above this theshold, some old parts of the map are temporarly forgotten to reduce time of next updates. + Memory Limit + Maximum nodes kept in working memory. Loop Closure Threshold Threshold at which loop closure hypotheses are accepted. Higher means more robust to false loop closures while rejecting more good loop closures. + Similarity Threshold + Threshold at which consecutive images are considered the same, so the corresponding node\'s weight is increased. The background turns dark blue when this happens. Max Optimization Error Reject any loop closures causing error corrections in the map higher than this threshold. - Max Features Extracted - Extracting more features per image would result in better loop closure detection but more processing time required. + Max Features Extracted (Vocabulary) + Extracting more features per image would result in better loop closure hypotheses but more processing time is required. + Max Features Extracted (Loop Closure) + Extracting more features per image would result in better loop closure transforms but more processing time is required. Feature Type BRIEF features are fast to compute but are not rotation invariant like FREAK. Warning: Changing feature type will automatically reset the map! + Save All Frames in Database + Discarded frames while not moving are still saved in database. Useful to replay exactly the scanning on RTAB-Map Desktop. + Save Raw Scan + Save raw point clouds in database. + Database In Memory + The database is kept in RAM for fast access. Set to false to reduce RAM used at the cost of slower access. This parameter is applied on reset or when a database is opened. "Max" @@ -223,6 +246,25 @@ "400" + + "No Limit" + "500 nodes" + "400 nodes" + "300 nodes" + "200 nodes" + "100 nodes" + "50 nodes" + + + "0" + "500" + "400" + "300" + "200" + "100" + "50" + + "0.90" "0.80" @@ -232,7 +274,9 @@ "0.40" "0.30" "0.20" + "0.15" "0.11" + "0.10" "0.90" @@ -243,7 +287,28 @@ "0.40" "0.30" "0.20" + "0.15" "0.11" + "0.10" + + + + "Disabled" + "0.60" + "0.50" + "0.40" + "0.30" + "0.20" + "0.10" + + + "0" + "0.6" + "0.5" + "0.4" + "0.3" + "0.2" + "0.1" @@ -269,7 +334,7 @@ "0" - + "No Limit" "1000" "900" @@ -283,7 +348,7 @@ "100" "Disabled" - + "0" "1000" "900" @@ -298,6 +363,33 @@ "-1" + + "No Limit" + "1000" + "900" + "800" + "700" + "600" + "500" + "400" + "300" + "200" + "100" + + + "0" + "1000" + "900" + "800" + "700" + "600" + "500" + "400" + "300" + "200" + "100" + + "BRIEF" "FREAK" @@ -307,6 +399,7 @@ "5" + Exporting... Exporting Advanced parameters used when exporting the map. @@ -316,8 +409,10 @@ If the map is large, you may want to increase this to maximize the texture resolution. Normal K K-nearest neighbors used for normal computation when a mesh is created. + Max Texture Distance + Maximum distance from a camera for polygons to be textured by this camera. Block Rendering Thread While Exporting - This decreases exporting time, but freezes rendering while exporting. + This decreases exporting time, but freezes rendering while exporting. This also clears temporary the rendered clouds/meshes from memory during exporting, this can be useful to avoid out of memory errors. "0.2 m" @@ -325,6 +420,7 @@ "0.05 m" "0.02 m" "0.01 m" + "0.005 m" "Disabled" @@ -333,6 +429,7 @@ "0.05" "0.02" "0.01" + "0.005" "0" @@ -358,6 +455,27 @@ "12" "6" + + + "No Limit" + "5 m" + "4.5 m" + "4 m" + "3.5 m" + "3 m" + "2.5 m" + "2 m" + + + "0" + "5" + "4.5" + "4" + "3.5" + "3" + "2.5" + "2" + Optimized @@ -431,7 +549,7 @@ General Color Correction Radius - Radius used to find pixel correspondences for color correction. + Radius used to find pixel correspondences for Adjust Colors optimization. Min Cluster Size Minimum number of polygons for a cluster to be kept after Noise Filtering optimization. diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index b2936ba1..0b79a015 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -7,6 +7,8 @@ import java.io.FileInputStream; import java.io.FileOutputStream; import java.io.FilenameFilter; import java.io.IOException; +import java.io.InputStream; +import java.io.OutputStream; import java.text.SimpleDateFormat; import java.util.Arrays; import java.util.Date; @@ -15,6 +17,8 @@ import java.util.zip.ZipEntry; import java.util.zip.ZipOutputStream; import android.app.Activity; +import android.app.ActivityManager; +import android.app.ActivityManager.MemoryInfo; import android.app.AlertDialog; import android.app.Dialog; import android.app.Notification; @@ -30,8 +34,13 @@ import android.content.SharedPreferences; import android.content.pm.PackageInfo; import android.content.pm.PackageManager; import android.content.pm.PackageManager.NameNotFoundException; +import android.content.res.Configuration; import android.graphics.Bitmap; +import android.hardware.Camera; import android.graphics.Point; +import android.hardware.display.DisplayManager; +import android.net.ConnectivityManager; +import android.net.NetworkInfo; import android.net.Uri; import android.opengl.GLSurfaceView; import android.os.AsyncTask; @@ -49,6 +58,7 @@ import android.view.Menu; import android.view.MenuItem; import android.view.MenuInflater; import android.view.MotionEvent; +import android.view.Surface; import android.view.View; import android.view.View.OnClickListener; import android.view.WindowManager; @@ -80,6 +90,8 @@ public class RTABMapActivity extends Activity implements OnClickListener { public static final String EXTRA_VALUE_ADF = "ADF_LOAD_SAVE_PERMISSION"; public static final int ZIP_BUFFER_SIZE = 1<<20; // 1MB + + public static final String RTABMAP_TMP_DB = "rtabmap.tmp.db"; private static final String AUTHORIZE_PATH = "https://sketchfab.com/oauth2/authorize"; private static final String CLIENT_ID = "RXrIJYAwlTELpySsyM8TrK9r3kOGQ5Qjj9VVDIfV"; @@ -105,6 +117,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { // Screen size for normalizing the touch input for orbiting the render camera. private Point mScreenSize = new Point(); + private long mOnPauseStamp = 0; private boolean mOnPause = false; private MenuItem mItemSave; @@ -145,19 +158,28 @@ public class RTABMapActivity extends Activity implements OnClickListener { //Tango Service connection. ServiceConnection mTangoServiceConnection = new ServiceConnection() { - public void onServiceConnected(ComponentName name, IBinder service) { - if(!RTABMapLib.onTangoServiceConnected(service)) - { - mToast.makeText(getApplicationContext(), - String.format("Failed to intialize Tango!"), mToast.LENGTH_SHORT).show(); - } + public void onServiceConnected(ComponentName name, final IBinder service) { + Thread bindThread = new Thread(new Runnable() { + public void run() { + if(!RTABMapLib.onTangoServiceConnected(service)) + { + runOnUiThread(new Runnable() { + public void run() { + mToast.makeText(getApplicationContext(), + String.format("Failed to intialize Tango!"), mToast.LENGTH_LONG).show(); + } + }); + } + } + }); + bindThread.start(); } public void onServiceDisconnected(ComponentName name) { // Handle this if you need to gracefully shutdown/retry // in the event that Tango itself crashes/gets upgraded while running. mToast.makeText(getApplicationContext(), - String.format("Tango disconnected!"), mToast.LENGTH_SHORT).show(); + String.format("Tango disconnected!"), mToast.LENGTH_LONG).show(); } }; @@ -206,7 +228,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { mGLView.setEGLContextClientVersion(2); // Configure the OpenGL renderer. - mRenderer = new Renderer(); + mRenderer = new Renderer(this); mGLView.setRenderer(mRenderer); mLayoutDebug = (LinearLayout) findViewById(R.id.debug_layout); @@ -243,7 +265,31 @@ public class RTABMapActivity extends Activity implements OnClickListener { } RTABMapLib.onCreate(this); - RTABMapLib.openEmptyDatabase(); + String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB; + (new File(tmpDatabase)).delete(); + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); + boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory))); + RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, false); + + DisplayManager displayManager = (DisplayManager) getSystemService(DISPLAY_SERVICE); + if (displayManager != null) { + displayManager.registerDisplayListener(new DisplayManager.DisplayListener() { + @Override + public void onDisplayAdded(int displayId) { + + } + + @Override + public void onDisplayChanged(int displayId) { + synchronized (this) { + setAndroidOrientation(); + } + } + + @Override + public void onDisplayRemoved(int displayId) {} + }, null); + } } @Override @@ -258,78 +304,126 @@ public class RTABMapActivity extends Activity implements OnClickListener { } } } + + @Override + protected void onPause() { + super.onPause(); + + Log.i(TAG, "onPause()"); + mOnPause = true; + + // This deletes OpenGL context! + mGLView.onPause(); + + RTABMapLib.onPause(); + + unbindService(mTangoServiceConnection); + + if(!mButtonPause.isChecked()) + { + mButtonPause.setChecked(true); + pauseMapping(); + } + + mOnPauseStamp = System.currentTimeMillis()/1000; + } @Override protected void onResume() { super.onResume(); - - // update preferences - SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); - mUpdateRate = sharedPref.getString(getString(R.string.pref_key_update_rate), getString(R.string.pref_default_update_rate)); - mTimeThr = sharedPref.getString(getString(R.string.pref_key_time_thr), getString(R.string.pref_default_time_thr)); - mLoopThr = sharedPref.getString(getString(R.string.pref_key_loop_thr), getString(R.string.pref_default_loop_thr)); - String optError = sharedPref.getString(getString(R.string.pref_key_opt_error), getString(R.string.pref_default_opt_error)); - mMaxFeatures = sharedPref.getString(getString(R.string.pref_key_features), getString(R.string.pref_default_features)); - String featureType = sharedPref.getString(getString(R.string.pref_key_features_type), getString(R.string.pref_default_features_type)); - - RTABMapLib.setNodesFiltering(sharedPref.getBoolean(getString(R.string.pref_key_nodes_filtering), Boolean.parseBoolean(getString(R.string.pref_default_nodes_filtering)))); - RTABMapLib.setDriftCorrection(sharedPref.getBoolean(getString(R.string.pref_key_drift_correction), Boolean.parseBoolean(getString(R.string.pref_default_drift_correction)))); - RTABMapLib.setAutoExposure(sharedPref.getBoolean(getString(R.string.pref_key_auto_exposure), Boolean.parseBoolean(getString(R.string.pref_default_auto_exposure)))); - RTABMapLib.setFullResolution(sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution)))); - RTABMapLib.setAppendMode(sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append)))); - RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRate.compareTo("Max")==0?"0":mUpdateRate); - RTABMapLib.setMappingParameter("Rtabmap/TimeThr", mTimeThr.compareTo("No Limit") == 0?"0":mTimeThr); - RTABMapLib.setMappingParameter("Kp/MaxFeatures", mMaxFeatures.compareTo("Disabled")==0?"-1":mMaxFeatures.compareTo("No Limit")==0?"0":mMaxFeatures); - RTABMapLib.setMappingParameter("Rtabmap/LoopThr", mLoopThr.compareTo("Disabled")==0?"1":mLoopThr); - RTABMapLib.setMappingParameter("RGBD/OptimizeMaxError", optError.compareTo("Disabled")==0?"0":optError); - RTABMapLib.setMappingParameter("Kp/DetectorStrategy", featureType); - - RTABMapLib.setMeshDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_decimation), getString(R.string.pref_default_decimation)))); - RTABMapLib.setMaxCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth), getString(R.string.pref_default_depth)))); - RTABMapLib.setPointSize(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_point_size), getString(R.string.pref_default_point_size)))); - RTABMapLib.setMeshAngleTolerance(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_angle), getString(R.string.pref_default_angle)))); - RTABMapLib.setMeshTriangleSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_triangle), getString(R.string.pref_default_triangle)))); - RTABMapLib.setMinClusterSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_min_cluster_size), getString(R.string.pref_default_min_cluster_size)))); - RTABMapLib.setMaxGainRadius(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_gain_max_radius), getString(R.string.pref_default_gain_max_radius)))); - - if(mItemRenderingPointCloud != null) + mProgressDialog.setTitle(""); + if(mOnPause) { - int renderingType = sharedPref.getInt(getString(R.string.pref_key_rendering), Integer.parseInt(getString(R.string.pref_default_rendering))); - if(renderingType == 0) + if(System.currentTimeMillis()/1000 - mOnPauseStamp < 1) { - mItemRenderingPointCloud.setChecked(true); - } - else if(renderingType == 1) - { - mItemRenderingMesh.setChecked(true); + mProgressDialog.setMessage(String.format("RTAB-Map has been interrupted by another application, Tango should be re-initialized! Set your phone/tablet in Airplane mode if this happens often.")); } else { - mItemRenderingTextureMesh.setChecked(true); + mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service...")); } - RTABMapLib.setMeshRendering( - mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), - mItemRenderingTextureMesh.isChecked()); - } - - - mProgressDialog.setTitle(""); - mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service...")); - mProgressDialog.show(); - - if(mOnPause) - { mToast.makeText(this, "Mapping is paused!", mToast.LENGTH_LONG).show(); } else { - mToast.makeText(this, "Tip: If the camera is still drifting just after the mapping has started, do \"Reset\".", mToast.LENGTH_LONG).show(); + mProgressDialog.setMessage(String.format("Hold Tight! Initializing Tango Service...\nTip: If the camera is still drifting just after the mapping has started, do \"Reset\".")); } - + mProgressDialog.show(); mOnPause = false; + + setAndroidOrientation(); - TangoInitializationHelper.bindTangoService(this, mTangoServiceConnection); + // update preferences + try + { + Log.d(TAG, "update preferences..."); + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); + mUpdateRate = sharedPref.getString(getString(R.string.pref_key_update_rate), getString(R.string.pref_default_update_rate)); + mTimeThr = sharedPref.getString(getString(R.string.pref_key_time_thr), getString(R.string.pref_default_time_thr)); + String memThr = sharedPref.getString(getString(R.string.pref_key_mem_thr), getString(R.string.pref_default_mem_thr)); + mLoopThr = sharedPref.getString(getString(R.string.pref_key_loop_thr), getString(R.string.pref_default_loop_thr)); + String simThr = sharedPref.getString(getString(R.string.pref_key_sim_thr), getString(R.string.pref_default_sim_thr)); + String optError = sharedPref.getString(getString(R.string.pref_key_opt_error), getString(R.string.pref_default_opt_error)); + mMaxFeatures = sharedPref.getString(getString(R.string.pref_key_features_voc), getString(R.string.pref_default_features_voc)); + String maxFeaturesLoop = sharedPref.getString(getString(R.string.pref_key_features), getString(R.string.pref_default_features)); + String featureType = sharedPref.getString(getString(R.string.pref_key_features_type), getString(R.string.pref_default_features_type)); + boolean keepAllDb = sharedPref.getBoolean(getString(R.string.pref_key_keep_all_db), Boolean.parseBoolean(getString(R.string.pref_default_keep_all_db))); + + Log.d(TAG, "set mapping parameters"); + RTABMapLib.setNodesFiltering(sharedPref.getBoolean(getString(R.string.pref_key_nodes_filtering), Boolean.parseBoolean(getString(R.string.pref_default_nodes_filtering)))); + RTABMapLib.setAutoExposure(sharedPref.getBoolean(getString(R.string.pref_key_auto_exposure), Boolean.parseBoolean(getString(R.string.pref_default_auto_exposure)))); + RTABMapLib.setRawScanSaved(sharedPref.getBoolean(getString(R.string.pref_key_raw_scan_saved), Boolean.parseBoolean(getString(R.string.pref_default_raw_scan_saved)))); + RTABMapLib.setFullResolution(sharedPref.getBoolean(getString(R.string.pref_key_resolution), Boolean.parseBoolean(getString(R.string.pref_default_resolution)))); + RTABMapLib.setAppendMode(sharedPref.getBoolean(getString(R.string.pref_key_append), Boolean.parseBoolean(getString(R.string.pref_default_append)))); + RTABMapLib.setMappingParameter("Rtabmap/DetectionRate", mUpdateRate); + RTABMapLib.setMappingParameter("Rtabmap/TimeThr", mTimeThr); + RTABMapLib.setMappingParameter("Rtabmap/MemoryThr", memThr); + RTABMapLib.setMappingParameter("Mem/RehearsalSimilarity", simThr); + RTABMapLib.setMappingParameter("Kp/MaxFeatures", mMaxFeatures); + RTABMapLib.setMappingParameter("Vis/MaxFeatures", maxFeaturesLoop); + RTABMapLib.setMappingParameter("Rtabmap/LoopThr", mLoopThr); + RTABMapLib.setMappingParameter("RGBD/OptimizeMaxError", optError); + RTABMapLib.setMappingParameter("Kp/DetectorStrategy", featureType); + RTABMapLib.setMappingParameter("Vis/FeatureType", featureType); + RTABMapLib.setMappingParameter("Mem/NotLinkedNodesKept", String.valueOf(keepAllDb)); + + Log.d(TAG, "set exporting parameters..."); + RTABMapLib.setMeshDecimation(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_density), getString(R.string.pref_default_density)))); + RTABMapLib.setMaxCloudDepth(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_depth), getString(R.string.pref_default_depth)))); + RTABMapLib.setPointSize(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_point_size), getString(R.string.pref_default_point_size)))); + RTABMapLib.setMeshAngleTolerance(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_angle), getString(R.string.pref_default_angle)))); + RTABMapLib.setMeshTriangleSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_triangle), getString(R.string.pref_default_triangle)))); + + Log.d(TAG, "set rendering parameters..."); + RTABMapLib.setMinClusterSize(Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_min_cluster_size), getString(R.string.pref_default_min_cluster_size)))); + RTABMapLib.setMaxGainRadius(Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_gain_max_radius), getString(R.string.pref_default_gain_max_radius)))); + + if(mItemRenderingPointCloud != null) + { + int renderingType = sharedPref.getInt(getString(R.string.pref_key_rendering), Integer.parseInt(getString(R.string.pref_default_rendering))); + if(renderingType == 0) + { + mItemRenderingPointCloud.setChecked(true); + } + else if(renderingType == 1) + { + mItemRenderingMesh.setChecked(true); + } + else + { + mItemRenderingTextureMesh.setChecked(true); + } + RTABMapLib.setMeshRendering( + mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), + mItemRenderingTextureMesh.isChecked()); + } + } + catch(Exception e) + { + Log.e(TAG, "Error parsing preferences: " + e.getMessage()); + mToast.makeText(this, String.format("Error parsing preferences: "+e.getMessage()), mToast.LENGTH_LONG).show(); + } Log.i(TAG, String.format("onResume()")); @@ -343,28 +437,10 @@ public class RTABMapActivity extends Activity implements OnClickListener { Tango.getRequestPermissionIntent(Tango.PERMISSIONTYPE_MOTION_TRACKING), Tango.TANGO_INTENT_ACTIVITYCODE); } + + TangoInitializationHelper.bindTangoService(getActivity(), mTangoServiceConnection); } - - @Override - protected void onPause() { - super.onPause(); - - // This deletes OpenGL context! - mGLView.onPause(); - - mOnPause = true; - - RTABMapLib.onPause(); - - unbindService(mTangoServiceConnection); - - if(!mButtonPause.isChecked()) - { - mButtonPause.setChecked(true); - pauseMapping(); - } - } - + private void setCamera(int type) { RTABMapLib.setCamera(type); @@ -400,6 +476,13 @@ public class RTABMapActivity extends Activity implements OnClickListener { return; } } + + private void setAndroidOrientation() { + Display display = getWindowManager().getDefaultDisplay(); + Camera.CameraInfo colorCameraInfo = new Camera.CameraInfo(); + Camera.getCameraInfo(0, colorCameraInfo); + RTABMapLib.setScreenRotation(display.getRotation(), colorCameraInfo.orientation); + } @Override public boolean onTouchEvent(MotionEvent event) { @@ -438,6 +521,9 @@ public class RTABMapActivity extends Activity implements OnClickListener { MenuInflater inflater = getMenuInflater(); inflater.inflate(R.menu.optionmenu, menu); + + getActionBar().setDisplayShowHomeEnabled(true); + getActionBar().setIcon(R.drawable.ic_launcher); mItemSave = menu.findItem(R.id.save); mItemOpen = menu.findItem(R.id.open); @@ -458,28 +544,44 @@ public class RTABMapActivity extends Activity implements OnClickListener { mItemPostProcessing.setEnabled(false); mItemDataRecorderMode.setEnabled(false); - SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); - int renderingType = sharedPref.getInt(getString(R.string.pref_key_rendering), Integer.parseInt(getString(R.string.pref_default_rendering))); - if(renderingType == 0) + try { - mItemRenderingPointCloud.setChecked(true); + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); + int renderingType = sharedPref.getInt(getString(R.string.pref_key_rendering), Integer.parseInt(getString(R.string.pref_default_rendering))); + if(renderingType == 0) + { + mItemRenderingPointCloud.setChecked(true); + } + else if(renderingType == 1) + { + mItemRenderingMesh.setChecked(true); + } + else + { + mItemRenderingTextureMesh.setChecked(true); + } + RTABMapLib.setMeshRendering( + mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), + mItemRenderingTextureMesh.isChecked()); } - else if(renderingType == 1) + catch(Exception e) { - mItemRenderingMesh.setChecked(true); + Log.e(TAG, "Error parsing rendering preferences: " + e.getMessage()); + mToast.makeText(this, String.format("Error parsing rendering preferences: "+e.getMessage()), mToast.LENGTH_LONG).show(); } - else - { - mItemRenderingTextureMesh.setChecked(true); - } - RTABMapLib.setMeshRendering( - mItemRenderingMesh.isChecked() || mItemRenderingTextureMesh.isChecked(), - mItemRenderingTextureMesh.isChecked()); updateState(mState); return true; } + + private long getFreeMemory() + { + MemoryInfo mi = new MemoryInfo(); + ActivityManager activityManager = (ActivityManager) getSystemService(ACTIVITY_SERVICE); + activityManager.getMemoryInfo(mi); + return mi.availMem / 0x100000L; // MB + } private void updateStatsUI( int nodes, @@ -495,7 +597,8 @@ public class RTABMapActivity extends Activity implements OnClickListener { float hypothesis, int nodesDrawn, float fps, - int rejected) + int rejected, + float rehearsalValue) { if(mButtonPause!=null) { @@ -505,7 +608,8 @@ public class RTABMapActivity extends Activity implements OnClickListener { } else { - ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", mUpdateRate):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", mUpdateRate):String.format("Mapping (%s Hz)", mUpdateRate)); + String updateValue = mUpdateRate.compareTo("0")==0?"Max":mUpdateRate; + ((TextView)findViewById(R.id.status)).setText(mItemLocalizationMode.isChecked()?String.format("Localization (%s Hz)", updateValue):mItemDataRecorderMode.isChecked()?String.format("Recording (%s Hz)", updateValue):String.format("Mapping (%s Hz)", updateValue)); } } @@ -514,10 +618,12 @@ public class RTABMapActivity extends Activity implements OnClickListener { ((TextView)findViewById(R.id.nodes)).setText(String.format("%d (%d shown)", nodes, nodesDrawn)); ((TextView)findViewById(R.id.words)).setText(String.valueOf(words)); ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024))); - ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(databaseMemoryUsed)); + ((TextView)findViewById(R.id.free_memory)).setText(String.valueOf(getFreeMemory())); + ((TextView)findViewById(R.id.database_size)).setText(String.valueOf(databaseMemoryUsed)); ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(inliers)); - ((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeatures)); - ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThr)); + ((TextView)findViewById(R.id.features)).setText(String.format("%d / %s", featuresExtracted, mMaxFeatures.compareTo("0")==0?"No Limit":mMaxFeatures.compareTo("-1")==0?"Disabled":mMaxFeatures)); + ((TextView)findViewById(R.id.rehearsal)).setText(String.format("%.3f", rehearsalValue)); + ((TextView)findViewById(R.id.update_time)).setText(String.format("%.3f / %s", updateTime, mTimeThr.compareTo("0")==0?"No Limit":mTimeThr)); ((TextView)findViewById(R.id.hypothesis)).setText(String.format("%.3f / %s (%d)", hypothesis, mLoopThr, loopClosureId>0?loopClosureId:highestHypId)); ((TextView)findViewById(R.id.fps)).setText(String.format("%.3f Hz", fps)); if(mButtonPause!=null && !mButtonPause.isChecked()) @@ -553,13 +659,14 @@ public class RTABMapActivity extends Activity implements OnClickListener { final float hypothesis, final int nodesDrawn, final float fps, - final int rejected) + final int rejected, + final float rehearsalValue) { Log.i(TAG, String.format("updateStatsCallback()")); runOnUiThread(new Runnable() { public void run() { - updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, fps, rejected); + updateStatsUI(nodes, words, points, polygons, updateTime, loopClosureId, highestHypId, databaseMemoryUsed, inliers, features, hypothesis, nodesDrawn, fps, rejected, rehearsalValue); } }); } @@ -692,7 +799,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { @Override public boolean accept(File dir, String filename) { File sel = new File(dir, filename); - return filename.endsWith(".db"); + return filename.compareTo(RTABMAP_TMP_DB) != 0 && filename.endsWith(".db"); } }; @@ -859,33 +966,6 @@ public class RTABMapActivity extends Activity implements OnClickListener { }); workingThread.start(); } - else if (itemId == R.id.icp_refining) - { - mProgressDialog.setTitle("Post-Processing"); - mProgressDialog.setMessage(String.format("Please wait while refining links...")); - mProgressDialog.show(); - updateState(State.STATE_PROCESSING); - Thread workingThread = new Thread(new Runnable() { - public void run() { - final int linksRefined = RTABMapLib.postProcessing(3); - runOnUiThread(new Runnable() { - public void run() { - mProgressDialog.dismiss(); - if(linksRefined >= 0) - { - mToast.makeText(getActivity(), String.format("Refining done! %d link(s) refined.", linksRefined), mToast.LENGTH_SHORT).show(); - } - else if(linksRefined < 0) - { - mToast.makeText(getActivity(), String.format("Refining failed!"), mToast.LENGTH_SHORT).show(); - } - updateState(State.STATE_IDLE); - } - }); - } - }); - workingThread.start(); - } else if (itemId == R.id.global_graph_optimization) { mProgressDialog.setTitle("Post-Processing"); @@ -1201,7 +1281,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { ((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.words)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024))); - ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0)); + ((TextView)findViewById(R.id.free_memory)).setText(String.valueOf(getFreeMemory())); ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.features)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0)); @@ -1210,19 +1290,18 @@ public class RTABMapActivity extends Activity implements OnClickListener { mTotalLoopClosures = 0; ((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures)); - if(mOpenedDatabasePath.isEmpty()) - { - RTABMapLib.resetMapping(); - } - else - { - mOpenedDatabasePath = ""; - RTABMapLib.openEmptyDatabase(); - } + mOpenedDatabasePath = ""; + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(this); + boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory))); + String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB; + (new File(tmpDatabase)).delete(); + RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, false); + mMapIsEmpty = true; mItemSave.setEnabled(false); mItemExport.setEnabled(false); mItemPostProcessing.setEnabled(false); + updateState(State.STATE_IDLE); } else if(itemId == R.id.data_recorder) { @@ -1238,7 +1317,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { ((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.words)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024))); - ((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0)); + ((TextView)findViewById(R.id.free_memory)).setText(String.valueOf(getFreeMemory())); ((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.features)).setText(String.valueOf(0)); ((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0)); @@ -1251,7 +1330,11 @@ public class RTABMapActivity extends Activity implements OnClickListener { RTABMapLib.setDataRecorderMode(mItemDataRecorderMode.isChecked()); mOpenedDatabasePath = ""; - RTABMapLib.openEmptyDatabase(); + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity()); + boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory))); + String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB; + (new File(tmpDatabase)).delete(); + RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, false); mItemOpen.setEnabled(!mItemDataRecorderMode.isChecked() && mButtonPause.isChecked()); mItemPostProcessing.setEnabled(!mItemDataRecorderMode.isChecked() && mButtonPause.isChecked()); @@ -1295,6 +1378,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { final float cloudVoxelSize = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_cloud_voxel), getString(R.string.pref_default_cloud_voxel))); final int textureSize = isOBJ?Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_texture_size), getString(R.string.pref_default_texture_size))):0; final int normalK = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_normal_k), getString(R.string.pref_default_normal_k))); + final float maxTextureDistance = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_max_texture_distance), getString(R.string.pref_default_max_texture_distance))); final float optimizedVoxelSize = cloudVoxelSize; final int optimizedDepth = Integer.parseInt(sharedPref.getString(getString(R.string.pref_key_opt_depth), getString(R.string.pref_default_opt_depth))); final float optimizedDecimationFactor = Float.parseFloat(sharedPref.getString(getString(R.string.pref_key_opt_decimation_factor), getString(R.string.pref_default_opt_decimation_factor)))/100.0f; @@ -1353,6 +1437,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { meshing, textureSize, normalK, + maxTextureDistance, optimized, optimizedVoxelSize, optimizedDepth, @@ -1454,6 +1539,7 @@ public class RTABMapActivity extends Activity implements OnClickListener { meshing, textureSize, normalK, + maxTextureDistance, optimized, optimizedVoxelSize, optimizedDepth, @@ -1553,18 +1639,24 @@ public class RTABMapActivity extends Activity implements OnClickListener { AlertDialog.Builder builder = new AlertDialog.Builder(this); builder.setTitle("Choose Your File (*.db)"); builder.setItems(filesWithSize, new DialogInterface.OnClickListener() { - public void onClick(DialogInterface dialog, int which) { - mOpenedDatabasePath = mWorkingDirectory + files[which]; - - if(!mItemTrajectoryMode.isChecked()) - { - mProgressDialog.setTitle("Loading"); - mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", files[which])); - mProgressDialog.show(); - } - - RTABMapLib.openDatabase(mOpenedDatabasePath); - setCamera(1); + public void onClick(DialogInterface dialog, final int which) { + + // Smooth and adjust color now? + new AlertDialog.Builder(getActivity()) + .setTitle("Opening database...") + .setMessage("Do you want to smooth and adjust colors now?\nThis can be done later under Optimize menu.") + .setPositiveButton("Yes", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int whichIn) { + openDatabase(files[which], true); + } + }) + .setNeutralButton("No", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int whichIn) { + openDatabase(files[which], false); + } + }) + .show(); + return; } }); builder.show(); @@ -1584,6 +1676,56 @@ public class RTABMapActivity extends Activity implements OnClickListener { return true; } + + private void openDatabase(String fileName, boolean optimize) + { + mOpenedDatabasePath = mWorkingDirectory + fileName; + + if(!mItemTrajectoryMode.isChecked()) + { + mProgressDialog.setTitle("Loading"); + mProgressDialog.setMessage(String.format("Database \"%s\" loaded. Please wait while creating point clouds and meshes...", fileName)); + mProgressDialog.show(); + updateState(State.STATE_PROCESSING); + } + + String tmpDatabase = mWorkingDirectory+RTABMAP_TMP_DB; + (new File(tmpDatabase)).delete(); + try{ + copy(new File(mOpenedDatabasePath), new File(tmpDatabase)); + } + catch(IOException e) + { + mToast.makeText(getActivity(), String.format("Failed to create temp database from %s!", mOpenedDatabasePath), mToast.LENGTH_LONG).show(); + } + + SharedPreferences sharedPref = PreferenceManager.getDefaultSharedPreferences(getActivity()); + boolean databaseInMemory = sharedPref.getBoolean(getString(R.string.pref_key_db_in_memory), Boolean.parseBoolean(getString(R.string.pref_default_db_in_memory))); + RTABMapLib.openDatabase(tmpDatabase, databaseInMemory, optimize); + setCamera(1); + updateState(State.STATE_IDLE); + } + + public void copy(File src, File dst) throws IOException { + InputStream in = new FileInputStream(src); + OutputStream out = new FileOutputStream(dst); + + // Transfer bytes from in to out + byte[] buf = new byte[1024]; + int len; + while ((len = in.read(buf)) > 0) { + out.write(buf, 0, len); + } + in.close(); + out.close(); + } + + private boolean isNetworkAvailable() { + ConnectivityManager connectivityManager + = (ConnectivityManager) getSystemService(Context.CONNECTIVITY_SERVICE); + NetworkInfo activeNetworkInfo = connectivityManager.getActiveNetworkInfo(); + return activeNetworkInfo != null && activeNetworkInfo.isConnected(); + } public static void zip(String file, String zipFile) throws IOException { Log.i(TAG, "Zipping " + file +" to " + zipFile); @@ -1662,54 +1804,78 @@ public class RTABMapActivity extends Activity implements OnClickListener { if(files.length > 0) { final String[] filesToZip = files; - // get token the first time - if(mAuthToken == null) - { - Log.i(TAG,"We don't have the token, get it!"); + authorizeAndPublish(filesToZip, fileName); + } + } + + private void authorizeAndPublish(final String[] filesToZip, final String fileName) + { + if(!isNetworkAvailable()) + { + // Visualize the result? + new AlertDialog.Builder(getActivity()) + .setTitle("Sharing to Sketchfab...") + .setMessage("Network is not available. Make sure you have internet before continuing.") + .setPositiveButton("Try Again", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int which) { + authorizeAndPublish(filesToZip, fileName); + } + }) + .setNeutralButton("Abort", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int which) { + } + }) + .show(); + return; + } - WebView web; - mAuthDialog = new Dialog(this); - mAuthDialog.setContentView(R.layout.auth_dialog); - web = (WebView)mAuthDialog.findViewById(R.id.webv); - web.getSettings().setJavaScriptEnabled(true); - String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID; - Log.i(TAG, "Auhorize url="+auth_url); - web.setWebViewClient(new WebViewClient() { + // get token the first time + if(mAuthToken == null) + { + Log.i(TAG,"We don't have the token, get it!"); - boolean authComplete = false; + WebView web; + mAuthDialog = new Dialog(this); + mAuthDialog.setContentView(R.layout.auth_dialog); + web = (WebView)mAuthDialog.findViewById(R.id.webv); + web.getSettings().setJavaScriptEnabled(true); + String auth_url = AUTHORIZE_PATH+"?redirect_uri="+REDIRECT_URI+"&response_type=token&client_id="+CLIENT_ID; + Log.i(TAG, "Auhorize url="+auth_url); + web.setWebViewClient(new WebViewClient() { - @Override - public void onPageFinished(WebView view, String url) { - super.onPageFinished(view, url); + boolean authComplete = false; - //Log.i(TAG,"onPageFinished url="+url); - if(url.contains("error=access_denied")){ - Log.e(TAG, "ACCESS_DENIED_HERE"); - authComplete = true; - Toast.makeText(getApplicationContext(), "Error Occured", Toast.LENGTH_SHORT).show(); - mAuthDialog.dismiss(); - } - else if (url.startsWith(REDIRECT_URI) && url.contains("access_token") && authComplete != true) { - //Log.i(TAG,"onPageFinished received token="+url); - String[] sArray = url.split("access_token="); - mAuthToken = (sArray[1].split("&token_type=Bearer"))[0]; - authComplete = true; + @Override + public void onPageFinished(WebView view, String url) { + super.onPageFinished(view, url); - mAuthDialog.dismiss(); - - zipAndPublish(filesToZip, fileName); - } + //Log.i(TAG,"onPageFinished url="+url); + if(url.contains("error=access_denied")){ + Log.e(TAG, "ACCESS_DENIED_HERE"); + authComplete = true; + Toast.makeText(getApplicationContext(), "Error Occured", Toast.LENGTH_SHORT).show(); + mAuthDialog.dismiss(); } - }); - mAuthDialog.show(); - mAuthDialog.setTitle("Authorize RTAB-Map"); - mAuthDialog.setCancelable(true); - web.loadUrl(auth_url); - } - else - { - zipAndPublish(filesToZip, fileName); - } + else if (url.startsWith(REDIRECT_URI) && url.contains("access_token") && authComplete != true) { + //Log.i(TAG,"onPageFinished received token="+url); + String[] sArray = url.split("access_token="); + mAuthToken = (sArray[1].split("&token_type=Bearer"))[0]; + authComplete = true; + + mAuthDialog.dismiss(); + + zipAndPublish(filesToZip, fileName); + } + } + }); + mAuthDialog.show(); + mAuthDialog.setTitle("Authorize RTAB-Map"); + mAuthDialog.setCancelable(true); + web.loadUrl(auth_url); + } + else + { + zipAndPublish(filesToZip, fileName); } } diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index 57b15ff6..0eed8d46 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapLib.java @@ -25,8 +25,9 @@ public class RTABMapLib // The activity object is used for checking if the API version is outdated. public static native void onCreate(RTABMapActivity activity); - public static native void openEmptyDatabase(); - public static native void openDatabase(String databasePath); + public static native void setScreenRotation(int displayRotation, int cameraRotation); + + public static native void openDatabase(String databasePath, boolean databaseInMemory, boolean optimize); /* * Called when the Tango service is connected. @@ -64,10 +65,10 @@ public class RTABMapLib public static native void setTrajectoryMode(boolean enabled); public static native void setGraphOptimization(boolean enabled); public static native void setNodesFiltering(boolean enabled); - public static native void setDriftCorrection(boolean enabled); public static native void setGraphVisible(boolean visible); public static native void setGridVisible(boolean visible); public static native void setAutoExposure(boolean enabled); + public static native void setRawScanSaved(boolean enabled); public static native void setFullResolution(boolean enabled); public static native void setAppendMode(boolean enabled); public static native void setDataRecorderMode(boolean enabled); @@ -89,6 +90,7 @@ public class RTABMapLib boolean meshing, int textureSize, int normalK, + float maxTextureDistance, boolean optimized, float optimizedVoxelSize, int optimizedDepth, diff --git a/app/android/src/com/introlab/rtabmap/Renderer.java b/app/android/src/com/introlab/rtabmap/Renderer.java index 588ecd92..1fd0ccf3 100644 --- a/app/android/src/com/introlab/rtabmap/Renderer.java +++ b/app/android/src/com/introlab/rtabmap/Renderer.java @@ -16,8 +16,12 @@ package com.introlab.rtabmap; +import android.app.Activity; import android.app.ProgressDialog; +import android.content.Context; import android.opengl.GLSurfaceView; +import android.util.Log; +import android.widget.Toast; import javax.microedition.khronos.egl.EGLConfig; import javax.microedition.khronos.opengles.GL10; @@ -27,6 +31,11 @@ import javax.microedition.khronos.opengles.GL10; // device's pose. public class Renderer implements GLSurfaceView.Renderer { + private static Activity mActivity; + public Renderer(Activity c) { + mActivity = c; + } + private ProgressDialog mProgressDialog; public void setProgressDialog(ProgressDialog progressDialog) @@ -34,14 +43,35 @@ public class Renderer implements GLSurfaceView.Renderer { mProgressDialog = progressDialog; } - // Render loop of the Gl context. - public void onDrawFrame(GL10 gl) { - int value = RTABMapLib.render(); - if(value == 1 && mProgressDialog != null) - { - mProgressDialog.dismiss(); - } - } + // Render loop of the Gl context. + public void onDrawFrame(GL10 gl) { + try + { + final int value = RTABMapLib.render(); + mActivity.runOnUiThread(new Runnable() { + public void run() { + if(value != 0 && mProgressDialog != null && mProgressDialog.isShowing()) + { + Log.i("RTABMapActivity", "Renderer: dismiss dialog, value received=" + String.valueOf(value)); + mProgressDialog.dismiss(); + } + if(value==-1) + { + Toast.makeText(mActivity, String.format("Out of Memory!"), Toast.LENGTH_LONG).show(); + } + } + }); + } + catch(final Exception e) + { + mActivity.runOnUiThread(new Runnable() { + public void run() { + Toast.makeText(mActivity, String.format("Rendering error! %s", e.getMessage()), Toast.LENGTH_LONG).show(); + } + + }); + } + } // Called when the surface size changes. public void onSurfaceChanged(GL10 gl, int width, int height) { diff --git a/app/android/src/com/introlab/rtabmap/SettingsActivity.java b/app/android/src/com/introlab/rtabmap/SettingsActivity.java index 43e90260..002693c1 100644 --- a/app/android/src/com/introlab/rtabmap/SettingsActivity.java +++ b/app/android/src/com/introlab/rtabmap/SettingsActivity.java @@ -28,7 +28,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref } }); - ((Preference)findPreference(getString(R.string.pref_key_decimation))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_decimation))).getEntry() + ") "+getString(R.string.pref_summary_decimation)); + ((Preference)findPreference(getString(R.string.pref_key_density))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_density))).getEntry() + ") "+getString(R.string.pref_summary_density)); ((Preference)findPreference(getString(R.string.pref_key_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_depth))).getEntry() + ") "+getString(R.string.pref_summary_depth)); ((Preference)findPreference(getString(R.string.pref_key_point_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_point_size))).getEntry() + ") "+getString(R.string.pref_summary_point_size)); ((Preference)findPreference(getString(R.string.pref_key_angle))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_angle))).getEntry() + ") "+getString(R.string.pref_summary_angle)); @@ -36,14 +36,18 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref ((Preference)findPreference(getString(R.string.pref_key_update_rate))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_update_rate))).getEntry() + ") "+getString(R.string.pref_summary_update_rate)); ((Preference)findPreference(getString(R.string.pref_key_time_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_time_thr))).getEntry() + ") "+getString(R.string.pref_summary_time_thr)); + ((Preference)findPreference(getString(R.string.pref_key_mem_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_mem_thr))).getEntry() + ") "+getString(R.string.pref_summary_mem_thr)); ((Preference)findPreference(getString(R.string.pref_key_loop_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_loop_thr))).getEntry() + ") "+getString(R.string.pref_summary_loop_thr)); + ((Preference)findPreference(getString(R.string.pref_key_sim_thr))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_sim_thr))).getEntry() + ") "+getString(R.string.pref_summary_sim_thr)); ((Preference)findPreference(getString(R.string.pref_key_opt_error))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_error))).getEntry() + ") "+getString(R.string.pref_summary_opt_error)); + ((Preference)findPreference(getString(R.string.pref_key_features_voc))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_voc))).getEntry() + ") "+getString(R.string.pref_summary_features_voc)); ((Preference)findPreference(getString(R.string.pref_key_features))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features))).getEntry() + ") "+getString(R.string.pref_summary_features)); ((Preference)findPreference(getString(R.string.pref_key_features_type))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_features_type))).getEntry() + ") "+getString(R.string.pref_summary_features_type)); ((Preference)findPreference(getString(R.string.pref_key_cloud_voxel))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_cloud_voxel))).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel)); ((Preference)findPreference(getString(R.string.pref_key_texture_size))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_texture_size))).getEntry() + ") "+getString(R.string.pref_summary_texture_size)); ((Preference)findPreference(getString(R.string.pref_key_normal_k))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_normal_k))).getEntry() + ") "+getString(R.string.pref_summary_normal_k)); + ((Preference)findPreference(getString(R.string.pref_key_max_texture_distance))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_max_texture_distance))).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance)); ((Preference)findPreference(getString(R.string.pref_key_opt_depth))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_depth))).getEntry() + ") "+getString(R.string.pref_summary_opt_depth)); ((Preference)findPreference(getString(R.string.pref_key_opt_decimation_factor))).setSummary("("+((ListPreference)findPreference(getString(R.string.pref_key_opt_decimation_factor))).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor)); @@ -57,7 +61,7 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref Preference pref = findPreference(key); if (pref instanceof ListPreference) { - if(key.compareTo(getString(R.string.pref_key_decimation))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_decimation)); + if(key.compareTo(getString(R.string.pref_key_density))==0) pref.setSummary("("+ ((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_density)); if(key.compareTo(getString(R.string.pref_key_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_depth)); if(key.compareTo(getString(R.string.pref_key_point_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_point_size)); if(key.compareTo(getString(R.string.pref_key_angle))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_angle)); @@ -65,14 +69,18 @@ public class SettingsActivity extends PreferenceActivity implements OnSharedPref if(key.compareTo(getString(R.string.pref_key_update_rate))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_update_rate)); if(key.compareTo(getString(R.string.pref_key_time_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_time_thr)); + if(key.compareTo(getString(R.string.pref_key_mem_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_mem_thr)); if(key.compareTo(getString(R.string.pref_key_loop_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_loop_thr)); + if(key.compareTo(getString(R.string.pref_key_sim_thr))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_sim_thr)); if(key.compareTo(getString(R.string.pref_key_opt_error))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_error)); + if(key.compareTo(getString(R.string.pref_key_features_voc))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_voc)); if(key.compareTo(getString(R.string.pref_key_features))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features)); if(key.compareTo(getString(R.string.pref_key_features_type))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_features_type)); if(key.compareTo(getString(R.string.pref_key_cloud_voxel))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_cloud_voxel)); if(key.compareTo(getString(R.string.pref_key_texture_size))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_texture_size)); if(key.compareTo(getString(R.string.pref_key_normal_k))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_normal_k)); + if(key.compareTo(getString(R.string.pref_key_max_texture_distance))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_max_texture_distance)); if(key.compareTo(getString(R.string.pref_key_opt_depth))==0) pref.setSummary("("+((ListPreference)pref).getEntry() + ") "+getString(R.string.pref_summary_opt_depth)); if(key.compareTo(getString(R.string.pref_key_opt_decimation_factor))==0) pref.setSummary("("+((ListPreference)pref).getValue() + "%%) "+getString(R.string.pref_summary_opt_decimation_factor)); diff --git a/corelib/include/rtabmap/core/Parameters.h b/corelib/include/rtabmap/core/Parameters.h index 16b96b6f..d65b2305 100644 --- a/corelib/include/rtabmap/core/Parameters.h +++ b/corelib/include/rtabmap/core/Parameters.h @@ -223,9 +223,9 @@ class RTABMAP_EXP Parameters RTABMAP_PARAM(Kp, BadSignRatio, float, 0.5, "Bad signature ratio (less than Ratio x AverageWordsPerImage = bad)."); RTABMAP_PARAM(Kp, NndrRatio, float, 0.8, "NNDR ratio (A matching pair is detected, if its distance is closer than X times the distance of the second nearest neighbor.)"); #ifdef RTABMAP_NONFREE - RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); + RTABMAP_PARAM(Kp, DetectorStrategy, int, 0, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK."); #else - RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); + RTABMAP_PARAM(Kp, DetectorStrategy, int, 2, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK."); #endif RTABMAP_PARAM(Kp, TfIdfLikelihoodUsed, bool, true, "Use of the td-idf strategy to compute the likelihood."); RTABMAP_PARAM(Kp, Parallelized, bool, true, "If the dictionary update and signature creation were parallelized."); @@ -412,12 +412,12 @@ class RTABMAP_EXP Parameters #ifndef RTABMAP_NONFREE #ifdef RTABMAP_OPENCV3 // OpenCV 3 without xFeatures2D module doesn't have BRIEF - RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); + RTABMAP_PARAM(Vis, FeatureType, int, 8, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK."); #else - RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); + RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK."); #endif #else - RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB."); + RTABMAP_PARAM(Vis, FeatureType, int, 6, "0=SURF 1=SIFT 2=ORB 3=FAST/FREAK 4=FAST/BRIEF 5=GFTT/FREAK 6=GFTT/BRIEF 7=BRISK 8=GFTT/ORB 9=FREAK."); #endif RTABMAP_PARAM(Vis, MaxFeatures, int, 1000, "0 no limits."); diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 48652305..346c93ed 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -440,18 +440,22 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks()); } } - else if(save && !outputUrl.empty() && outputUrl.compare(this->getUrl()) != 0) - { - UWARN("Output database path (%s) is different than the opened database " - "path (%s). Exporting to a different path is only available " - "when database is in memory (%s=true). Opened database path is overwritten.", - outputUrl.c_str(), this->getUrl().c_str(), Parameters::kDbSqlite3InMemory().c_str()); - } // Then close (delete) the database connection UINFO("Disconnecting database %s...", this->getUrl().c_str()); sqlite3_close(_ppDb); _ppDb = 0; + + if(save && !_dbInMemory && !outputUrl.empty() && !this->getUrl().empty() && outputUrl.compare(this->getUrl()) != 0) + { + UWARN("Output database path (%s) is different than the opened database " + "path (%s). Opened database path is overwritten then renamed to output path.", + outputUrl.c_str(), this->getUrl().c_str()); + if(UFile::rename(this->getUrl(), outputUrl) != 0) + { + UERROR("Failed to rename just closed db %s to %s", this->getUrl().c_str(), outputUrl.c_str()); + } + } } } diff --git a/corelib/src/Features2d.cpp b/corelib/src/Features2d.cpp index 6991030a..1d17d899 100644 --- a/corelib/src/Features2d.cpp +++ b/corelib/src/Features2d.cpp @@ -369,9 +369,9 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame if(type == Feature2D::kFeatureSurf || type == Feature2D::kFeatureSift) { #if CV_MAJOR_VERSION < 3 - UWARN("SURF/SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead."); + UWARN("SURF and SIFT features cannot be used because OpenCV was not built with nonfree module. ORB is used instead."); #else - UWARN("SURF/SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead."); + UWARN("SURF and SIFT features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead."); #endif type = Feature2D::kFeatureOrb; } @@ -379,14 +379,23 @@ Feature2D * Feature2D::create(Feature2D::Type type, const ParametersMap & parame if(type == Feature2D::kFeatureFastBrief || type == Feature2D::kFeatureFastFreak || type == Feature2D::kFeatureGfttBrief || - type == Feature2D::kFeatureGfttFreak) + type == Feature2D::kFeatureGfttFreak || + type == Feature2D::kFeatureFreak) { - UWARN("BRIEF/FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead."); + UWARN("BRIEF and FREAK features cannot be used because OpenCV was not built with xfeatures2d module. ORB is used instead."); type = Feature2D::kFeatureOrb; } #endif #endif +#if CV_MAJOR_VERSION < 3 + if(type == Feature2D::kFeatureFreak) + { + UWARN("FREAK detector/descriptor can be used only with OpenCV3. GFTT/FREAK is used instead."); + type = Feature2D::kFeatureGfttFreak; + } +#endif + Feature2D * feature2D = 0; switch(type) { diff --git a/corelib/src/FlannIndex.cpp b/corelib/src/FlannIndex.cpp index a5f243e1..4bf46faf 100644 --- a/corelib/src/FlannIndex.cpp +++ b/corelib/src/FlannIndex.cpp @@ -318,6 +318,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features) // Rebuild index if it doubles in size if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) { + UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount())); index->buildIndex(); } // if no more removed points, the index has been rebuilt @@ -334,6 +335,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features) // Rebuild index if it doubles in size if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) { + UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount())); index->buildIndex(); } // if no more removed points, the index has been rebuilt @@ -347,6 +349,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features) // Rebuild index if it doubles in size if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) { + UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount())); index->buildIndex(); } // if no more removed points, the index has been rebuilt @@ -360,6 +363,7 @@ unsigned int FlannIndex::addPoints(const cv::Mat & features) // Rebuild index if it doubles in size if(index->sizeAtBuild() * 2 < index->size()+index->removedCount()) { + UDEBUG("Rebuilding FLANN index: %d -> %d", (int)index->sizeAtBuild(), (int)(index->size()+index->removedCount())); index->buildIndex(); } // if no more removed points, the index has been rebuilt diff --git a/corelib/src/GainCompensator.cpp b/corelib/src/GainCompensator.cpp index 0db6d02a..782fa66c 100644 --- a/corelib/src/GainCompensator.cpp +++ b/corelib/src/GainCompensator.cpp @@ -305,11 +305,11 @@ void feedImpl( gains = cv::Mat_(); cv::solve(A, b, gains); - if(ULogger::kDebug) + //if(ULogger::kDebug) { for(int i=0; i 1) { preDecimation = _imagePreDecimation; + if(!decimatedData.rightRaw().empty() || + (decimatedData.depthRaw().rows == decimatedData.imageRaw().rows && decimatedData.depthRaw().cols == decimatedData.imageRaw().cols)) + { + decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation)); + } decimatedData.setImageRaw(util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation)); - decimatedData.setDepthOrRightRaw(util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation)); std::vector cameraModels = decimatedData.cameraModels(); for(unsigned int i=0; i 1 && !isIntermediateNode) { + if(!data.rightRaw().empty() || + (data.depthRaw().rows == image.rows && data.depthRaw().cols == image.cols)) + { + depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation); + } image = util2d::decimate(image, _imagePostDecimation); - depthOrRightImage = util2d::decimate(depthOrRightImage, _imagePostDecimation); for(unsigned int i=0; i #include +#define KDTREE_SIZE 4 +#define KNN_CHECKS 32 + namespace rtabmap { @@ -362,7 +365,7 @@ void VWDictionary::update() break; case kNNFlannKdTree: UASSERT_MSG(descriptor.type() == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->buildKDTreeIndex(descriptor, 4, useDistanceL1_); + _flannIndex->buildKDTreeIndex(descriptor, KDTREE_SIZE, useDistanceL1_); break; case kNNFlannLSH: UASSERT_MSG(descriptor.type() == CV_8U, "To use LSH dictionary, binary descriptors are required!"); @@ -485,7 +488,7 @@ void VWDictionary::update() break; case kNNFlannKdTree: UASSERT_MSG(type == CV_32F, "To use KdTree dictionary, float descriptors are required!"); - _flannIndex->buildKDTreeIndex(_dataTree, 4, useDistanceL1_); + _flannIndex->buildKDTreeIndex(_dataTree, KDTREE_SIZE, useDistanceL1_); break; case kNNFlannLSH: UASSERT_MSG(type == CV_8U, "To use LSH dictionary, binary descriptors are required!"); @@ -682,7 +685,7 @@ std::list VWDictionary::addNewWords(const cv::Mat & descriptorsIn, if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH) { - _flannIndex->knnSearch(descriptors, results, dists, k); + _flannIndex->knnSearch(descriptors, results, dists, k, KNN_CHECKS); } else if(_strategy == kNNBruteForce) { @@ -992,7 +995,7 @@ std::vector VWDictionary::findNN(const cv::Mat & queryIn) const if(_strategy == kNNFlannNaive || _strategy == kNNFlannKdTree || _strategy == kNNFlannLSH) { - _flannIndex->knnSearch(query, results, dists, k); + _flannIndex->knnSearch(query, results, dists, k, KNN_CHECKS); } else if(_strategy == kNNBruteForce) { diff --git a/corelib/src/util2d.cpp b/corelib/src/util2d.cpp index 0f5b01b9..f1842364 100644 --- a/corelib/src/util2d.cpp +++ b/corelib/src/util2d.cpp @@ -1186,7 +1186,9 @@ cv::Mat decimate(const cv::Mat & image, int decimation) { if((image.type() == CV_32FC1 || image.type()==CV_16UC1)) { - UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, "Decimation of depth images should be exact!"); + UASSERT_MSG(image.rows % decimation == 0 && image.cols % decimation == 0, + uFormat("Decimation of depth images should be exact! (decimation=%d, size=%dx%d)", + decimation, image.cols, image.rows).c_str()); out = cv::Mat(image.rows/decimation, image.cols/decimation, image.type()); if(image.type() == CV_32FC1)