/* Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke All rights reserved. Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: * Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. * Neither the name of the Universite de Sherbrooke nor the names of its contributors may be used to endorse or promote products derived from this software without specific prior written permission. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include #include "RTABMapApp.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #define LOW_RES_PIX 2 //#define DEBUG_RENDERING_PERFORMANCE; const int g_optMeshId = -100; static JavaVM *jvm; static jobject RTABMapActivity = 0; namespace { constexpr int kTangoCoreMinimumVersion = 9377; } // anonymous namespace. rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() { rtabmap::ParametersMap parameters; parameters.insert(mappingParameters_.begin(), mappingParameters_.end()); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false"))); 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::kOptimizerIterations(), graphOptimization_?"10":"0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemCompressionParallelized(), std::string("false"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words) parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.1"))); 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 if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end()) { if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0) // GTSAM { parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0")); } else if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("1") == 0) // g2o { parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0")); } else // TORO { parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"100":"0")); } } parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("0"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05"))); parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures())); parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemRehearsalSimilarity())); parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded())); if(dataRecorderMode_) { 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; } RTABMapApp::RTABMapApp() : camera_(0), rtabmapThread_(0), rtabmap_(0), logHandler_(0), odomCloudShown_(true), graphOptimization_(true), nodesFiltering_(false), localizationMode_(false), trajectoryMode_(false), rawScanSaved_(false), smoothing_(true), cameraColor_(true), fullResolution_(false), appendMode_(true), maxCloudDepth_(0.0), minCloudDepth_(0.0), cloudDensityLevel_(1), meshTrianglePix_(1), meshAngleToleranceDeg_(15.0), clusterRatio_(0.1), maxGainRadius_(0.02f), renderingTextureDecimation_(4), backgroundColor_(0.2f), paused_(false), dataRecorderMode_(false), clearSceneOnNextRender_(false), openingDatabase_(false), exporting_(false), postProcessing_(false), filterPolygonsOnNextRender_(false), gainCompensationOnNextRender_(0), bilateralFilteringOnNextRender_(false), takeScreenshotOnNextRender_(false), cameraJustInitialized_(false), meshDecimation_(1), totalPoints_(0), totalPolygons_(0), lastDrawnCloudsCount_(0), renderingTime_(0.0f), lastPostRenderEventTime_(0.0), processMemoryUsedBytes(0), processGPUMemoryUsedBytes(0), visualizingMesh_(false), exportedMeshUpdated_(false), optMesh_(new pcl::TextureMesh), mapToOdom_(rtabmap::Transform::getIdentity()) { mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK } RTABMapApp::~RTABMapApp() { if(camera_) { delete camera_; } if(rtabmapThread_) { rtabmapThread_->close(false); delete rtabmapThread_; } if(logHandler_) { delete logHandler_; } boost::mutex::scoped_lock lock(rtabmapMutex_); if(rtabmapEvents_.size()) { for(std::list::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter) { delete *iter; } } rtabmapEvents_.clear(); } void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity) { env->GetJavaVM(&jvm); RTABMapActivity = env->NewGlobalRef(caller_activity); LOGI("RTABMapApp::onCreate()"); createdMeshes_.clear(); rawPoses_.clear(); clearSceneOnNextRender_ = true; openingDatabase_ = false; exporting_ = false; postProcessing_=false; totalPoints_ = 0; totalPolygons_ = 0; lastDrawnCloudsCount_ = 0; renderingTime_ = 0.0f; lastPostRenderEventTime_ = 0.0; processMemoryUsedBytes = 0; processGPUMemoryUsedBytes = 0; bufferedStatsData_.clear(); progressionStatus_.setJavaObjects(jvm, RTABMapActivity); main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_); if(camera_) { delete camera_; camera_ = 0; } if(rtabmapThread_) { rtabmapThread_->close(false); delete rtabmapThread_; rtabmapThread_ = 0; rtabmap_ = 0; } if(logHandler_ == 0) { logHandler_ = new LogHandler(); } this->registerToEventsManager(); camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_); } void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation) { TangoSupportRotation rotation = tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(displayRotation, cameraRotation); LOGI("Set orientation: display=%d camera=%d -> %d", displayRotation, cameraRotation, (int)rotation); main_scene_.setScreenRotation(rotation); camera_->setScreenRotation(rotation); } int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource) { LOGI("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0); this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap status_.first = rtabmap::RtabmapEventInit::kInitializing; rtabmapMutex_.lock(); rtabmapEvents_.clear(); openingDatabase_ = true; if(rtabmapThread_) { rtabmapThread_->close(false); delete rtabmapThread_; rtabmapThread_ = 0; rtabmap_ = 0; } this->registerToEventsManager(); int status = 0; // Open visualization while we load (if there is an optimized mesh saved in database) optMesh_.reset(new pcl::TextureMesh); optTexture_ = cv::Mat(); cv::Mat cloudMat; std::vector > > polygons; #if PCL_VERSION_COMPARE(>=, 1, 8, 0) std::vector > > texCoords; #else std::vector > texCoords; #endif cv::Mat textures; std::map optPoses; if(!databaseSource.empty()) { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh...")); rtabmap::DBDriver * driver = rtabmap::DBDriver::create(); if(driver->openConnection(databaseSource)) { cloudMat = driver->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures); if(!cloudMat.empty()) { LOGI("Open: Found optimized mesh! Visualizing it."); optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true); optTexture_ = textures; if(!optTexture_.empty()) { LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows); status=3; } else if(optMesh_->tex_polygons.size()) { LOGI("Open: Polygon mesh"); status=2; } else if(!optMesh_->cloud.data.empty()) { LOGI("Open: Point cloud"); status=1; } } else { LOGI("Open: No optimized mesh found."); } delete driver; } } if(status > 0) { if(status==1) { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud...done!")); } else if(status==2) { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized mesh...done!")); } else { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized texture mesh...done!")); } boost::mutex::scoped_lock lockRender(renderingMutex_); visualizingMesh_ = true; exportedMeshUpdated_ = true; } UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database...")); LOGI("Erasing database \"%s\"...", databasePath.c_str()); UFile::erase(databasePath); if(!databaseSource.empty()) { LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str()); UFile::copy(databaseSource, databasePath); } //Rtabmap mapToOdom_.setIdentity(); rtabmap_ = new rtabmap::Rtabmap(); rtabmap::ParametersMap parameters = getRtabmapParameters(); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory))); LOGI("Initializing database..."); rtabmap_->init(parameters, databasePath); rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_); if(parameters.find(rtabmap::Parameters::kRtabmapDetectionRate()) != parameters.end()) { rtabmapThread_->setDetectorRate(uStr2Float(parameters.at(rtabmap::Parameters::kRtabmapDetectionRate()))); } // Generate all meshes std::map signatures; std::map poses; std::multimap links; LOGI("Loading full map from database..."); UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading data from database...")); rtabmap_->get3DMap( signatures, poses, links, true, true); if(signatures.size() && poses.empty()) { LOGE("Failed to optimize the graph!"); status = -1; } { LOGI("Creating the meshes (%d)....", poses.size()); boost::mutex::scoped_lock lock(meshesMutex_); createdMeshes_.clear(); int i=0; UTimer addTime; for(std::map::iterator iter=poses.begin(); iter!=poses.end() && status>=0; ++iter) { try { int id = iter->first; if(!iter->second.isNull()) { if(uContains(signatures, id)) { UTimer timer; rtabmap::SensorData data = signatures.at(id).sensorData(); cv::Mat tmpA, depth; data.uncompressData(&tmpA, &depth); 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); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); if(cloud->size() && indices->size()) { std::vector polygons; std::vector polygonsLowRes; if(main_scene_.isMeshRendering() && main_scene_.isMapRendering()) { polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX); } if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering()) { 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.polygonsLowRes = polygonsLowRes; inserted.first->second.visible = true; inserted.first->second.cameraModel = data.cameraModels()[0]; inserted.first->second.gains[0] = 1.0; inserted.first->second.gains[1] = 1.0; inserted.first->second.gains[2] = 1.0; if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering()) { if(renderingTextureDecimation_>1) { cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_); cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); } else { inserted.first->second.texture = data.imageRaw(); } } LOGI("Created cloud %d (%fs)", id, timer.ticks()); } else { LOGI("Cloud %d not added to created meshes", id); } } else { UWARN("Cloud %d is empty", id); } } else { UERROR("Failed to uncompress data!"); status=-2; } const rtabmap::Signature & s = signatures.at(id); processMemoryUsedBytes += data.imageCompressed().total(); processMemoryUsedBytes += data.depthOrRightCompressed().total(); processMemoryUsedBytes += data.laserScanCompressed().total(); processMemoryUsedBytes += s.getWords().size()*4*8; processMemoryUsedBytes += s.getWords3().size()*4*4; if(!s.getWordsDescriptors().empty()) { processMemoryUsedBytes +=s.getWordsDescriptors().size()*(4+s.getWordsDescriptors().begin()->second.total()); } } else { UWARN("Data for node %d not found", id); } } else { UWARN("Pose %d is null !?", id); } ++i; if(addTime.elapsed() >= 4.0f) { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, uFormat("Created clouds %d/%d", i, (int)poses.size()))); addTime.restart(); } } catch(const UException & e) { UERROR("Exception! msg=\"%s\"", e.what()); status = -2; } catch (const cv::Exception & e) { UERROR("Exception! msg=\"%s\"", e.what()); status = -2; } catch (const std::exception & e) { UERROR("Exception! msg=\"%s\"", e.what()); status = -2; } } if(status < 0) { createdMeshes_.clear(); } else { LOGI("Created %d meshes...", (int)createdMeshes_.size()); } } if(optimize && status>=0) { UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization...")); gainCompensation(); LOGI("Polygon filtering..."); boost::mutex::scoped_lock lock(meshesMutex_); UTimer time; for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { if(iter->second.polygons.size()) { // filter polygons iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size()); } } } UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Updating scene...")); LOGI("Open: add rtabmap event to update the scene"); rtabmap::Statistics stats; stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize()); stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size()); stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed()); stats.setPoses(poses); stats.setConstraints(links); rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats)); rtabmap_->setOptimizedPoses(poses); // Start threads LOGI("Start rtabmap thread"); rtabmapThread_->registerToEventsManager(); rtabmapThread_->start(); UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInitialized, "")); status_.first = rtabmap::RtabmapEventInit::kInitialized; status_.second = ""; rtabmapMutex_.unlock(); boost::mutex::scoped_lock lockRender(renderingMutex_); if(poses.empty() || status>0) { openingDatabase_ = false; } clearSceneOnNextRender_ = status<=0; return status; } bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder) { LOGW("onTangoServiceConnected()"); if(camera_) { camera_->join(true); if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) { UERROR("TangoHandler::ConnectTango, TangoService_setBinder error"); return false; } camera_->setColorCamera(cameraColor_); if(camera_->init()) { //update mesh decimation based on camera calibration LOGI("Cloud density level %d", cloudDensityLevel_); meshDecimation_ = 1; if(camera_) { // Google Tango Tablet 160x90 // Phab2Pro 240x135 // FishEye 640x480 int width = camera_->getCameraModel().imageWidth()/(cameraColor_?8:1); int height = camera_->getCameraModel().imageHeight()/(cameraColor_?8:1); if(cloudDensityLevel_ == 3) // high { if(height >= 480 && width % 20 == 0 && height % 20 == 0) { meshDecimation_ = 20; } else if(width % 10 == 0 && height % 10 == 0) { meshDecimation_ = 10; } else if(width % 15 == 0 && height % 15 == 0) { meshDecimation_ = 15; } else { UERROR("Could not set decimation to high (size=%dx%d)", width, height); } } else if(cloudDensityLevel_ == 2) // medium { if(height >= 480 && width % 10 == 0 && height % 10 == 0) { meshDecimation_ = 10; } else if(width % 5 == 0 && height % 5 == 0) { meshDecimation_ = 5; } else { UERROR("Could not set decimation to medium (size=%dx%d)", width, height); } } else if(cloudDensityLevel_ == 1) // low { if(height >= 480 && width % 5 == 0 && height % 5 == 0) { meshDecimation_ = 5; } else 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); } } } LOGI("Set decimation to %d", meshDecimation_); LOGI("Start camera thread"); if(!paused_) { camera_->start(); } cameraJustInitialized_ = true; return true; } UERROR("Failed camera initialization!"); } return false; } void RTABMapApp::onPause() { LOGI("onPause()"); if(camera_) { camera_->join(true); camera_->close(); } } void RTABMapApp::TangoResetMotionTracking() { TangoService_resetMotionTracking(); } std::vector RTABMapApp::filterOrganizedPolygons( const std::vector & polygons, int cloudSize) const { std::vector vertexToCluster(cloudSize, 0); std::map > clusters; int lastClusterID = 0; for(unsigned int i=0; i0) { clusterID = vertexToCluster[polygons[i].vertices[j]]; break; } } if(clusterID>0) { clusters.at(clusterID).push_back(i); } else { clusterID = ++lastClusterID; std::list polygons; polygons.push_back(i); clusters.insert(std::make_pair(clusterID, polygons)); } for(unsigned int j=0;j >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) { LOGD("cluster %d = %d", iter->first, iter->second.size()); if(iter->second.size() > biggestClusterSize) { biggestClusterSize = iter->second.size(); } } unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_); LOGI("Biggest cluster %d -> minClusterSize(ratio=%f)=%d", biggestClusterSize, clusterRatio_, (int)minClusterSize); std::vector filteredPolygons(polygons.size()); int oi = 0; for(std::map >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) { if(iter->second.size() >= minClusterSize) { for(std::list::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter) { filteredPolygons[oi++] = polygons[*jter]; } } } filteredPolygons.resize(oi); return filteredPolygons; } std::vector RTABMapApp::filterPolygons( const std::vector & polygons, int cloudSize) const { // filter polygons std::vector > neighbors; std::vector > vertexToPolygons; rtabmap::util3d::createPolygonIndexes( polygons, cloudSize, neighbors, vertexToPolygons); std::list > clusters = rtabmap::util3d::clusterPolygons(neighbors); unsigned int biggestClusterSize = 0; for(std::list >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter) { if(iter->size() > biggestClusterSize) { biggestClusterSize = iter->size(); } } unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_); LOGI("Biggest cluster = %d -> minClusterSize(ratio=%f)=%d", biggestClusterSize, clusterRatio_, (int)minClusterSize); std::vector filteredPolygons(polygons.size()); int oi=0; for(std::list >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter) { if(jter->size() >= minClusterSize) { for(std::list::iterator kter=jter->begin(); kter!=jter->end(); ++kter) { filteredPolygons[oi++] = polygons.at(*kter); } } } filteredPolygons.resize(oi); return filteredPolygons; } // OpenGL thread void RTABMapApp::InitializeGLContent() { UINFO(""); main_scene_.InitGLContent(); float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_; main_scene_.setGridColor(v, v, v); } // OpenGL thread void RTABMapApp::SetViewPort(int width, int height) { UINFO(""); main_scene_.SetupViewPort(width, height); } class PostRenderEvent : public UEvent { public: PostRenderEvent(rtabmap::RtabmapEvent * event = 0) : rtabmapEvent_(event) { } ~PostRenderEvent() { if(rtabmapEvent_!=0) { delete rtabmapEvent_; } } virtual std::string getClassName() const {return "PostRenderEvent";} const rtabmap::RtabmapEvent * getRtabmapEvent() const {return rtabmapEvent_;} private: rtabmap::RtabmapEvent * rtabmapEvent_; }; // OpenGL thread bool RTABMapApp::smoothMesh(int id, Mesh & mesh) { UTimer t; // reconstruct depth image UASSERT(mesh.indices.get() && mesh.indices->size()); cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1); rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse(); for(unsigned int i=0; isize(); ++i) { int index = mesh.indices->at(i); // FastBilateralFilter works in camera frame if(mesh.cloud->at(index).x > 0) { pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv); depth.at(index) = pt.z; } } depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f); LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks()); if(!depth.empty() && mesh.indices->size()) { pcl::IndicesPtr newIndices(new std::vector(mesh.indices->size())); int oi = 0; for(unsigned int i=0; isize(); ++i) { int index = mesh.indices->at(i); pcl::PointXYZRGB & pt = mesh.cloud->at(index); pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv); if(depth.at(index) > 0) { newPt.z = depth.at(index); newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform()); newIndices->at(oi++) = index; } else { newPt.x = newPt.y = newPt.z = std::numeric_limits::quiet_NaN(); } pt.x = newPt.x; pt.y = newPt.y; pt.z = newPt.z; } newIndices->resize(oi); mesh.indices = newIndices; //reconstruct the mesh with smoothed surfaces std::vector polygons; if(main_scene_.isMeshRendering()) { polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); } LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks()); mesh.polygons = polygons; } else { UERROR("smoothMesh() Failed to smooth surface %d", id); return false; } return true; } void RTABMapApp::gainCompensation(bool full) { 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(full) { // 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_, 0.0f, 0.01f, 1.0f); 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()) { compensator.getGain(iter->first, &iter->second.gains[0], &iter->second.gains[1], &iter->second.gains[2]); LOGI("%d mesh has gain %f,%f,%f", iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]); } } } LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks()); } // OpenGL thread int RTABMapApp::Render() { std::list rtabmapEvents; try { UASSERT(camera_!=0); UTimer fpsTime; #ifdef DEBUG_RENDERING_PERFORMANCE UTimer time; #endif boost::mutex::scoped_lock lock(renderingMutex_); bool notifyDataLoaded = false; bool notifyCameraStarted = false; if(clearSceneOnNextRender_) { visualizingMesh_ = false; } // process only pose events in visualization mode rtabmap::Transform pose; { boost::mutex::scoped_lock lock(poseMutex_); if(poseEvents_.size()) { pose = poseEvents_.back(); poseEvents_.clear(); } } rtabmap::Transform mapOdom = rtabmap::Transform::getIdentity(); if(!pose.isNull()) { // update camera pose? if(graphOptimization_ && !visualizingMesh_ && !mapToOdom_.isIdentity()) { mapOdom = mapToOdom_; main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapOdom*rtabmap_world_T_tango_world*pose); } else { main_scene_.SetCameraPose(opengl_world_T_tango_world*pose); } if(!camera_->isRunning() && cameraJustInitialized_) { 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_) { notifyCameraStarted = true; cameraJustInitialized_ = false; } } } if(visualizingMesh_) { if(exportedMeshUpdated_) { main_scene_.clear(); exportedMeshUpdated_ = false; } if(!main_scene_.hasCloud(g_optMeshId)) { LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...", optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step, optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(), optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(), (int)optMesh_->tex_materials.size(), optTexture_.cols, optTexture_.rows); if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size()) { Mesh mesh; mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0; mesh.cloud.reset(new pcl::PointCloud); mesh.normals.reset(new pcl::PointCloud); pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud); pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals); mesh.polygons = optMesh_->tex_polygons[0]; mesh.pose.setIdentity(); if(optMesh_->tex_coordinates.size()) { mesh.texCoords = optMesh_->tex_coordinates[0]; mesh.texture = optTexture_; } main_scene_.addMesh(g_optMeshId, mesh, opengl_world_T_rtabmap_world, true); } else { pcl::IndicesPtr indices(new std::vector); // null pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud); main_scene_.addCloud(g_optMeshId, cloud, indices, opengl_world_T_rtabmap_world); } } //backup state bool isMeshRendering = main_scene_.isMeshRendering(); bool isTextureRendering = main_scene_.isMeshTexturing(); main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId)); fpsTime.restart(); lastDrawnCloudsCount_ = main_scene_.Render(); if(renderingTime_ < fpsTime.elapsed()) { renderingTime_ = fpsTime.elapsed(); } // revert state main_scene_.setMeshRendering(isMeshRendering, isTextureRendering); } else { if(main_scene_.hasCloud(g_optMeshId)) { main_scene_.clear(); optMesh_.reset(new pcl::TextureMesh); optTexture_ = cv::Mat(); } // should be before clearSceneOnNextRender_ in case database is reset if(!openingDatabase_) { rtabmapMutex_.lock(); rtabmapEvents = rtabmapEvents_; rtabmapEvents_.clear(); rtabmapMutex_.unlock(); if(!clearSceneOnNextRender_ && rtabmapEvents.size()) { boost::mutex::scoped_lock lockMesh(meshesMutex_); if(createdMeshes_.size()) { if(rtabmapEvents.front()->getStats().refImageId()>0 && rtabmapEvents.front()->getStats().refImageId() < createdMeshes_.rbegin()->first) { LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front()->getStats().refImageId(), createdMeshes_.rbegin()->first); clearSceneOnNextRender_ = true; } } } #ifdef DEBUG_RENDERING_PERFORMANCE if(rtabmapEvents.size()) { LOGW("begin and getting rtabmap events %fs", time.ticks()); } #endif } if(clearSceneOnNextRender_) { LOGI("Clearing all rendering data..."); odomMutex_.lock(); odomEvents_.clear(); odomMutex_.unlock(); poseMutex_.lock(); poseEvents_.clear(); poseMutex_.unlock(); main_scene_.clear(); clearSceneOnNextRender_ = false; if(!openingDatabase_) { boost::mutex::scoped_lock lock(meshesMutex_); LOGI("Clearing meshes..."); createdMeshes_.clear(); } else { notifyDataLoaded = true; } rawPoses_.clear(); totalPoints_ = 0; totalPolygons_ = 0; lastDrawnCloudsCount_ = 0; renderingTime_ = 0.0f; lastPostRenderEventTime_ = 0.0; processMemoryUsedBytes = 0; processGPUMemoryUsedBytes = 0; bufferedStatsData_.clear(); } // Did we lose OpenGL context? If so, recreate the context; std::set added = main_scene_.getAddedClouds(); added.erase(-1); if(!openingDatabase_) { boost::mutex::scoped_lock lock(meshesMutex_); unsigned int meshes = createdMeshes_.size(); if(added.size() != meshes) { LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes); processGPUMemoryUsedBytes = 0; boost::mutex::scoped_lock lockRtabmap(rtabmapMutex_); UASSERT(rtabmap_!=0); for(std::map::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull()) { LOGI("Re-add mesh %d to OpenGL context", iter->first); if(main_scene_.isMeshRendering() && iter->second.polygons.size() == 0) { iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); iter->second.polygonsLowRes = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX); } if(main_scene_.isMeshTexturing()) { cv::Mat textureRaw; textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first)); if(!textureRaw.empty()) { if(renderingTextureDecimation_ > 1) { cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_); LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height); cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); } else { iter->second.texture = textureRaw; } } } main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose); main_scene_.setCloudVisible(iter->first, iter->second.visible); long estimateGPUMem = 0; estimateGPUMem += iter->second.cloud->size()*16; // 3*float + 1 float rgb estimateGPUMem += iter->second.indices->size()*4; // int estimateGPUMem += iter->second.polygons.size()*4*3; // 3 indices per polygon estimateGPUMem += iter->second.polygonsLowRes.size()*4*3; // 3 indices per polygon processGPUMemoryUsedBytes += estimateGPUMem + (iter->second.texture.empty()?0:iter->second.polygons.size()*3*8+iter->second.texture.total()); iter->second.texture = cv::Mat(); // don't keep textures in memory } } } } else if(notifyDataLoaded) { rtabmapMutex_.lock(); rtabmapEvents = rtabmapEvents_; rtabmapEvents_.clear(); rtabmapMutex_.unlock(); openingDatabase_ = false; } if(rtabmapEvents.size()) { #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Process rtabmap events %fs", time.ticks()); #else LOGI("Process rtabmap events"); #endif // update buffered signatures std::map bufferedSensorData; if(!trajectoryMode_ && !dataRecorderMode_) { for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { const rtabmap::Statistics & stats = (*iter)->getStats(); // Don't create mesh for the last node added if rehearsal happened or if discarded (small movement) int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f); int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f); int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f); if(smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0) { for(std::map::const_iterator jter=stats.getSignatures().begin(); jter!=stats.getSignatures().end(); ++jter) { bool dataDetected = false; if(!jter->second.sensorData().imageRaw().empty() && !jter->second.sensorData().depthRaw().empty()) { if(!localizationMode_) { uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData())); uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); dataDetected = true; } } if(dataDetected) { processMemoryUsedBytes += jter->second.sensorData().imageCompressed().total(); processMemoryUsedBytes += jter->second.sensorData().depthOrRightCompressed().total(); processMemoryUsedBytes += jter->second.sensorData().laserScanCompressed().total(); processMemoryUsedBytes += jter->second.getWords().size()*4*8; processMemoryUsedBytes += jter->second.getWords3().size()*4*4; if(!jter->second.getWordsDescriptors().empty()) { processMemoryUsedBytes += jter->second.getWordsDescriptors().size()*(4+jter->second.getWordsDescriptors().begin()->second.total()); } } } } int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f); int rejected = (int)uValue(stats.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.2f, 0); // dark green } else if(!paused_ && rehearsalMerged>0) { main_scene_.setBackgroundColor(0, 0, 0.2f); // blue } else if(!paused_ && fastMovement) { main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta } else { main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_); } } } #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Looking fo data to load (%d) %fs", bufferedSensorData.size(), time.ticks()); #endif std::map poses = rtabmapEvents.back()->getStats().poses(); if(!rtabmapEvents.back()->getStats().mapCorrection().isNull()) { mapToOdom_ = rtabmapEvents.back()->getStats().mapCorrection(); } // 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 { iter->second = opengl_world_T_rtabmap_world*iter->second; } } const std::multimap & links = rtabmapEvents.back()->getStats().constraints(); if(poses.size()) { //update graph main_scene_.updateGraph(poses, links); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Update graph: %fs", time.ticks()); #endif // 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(createdMeshes_.find(id) == createdMeshes_.end() && bufferedSensorData.find(id) != bufferedSensorData.end()) { rtabmap::SensorData data = bufferedSensorData.at(id); cv::Mat tmpA, depth; data.uncompressData(&tmpA, &depth); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Decompressing data: %fs", time.ticks()); #endif 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); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get()); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Creating node cloud %d (depth=%dx%d rgb=%dx%d, %fs)", id, data.depthRaw().cols, data.depthRaw().rows, data.imageRaw().cols, data.imageRaw().rows, time.ticks()); #endif if(cloud->size() && indices->size()) { std::vector polygons; std::vector polygonsLowRes; if(main_scene_.isMeshRendering() && main_scene_.isMapRendering()) { polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks()); #endif polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks()); #endif } if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering()) { 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.polygonsLowRes = polygonsLowRes; inserted.first->second.visible = true; inserted.first->second.cameraModel = data.cameraModels()[0]; inserted.first->second.gains[0] = 1.0; inserted.first->second.gains[1] = 1.0; inserted.first->second.gains[2] = 1.0; if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering()) { if(renderingTextureDecimation_ > 1) { cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_); cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks()); #endif } else { inserted.first->second.texture = data.imageRaw(); } } } } } } if(createdMeshes_.find(id) != createdMeshes_.end()) { Mesh & mesh = createdMeshes_.at(id); totalPoints_+=mesh.indices->size(); totalPolygons_ += mesh.polygons.size(); mesh.pose = opengl_world_T_rtabmap_world.inverse()*iter->second; main_scene_.addMesh(id, mesh, iter->second); #ifdef DEBUG_RENDERING_PERFORMANCE LOGW("Adding mesh to scene: %fs", time.ticks()); #endif long estimateCPUMem = 0; estimateCPUMem += mesh.cloud->size()*16; // 3*float + 1 float rgb estimateCPUMem += mesh.indices->size()*4; // int estimateCPUMem += mesh.polygons.size()*4*3; // 3 indices per polygon processMemoryUsedBytes += estimateCPUMem; processGPUMemoryUsedBytes += estimateCPUMem + (mesh.texture.empty()?0:mesh.polygons.size()*3*8+mesh.texture.total()); mesh.texture = cv::Mat(); // don't keep textures in memory } } } } } //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; } } } } else { main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_); //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_, minCloudDepth_, 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*mapOdom*odomEvent.pose()); main_scene_.setCloudVisible(-1, true); } else { UERROR("Generated cloud is empty!"); } } else { UERROR("Odom data images are empty!"); } } } } if(gainCompensationOnNextRender_>0) { gainCompensation(gainCompensationOnNextRender_==2); for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { main_scene_.updateGains(iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]); } 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); } } } notifyDataLoaded = true; } if(filterPolygonsOnNextRender_ && clusterRatio_>0.0f) { LOGI("Polygon filtering..."); filterPolygonsOnNextRender_ = false; boost::mutex::scoped_lock lock(meshesMutex_); UTimer time; for(std::map::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter) { if(iter->second.polygons.size()) { // filter polygons iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size()); main_scene_.updateCloudPolygons(iter->first, iter->second.polygons); } } notifyDataLoaded = true; } fpsTime.restart(); lastDrawnCloudsCount_ = main_scene_.Render(); if(renderingTime_ < fpsTime.elapsed()) { renderingTime_ = fpsTime.elapsed(); } if(rtabmapEvents.size()) { // send statistics to GUI LOGI("New data added to map, rendering time: %fs", renderingTime_); UEventsManager::post(new PostRenderEvent(rtabmapEvents.back())); rtabmapEvents.pop_back(); for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { delete *iter; } rtabmapEvents.clear(); lastPostRenderEventTime_ = UTimer::now(); } } if(takeScreenshotOnNextRender_) { takeScreenshotOnNextRender_ = false; int w = main_scene_.getViewPortWidth(); int h = main_scene_.getViewPortHeight(); cv::Mat image(h, w, CV_8UC4); glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data); cv::flip(image, image, 0); cv::cvtColor(image, image, CV_RGBA2BGRA); cv::Mat roi; if(w>h) { int offset = (w-h)/2; roi = image(cv::Range::all(), cv::Range(offset,offset+h)); } else { int offset = (h-w)/2; roi = image(cv::Range(offset,offset+w), cv::Range::all()); } rtabmapMutex_.lock(); LOGI("Saving screenshot %dx%d...", roi.cols, roi.rows); rtabmap_->getMemory()->savePreviewImage(roi); rtabmapMutex_.unlock(); screenshotReady_.release(); } if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_) { // throttle rendering max 5Hz if we are doing some processing double renderTime = fpsTime.elapsed(); if(0.2 - renderTime > 0.0) { uSleep((0.2 - renderTime)*1000); } } if(paused_ && lastPostRenderEventTime_ > 0.0) { double interval = UTimer::now() - lastPostRenderEventTime_; double updateInterval = 1.0; if(!openingDatabase_ && rtabmapThread_) { boost::mutex::scoped_lock lock(rtabmapMutex_); if(rtabmapThread_ && rtabmapThread_->getDetectorRate()>0.0f) { updateInterval = 1.0f/rtabmapThread_->getDetectorRate(); } } if(interval >= updateInterval) { if(!openingDatabase_) { // don't send event when we are opening the database (init events already sent) UEventsManager::post(new PostRenderEvent()); } lastPostRenderEventTime_ = UTimer::now(); } } return notifyDataLoaded||notifyCameraStarted?1:0; } catch(const UException & e) { for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { delete *iter; } rtabmapEvents.clear(); UERROR("Exception! msg=\"%s\"", e.what()); return -2; } catch(const cv::Exception & e) { for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { delete *iter; } rtabmapEvents.clear(); UERROR("Exception! msg=\"%s\"", e.what()); return -1; } catch(const std::exception & e) { for(std::list::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { delete *iter; } rtabmapEvents.clear(); UERROR("Exception! msg=\"%s\"", e.what()); return -2; } } void RTABMapApp::SetCameraType( tango_gl::GestureCamera::CameraType camera_type) { main_scene_.SetCameraType(camera_type); } void RTABMapApp::OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event, float x0, float y0, float x1, float y1) { main_scene_.OnTouchEvent(touch_count, event, x0, y0, x1, y1); } void RTABMapApp::setPausedMapping(bool paused) { { boost::mutex::scoped_lock lock(renderingMutex_); visualizingMesh_ = false; main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_); } paused_ = paused; if(camera_) { if(paused_) { LOGW("Pause!"); camera_->kill(); } else { LOGW("Resume!"); UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdTriggerNewMap)); camera_->start(); } } } void RTABMapApp::setOnlineBlending(bool enabled) { main_scene_.setBlending(enabled); } void RTABMapApp::setMapCloudShown(bool shown) { main_scene_.setMapRendering(shown); } void RTABMapApp::setOdomCloudShown(bool shown) { odomCloudShown_ = shown; main_scene_.setTraceVisible(shown); } void RTABMapApp::setMeshRendering(bool enabled, bool withTexture) { main_scene_.setMeshRendering(enabled, withTexture); } void RTABMapApp::setPointSize(float value) { main_scene_.setPointSize(value); } void RTABMapApp::setFOV(float angle) { main_scene_.setFOV(angle); } void RTABMapApp::setOrthoCropFactor(float value) { main_scene_.setOrthoCropFactor(value); } void RTABMapApp::setGridRotation(float value) { main_scene_.setGridRotation(value); } void RTABMapApp::setLighting(bool enabled) { main_scene_.setLighting(enabled); } void RTABMapApp::setBackfaceCulling(bool enabled) { main_scene_.setBackfaceCulling(enabled); } void RTABMapApp::setWireframe(bool enabled) { main_scene_.setWireframe(enabled); } void RTABMapApp::setLocalizationMode(bool enabled) { localizationMode_ = enabled; this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_))); } void RTABMapApp::setTrajectoryMode(bool enabled) { trajectoryMode_ = enabled; this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_))); } void RTABMapApp::setGraphOptimization(bool enabled) { graphOptimization_ = enabled; UASSERT(camera_ != 0 && rtabmap_!=0 && rtabmap_->getMemory()!=0); if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0) { std::map poses; std::multimap links; rtabmap_->getGraph(poses, links, true, true); if(poses.size()) { boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmap::Statistics stats = rtabmap_->getStatistics(); stats.setPoses(poses); stats.setConstraints(links); LOGI("Send rtabmap event to update graph..."); rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats)); rtabmap_->setOptimizedPoses(poses); } } } void RTABMapApp::setNodesFiltering(bool enabled) { nodesFiltering_ = enabled; setGraphOptimization(graphOptimization_); // this will resend the graph if paused } void RTABMapApp::setGraphVisible(bool visible) { main_scene_.setGraphVisible(visible); main_scene_.setTraceVisible(visible); } void RTABMapApp::setGridVisible(bool visible) { main_scene_.setGridVisible(visible); } void RTABMapApp::setRawScanSaved(bool enabled) { if(rawScanSaved_ != enabled) { rawScanSaved_ = enabled; if(camera_) { camera_->setRawScanPublished(rawScanSaved_); } } } void RTABMapApp::setCameraColor(bool enabled) { if(cameraColor_ != enabled) { cameraColor_ = enabled; } } void RTABMapApp::setFullResolution(bool enabled) { if(fullResolution_ != enabled) { fullResolution_ = enabled; if(camera_) { camera_->setDecimation(fullResolution_?1:2); } rtabmap::ParametersMap parameters; parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1"))); this->post(new rtabmap::ParamEvent(parameters)); } } void RTABMapApp::setSmoothing(bool enabled) { if(smoothing_ != enabled) { smoothing_ = enabled; if(camera_) { camera_->setSmoothing(smoothing_); } } } void RTABMapApp::setAppendMode(bool enabled) { if(appendMode_ != enabled) { appendMode_ = enabled; rtabmap::ParametersMap parameters; parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_))); this->post(new rtabmap::ParamEvent(parameters)); } } void RTABMapApp::setDataRecorderMode(bool enabled) { if(dataRecorderMode_ != enabled) { dataRecorderMode_ = enabled; // parameters will be set when resuming (we assume we are paused) } } void RTABMapApp::setMaxCloudDepth(float value) { maxCloudDepth_ = value; } void RTABMapApp::setMinCloudDepth(float value) { minCloudDepth_ = value; } void RTABMapApp::setCloudDensityLevel(int value) { cloudDensityLevel_ = value; } void RTABMapApp::setMeshAngleTolerance(float value) { meshAngleToleranceDeg_ = value; } void RTABMapApp::setMeshTriangleSize(int value) { meshTrianglePix_ = value; } void RTABMapApp::setClusterRatio(float value) { clusterRatio_ = value; } void RTABMapApp::setMaxGainRadius(float value) { maxGainRadius_ = value; } void RTABMapApp::setRenderingTextureDecimation(int value) { UASSERT(value>=1); renderingTextureDecimation_ = value; } void RTABMapApp::setBackgroundColor(float gray) { backgroundColor_ = gray; float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_; main_scene_.setGridColor(v, v, v); } int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value) { std::string compatibleKey = key; // Backward compatibility std::map >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().find(key); if(iter != rtabmap::Parameters::getRemovedParameters().end()) { if(iter->second.first) { // can be migrated compatibleKey = iter->second.second; LOGW("Parameter name changed: \"%s\" -> \"%s\". Please update the code accordingly. Value \"%s\" is still set to the new parameter name.", iter->first.c_str(), iter->second.second.c_str(), value.c_str()); } else { if(iter->second.second.empty()) { UERROR("Parameter \"%s\" doesn't exist anymore!", iter->first.c_str()); } else { UERROR("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"", iter->first.c_str(), iter->second.second.c_str()); } } } if(rtabmap::Parameters::getDefaultParameters().find(compatibleKey) != rtabmap::Parameters::getDefaultParameters().end()) { LOGI(uFormat("Setting param \"%s\" to \"%s\"", compatibleKey.c_str(), value.c_str()).c_str()); if(compatibleKey.compare(rtabmap::Parameters::kKpDetectorStrategy()) == 0 && mappingParameters_.at(rtabmap::Parameters::kKpDetectorStrategy()).compare(value) != 0) { // Changing feature type should reset mapping! resetMapping(); } uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value)); UEventsManager::post(new rtabmap::ParamEvent(this->getRtabmapParameters())); return 0; } else { UERROR(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str()); return -1; } } void RTABMapApp::setGPS(const rtabmap::GPS & gps) { if(camera_) { camera_->setGPS(gps); } } void RTABMapApp::resetMapping() { LOGW("Reset!"); status_.first = rtabmap::RtabmapEventInit::kInitializing; status_.second = ""; mapToOdom_.setIdentity(); clearSceneOnNextRender_ = true; UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory)); } void RTABMapApp::save(const std::string & databasePath) { LOGI("Saving database to %s", databasePath.c_str()); rtabmapThread_->join(true); LOGI("Taking screenshot..."); takeScreenshotOnNextRender_ = true; if(!screenshotReady_.acquire(1, 2000)) { UERROR("Failed to take a screenshot after 2 sec!"); } // save mapping parameters in the database bool appendModeBackup = appendMode_; if(appendMode_) { appendMode_ = false; } bool dataRecorderModeBackup = dataRecorderMode_; if(dataRecorderMode_) { dataRecorderMode_ = false; } bool localizationModeBackup = localizationMode_; if(localizationMode_) { localizationMode_ = false; } if(appendModeBackup || dataRecorderModeBackup || localizationModeBackup) { rtabmap::ParametersMap parameters = getRtabmapParameters(); rtabmap_->parseParameters(parameters); appendMode_ = appendModeBackup; dataRecorderMode_ = dataRecorderModeBackup; localizationMode_ = localizationModeBackup; } std::map poses = rtabmap_->getLocalOptimizedPoses(); rtabmap_->close(true, databasePath); rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath); rtabmap_->setOptimizedPoses(poses); if(dataRecorderMode_) { clearSceneOnNextRender_ = true; } rtabmapThread_->start(); } void RTABMapApp::cancelProcessing() { UWARN("Processing canceled!"); progressionStatus_.setCanceled(true); } bool RTABMapApp::exportMesh( float cloudVoxelSize, bool regenerateCloud, bool meshing, int textureSize, int textureCount, int normalK, bool optimized, float optimizedVoxelSize, int optimizedDepth, int optimizedMaxPolygons, float optimizedColorRadius, bool optimizedCleanWhitePolygons, int optimizedMinClusterSize, float optimizedMaxTextureDistance, int optimizedMinTextureClusterSize, bool blockRendering) { // make sure createdMeshes_ is not modified while exporting! We don't // lock the meshesMutex_ because we want to continue rendering. std::map poses = rtabmap_->getLocalOptimizedPoses(); if(poses.empty()) { // look if we just triggered new map without localizing afterward (pause/resume in append Mode) std::multimap links; rtabmap_->getGraph( poses, links, true, false); if(poses.empty()) { UERROR("Empty optimized poses!"); return false; } rtabmap_->setOptimizedPoses(poses); } if(blockRendering) { renderingMutex_.lock(); main_scene_.clear(); } exporting_ = true; bool success = false; try { int totalSteps = 0; totalSteps+=poses.size(); // assemble if(meshing) { if(optimized) { totalSteps += poses.size(); // meshing if(textureSize > 0) { totalSteps += 1; // gain totalSteps += 1; // blending if(optimizedMaxPolygons > 0) { totalSteps += 1; // decimation } } totalSteps += 1; // texture/coloring if(textureSize > 0) { totalSteps+=poses.size()+1; // texture cameras + apply polygons } } if(textureSize>0) { totalSteps += poses.size()+1; // uncompress and merge textures } } totalSteps += 1; // save file progressionStatus_.reset(totalSteps); //Assemble the meshes if(meshing) // Mesh or Texture Mesh { pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh); pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); std::vector > vertexToPixels; cv::Mat globalTextures; int totalPolygons = 0; { if(optimized) { std::map cameraPoses; std::map cameraModels; std::map cameraDepths; UTimer timer; LOGI("Assemble clouds (%d)...", (int)poses.size()); #ifndef DISABLE_LOG int cloudCount=0; #endif 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; pcl::IndicesPtr indices(new std::vector); rtabmap::CameraModel model; cv::Mat depth; float gains[3] = {1.0f}; if(jter != createdMeshes_.end()) { cloud = jter->second.cloud; indices = jter->second.indices; model = jter->second.cameraModel; gains[0] = jter->second.gains[0]; gains[1] = jter->second.gains[1]; gains[2] = jter->second.gains[2]; rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, false); data.uncompressData(0, &depth); } 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_, minCloudDepth_, indices.get()); model = data.cameraModels()[0]; depth = data.depthRaw(); } } 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); } 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, 0.0f, viewpoint); pcl::PointCloud::Ptr cloudWithNormals(new pcl::PointCloud); pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals); if(textureSize == 0 && (gains[0] != 1.0 || gains[1] != 1.0 || gains[2] != 1.0)) { 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) * gains[0]))); pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } if(mergedClouds->size() == 0) { *mergedClouds = *cloudWithNormals; } else { *mergedClouds += *cloudWithNormals; } cameraPoses.insert(std::make_pair(iter->first, iter->second)); cameraModels.insert(std::make_pair(iter->first, model)); if(!depth.empty()) { cameraDepths.insert(std::make_pair(iter->first, depth)); } 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); } if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(); } LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size()); if(mergedClouds->size()>=3) { if(optimizedDepth == 0) { Eigen::Vector4f min,max; pcl::getMinMax3D(*mergedClouds, min, max); float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]); optimizedDepth = 12; for(int i=6; i<12; ++i) { if(mapLength/float(1< poisson; poisson.setDepth(optimizedDepth); poisson.setInputCloud(mergedClouds); poisson.reconstruct(*mesh); LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), mesh->polygons.size()); if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(poses.size()); if(mesh->polygons.size()) { totalPolygons=(int)mesh->polygons.size(); if(optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size()) { #ifndef DISABLE_VTK unsigned int count = mesh->polygons.size(); float factor = 1.0f-float(optimizedMaxPolygons)/float(count); LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor); progressionStatus_.setMax(progressionStatus_.getMax() + optimizedMaxPolygons/10000); pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh); pcl::MeshQuadricDecimationVTK mqd; mqd.setTargetReductionFactor(factor); 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)", factor, 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(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(); rtabmap::util3d::denseMeshPostProcessing( mesh, 0.0f, 0, mergedClouds, optimizedColorRadius, textureSize == 0, optimizedCleanWhitePolygons, optimizedMinClusterSize); if(textureSize>0) { LOGI("Texturing... cameraPoses=%d, cameraDepths=%d", (int)cameraPoses.size(), (int)cameraDepths.size()); textureMesh = rtabmap::util3d::createTextureMesh( mesh, cameraPoses, cameraModels, cameraDepths, optimizedMaxTextureDistance, 0.0f, 0.0f, optimizedMinTextureClusterSize, std::vector(), &progressionStatus_, &vertexToPixels); LOGI("Texturing... done! %fs", timer.ticks()); if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } // Remove occluded polygons (polygons with no texture) if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons) { LOGI("Cleanup mesh..."); rtabmap::util3d::cleanTextureMesh(*textureMesh, 0); LOGI("Cleanup mesh... done! %fs", timer.ticks()); } totalPolygons = 0; for(unsigned int t=0; ttex_polygons.size(); ++t) { totalPolygons+=textureMesh->tex_polygons[t].size(); } } else { totalPolygons = (int)mesh->polygons.size(); polygonMesh = mesh; } } } else { UERROR("Merged cloud too small (%d points) to create polygons!", (int)mergedClouds->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 gains[3] = {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_); } gains[0] = jter->second.gains[0]; gains[1] = jter->second.gains[1]; gains[2] = jter->second.gains[2]; } 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_, minCloudDepth_); 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(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 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) * gains[0]))); pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } 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 { UERROR("Mesh not found for mesh %d", iter->first); } if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(); } if(textureSize == 0) { if(mergedClouds->size()) { pcl::toPCLPointCloud2(*mergedClouds, polygonMesh->cloud); } else { polygonMesh->polygons.clear(); } } else { 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()); globalTextures = rtabmap::util3d::mergeTextures( *textureMesh, std::map(), std::map >(), rtabmap_->getMemory(), 0, textureSize, textureCount, vertexToPixels, true, 10.0f, true ,true, 0, 0, 0, false, &progressionStatus_); } if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(); } if(totalPolygons) { if(textureSize == 0) { UASSERT((int)polygonMesh->polygons.size() == totalPolygons); if(polygonMesh->polygons.size()) { // save in database pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud); cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database std::vector > > polygons(1); polygons[0].resize(polygonMesh->polygons.size()); for(unsigned int p=0; ppolygons.size(); ++p) { polygons[0][p] = polygonMesh->polygons[p].vertices; } boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons); success = true; } } else if(textureMesh->tex_materials.size()) { pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud); cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud)); // for database // save in database std::vector > > polygons(textureMesh->tex_polygons.size()); for(unsigned int t=0; ttex_polygons.size(); ++t) { polygons[t].resize(textureMesh->tex_polygons[t].size()); for(unsigned int p=0; ptex_polygons[t].size(); ++p) { polygons[t][p] = textureMesh->tex_polygons[t][p].vertices; } } boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses, polygons, textureMesh->tex_coordinates, globalTextures); success = true; } else { UERROR("Failed exporting texture mesh! There are no textures!"); } } else { UERROR("Failed exporting mesh! There are no polygons!"); } } 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 gains[3] = {1.0f}; if(regenerateCloud) { if(jter != createdMeshes_.end()) { gains[0] = jter->second.gains[0]; gains[1] = jter->second.gains[1]; gains[2] = jter->second.gains[2]; } rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty()) { // full resolution cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get()); } } else { if(jter != createdMeshes_.end()) { cloud = jter->second.cloud; indices = jter->second.indices; gains[0] = jter->second.gains[0]; gains[1] = jter->second.gains[1]; gains[2] = jter->second.gains[2]; } else { rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true); if(!data.imageRaw().empty() && !data.depthRaw().empty()) { cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, 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(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 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) * gains[0]))); pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1]))); pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2]))); } } if(mergedClouds->size() == 0) { *mergedClouds = *transformedCloud; } else { *mergedClouds += *transformedCloud; } } if(progressionStatus_.isCanceled()) { if(blockRendering) { renderingMutex_.unlock(); } exporting_ = false; return false; } progressionStatus_.increment(); } if(mergedClouds->size()) { if(cloudVoxelSize > 0.0f) { mergedClouds = rtabmap::util3d::voxelize(mergedClouds, cloudVoxelSize); } // save in database { cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds)); // for database boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, poses); success = true; } } else { UERROR("Merged cloud is empty!"); } } progressionStatus_.finish(); if(blockRendering) { renderingMutex_.unlock(); } } catch (std::exception & e) { UERROR("Out of memory! %s", e.what()); if(blockRendering) { renderingMutex_.unlock(); } success = false; } exporting_ = false; return success; } bool RTABMapApp::postExportation(bool visualize) { LOGI("postExportation(visualize=%d)", visualize?1:0); optMesh_.reset(new pcl::TextureMesh); optTexture_ = cv::Mat(); exportedMeshUpdated_ = false; visualizingMesh_ = false; if(visualize) { cv::Mat cloudMat; std::vector > > polygons; #if PCL_VERSION_COMPARE(>=, 1, 8, 0) std::vector > > texCoords; #else std::vector > texCoords; #endif cv::Mat textures; std::map optPoses; if(rtabmap_ && rtabmap_->getMemory()) { cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures); if(!cloudMat.empty()) { LOGI("postExportation: Found optimized mesh! Visualizing it."); optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true); optTexture_ = textures; boost::mutex::scoped_lock lock(renderingMutex_); visualizingMesh_ = true; exportedMeshUpdated_ = true; } else { LOGI("postExportation: No optimized mesh found."); } } } return visualizingMesh_; } bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::string & name) { LOGI("writeExportedMesh: dir=%s name=%s", directory.c_str(), name.c_str()); exporting_ = true; bool success = false; pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh); pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh); cv::Mat cloudMat; std::vector > > polygons; #if PCL_VERSION_COMPARE(>=, 1, 8, 0) std::vector > > texCoords; #else std::vector > texCoords; #endif cv::Mat textures; std::map optPoses; if(rtabmap_ && rtabmap_->getMemory()) { cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&optPoses, &polygons, &texCoords, &textures); if(!cloudMat.empty()) { LOGI("writeExportedMesh: Found optimized mesh!"); if(textures.empty()) { polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector >()); } else { textureMesh = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, false); } } else { LOGI("writeExportedMesh: No optimized mesh found."); } } if(polygonMesh->cloud.data.size()) { // Point cloud PLY std::string filePath = directory + UDirectory::separator() + name + ".ply"; LOGI("Saving ply (%d vertices, %d polygons) to %s.", (int)polygonMesh->cloud.data.size()/polygonMesh->cloud.point_step, (int)polygonMesh->polygons.size(), filePath.c_str()); success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0; if(success) { LOGI("Saved ply to %s!", filePath.c_str()); } else { UERROR("Failed saving ply to %s!", filePath.c_str()); } } else if(textureMesh->cloud.data.size()) { // TextureMesh OBJ LOGD("Saving texture(s) (%d)", textures.empty()?0:textures.cols/textures.rows); UASSERT(textures.empty() || textures.cols % textures.rows == 0); UASSERT((int)textureMesh->tex_materials.size() == textures.cols/textures.rows); for(unsigned int i=0; itex_materials.size(); ++i) { std::string baseNameNum = name; if(textureMesh->tex_materials.size()>1) { baseNameNum+=uNumber2Str(i); } std::string fullPath = directory+UDirectory::separator()+baseNameNum+".jpg"; textureMesh->tex_materials[i].tex_file = baseNameNum+".jpg"; LOGI("Saving texture to %s.", fullPath.c_str()); success = cv::imwrite(fullPath, textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows))); if(!success) { LOGI("Failed saving %s!", fullPath.c_str()); } else { LOGI("Saved %s.", fullPath.c_str()); } } if(success) { // With Sketchfab, the OBJ models are rotated 90 degrees on x axis, so rotate -90 to have model in right position pcl::PointCloud::Ptr cloud(new pcl::PointCloud); pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud); cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform(1,0,0,0, 0,0,1,0, 0,-1,0,0)); pcl::toPCLPointCloud2(*cloud, textureMesh->cloud); std::string filePath = directory + UDirectory::separator() + name + ".obj"; int totalPolygons = 0; for(unsigned int i=0;itex_polygons.size(); ++i) { totalPolygons += textureMesh->tex_polygons[i].size(); } 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()); } else { UERROR("Failed saving obj to %s!", filePath.c_str()); } } } exporting_ = false; return success; } int RTABMapApp::postProcessing(int approach) { postProcessing_ = true; LOGI("postProcessing(%d)", approach); int returnedValue = 0; if(rtabmap_) { std::map poses; std::multimap links; // detect more loop closures if(approach == -1 || approach == 2) { if(approach == -1) { progressionStatus_.reset(6); } returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, approach==-1?&progressionStatus_:0); if(approach == -1 && progressionStatus_.isCanceled()) { postProcessing_ = false; return -1; } } // graph optimization if(returnedValue >=0) { if (approach == 1) { if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O)) { std::map signatures; rtabmap_->getGraph(poses, links, true, true, &signatures); rtabmap::ParametersMap param; param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30")); param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0")); rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param); poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures); delete sba; } else { UERROR("g2o not available!"); } } else if(approach!=4 && approach!=5 && approach != 7) { // simple graph optmimization rtabmap_->getGraph(poses, links, true, true); } } if(poses.size()) { boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmap::Statistics stats = rtabmap_->getStatistics(); stats.setPoses(poses); stats.setConstraints(links); LOGI("PostProcessing, sending rtabmap event to update graph..."); rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats)); rtabmap_->setOptimizedPoses(poses); } else if(approach!=4 && approach!=5 && approach != 7) { returnedValue = -1; } if(returnedValue >=0) { boost::mutex::scoped_lock lock(renderingMutex_); // filter polygons if(approach == -1 || approach == 4) { filterPolygonsOnNextRender_ = true; } // gain compensation if(approach == -1 || approach == 5 || approach == 6) { gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast } // bilateral filtering if(approach == 7) { bilateralFilteringOnNextRender_ = true; } } } postProcessing_ = false; return returnedValue; } bool RTABMapApp::handleEvent(UEvent * event) { if(camera_ && camera_->isRunning()) { // called from events manager thread, so protect the data if(event->getClassName().compare("OdometryEvent") == 0) { LOGI("Received OdometryEvent!"); if(odomMutex_.try_lock()) { odomEvents_.clear(); if(camera_->isRunning()) { odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event))); } odomMutex_.unlock(); } } if(status_.first == rtabmap::RtabmapEventInit::kInitialized && event->getClassName().compare("RtabmapEvent") == 0) { LOGI("Received RtabmapEvent event!"); if(camera_->isRunning()) { boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event); return true; } } } if(event->getClassName().compare("PoseEvent") == 0) { if(poseMutex_.try_lock()) { poseEvents_.clear(); poseEvents_.push_back(((rtabmap::PoseEvent*)event)->pose()); poseMutex_.unlock(); } } if(event->getClassName().compare("CameraTangoEvent") == 0) { rtabmap::CameraTangoEvent * tangoEvent = (rtabmap::CameraTangoEvent*)event; // Call JAVA callback with tango event msg bool success = false; if(jvm && RTABMapActivity) { JNIEnv *env = 0; jint rs = jvm->AttachCurrentThread(&env, NULL); if(rs == JNI_OK && env) { jclass clazz = env->GetObjectClass(RTABMapActivity); if(clazz) { jmethodID methodID = env->GetMethodID(clazz, "tangoEventCallback", "(ILjava/lang/String;Ljava/lang/String;)V" ); if(methodID) { env->CallVoidMethod(RTABMapActivity, methodID, tangoEvent->type(), env->NewStringUTF(tangoEvent->key().c_str()), env->NewStringUTF(tangoEvent->value().c_str())); success = true; } } } jvm->DetachCurrentThread(); } if(!success) { UERROR("Failed to call RTABMapActivity::tangoEventCallback"); } } if(event->getClassName().compare("RtabmapEventInit") == 0) { LOGI("Received RtabmapEventInit!"); status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus(); status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo(); // Call JAVA callback with init msg bool success = false; if(jvm && RTABMapActivity) { JNIEnv *env = 0; jint rs = jvm->AttachCurrentThread(&env, NULL); if(rs == JNI_OK && env) { jclass clazz = env->GetObjectClass(RTABMapActivity); if(clazz) { jmethodID methodID = env->GetMethodID(clazz, "rtabmapInitEventCallback", "(ILjava/lang/String;)V" ); if(methodID) { env->CallVoidMethod(RTABMapActivity, methodID, status_.first, env->NewStringUTF(status_.second.c_str())); success = true; } } } jvm->DetachCurrentThread(); } if(!success) { UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback"); } } if(event->getClassName().compare("PostRenderEvent") == 0) { LOGI("Received PostRenderEvent!"); int loopClosureId = 0; int featuresExtracted = 0; if(((PostRenderEvent*)event)->getRtabmapEvent()) { const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getRtabmapEvent()->getStats(); loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0; featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0; uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryWorking_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryShort_time_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kKeypointDictionary_size(), uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kTimingTotal(), uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopHighest_hypothesis_id(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryDatabase_memory_used(), uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopVisual_inliers(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopVisual_matches(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_matches(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopRejectedHypothesis(), uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f))); uInsert(bufferedStatsData_, std::make_pair(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f))); } // else use last data int nodes = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) + uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f); int words = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kKeypointDictionary_size(), 0.0f); float updateTime = uValue(bufferedStatsData_, rtabmap::Statistics::kTimingTotal(), 0.0f); int highestHypId = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f); int databaseMemoryUsed = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f); int inliers = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_inliers(), 0.0f); int matches = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_matches(), 0.0f); int rejected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f); float optimizationMaxError = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f); float rehearsalValue = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f); float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f); float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f); int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f); // Call JAVA callback with some stats UINFO("Send statistics to GUI"); bool success = false; if(jvm && RTABMapActivity) { JNIEnv *env = 0; jint rs = jvm->AttachCurrentThread(&env, NULL); if(rs == JNI_OK && env) { jclass clazz = env->GetObjectClass(RTABMapActivity); if(clazz) { jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIIFIFIFFFI)V" ); if(methodID) { env->CallVoidMethod(RTABMapActivity, methodID, nodes, words, totalPoints_, totalPolygons_, updateTime, loopClosureId, highestHypId, (int)((processMemoryUsedBytes+processGPUMemoryUsedBytes)/(1024*1024)), databaseMemoryUsed, inliers, matches, featuresExtracted, hypothesis, lastDrawnCloudsCount_, renderingTime_>0.0f?1.0f/renderingTime_:0.0f, rejected, rehearsalValue, optimizationMaxError, distanceTravelled, fastMovement); success = true; } } } jvm->DetachCurrentThread(); } if(!success) { UERROR("Failed to call RTABMapActivity::updateStatsCallback"); } renderingTime_ = 0.0f; } return false; }