void ULogger::registerCurrentThread(const std::string & name) { loggerMutex_.lock(); UASSERT(!name.empty()); uInsert(registeredThreads_, std::make_pair(name, UThread::currentThreadId())); loggerMutex_.unlock(); }
int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value) { if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end()) { LOGI(uFormat("Setting param \"%s\" to \"\"", key.c_str(), value.c_str()).c_str()); uInsert(mappingParameters_, rtabmap::ParametersPair(key, value)); UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_)); return 0; } else { LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str()); return -1; } }
// OpenGL thread int RTABMapApp::Render() { // should be before clearSceneOnNextRender_ in case openDatabase is called std::list<rtabmap::Statistics> rtabmapEvents; { boost::mutex::scoped_lock lock(rtabmapMutex_); rtabmapEvents = rtabmapEvents_; rtabmapEvents_.clear(); } if(clearSceneOnNextRender_) { 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; renderingFPS_ = 0.0f; } // Process events rtabmap::Transform pose; { boost::mutex::scoped_lock lock(poseMutex_); if(poseEvents_.size()) { pose = poseEvents_.back(); poseEvents_.clear(); } } if(!pose.isNull()) { // update camera pose? main_scene_.SetCameraPose(pose); } bool notifyDataLoaded = false; if(rtabmapEvents.size()) { LOGI("Process rtabmap events"); // update buffered signatures std::map<int, rtabmap::SensorData> bufferedSensorData; if(!trajectoryMode_) { for(std::list<rtabmap::Statistics>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter) { for(std::map<int, rtabmap::Signature>::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(!jter->second.sensorData().imageCompressed().empty() && !jter->second.sensorData().depthOrRightCompressed().empty()) { // uncompress rtabmap::SensorData data = jter->second.sensorData(); cv::Mat tmpA,tmpB; data.uncompressData(&tmpA, &tmpB); uInsert(bufferedSensorData, std::make_pair(jter->first, data)); uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose())); notifyDataLoaded = true; } } } } std::map<int, rtabmap::Transform> poses = rtabmapEvents.back().poses(); // Transform pose in OpenGL world for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter) { if(!graphOptimization_) { std::map<int, rtabmap::Transform>::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<int, rtabmap::Link> & links = rtabmapEvents.back().constraints(); if(poses.size()) { //update graph main_scene_.updateGraph(poses, links); // update clouds boost::mutex::scoped_lock lock(meshesMutex_); std::set<std::string> strIds; for(std::map<int, rtabmap::Transform>::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<int, Mesh>::iterator meshIter = createdMeshes_.find(id); if(meshIter!=createdMeshes_.end()) { meshIter->second.pose = iter->second; } else { UERROR("Not found mesh %d !?!?", id); } } 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<pcl::PointXYZRGB>::Ptr cloud; pcl::IndicesPtr indices(new std::vector<int>); LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows); cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get()); if(cloud->size() && indices->size()) { UTimer time; // pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>); pcl::ExtractIndices<pcl::PointXYZRGB> filter; filter.setIndices(indices); filter.setKeepOrganized(true); filter.setInputCloud(cloud); filter.filter(*output); std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>); std::vector<pcl::Vertices> outputPolygons; outputCloud = output; outputPolygons = polygons; LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks()); if(outputCloud->size() && outputPolygons.size()) { totalPolygons_ += outputPolygons.size(); main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw()); // protect createdMeshes_ used also by exportMesh() method std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh())); UASSERT(inserted.second); inserted.first->second.cloud = outputCloud; inserted.first->second.polygons = outputPolygons; inserted.first->second.pose = iter->second; inserted.first->second.texture = data.imageCompressed(); } else { LOGE("Not mesh could be created for node %d", id); } } totalPoints_+=indices->size(); } } } } } //filter poses? if(poses.size() > 2) { if(nodesFiltering_) { for(std::multimap<int, rtabmap::Link>::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); } } } } //update cloud visibility std::set<int> addedClouds = main_scene_.getAddedClouds(); for(std::set<int>::const_iterator iter=addedClouds.begin(); iter!=addedClouds.end(); ++iter) { if(*iter > 0 && poses.find(*iter) == poses.end()) { main_scene_.setCloudVisible(*iter, false); } } } else { rtabmap::OdometryEvent event; bool set = false; { boost::mutex::scoped_lock lock(odomMutex_); if(odomEvents_.size()) { LOGI("Process odom events"); event = odomEvents_.back(); odomEvents_.clear(); set = true; } } main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_); //just process the last one if(set && !event.pose().isNull()) { if(odomCloudShown_ && !trajectoryMode_) { if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty()) { pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud; cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_); if(cloud->size()) { LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)", event.data().imageRaw().cols, event.data().imageRaw().rows, event.data().depthRaw().cols, event.data().depthRaw().rows, (int)cloud->width, (int)cloud->height); std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_); main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw()); main_scene_.setCloudVisible(-1, true); } else { LOGE("Generated cloud is empty!"); } } else { LOGE("Odom data images are empty!"); } } } } UTimer fpsTime; lastDrawnCloudsCount_ = main_scene_.Render(); renderingFPS_ = 1.0/fpsTime.elapsed(); if(rtabmapEvents.size()) { // send statistics to GUI LOGI("Posting PostRenderEvent!"); UEventsManager::post(new PostRenderEvent(rtabmapEvents.back())); } return notifyDataLoaded?1:0; }