00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <tango-gl/conversions.h>
00029
00030 #include "RTABMapApp.h"
00031
00032 #include <rtabmap/core/Rtabmap.h>
00033 #include <rtabmap/core/util2d.h>
00034 #include <rtabmap/core/util3d.h>
00035 #include <rtabmap/core/util3d_transforms.h>
00036 #include <rtabmap/core/util3d_filtering.h>
00037 #include <rtabmap/core/util3d_surface.h>
00038 #include <rtabmap/core/Graph.h>
00039 #include <rtabmap/utilite/UEventsManager.h>
00040 #include <rtabmap/utilite/UStl.h>
00041 #include <rtabmap/utilite/UDirectory.h>
00042 #include <rtabmap/utilite/UFile.h>
00043 #include <opencv2/opencv_modules.hpp>
00044 #include <rtabmap/core/util3d_surface.h>
00045 #include <rtabmap/utilite/UConversion.h>
00046 #include <rtabmap/utilite/UTimer.h>
00047 #include <rtabmap/core/ParamEvent.h>
00048 #include <rtabmap/core/Compression.h>
00049 #include <rtabmap/core/Optimizer.h>
00050 #include <rtabmap/core/VWDictionary.h>
00051 #include <rtabmap/core/Memory.h>
00052 #include <rtabmap/core/GainCompensator.h>
00053 #include <rtabmap/core/DBDriver.h>
00054 #include <pcl/common/common.h>
00055 #include <pcl/filters/extract_indices.h>
00056 #include <pcl/io/ply_io.h>
00057 #include <pcl/io/obj_io.h>
00058 #include <pcl/surface/poisson.h>
00059 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
00060
00061 #define LOW_RES_PIX 2
00062
00063
00064 const int g_optMeshId = -100;
00065
00066 static JavaVM *jvm;
00067 static jobject RTABMapActivity = 0;
00068
00069 namespace {
00070 constexpr int kTangoCoreMinimumVersion = 9377;
00071 }
00072
00073 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
00074 {
00075 rtabmap::ParametersMap parameters;
00076
00077 parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
00078
00079 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
00080 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
00081 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
00082 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
00083 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
00084 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
00085 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
00086 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
00087 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
00088 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
00089 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
00090 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1"));
00091 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));
00092 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemCompressionParallelized(), std::string("false")));
00093 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
00094 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10")));
00095 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
00096 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
00097 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0")));
00098 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("1")));
00099 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0")));
00100 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false")));
00101 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
00102 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
00103
00104 if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
00105 {
00106 if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0)
00107 {
00108 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
00109 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
00110 }
00111 else if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("1") == 0)
00112 {
00113 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.0"));
00114 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
00115 }
00116 else
00117 {
00118 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
00119 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"100":"0"));
00120 }
00121 }
00122
00123 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true")));
00124 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("0")));
00125 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10")));
00126 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
00127 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17")));
00128 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
00129 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5")));
00130 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
00131
00132 parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
00133 parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemRehearsalSimilarity()));
00134 parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded()));
00135 if(dataRecorderMode_)
00136 {
00137 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1")));
00138 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0")));
00139 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false"));
00140 uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true")));
00141 }
00142
00143 return parameters;
00144 }
00145
00146 RTABMapApp::RTABMapApp() :
00147 camera_(0),
00148 rtabmapThread_(0),
00149 rtabmap_(0),
00150 logHandler_(0),
00151 odomCloudShown_(true),
00152 graphOptimization_(true),
00153 nodesFiltering_(false),
00154 localizationMode_(false),
00155 trajectoryMode_(false),
00156 rawScanSaved_(false),
00157 smoothing_(true),
00158 cameraColor_(true),
00159 fullResolution_(false),
00160 appendMode_(true),
00161 maxCloudDepth_(0.0),
00162 minCloudDepth_(0.0),
00163 cloudDensityLevel_(1),
00164 meshTrianglePix_(1),
00165 meshAngleToleranceDeg_(15.0),
00166 clusterRatio_(0.1),
00167 maxGainRadius_(0.02f),
00168 renderingTextureDecimation_(4),
00169 backgroundColor_(0.2f),
00170 paused_(false),
00171 dataRecorderMode_(false),
00172 clearSceneOnNextRender_(false),
00173 openingDatabase_(false),
00174 exporting_(false),
00175 postProcessing_(false),
00176 filterPolygonsOnNextRender_(false),
00177 gainCompensationOnNextRender_(0),
00178 bilateralFilteringOnNextRender_(false),
00179 takeScreenshotOnNextRender_(false),
00180 cameraJustInitialized_(false),
00181 meshDecimation_(1),
00182 totalPoints_(0),
00183 totalPolygons_(0),
00184 lastDrawnCloudsCount_(0),
00185 renderingTime_(0.0f),
00186 lastPostRenderEventTime_(0.0),
00187 lastPoseEventTime_(0.0),
00188 visualizingMesh_(false),
00189 exportedMeshUpdated_(false),
00190 optMesh_(new pcl::TextureMesh),
00191 optRefId_(0),
00192 optRefPose_(0),
00193 mapToOdom_(rtabmap::Transform::getIdentity())
00194
00195 {
00196 mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5"));
00197 }
00198
00199 RTABMapApp::~RTABMapApp() {
00200 if(camera_)
00201 {
00202 delete camera_;
00203 }
00204 if(rtabmapThread_)
00205 {
00206 rtabmapThread_->close(false);
00207 delete rtabmapThread_;
00208 }
00209 if(logHandler_)
00210 {
00211 delete logHandler_;
00212 }
00213 if(optRefPose_)
00214 {
00215 delete optRefPose_;
00216 }
00217 {
00218 boost::mutex::scoped_lock lock(rtabmapMutex_);
00219 if(rtabmapEvents_.size())
00220 {
00221 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
00222 {
00223 delete *iter;
00224 }
00225 }
00226 rtabmapEvents_.clear();
00227 }
00228 {
00229 boost::mutex::scoped_lock lock(visLocalizationMutex_);
00230 if(visLocalizationEvents_.size())
00231 {
00232 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
00233 {
00234 delete *iter;
00235 }
00236 }
00237 visLocalizationEvents_.clear();
00238 }
00239 }
00240
00241 void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
00242 {
00243 env->GetJavaVM(&jvm);
00244 RTABMapActivity = env->NewGlobalRef(caller_activity);
00245
00246 LOGI("RTABMapApp::onCreate()");
00247 createdMeshes_.clear();
00248 rawPoses_.clear();
00249 clearSceneOnNextRender_ = true;
00250 openingDatabase_ = false;
00251 exporting_ = false;
00252 postProcessing_=false;
00253 totalPoints_ = 0;
00254 totalPolygons_ = 0;
00255 lastDrawnCloudsCount_ = 0;
00256 renderingTime_ = 0.0f;
00257 lastPostRenderEventTime_ = 0.0;
00258 lastPoseEventTime_ = 0.0;
00259 bufferedStatsData_.clear();
00260 progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
00261 main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
00262
00263 if(camera_)
00264 {
00265 delete camera_;
00266 camera_ = 0;
00267 }
00268 if(rtabmapThread_)
00269 {
00270 rtabmapThread_->close(false);
00271 delete rtabmapThread_;
00272 rtabmapThread_ = 0;
00273 rtabmap_ = 0;
00274 }
00275
00276 if(logHandler_ == 0)
00277 {
00278 logHandler_ = new LogHandler();
00279 }
00280
00281 this->registerToEventsManager();
00282
00283 camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
00284 }
00285
00286 void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
00287 {
00288 TangoSupportRotation rotation = tango_gl::util::GetAndroidRotationFromColorCameraToDisplay(displayRotation, cameraRotation);
00289 LOGI("Set orientation: display=%d camera=%d -> %d", displayRotation, cameraRotation, (int)rotation);
00290 main_scene_.setScreenRotation(rotation);
00291 camera_->setScreenRotation(rotation);
00292 }
00293
00294 int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, const std::string & databaseSource)
00295 {
00296 LOGI("Opening database %s (inMemory=%d, optimize=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0);
00297 this->unregisterFromEventsManager();
00298 status_.first = rtabmap::RtabmapEventInit::kInitializing;
00299 rtabmapMutex_.lock();
00300 if(rtabmapEvents_.size())
00301 {
00302 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
00303 {
00304 delete *iter;
00305 }
00306 }
00307 rtabmapEvents_.clear();
00308 openingDatabase_ = true;
00309 if(rtabmapThread_)
00310 {
00311 rtabmapThread_->close(false);
00312 delete rtabmapThread_;
00313 rtabmapThread_ = 0;
00314 rtabmap_ = 0;
00315 }
00316
00317 this->registerToEventsManager();
00318
00319 int status = 0;
00320
00321
00322 optMesh_.reset(new pcl::TextureMesh);
00323 optTexture_ = cv::Mat();
00324 optRefId_ = 0;
00325 if(optRefPose_)
00326 {
00327 delete optRefPose_;
00328 optRefPose_ = 0;
00329 }
00330 cv::Mat cloudMat;
00331 std::vector<std::vector<std::vector<unsigned int> > > polygons;
00332 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
00333 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
00334 #else
00335 std::vector<std::vector<Eigen::Vector2f> > texCoords;
00336 #endif
00337 cv::Mat textures;
00338 if(!databaseSource.empty())
00339 {
00340 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
00341 rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
00342 if(driver->openConnection(databaseSource))
00343 {
00344 cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
00345 if(!cloudMat.empty())
00346 {
00347 LOGI("Open: Found optimized mesh! Visualizing it.");
00348 optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
00349 optTexture_ = textures;
00350 if(!optTexture_.empty())
00351 {
00352 LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
00353 status=3;
00354 }
00355 else if(optMesh_->tex_polygons.size())
00356 {
00357 LOGI("Open: Polygon mesh");
00358 status=2;
00359 }
00360 else if(!optMesh_->cloud.data.empty())
00361 {
00362 LOGI("Open: Point cloud");
00363 status=1;
00364 }
00365 }
00366 else
00367 {
00368 LOGI("Open: No optimized mesh found.");
00369 }
00370 delete driver;
00371 }
00372 }
00373
00374 if(status > 0)
00375 {
00376 if(status==1)
00377 {
00378 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud...done!"));
00379 }
00380 else if(status==2)
00381 {
00382 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized mesh...done!"));
00383 }
00384 else
00385 {
00386 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized texture mesh...done!"));
00387 }
00388 boost::mutex::scoped_lock lockRender(renderingMutex_);
00389 visualizingMesh_ = true;
00390 exportedMeshUpdated_ = true;
00391 }
00392
00393 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database..."));
00394 LOGI("Erasing database \"%s\"...", databasePath.c_str());
00395 UFile::erase(databasePath);
00396 if(!databaseSource.empty())
00397 {
00398 LOGI("Copying database source \"%s\" to \"%s\"...", databaseSource.c_str(), databasePath.c_str());
00399 UFile::copy(databaseSource, databasePath);
00400 }
00401
00402
00403 mapToOdom_.setIdentity();
00404 rtabmap_ = new rtabmap::Rtabmap();
00405 rtabmap::ParametersMap parameters = getRtabmapParameters();
00406
00407 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory)));
00408 LOGI("Initializing database...");
00409 rtabmap_->init(parameters, databasePath);
00410 rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
00411 if(parameters.find(rtabmap::Parameters::kRtabmapDetectionRate()) != parameters.end())
00412 {
00413 rtabmapThread_->setDetectorRate(uStr2Float(parameters.at(rtabmap::Parameters::kRtabmapDetectionRate())));
00414 }
00415
00416
00417 std::map<int, rtabmap::Signature> signatures;
00418 std::map<int, rtabmap::Transform> poses;
00419 std::multimap<int, rtabmap::Link> links;
00420 LOGI("Loading full map from database...");
00421 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading data from database..."));
00422 rtabmap_->get3DMap(
00423 signatures,
00424 poses,
00425 links,
00426 true,
00427 true);
00428
00429 if(signatures.size() && poses.empty())
00430 {
00431 LOGE("Failed to optimize the graph!");
00432 status = -1;
00433 }
00434
00435 {
00436 LOGI("Creating the meshes (%d)....", (int)poses.size());
00437 boost::mutex::scoped_lock lock(meshesMutex_);
00438 createdMeshes_.clear();
00439 int i=0;
00440 UTimer addTime;
00441 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status>=0; ++iter)
00442 {
00443 try
00444 {
00445 int id = iter->first;
00446 if(!iter->second.isNull())
00447 {
00448 if(uContains(signatures, id))
00449 {
00450 UTimer timer;
00451 rtabmap::SensorData data = signatures.at(id).sensorData();
00452
00453 cv::Mat tmpA, depth;
00454 data.uncompressData(&tmpA, &depth);
00455 if(!data.imageRaw().empty() && !data.depthRaw().empty())
00456 {
00457
00458 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00459 pcl::IndicesPtr indices(new std::vector<int>);
00460 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
00461 if(cloud->size() && indices->size())
00462 {
00463 std::vector<pcl::Vertices> polygons;
00464 std::vector<pcl::Vertices> polygonsLowRes;
00465 if(main_scene_.isMeshRendering() && main_scene_.isMapRendering())
00466 {
00467 polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
00468 polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
00469 }
00470
00471 if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering())
00472 {
00473 std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
00474 UASSERT(inserted.second);
00475 inserted.first->second.cloud = cloud;
00476 inserted.first->second.indices = indices;
00477 inserted.first->second.polygons = polygons;
00478 inserted.first->second.polygonsLowRes = polygonsLowRes;
00479 inserted.first->second.visible = true;
00480 inserted.first->second.cameraModel = data.cameraModels()[0];
00481 inserted.first->second.gains[0] = 1.0;
00482 inserted.first->second.gains[1] = 1.0;
00483 inserted.first->second.gains[2] = 1.0;
00484 if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
00485 {
00486 if(renderingTextureDecimation_>1)
00487 {
00488 cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
00489 cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
00490 }
00491 else
00492 {
00493 inserted.first->second.texture = data.imageRaw();
00494 }
00495 }
00496 LOGI("Created cloud %d (%fs)", id, timer.ticks());
00497 }
00498 else
00499 {
00500 LOGI("Cloud %d not added to created meshes", id);
00501 }
00502 }
00503 else
00504 {
00505 UWARN("Cloud %d is empty", id);
00506 }
00507 }
00508 else
00509 {
00510 UERROR("Failed to uncompress data!");
00511 status=-2;
00512 }
00513 }
00514 else
00515 {
00516 UWARN("Data for node %d not found", id);
00517 }
00518 }
00519 else
00520 {
00521 UWARN("Pose %d is null !?", id);
00522 }
00523 ++i;
00524 if(addTime.elapsed() >= 4.0f)
00525 {
00526 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, uFormat("Created clouds %d/%d", i, (int)poses.size())));
00527 addTime.restart();
00528 }
00529 }
00530 catch(const UException & e)
00531 {
00532 UERROR("Exception! msg=\"%s\"", e.what());
00533 status = -2;
00534 }
00535 catch (const cv::Exception & e)
00536 {
00537 UERROR("Exception! msg=\"%s\"", e.what());
00538 status = -2;
00539 }
00540 catch (const std::exception & e)
00541 {
00542 UERROR("Exception! msg=\"%s\"", e.what());
00543 status = -2;
00544 }
00545 }
00546 if(status < 0)
00547 {
00548 createdMeshes_.clear();
00549 }
00550 else
00551 {
00552 LOGI("Created %d meshes...", (int)createdMeshes_.size());
00553 }
00554 }
00555
00556
00557
00558 if(optimize && status>=0)
00559 {
00560 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization..."));
00561 gainCompensation();
00562
00563 LOGI("Polygon filtering...");
00564 boost::mutex::scoped_lock lock(meshesMutex_);
00565 UTimer time;
00566 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
00567 {
00568 if(iter->second.polygons.size())
00569 {
00570
00571 iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size());
00572 }
00573 }
00574 }
00575
00576 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Updating scene..."));
00577 LOGI("Open: add rtabmap event to update the scene");
00578 rtabmap::Statistics stats;
00579 stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
00580 stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
00581 stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
00582 stats.setPoses(poses);
00583 stats.setConstraints(links);
00584 rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
00585
00586 rtabmap_->setOptimizedPoses(poses);
00587
00588
00589 if(poses.size())
00590 {
00591
00592 optRefId_ = poses.rbegin()->first;
00593 optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
00594 }
00595
00596 if(camera_)
00597 {
00598 camera_->resetOrigin();
00599 }
00600
00601
00602 LOGI("Start rtabmap thread");
00603 rtabmapThread_->registerToEventsManager();
00604 rtabmapThread_->start();
00605
00606 UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInitialized, ""));
00607
00608 status_.first = rtabmap::RtabmapEventInit::kInitialized;
00609 status_.second = "";
00610 rtabmapMutex_.unlock();
00611
00612 boost::mutex::scoped_lock lockRender(renderingMutex_);
00613 if(poses.empty() || status>0)
00614 {
00615 openingDatabase_ = false;
00616 }
00617
00618 clearSceneOnNextRender_ = status<=0;
00619
00620 return status;
00621 }
00622
00623 bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
00624 {
00625 LOGW("onTangoServiceConnected()");
00626 if(camera_)
00627 {
00628 camera_->join(true);
00629
00630 if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
00631 UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
00632 return false;
00633 }
00634
00635 camera_->setColorCamera(cameraColor_);
00636 if(camera_->init())
00637 {
00638
00639 LOGI("Cloud density level %d", cloudDensityLevel_);
00640 meshDecimation_ = 1;
00641 if(camera_)
00642 {
00643
00644
00645
00646 int width = camera_->getCameraModel().imageWidth()/(cameraColor_?8:1);
00647 int height = camera_->getCameraModel().imageHeight()/(cameraColor_?8:1);
00648 if(cloudDensityLevel_ == 3)
00649 {
00650 if(height >= 480 && width % 20 == 0 && height % 20 == 0)
00651 {
00652 meshDecimation_ = 20;
00653 }
00654 else if(width % 10 == 0 && height % 10 == 0)
00655 {
00656 meshDecimation_ = 10;
00657 }
00658 else if(width % 15 == 0 && height % 15 == 0)
00659 {
00660 meshDecimation_ = 15;
00661 }
00662 else
00663 {
00664 UERROR("Could not set decimation to high (size=%dx%d)", width, height);
00665 }
00666 }
00667 else if(cloudDensityLevel_ == 2)
00668 {
00669 if(height >= 480 && width % 10 == 0 && height % 10 == 0)
00670 {
00671 meshDecimation_ = 10;
00672 }
00673 else if(width % 5 == 0 && height % 5 == 0)
00674 {
00675 meshDecimation_ = 5;
00676 }
00677 else
00678 {
00679 UERROR("Could not set decimation to medium (size=%dx%d)", width, height);
00680 }
00681 }
00682 else if(cloudDensityLevel_ == 1)
00683 {
00684 if(height >= 480 && width % 5 == 0 && height % 5 == 0)
00685 {
00686 meshDecimation_ = 5;
00687 }
00688 else if(width % 3 == 0 && width % 3 == 0)
00689 {
00690 meshDecimation_ = 3;
00691 }
00692 else if(width % 2 == 0 && width % 2 == 0)
00693 {
00694 meshDecimation_ = 2;
00695 }
00696 else
00697 {
00698 UERROR("Could not set decimation to low (size=%dx%d)", width, height);
00699 }
00700 }
00701 }
00702 LOGI("Set decimation to %d", meshDecimation_);
00703
00704 LOGI("Start camera thread");
00705 if(!paused_)
00706 {
00707 camera_->start();
00708 }
00709 cameraJustInitialized_ = true;
00710 return true;
00711 }
00712 UERROR("Failed camera initialization!");
00713 }
00714 return false;
00715 }
00716
00717 void RTABMapApp::onPause()
00718 {
00719 LOGI("onPause()");
00720 if(camera_)
00721 {
00722 camera_->join(true);
00723 camera_->close();
00724 }
00725 }
00726
00727
00728 void RTABMapApp::TangoResetMotionTracking() {
00729 TangoService_resetMotionTracking();
00730 }
00731
00732 std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
00733 const std::vector<pcl::Vertices> & polygons,
00734 int cloudSize) const
00735 {
00736 std::vector<int> vertexToCluster(cloudSize, 0);
00737 std::map<int, std::list<int> > clusters;
00738 int lastClusterID = 0;
00739
00740 for(unsigned int i=0; i<polygons.size(); ++i)
00741 {
00742 int clusterID = 0;
00743 for(unsigned int j=0;j<polygons[i].vertices.size(); ++j)
00744 {
00745 if(vertexToCluster[polygons[i].vertices[j]]>0)
00746 {
00747 clusterID = vertexToCluster[polygons[i].vertices[j]];
00748 break;
00749 }
00750 }
00751 if(clusterID>0)
00752 {
00753 clusters.at(clusterID).push_back(i);
00754 }
00755 else
00756 {
00757 clusterID = ++lastClusterID;
00758 std::list<int> polygons;
00759 polygons.push_back(i);
00760 clusters.insert(std::make_pair(clusterID, polygons));
00761 }
00762 for(unsigned int j=0;j<polygons[i].vertices.size(); ++j)
00763 {
00764 vertexToCluster[polygons[i].vertices[j]] = clusterID;
00765 }
00766 }
00767
00768 unsigned int biggestClusterSize = 0;
00769 for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00770 {
00771 LOGD("cluster %d = %d", iter->first, (int)iter->second.size());
00772
00773 if(iter->second.size() > biggestClusterSize)
00774 {
00775 biggestClusterSize = iter->second.size();
00776 }
00777 }
00778 unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_);
00779 LOGI("Biggest cluster %d -> minClusterSize(ratio=%f)=%d",
00780 biggestClusterSize, clusterRatio_, (int)minClusterSize);
00781
00782 std::vector<pcl::Vertices> filteredPolygons(polygons.size());
00783 int oi = 0;
00784 for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00785 {
00786 if(iter->second.size() >= minClusterSize)
00787 {
00788 for(std::list<int>::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
00789 {
00790 filteredPolygons[oi++] = polygons[*jter];
00791 }
00792 }
00793 }
00794 filteredPolygons.resize(oi);
00795 return filteredPolygons;
00796 }
00797
00798
00799 std::vector<pcl::Vertices> RTABMapApp::filterPolygons(
00800 const std::vector<pcl::Vertices> & polygons,
00801 int cloudSize) const
00802 {
00803
00804 std::vector<std::set<int> > neighbors;
00805 std::vector<std::set<int> > vertexToPolygons;
00806 rtabmap::util3d::createPolygonIndexes(
00807 polygons,
00808 cloudSize,
00809 neighbors,
00810 vertexToPolygons);
00811 std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(neighbors);
00812
00813 unsigned int biggestClusterSize = 0;
00814 for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
00815 {
00816 if(iter->size() > biggestClusterSize)
00817 {
00818 biggestClusterSize = iter->size();
00819 }
00820 }
00821 unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_);
00822 LOGI("Biggest cluster = %d -> minClusterSize(ratio=%f)=%d",
00823 biggestClusterSize, clusterRatio_, (int)minClusterSize);
00824
00825 std::vector<pcl::Vertices> filteredPolygons(polygons.size());
00826 int oi=0;
00827 for(std::list<std::list<int> >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter)
00828 {
00829 if(jter->size() >= minClusterSize)
00830 {
00831 for(std::list<int>::iterator kter=jter->begin(); kter!=jter->end(); ++kter)
00832 {
00833 filteredPolygons[oi++] = polygons.at(*kter);
00834 }
00835 }
00836 }
00837 filteredPolygons.resize(oi);
00838 return filteredPolygons;
00839 }
00840
00841
00842 void RTABMapApp::InitializeGLContent()
00843 {
00844 UINFO("");
00845 main_scene_.InitGLContent();
00846
00847 float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
00848 main_scene_.setGridColor(v, v, v);
00849 }
00850
00851
00852 void RTABMapApp::SetViewPort(int width, int height)
00853 {
00854 UINFO("");
00855 main_scene_.SetupViewPort(width, height);
00856 }
00857
00858 class PostRenderEvent : public UEvent
00859 {
00860 public:
00861 PostRenderEvent(rtabmap::RtabmapEvent * event = 0) :
00862 rtabmapEvent_(event)
00863 {
00864 }
00865 ~PostRenderEvent()
00866 {
00867 if(rtabmapEvent_!=0)
00868 {
00869 delete rtabmapEvent_;
00870 }
00871 }
00872 virtual std::string getClassName() const {return "PostRenderEvent";}
00873 const rtabmap::RtabmapEvent * getRtabmapEvent() const {return rtabmapEvent_;}
00874 private:
00875 rtabmap::RtabmapEvent * rtabmapEvent_;
00876 };
00877
00878
00879 bool RTABMapApp::smoothMesh(int id, Mesh & mesh)
00880 {
00881 UTimer t;
00882
00883 UASSERT(mesh.indices.get() && mesh.indices->size());
00884 cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1);
00885 rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
00886 for(unsigned int i=0; i<mesh.indices->size(); ++i)
00887 {
00888 int index = mesh.indices->at(i);
00889
00890 if(mesh.cloud->at(index).x > 0)
00891 {
00892 pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
00893 depth.at<float>(index) = pt.z;
00894 }
00895 }
00896
00897 depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
00898 LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
00899
00900 if(!depth.empty() && mesh.indices->size())
00901 {
00902 pcl::IndicesPtr newIndices(new std::vector<int>(mesh.indices->size()));
00903 int oi = 0;
00904 for(unsigned int i=0; i<mesh.indices->size(); ++i)
00905 {
00906 int index = mesh.indices->at(i);
00907
00908 pcl::PointXYZRGB & pt = mesh.cloud->at(index);
00909 pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
00910 if(depth.at<float>(index) > 0)
00911 {
00912 newPt.z = depth.at<float>(index);
00913 newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform());
00914 newIndices->at(oi++) = index;
00915 }
00916 else
00917 {
00918 newPt.x = newPt.y = newPt.z = std::numeric_limits<float>::quiet_NaN();
00919 }
00920 pt.x = newPt.x;
00921 pt.y = newPt.y;
00922 pt.z = newPt.z;
00923 }
00924 newIndices->resize(oi);
00925 mesh.indices = newIndices;
00926
00927
00928 std::vector<pcl::Vertices> polygons;
00929 if(main_scene_.isMeshRendering())
00930 {
00931 polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
00932 }
00933 LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
00934 mesh.polygons = polygons;
00935 }
00936 else
00937 {
00938 UERROR("smoothMesh() Failed to smooth surface %d", id);
00939 return false;
00940 }
00941 return true;
00942 }
00943
00944 void RTABMapApp::gainCompensation(bool full)
00945 {
00946 UTimer tGainCompensation;
00947 LOGI("Gain compensation...");
00948 boost::mutex::scoped_lock lock(meshesMutex_);
00949
00950 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
00951 std::map<int, pcl::IndicesPtr> indices;
00952 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
00953 {
00954 clouds.insert(std::make_pair(iter->first, iter->second.cloud));
00955 indices.insert(std::make_pair(iter->first, iter->second.indices));
00956 }
00957 std::map<int, rtabmap::Transform> poses;
00958 std::multimap<int, rtabmap::Link> links;
00959 rtabmap_->getGraph(poses, links, true, true);
00960 if(full)
00961 {
00962
00963 links.clear();
00964 for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
00965 {
00966 int from = iter->first;
00967 std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator jter = iter;
00968 ++jter;
00969 for(;jter!=clouds.end(); ++jter)
00970 {
00971 int to = jter->first;
00972 links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to))));
00973 }
00974 }
00975 }
00976
00977 UASSERT(maxGainRadius_>0.0f);
00978 rtabmap::GainCompensator compensator(maxGainRadius_, 0.0f, 0.01f, 1.0f);
00979 if(clouds.size() > 1 && links.size())
00980 {
00981 compensator.feed(clouds, indices, links);
00982 LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
00983 }
00984
00985 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
00986 {
00987 if(!iter->second.cloud->empty())
00988 {
00989 if(clouds.size() > 1 && links.size())
00990 {
00991 compensator.getGain(iter->first, &iter->second.gains[0], &iter->second.gains[1], &iter->second.gains[2]);
00992 LOGI("%d mesh has gain %f,%f,%f", iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]);
00993 }
00994 }
00995 }
00996 LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
00997 }
00998
00999
01000 int RTABMapApp::Render()
01001 {
01002 std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
01003 try
01004 {
01005 UASSERT(camera_!=0);
01006
01007 UTimer fpsTime;
01008 #ifdef DEBUG_RENDERING_PERFORMANCE
01009 UTimer time;
01010 #endif
01011 boost::mutex::scoped_lock lock(renderingMutex_);
01012
01013 bool notifyDataLoaded = false;
01014 bool notifyCameraStarted = false;
01015
01016 if(clearSceneOnNextRender_)
01017 {
01018 visualizingMesh_ = false;
01019 }
01020
01021
01022 rtabmap::Transform pose;
01023 {
01024 boost::mutex::scoped_lock lock(poseMutex_);
01025 if(poseEvents_.size())
01026 {
01027 pose = poseEvents_.back();
01028 poseEvents_.clear();
01029 }
01030 }
01031 if(!pose.isNull())
01032 {
01033
01034 if(graphOptimization_ && !mapToOdom_.isIdentity())
01035 {
01036 main_scene_.SetCameraPose(opengl_world_T_rtabmap_world*mapToOdom_*rtabmap_world_T_tango_world*pose);
01037 }
01038 else
01039 {
01040 main_scene_.SetCameraPose(opengl_world_T_tango_world*pose);
01041 }
01042 if(!camera_->isRunning() && cameraJustInitialized_)
01043 {
01044 notifyCameraStarted = true;
01045 cameraJustInitialized_ = false;
01046 }
01047 lastPoseEventTime_ = UTimer::now();
01048 }
01049
01050 rtabmap::OdometryEvent odomEvent;
01051 {
01052 boost::mutex::scoped_lock lock(odomMutex_);
01053 if(odomEvents_.size())
01054 {
01055 LOGI("Process odom events");
01056 odomEvent = odomEvents_.back();
01057 odomEvents_.clear();
01058 if(cameraJustInitialized_)
01059 {
01060 notifyCameraStarted = true;
01061 cameraJustInitialized_ = false;
01062 }
01063 }
01064 }
01065
01066 if(visualizingMesh_)
01067 {
01068 if(exportedMeshUpdated_)
01069 {
01070 main_scene_.clear();
01071 exportedMeshUpdated_ = false;
01072 }
01073 if(!main_scene_.hasCloud(g_optMeshId))
01074 {
01075 LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...",
01076 optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
01077 optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
01078 optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
01079 (int)optMesh_->tex_materials.size(),
01080 optTexture_.cols, optTexture_.rows);
01081 if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
01082 {
01083 Mesh mesh;
01084 mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
01085 mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
01086 mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
01087 pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
01088 pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
01089 mesh.polygons = optMesh_->tex_polygons[0];
01090 mesh.pose.setIdentity();
01091 if(optMesh_->tex_coordinates.size())
01092 {
01093 mesh.texCoords = optMesh_->tex_coordinates[0];
01094 mesh.texture = optTexture_;
01095 }
01096 main_scene_.addMesh(g_optMeshId, mesh, opengl_world_T_rtabmap_world, true);
01097 }
01098 else
01099 {
01100 pcl::IndicesPtr indices(new std::vector<int>);
01101 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
01102 pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
01103 main_scene_.addCloud(g_optMeshId, cloud, indices, opengl_world_T_rtabmap_world);
01104 }
01105
01106
01107 boost::mutex::scoped_lock lock(visLocalizationMutex_);
01108 if(visLocalizationEvents_.size())
01109 {
01110 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents_.begin(); iter!=visLocalizationEvents_.end(); ++iter)
01111 {
01112 delete *iter;
01113 }
01114 }
01115 visLocalizationEvents_.clear();
01116 }
01117
01118 std::list<rtabmap::RtabmapEvent*> visLocalizationEvents;
01119 visLocalizationMutex_.lock();
01120 visLocalizationEvents = visLocalizationEvents_;
01121 visLocalizationEvents_.clear();
01122 visLocalizationMutex_.unlock();
01123
01124 if(visLocalizationEvents.size())
01125 {
01126 const rtabmap::Statistics & stats = visLocalizationEvents.back()->getStats();
01127 if(!stats.mapCorrection().isNull())
01128 {
01129 mapToOdom_ = stats.mapCorrection();
01130 }
01131
01132 std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
01133 if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
01134 {
01135
01136 main_scene_.setCloudPose(g_optMeshId, opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
01137 }
01138 int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
01139 int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
01140 int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
01141 if(!paused_ && loopClosure>0)
01142 {
01143 main_scene_.setBackgroundColor(0, 0.5f, 0);
01144 }
01145 else if(!paused_ && rejected>0)
01146 {
01147 main_scene_.setBackgroundColor(0, 0.2f, 0);
01148 }
01149 else if(!paused_ && fastMovement)
01150 {
01151 main_scene_.setBackgroundColor(0.2f, 0, 0.2f);
01152 }
01153 else
01154 {
01155 main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
01156 }
01157 }
01158
01159
01160 bool isMeshRendering = main_scene_.isMeshRendering();
01161 bool isTextureRendering = main_scene_.isMeshTexturing();
01162
01163 main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId));
01164
01165 fpsTime.restart();
01166 lastDrawnCloudsCount_ = main_scene_.Render();
01167 if(renderingTime_ < fpsTime.elapsed())
01168 {
01169 renderingTime_ = fpsTime.elapsed();
01170 }
01171
01172
01173 main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
01174
01175 if(visLocalizationEvents.size())
01176 {
01177
01178 UEventsManager::post(new PostRenderEvent(visLocalizationEvents.back()));
01179 visLocalizationEvents.pop_back();
01180
01181 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=visLocalizationEvents.begin(); iter!=visLocalizationEvents.end(); ++iter)
01182 {
01183 delete *iter;
01184 }
01185 visLocalizationEvents.clear();
01186 lastPostRenderEventTime_ = UTimer::now();
01187 }
01188 }
01189 else
01190 {
01191 if(main_scene_.hasCloud(g_optMeshId))
01192 {
01193 main_scene_.clear();
01194 optMesh_.reset(new pcl::TextureMesh);
01195 optTexture_ = cv::Mat();
01196 }
01197
01198
01199 if(!openingDatabase_)
01200 {
01201 rtabmapMutex_.lock();
01202 rtabmapEvents = rtabmapEvents_;
01203 rtabmapEvents_.clear();
01204 rtabmapMutex_.unlock();
01205
01206 if(!clearSceneOnNextRender_ && rtabmapEvents.size())
01207 {
01208 boost::mutex::scoped_lock lockMesh(meshesMutex_);
01209 if(createdMeshes_.size())
01210 {
01211 if(rtabmapEvents.front()->getStats().refImageId()>0 && rtabmapEvents.front()->getStats().refImageId() < createdMeshes_.rbegin()->first)
01212 {
01213 LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front()->getStats().refImageId(), createdMeshes_.rbegin()->first);
01214 clearSceneOnNextRender_ = true;
01215 }
01216 }
01217 }
01218 #ifdef DEBUG_RENDERING_PERFORMANCE
01219 if(rtabmapEvents.size())
01220 {
01221 LOGW("begin and getting rtabmap events %fs", time.ticks());
01222 }
01223 #endif
01224 }
01225
01226 if(clearSceneOnNextRender_)
01227 {
01228 LOGI("Clearing all rendering data...");
01229 odomMutex_.lock();
01230 odomEvents_.clear();
01231 odomMutex_.unlock();
01232
01233 poseMutex_.lock();
01234 poseEvents_.clear();
01235 poseMutex_.unlock();
01236
01237 main_scene_.clear();
01238 clearSceneOnNextRender_ = false;
01239 if(!openingDatabase_)
01240 {
01241 boost::mutex::scoped_lock lock(meshesMutex_);
01242 LOGI("Clearing meshes...");
01243 createdMeshes_.clear();
01244 }
01245 else
01246 {
01247 notifyDataLoaded = true;
01248 }
01249 rawPoses_.clear();
01250 totalPoints_ = 0;
01251 totalPolygons_ = 0;
01252 lastDrawnCloudsCount_ = 0;
01253 renderingTime_ = 0.0f;
01254 lastPostRenderEventTime_ = 0.0;
01255 lastPoseEventTime_ = 0.0;
01256 bufferedStatsData_.clear();
01257 }
01258
01259
01260 std::set<int> added = main_scene_.getAddedClouds();
01261 added.erase(-1);
01262 if(!openingDatabase_)
01263 {
01264 boost::mutex::scoped_lock lock(meshesMutex_);
01265 unsigned int meshes = createdMeshes_.size();
01266 if(added.size() != meshes)
01267 {
01268 LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes);
01269 boost::mutex::scoped_lock lockRtabmap(rtabmapMutex_);
01270 UASSERT(rtabmap_!=0);
01271 for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
01272 {
01273 if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
01274 {
01275 LOGI("Re-add mesh %d to OpenGL context", iter->first);
01276 if(main_scene_.isMeshRendering() && iter->second.polygons.size() == 0)
01277 {
01278 iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
01279 iter->second.polygonsLowRes = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
01280 }
01281
01282 if(main_scene_.isMeshTexturing())
01283 {
01284 cv::Mat textureRaw;
01285 textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
01286 if(!textureRaw.empty())
01287 {
01288 if(renderingTextureDecimation_ > 1)
01289 {
01290 cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_);
01291 LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
01292 cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
01293 }
01294 else
01295 {
01296 iter->second.texture = textureRaw;
01297 }
01298 }
01299 }
01300 main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
01301 main_scene_.setCloudVisible(iter->first, iter->second.visible);
01302
01303 iter->second.texture = cv::Mat();
01304 }
01305 }
01306 }
01307 }
01308 else if(notifyDataLoaded)
01309 {
01310 rtabmapMutex_.lock();
01311 rtabmapEvents = rtabmapEvents_;
01312 rtabmapEvents_.clear();
01313 rtabmapMutex_.unlock();
01314 openingDatabase_ = false;
01315 }
01316
01317 if(rtabmapEvents.size())
01318 {
01319 #ifdef DEBUG_RENDERING_PERFORMANCE
01320 LOGW("Process rtabmap events %fs", time.ticks());
01321 #else
01322 LOGI("Process rtabmap events");
01323 #endif
01324
01325
01326 std::map<int, rtabmap::SensorData> bufferedSensorData;
01327 if(!dataRecorderMode_)
01328 {
01329 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
01330 {
01331 const rtabmap::Statistics & stats = (*iter)->getStats();
01332
01333
01334 int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
01335 int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
01336 int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
01337 if(!localizationMode_ && stats.getSignatures().size() &&
01338 smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0)
01339 {
01340 int id = stats.getSignatures().rbegin()->first;
01341 const rtabmap::Signature & s = stats.getSignatures().rbegin()->second;
01342
01343 if(!trajectoryMode_ &&
01344 !s.sensorData().imageRaw().empty() &&
01345 !s.sensorData().depthRaw().empty())
01346 {
01347 uInsert(bufferedSensorData, std::make_pair(id, s.sensorData()));
01348 }
01349
01350 uInsert(rawPoses_, std::make_pair(id, s.getPose()));
01351 }
01352
01353 int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
01354 int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
01355 if(!paused_ && loopClosure>0)
01356 {
01357 main_scene_.setBackgroundColor(0, 0.5f, 0);
01358 }
01359 else if(!paused_ && rejected>0)
01360 {
01361 main_scene_.setBackgroundColor(0, 0.2f, 0);
01362 }
01363 else if(!paused_ && rehearsalMerged>0)
01364 {
01365 main_scene_.setBackgroundColor(0, 0, 0.2f);
01366 }
01367 else if(!paused_ && fastMovement)
01368 {
01369 main_scene_.setBackgroundColor(0.2f, 0, 0.2f);
01370 }
01371 else
01372 {
01373 main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
01374 }
01375 }
01376 }
01377
01378 #ifdef DEBUG_RENDERING_PERFORMANCE
01379 LOGW("Looking fo data to load (%d) %fs", bufferedSensorData.size(), time.ticks());
01380 #endif
01381
01382 std::map<int, rtabmap::Transform> poses = rtabmapEvents.back()->getStats().poses();
01383 if(!rtabmapEvents.back()->getStats().mapCorrection().isNull())
01384 {
01385 mapToOdom_ = rtabmapEvents.back()->getStats().mapCorrection();
01386 }
01387
01388
01389 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
01390 {
01391 if(!graphOptimization_)
01392 {
01393 std::map<int, rtabmap::Transform>::iterator jter = rawPoses_.find(iter->first);
01394 if(jter != rawPoses_.end())
01395 {
01396 iter->second = opengl_world_T_rtabmap_world*jter->second;
01397 }
01398 }
01399 else
01400 {
01401 iter->second = opengl_world_T_rtabmap_world*iter->second;
01402 }
01403 }
01404
01405 const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back()->getStats().constraints();
01406 if(poses.size())
01407 {
01408
01409 main_scene_.updateGraph(poses, links);
01410
01411 #ifdef DEBUG_RENDERING_PERFORMANCE
01412 LOGW("Update graph: %fs", time.ticks());
01413 #endif
01414
01415
01416 boost::mutex::scoped_lock lock(meshesMutex_);
01417 std::set<std::string> strIds;
01418 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
01419 {
01420 int id = iter->first;
01421 if(!iter->second.isNull())
01422 {
01423 if(main_scene_.hasCloud(id))
01424 {
01425
01426 main_scene_.setCloudPose(id, iter->second);
01427 main_scene_.setCloudVisible(id, true);
01428 std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
01429 UASSERT(meshIter!=createdMeshes_.end());
01430 meshIter->second.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
01431 meshIter->second.visible = true;
01432 }
01433 else
01434 {
01435 if(createdMeshes_.find(id) == createdMeshes_.end() &&
01436 bufferedSensorData.find(id) != bufferedSensorData.end())
01437 {
01438 rtabmap::SensorData data = bufferedSensorData.at(id);
01439
01440 cv::Mat tmpA, depth;
01441 data.uncompressData(&tmpA, &depth);
01442 #ifdef DEBUG_RENDERING_PERFORMANCE
01443 LOGW("Decompressing data: %fs", time.ticks());
01444 #endif
01445
01446 if(!data.imageRaw().empty() && !data.depthRaw().empty())
01447 {
01448
01449 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01450 pcl::IndicesPtr indices(new std::vector<int>);
01451 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
01452 #ifdef DEBUG_RENDERING_PERFORMANCE
01453 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());
01454 #endif
01455 if(cloud->size() && indices->size())
01456 {
01457 std::vector<pcl::Vertices> polygons;
01458 std::vector<pcl::Vertices> polygonsLowRes;
01459 if(main_scene_.isMeshRendering() && main_scene_.isMapRendering())
01460 {
01461 polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
01462 #ifdef DEBUG_RENDERING_PERFORMANCE
01463 LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
01464 #endif
01465 polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
01466 #ifdef DEBUG_RENDERING_PERFORMANCE
01467 LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
01468 #endif
01469 }
01470
01471 if((main_scene_.isMeshRendering() && polygons.size()) || !main_scene_.isMeshRendering() || !main_scene_.isMapRendering())
01472 {
01473 std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
01474 UASSERT(inserted.second);
01475 inserted.first->second.cloud = cloud;
01476 inserted.first->second.indices = indices;
01477 inserted.first->second.polygons = polygons;
01478 inserted.first->second.polygonsLowRes = polygonsLowRes;
01479 inserted.first->second.visible = true;
01480 inserted.first->second.cameraModel = data.cameraModels()[0];
01481 inserted.first->second.gains[0] = 1.0;
01482 inserted.first->second.gains[1] = 1.0;
01483 inserted.first->second.gains[2] = 1.0;
01484 if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
01485 {
01486 if(renderingTextureDecimation_ > 1)
01487 {
01488 cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
01489 cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, CV_INTER_LINEAR);
01490 #ifdef DEBUG_RENDERING_PERFORMANCE
01491 LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks());
01492 #endif
01493 }
01494 else
01495 {
01496 inserted.first->second.texture = data.imageRaw();
01497 }
01498 }
01499 }
01500 }
01501 }
01502 }
01503 if(createdMeshes_.find(id) != createdMeshes_.end())
01504 {
01505 Mesh & mesh = createdMeshes_.at(id);
01506 totalPoints_+=mesh.indices->size();
01507 totalPolygons_ += mesh.polygons.size();
01508 mesh.pose = opengl_world_T_rtabmap_world.inverse()*iter->second;
01509 main_scene_.addMesh(id, mesh, iter->second);
01510 #ifdef DEBUG_RENDERING_PERFORMANCE
01511 LOGW("Adding mesh to scene: %fs", time.ticks());
01512 #endif
01513 mesh.texture = cv::Mat();
01514 }
01515 }
01516 }
01517 }
01518 }
01519
01520
01521 if(poses.size() > 2)
01522 {
01523 if(nodesFiltering_)
01524 {
01525 for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
01526 {
01527 if(iter->second.type() != rtabmap::Link::kNeighbor)
01528 {
01529 int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
01530 poses.erase(oldId);
01531 }
01532 }
01533 }
01534 }
01535
01536 if(poses.size())
01537 {
01538
01539 boost::mutex::scoped_lock lock(meshesMutex_);
01540 std::set<int> addedClouds = main_scene_.getAddedClouds();
01541 for(std::set<int>::const_iterator iter=addedClouds.begin();
01542 iter!=addedClouds.end();
01543 ++iter)
01544 {
01545 if(*iter > 0 && poses.find(*iter) == poses.end())
01546 {
01547 main_scene_.setCloudVisible(*iter, false);
01548 std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
01549 UASSERT(meshIter!=createdMeshes_.end());
01550 meshIter->second.visible = false;
01551 }
01552 }
01553 }
01554 }
01555 else
01556 {
01557 main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && !paused_);
01558
01559
01560 if(!odomEvent.pose().isNull())
01561 {
01562 if(odomCloudShown_ && !trajectoryMode_)
01563 {
01564 if(!odomEvent.data().imageRaw().empty() && !odomEvent.data().depthRaw().empty())
01565 {
01566 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
01567 pcl::IndicesPtr indices(new std::vector<int>);
01568 cloud = rtabmap::util3d::cloudRGBFromSensorData(odomEvent.data(), meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
01569 if(cloud->size() && indices->size())
01570 {
01571 LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
01572 odomEvent.data().imageRaw().cols, odomEvent.data().imageRaw().rows,
01573 odomEvent.data().depthRaw().cols, odomEvent.data().depthRaw().rows,
01574 (int)cloud->width, (int)cloud->height);
01575 main_scene_.addCloud(-1, cloud, indices, opengl_world_T_rtabmap_world*mapToOdom_*odomEvent.pose());
01576 main_scene_.setCloudVisible(-1, true);
01577 }
01578 else
01579 {
01580 UERROR("Generated cloud is empty!");
01581 }
01582 }
01583 else
01584 {
01585 UERROR("Odom data images are empty!");
01586 }
01587 }
01588 }
01589 }
01590
01591 if(gainCompensationOnNextRender_>0)
01592 {
01593 gainCompensation(gainCompensationOnNextRender_==2);
01594 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
01595 {
01596 main_scene_.updateGains(iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]);
01597 }
01598 gainCompensationOnNextRender_ = 0;
01599 notifyDataLoaded = true;
01600 }
01601
01602 if(bilateralFilteringOnNextRender_)
01603 {
01604 LOGI("Bilateral filtering...");
01605 bilateralFilteringOnNextRender_ = false;
01606 boost::mutex::scoped_lock lock(meshesMutex_);
01607 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
01608 {
01609 if(iter->second.cloud->size() && iter->second.indices->size())
01610 {
01611 if(smoothMesh(iter->first, iter->second))
01612 {
01613 main_scene_.updateMesh(iter->first, iter->second);
01614 }
01615 }
01616 }
01617 notifyDataLoaded = true;
01618 }
01619
01620 if(filterPolygonsOnNextRender_ && clusterRatio_>0.0f)
01621 {
01622 LOGI("Polygon filtering...");
01623 filterPolygonsOnNextRender_ = false;
01624 boost::mutex::scoped_lock lock(meshesMutex_);
01625 UTimer time;
01626 for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
01627 {
01628 if(iter->second.polygons.size())
01629 {
01630
01631 iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size());
01632 main_scene_.updateCloudPolygons(iter->first, iter->second.polygons);
01633 }
01634 }
01635 notifyDataLoaded = true;
01636 }
01637
01638 fpsTime.restart();
01639 lastDrawnCloudsCount_ = main_scene_.Render();
01640 if(renderingTime_ < fpsTime.elapsed())
01641 {
01642 renderingTime_ = fpsTime.elapsed();
01643 }
01644
01645 if(rtabmapEvents.size())
01646 {
01647
01648 LOGI("New data added to map, rendering time: %fs", renderingTime_);
01649 UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
01650 rtabmapEvents.pop_back();
01651
01652 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
01653 {
01654 delete *iter;
01655 }
01656 rtabmapEvents.clear();
01657
01658 lastPostRenderEventTime_ = UTimer::now();
01659
01660 if(lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
01661 {
01662 UERROR("TangoPoseEventNotReceived");
01663 UEventsManager::post(new rtabmap::CameraTangoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_)));
01664 }
01665 }
01666 }
01667
01668 if(takeScreenshotOnNextRender_)
01669 {
01670 takeScreenshotOnNextRender_ = false;
01671 int w = main_scene_.getViewPortWidth();
01672 int h = main_scene_.getViewPortHeight();
01673 cv::Mat image(h, w, CV_8UC4);
01674 glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
01675 cv::flip(image, image, 0);
01676 cv::cvtColor(image, image, CV_RGBA2BGRA);
01677 cv::Mat roi;
01678 if(w>h)
01679 {
01680 int offset = (w-h)/2;
01681 roi = image(cv::Range::all(), cv::Range(offset,offset+h));
01682 }
01683 else
01684 {
01685 int offset = (h-w)/2;
01686 roi = image(cv::Range(offset,offset+w), cv::Range::all());
01687 }
01688 rtabmapMutex_.lock();
01689 LOGI("Saving screenshot %dx%d...", roi.cols, roi.rows);
01690 rtabmap_->getMemory()->savePreviewImage(roi);
01691 rtabmapMutex_.unlock();
01692 screenshotReady_.release();
01693 }
01694
01695 if((openingDatabase_ && !visualizingMesh_) || exporting_ || postProcessing_)
01696 {
01697
01698 double renderTime = fpsTime.elapsed();
01699 if(0.2 - renderTime > 0.0)
01700 {
01701 uSleep((0.2 - renderTime)*1000);
01702 }
01703 }
01704
01705 if(paused_ && lastPostRenderEventTime_ > 0.0)
01706 {
01707 double interval = UTimer::now() - lastPostRenderEventTime_;
01708 double updateInterval = 1.0;
01709 if(!openingDatabase_ && rtabmapThread_)
01710 {
01711 boost::mutex::scoped_lock lock(rtabmapMutex_);
01712 if(rtabmapThread_ && rtabmapThread_->getDetectorRate()>0.0f)
01713 {
01714 updateInterval = 1.0f/rtabmapThread_->getDetectorRate();
01715 }
01716 }
01717
01718 if(interval >= updateInterval)
01719 {
01720 if(!openingDatabase_)
01721 {
01722
01723 UEventsManager::post(new PostRenderEvent());
01724 }
01725 lastPostRenderEventTime_ = UTimer::now();
01726 }
01727 }
01728
01729 return notifyDataLoaded||notifyCameraStarted?1:0;
01730 }
01731 catch(const UException & e)
01732 {
01733 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
01734 {
01735 delete *iter;
01736 }
01737 rtabmapEvents.clear();
01738 UERROR("Exception! msg=\"%s\"", e.what());
01739 return -2;
01740 }
01741 catch(const cv::Exception & e)
01742 {
01743 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
01744 {
01745 delete *iter;
01746 }
01747 rtabmapEvents.clear();
01748 UERROR("Exception! msg=\"%s\"", e.what());
01749 return -1;
01750 }
01751 catch(const std::exception & e)
01752 {
01753 for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
01754 {
01755 delete *iter;
01756 }
01757 rtabmapEvents.clear();
01758 UERROR("Exception! msg=\"%s\"", e.what());
01759 return -2;
01760 }
01761 }
01762
01763 void RTABMapApp::SetCameraType(
01764 tango_gl::GestureCamera::CameraType camera_type) {
01765 main_scene_.SetCameraType(camera_type);
01766 }
01767
01768 void RTABMapApp::OnTouchEvent(int touch_count,
01769 tango_gl::GestureCamera::TouchEvent event,
01770 float x0, float y0, float x1, float y1) {
01771 main_scene_.OnTouchEvent(touch_count, event, x0, y0, x1, y1);
01772 }
01773
01774 void RTABMapApp::setPausedMapping(bool paused)
01775 {
01776 {
01777 boost::mutex::scoped_lock lock(renderingMutex_);
01778 if(!localizationMode_)
01779 {
01780 visualizingMesh_ = false;
01781 }
01782 main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
01783 }
01784 paused_ = paused;
01785 if(camera_)
01786 {
01787 if(paused_)
01788 {
01789 LOGW("Pause!");
01790 camera_->kill();
01791 }
01792 else
01793 {
01794 LOGW("Resume!");
01795 UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdTriggerNewMap));
01796 camera_->start();
01797 }
01798 }
01799 }
01800 void RTABMapApp::setOnlineBlending(bool enabled)
01801 {
01802 main_scene_.setBlending(enabled);
01803 }
01804 void RTABMapApp::setMapCloudShown(bool shown)
01805 {
01806 main_scene_.setMapRendering(shown);
01807 }
01808 void RTABMapApp::setOdomCloudShown(bool shown)
01809 {
01810 odomCloudShown_ = shown;
01811 main_scene_.setTraceVisible(shown);
01812 }
01813 void RTABMapApp::setMeshRendering(bool enabled, bool withTexture)
01814 {
01815 main_scene_.setMeshRendering(enabled, withTexture);
01816 }
01817 void RTABMapApp::setPointSize(float value)
01818 {
01819 main_scene_.setPointSize(value);
01820 }
01821 void RTABMapApp::setFOV(float angle)
01822 {
01823 main_scene_.setFOV(angle);
01824 }
01825 void RTABMapApp::setOrthoCropFactor(float value)
01826 {
01827 main_scene_.setOrthoCropFactor(value);
01828 }
01829 void RTABMapApp::setGridRotation(float value)
01830 {
01831 main_scene_.setGridRotation(value);
01832 }
01833 void RTABMapApp::setLighting(bool enabled)
01834 {
01835 main_scene_.setLighting(enabled);
01836 }
01837 void RTABMapApp::setBackfaceCulling(bool enabled)
01838 {
01839 main_scene_.setBackfaceCulling(enabled);
01840 }
01841 void RTABMapApp::setWireframe(bool enabled)
01842 {
01843 main_scene_.setWireframe(enabled);
01844 }
01845
01846 void RTABMapApp::setLocalizationMode(bool enabled)
01847 {
01848 localizationMode_ = enabled;
01849 this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
01850 }
01851 void RTABMapApp::setTrajectoryMode(bool enabled)
01852 {
01853 trajectoryMode_ = enabled;
01854 this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
01855 }
01856
01857 void RTABMapApp::setGraphOptimization(bool enabled)
01858 {
01859 graphOptimization_ = enabled;
01860 UASSERT(camera_ != 0 && rtabmap_!=0 && rtabmap_->getMemory()!=0);
01861 if(!camera_->isRunning() && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
01862 {
01863 std::map<int, rtabmap::Transform> poses;
01864 std::multimap<int, rtabmap::Link> links;
01865 rtabmap_->getGraph(poses, links, true, true);
01866 if(poses.size())
01867 {
01868 boost::mutex::scoped_lock lock(rtabmapMutex_);
01869 rtabmap::Statistics stats = rtabmap_->getStatistics();
01870 stats.setPoses(poses);
01871 stats.setConstraints(links);
01872
01873 LOGI("Send rtabmap event to update graph...");
01874 rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
01875
01876 rtabmap_->setOptimizedPoses(poses);
01877 }
01878 }
01879 }
01880 void RTABMapApp::setNodesFiltering(bool enabled)
01881 {
01882 nodesFiltering_ = enabled;
01883 setGraphOptimization(graphOptimization_);
01884 }
01885 void RTABMapApp::setGraphVisible(bool visible)
01886 {
01887 main_scene_.setGraphVisible(visible);
01888 main_scene_.setTraceVisible(visible);
01889 }
01890 void RTABMapApp::setGridVisible(bool visible)
01891 {
01892 main_scene_.setGridVisible(visible);
01893 }
01894
01895 void RTABMapApp::setRawScanSaved(bool enabled)
01896 {
01897 if(rawScanSaved_ != enabled)
01898 {
01899 rawScanSaved_ = enabled;
01900 if(camera_)
01901 {
01902 camera_->setRawScanPublished(rawScanSaved_);
01903 }
01904 }
01905 }
01906
01907 void RTABMapApp::setCameraColor(bool enabled)
01908 {
01909 if(cameraColor_ != enabled)
01910 {
01911 cameraColor_ = enabled;
01912 }
01913 }
01914
01915 void RTABMapApp::setFullResolution(bool enabled)
01916 {
01917 if(fullResolution_ != enabled)
01918 {
01919 fullResolution_ = enabled;
01920 if(camera_)
01921 {
01922 camera_->setDecimation(fullResolution_?1:2);
01923 }
01924 rtabmap::ParametersMap parameters;
01925 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
01926 this->post(new rtabmap::ParamEvent(parameters));
01927 }
01928 }
01929
01930 void RTABMapApp::setSmoothing(bool enabled)
01931 {
01932 if(smoothing_ != enabled)
01933 {
01934 smoothing_ = enabled;
01935 if(camera_)
01936 {
01937 camera_->setSmoothing(smoothing_);
01938 }
01939 }
01940 }
01941
01942 void RTABMapApp::setAppendMode(bool enabled)
01943 {
01944 if(appendMode_ != enabled)
01945 {
01946 appendMode_ = enabled;
01947 rtabmap::ParametersMap parameters;
01948 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
01949 this->post(new rtabmap::ParamEvent(parameters));
01950 }
01951 }
01952
01953 void RTABMapApp::setDataRecorderMode(bool enabled)
01954 {
01955 if(dataRecorderMode_ != enabled)
01956 {
01957 dataRecorderMode_ = enabled;
01958 }
01959 }
01960
01961 void RTABMapApp::setMaxCloudDepth(float value)
01962 {
01963 maxCloudDepth_ = value;
01964 }
01965
01966 void RTABMapApp::setMinCloudDepth(float value)
01967 {
01968 minCloudDepth_ = value;
01969 }
01970
01971 void RTABMapApp::setCloudDensityLevel(int value)
01972 {
01973 cloudDensityLevel_ = value;
01974 }
01975
01976 void RTABMapApp::setMeshAngleTolerance(float value)
01977 {
01978 meshAngleToleranceDeg_ = value;
01979 }
01980
01981 void RTABMapApp::setMeshTriangleSize(int value)
01982 {
01983 meshTrianglePix_ = value;
01984 }
01985
01986 void RTABMapApp::setClusterRatio(float value)
01987 {
01988 clusterRatio_ = value;
01989 }
01990
01991 void RTABMapApp::setMaxGainRadius(float value)
01992 {
01993 maxGainRadius_ = value;
01994 }
01995
01996 void RTABMapApp::setRenderingTextureDecimation(int value)
01997 {
01998 UASSERT(value>=1);
01999 renderingTextureDecimation_ = value;
02000 }
02001
02002 void RTABMapApp::setBackgroundColor(float gray)
02003 {
02004 backgroundColor_ = gray;
02005 float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
02006 main_scene_.setGridColor(v, v, v);
02007 }
02008
02009 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
02010 {
02011 std::string compatibleKey = key;
02012
02013
02014 std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().find(key);
02015 if(iter != rtabmap::Parameters::getRemovedParameters().end())
02016 {
02017 if(iter->second.first)
02018 {
02019
02020 compatibleKey = iter->second.second;
02021 LOGW("Parameter name changed: \"%s\" -> \"%s\". Please update the code accordingly. Value \"%s\" is still set to the new parameter name.",
02022 iter->first.c_str(), iter->second.second.c_str(), value.c_str());
02023 }
02024 else
02025 {
02026 if(iter->second.second.empty())
02027 {
02028 UERROR("Parameter \"%s\" doesn't exist anymore!",
02029 iter->first.c_str());
02030 }
02031 else
02032 {
02033 UERROR("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
02034 iter->first.c_str(), iter->second.second.c_str());
02035 }
02036 }
02037 }
02038
02039 if(rtabmap::Parameters::getDefaultParameters().find(compatibleKey) != rtabmap::Parameters::getDefaultParameters().end())
02040 {
02041 LOGI(uFormat("Setting param \"%s\" to \"%s\"", compatibleKey.c_str(), value.c_str()).c_str());
02042 if(compatibleKey.compare(rtabmap::Parameters::kKpDetectorStrategy()) == 0 &&
02043 mappingParameters_.at(rtabmap::Parameters::kKpDetectorStrategy()).compare(value) != 0)
02044 {
02045
02046 resetMapping();
02047 }
02048 uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value));
02049 UEventsManager::post(new rtabmap::ParamEvent(this->getRtabmapParameters()));
02050 return 0;
02051 }
02052 else
02053 {
02054 UERROR(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str());
02055 return -1;
02056 }
02057 }
02058
02059 void RTABMapApp::setGPS(const rtabmap::GPS & gps)
02060 {
02061 if(camera_)
02062 {
02063 camera_->setGPS(gps);
02064 }
02065 }
02066
02067 void RTABMapApp::resetMapping()
02068 {
02069 LOGW("Reset!");
02070 status_.first = rtabmap::RtabmapEventInit::kInitializing;
02071 status_.second = "";
02072
02073 mapToOdom_.setIdentity();
02074 clearSceneOnNextRender_ = true;
02075
02076 if(camera_)
02077 {
02078 camera_->resetOrigin();
02079 }
02080
02081 UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
02082 }
02083
02084 void RTABMapApp::save(const std::string & databasePath)
02085 {
02086 LOGI("Saving database to %s", databasePath.c_str());
02087 rtabmapThread_->join(true);
02088
02089 LOGI("Taking screenshot...");
02090 takeScreenshotOnNextRender_ = true;
02091 if(!screenshotReady_.acquire(1, 2000))
02092 {
02093 UERROR("Failed to take a screenshot after 2 sec!");
02094 }
02095
02096
02097 bool appendModeBackup = appendMode_;
02098 if(appendMode_)
02099 {
02100 appendMode_ = false;
02101 }
02102
02103 bool dataRecorderModeBackup = dataRecorderMode_;
02104 if(dataRecorderMode_)
02105 {
02106 dataRecorderMode_ = false;
02107 }
02108
02109 bool localizationModeBackup = localizationMode_;
02110 if(localizationMode_)
02111 {
02112 localizationMode_ = false;
02113 }
02114
02115 if(appendModeBackup || dataRecorderModeBackup || localizationModeBackup)
02116 {
02117 rtabmap::ParametersMap parameters = getRtabmapParameters();
02118 rtabmap_->parseParameters(parameters);
02119 appendMode_ = appendModeBackup;
02120 dataRecorderMode_ = dataRecorderModeBackup;
02121 localizationMode_ = localizationModeBackup;
02122 }
02123
02124 std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
02125 rtabmap_->close(true, databasePath);
02126 rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath);
02127 rtabmap_->setOptimizedPoses(poses);
02128 if(dataRecorderMode_)
02129 {
02130 clearSceneOnNextRender_ = true;
02131 }
02132 rtabmapThread_->start();
02133
02134 }
02135
02136 void RTABMapApp::cancelProcessing()
02137 {
02138 UWARN("Processing canceled!");
02139 progressionStatus_.setCanceled(true);
02140 }
02141
02142 bool RTABMapApp::exportMesh(
02143 float cloudVoxelSize,
02144 bool regenerateCloud,
02145 bool meshing,
02146 int textureSize,
02147 int textureCount,
02148 int normalK,
02149 bool optimized,
02150 float optimizedVoxelSize,
02151 int optimizedDepth,
02152 int optimizedMaxPolygons,
02153 float optimizedColorRadius,
02154 bool optimizedCleanWhitePolygons,
02155 int optimizedMinClusterSize,
02156 float optimizedMaxTextureDistance,
02157 int optimizedMinTextureClusterSize,
02158 bool blockRendering)
02159 {
02160
02161
02162
02163 std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
02164 if(poses.empty())
02165 {
02166
02167 std::multimap<int, rtabmap::Link> links;
02168 rtabmap_->getGraph(
02169 poses,
02170 links,
02171 true,
02172 false);
02173 if(poses.empty())
02174 {
02175 UERROR("Empty optimized poses!");
02176 return false;
02177 }
02178 rtabmap_->setOptimizedPoses(poses);
02179 }
02180
02181 if(blockRendering)
02182 {
02183 renderingMutex_.lock();
02184 main_scene_.clear();
02185 }
02186
02187 exporting_ = true;
02188
02189 bool success = false;
02190
02191 try
02192 {
02193 int totalSteps = 0;
02194 totalSteps+=poses.size();
02195 if(meshing)
02196 {
02197 if(optimized)
02198 {
02199 totalSteps += poses.size();
02200 if(textureSize > 0)
02201 {
02202 totalSteps += 1;
02203 totalSteps += 1;
02204
02205 if(optimizedMaxPolygons > 0)
02206 {
02207 totalSteps += 1;
02208 }
02209 }
02210
02211 totalSteps += 1;
02212
02213 if(textureSize > 0)
02214 {
02215 totalSteps+=poses.size()+1;
02216 }
02217 }
02218 if(textureSize>0)
02219 {
02220 totalSteps += poses.size()+1;
02221 }
02222 }
02223 totalSteps += 1;
02224
02225 progressionStatus_.reset(totalSteps);
02226
02227
02228 if(meshing)
02229 {
02230 pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
02231 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
02232 std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
02233 cv::Mat globalTextures;
02234 int totalPolygons = 0;
02235 {
02236 if(optimized)
02237 {
02238 std::map<int, rtabmap::Transform> cameraPoses;
02239 std::map<int, rtabmap::CameraModel> cameraModels;
02240 std::map<int, cv::Mat> cameraDepths;
02241
02242 UTimer timer;
02243 LOGI("Assemble clouds (%d)...", (int)poses.size());
02244 #ifndef DISABLE_LOG
02245 int cloudCount=0;
02246 #endif
02247 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02248 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
02249 iter!= poses.end();
02250 ++iter)
02251 {
02252 std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
02253 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02254 pcl::IndicesPtr indices(new std::vector<int>);
02255 rtabmap::CameraModel model;
02256 cv::Mat depth;
02257 float gains[3];
02258 gains[0] = gains[1] = gains[2] = 1.0f;
02259 if(jter != createdMeshes_.end())
02260 {
02261 cloud = jter->second.cloud;
02262 indices = jter->second.indices;
02263 model = jter->second.cameraModel;
02264 gains[0] = jter->second.gains[0];
02265 gains[1] = jter->second.gains[1];
02266 gains[2] = jter->second.gains[2];
02267
02268 rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, false);
02269 data.uncompressData(0, &depth);
02270 }
02271 else
02272 {
02273 rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
02274 if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
02275 {
02276 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
02277 model = data.cameraModels()[0];
02278 depth = data.depthRaw();
02279 }
02280 }
02281 if(cloud->size() && indices->size() && model.isValidForProjection())
02282 {
02283 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02284 if(optimizedVoxelSize > 0.0f)
02285 {
02286 transformedCloud = rtabmap::util3d::voxelize(cloud, indices, optimizedVoxelSize);
02287 transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
02288 }
02289 else
02290 {
02291
02292
02293 pcl::copyPointCloud(*cloud, *indices, *transformedCloud);
02294 transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
02295 }
02296
02297 Eigen::Vector3f viewpoint( iter->second.x(), iter->second.y(), iter->second.z());
02298 pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, 0.0f, viewpoint);
02299
02300 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02301 pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
02302
02303 if(textureSize == 0 && (gains[0] != 1.0 || gains[1] != 1.0 || gains[2] != 1.0))
02304 {
02305 for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
02306 {
02307 pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
02308 pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
02309 pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
02310 pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
02311 }
02312 }
02313
02314
02315 if(mergedClouds->size() == 0)
02316 {
02317 *mergedClouds = *cloudWithNormals;
02318 }
02319 else
02320 {
02321 *mergedClouds += *cloudWithNormals;
02322 }
02323
02324 cameraPoses.insert(std::make_pair(iter->first, iter->second));
02325 cameraModels.insert(std::make_pair(iter->first, model));
02326 if(!depth.empty())
02327 {
02328 cameraDepths.insert(std::make_pair(iter->first, depth));
02329 }
02330
02331 LOGI("Assembled %d points (%d/%d total=%d)", (int)cloudWithNormals->size(), ++cloudCount, (int)poses.size(), (int)mergedClouds->size());
02332 }
02333 else
02334 {
02335 UERROR("Cloud %d not found or empty", iter->first);
02336 }
02337
02338 if(progressionStatus_.isCanceled())
02339 {
02340 if(blockRendering)
02341 {
02342 renderingMutex_.unlock();
02343 }
02344 exporting_ = false;
02345 return false;
02346 }
02347 progressionStatus_.increment();
02348 }
02349 LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size());
02350
02351 if(mergedClouds->size()>=3)
02352 {
02353 if(optimizedDepth == 0)
02354 {
02355 Eigen::Vector4f min,max;
02356 pcl::getMinMax3D(*mergedClouds, min, max);
02357 float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
02358 optimizedDepth = 12;
02359 for(int i=6; i<12; ++i)
02360 {
02361 if(mapLength/float(1<<i) < 0.03f)
02362 {
02363 optimizedDepth = i;
02364 break;
02365 }
02366 }
02367 LOGI("optimizedDepth=%d (map length=%f)", optimizedDepth, mapLength);
02368 }
02369
02370
02371 LOGI("Mesh reconstruction...");
02372 pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
02373 pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
02374 poisson.setDepth(optimizedDepth);
02375 poisson.setInputCloud(mergedClouds);
02376 poisson.reconstruct(*mesh);
02377 LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), (int)mesh->polygons.size());
02378
02379 if(progressionStatus_.isCanceled())
02380 {
02381 if(blockRendering)
02382 {
02383 renderingMutex_.unlock();
02384 }
02385 exporting_ = false;
02386 return false;
02387 }
02388
02389 progressionStatus_.increment(poses.size());
02390
02391 if(mesh->polygons.size())
02392 {
02393 totalPolygons=(int)mesh->polygons.size();
02394
02395 if(optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
02396 {
02397 #ifndef DISABLE_VTK
02398 unsigned int count = mesh->polygons.size();
02399 float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
02400 LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
02401
02402 progressionStatus_.setMax(progressionStatus_.getMax() + optimizedMaxPolygons/10000);
02403
02404 pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
02405 pcl::MeshQuadricDecimationVTK mqd;
02406 mqd.setTargetReductionFactor(factor);
02407 mqd.setInputMesh(mesh);
02408 mqd.process (*output);
02409 mesh = output;
02410
02411
02412
02413
02414
02415
02416 LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", factor, count, (int)mesh->polygons.size(), timer.ticks());
02417 if(count < mesh->polygons.size())
02418 {
02419 UWARN("Decimated mesh has more polygons than before!");
02420 }
02421 #else
02422 UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!");
02423 #endif
02424 }
02425
02426 if(progressionStatus_.isCanceled())
02427 {
02428 if(blockRendering)
02429 {
02430 renderingMutex_.unlock();
02431 }
02432 exporting_ = false;
02433 return false;
02434 }
02435
02436 progressionStatus_.increment();
02437
02438 rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
02439 mesh,
02440 0.0f,
02441 0,
02442 mergedClouds,
02443 optimizedColorRadius,
02444 textureSize == 0,
02445 optimizedCleanWhitePolygons,
02446 optimizedMinClusterSize);
02447
02448 if(textureSize>0)
02449 {
02450 LOGI("Texturing... cameraPoses=%d, cameraDepths=%d", (int)cameraPoses.size(), (int)cameraDepths.size());
02451 textureMesh = rtabmap::util3d::createTextureMesh(
02452 mesh,
02453 cameraPoses,
02454 cameraModels,
02455 cameraDepths,
02456 optimizedMaxTextureDistance,
02457 0.0f,
02458 0.0f,
02459 optimizedMinTextureClusterSize,
02460 std::vector<float>(),
02461 &progressionStatus_,
02462 &vertexToPixels);
02463 LOGI("Texturing... done! %fs", timer.ticks());
02464
02465 if(progressionStatus_.isCanceled())
02466 {
02467 if(blockRendering)
02468 {
02469 renderingMutex_.unlock();
02470 }
02471 exporting_ = false;
02472 return false;
02473 }
02474
02475
02476 if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
02477 {
02478 LOGI("Cleanup mesh...");
02479 rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
02480 LOGI("Cleanup mesh... done! %fs", timer.ticks());
02481 }
02482
02483 totalPolygons = 0;
02484 for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
02485 {
02486 totalPolygons+=textureMesh->tex_polygons[t].size();
02487 }
02488 }
02489 else
02490 {
02491 totalPolygons = (int)mesh->polygons.size();
02492 polygonMesh = mesh;
02493 }
02494 }
02495 }
02496 else
02497 {
02498 UERROR("Merged cloud too small (%d points) to create polygons!", (int)mergedClouds->size());
02499 }
02500 }
02501 else
02502 {
02503 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02504
02505 if(textureSize > 0)
02506 {
02507 textureMesh->tex_materials.resize(poses.size());
02508 textureMesh->tex_polygons.resize(poses.size());
02509 textureMesh->tex_coordinates.resize(poses.size());
02510 }
02511
02512 int polygonsStep = 0;
02513 int oi = 0;
02514 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
02515 iter!= poses.end();
02516 ++iter)
02517 {
02518 LOGI("Assembling cloud %d (total=%d)...", iter->first, (int)poses.size());
02519
02520 std::map<int, Mesh>::iterator jter = createdMeshes_.find(iter->first);
02521 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02522 std::vector<pcl::Vertices> polygons;
02523 float gains[3] = {1.0f};
02524 if(jter != createdMeshes_.end())
02525 {
02526 cloud = jter->second.cloud;
02527 polygons= jter->second.polygons;
02528 if(cloud->size() && polygons.size() == 0)
02529 {
02530 polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
02531 }
02532 gains[0] = jter->second.gains[0];
02533 gains[1] = jter->second.gains[1];
02534 gains[2] = jter->second.gains[2];
02535 }
02536 else
02537 {
02538 rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
02539 if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
02540 {
02541 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_);
02542 polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
02543 }
02544 }
02545
02546 if(cloud->size() && polygons.size())
02547 {
02548
02549 pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02550 std::vector<pcl::Vertices> outputPolygons;
02551 std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloud, polygons, *outputCloud, outputPolygons);
02552
02553 pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, normalK);
02554
02555 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02556 pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals);
02557
02558 UASSERT(outputPolygons.size());
02559
02560 totalPolygons+=outputPolygons.size();
02561
02562 if(textureSize == 0)
02563 {
02564
02565 cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second);
02566
02567 if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f)
02568 {
02569 for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
02570 {
02571 pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
02572 pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
02573 pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
02574 pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
02575 }
02576 }
02577
02578 if(mergedClouds->size() == 0)
02579 {
02580 *mergedClouds = *cloudWithNormals;
02581 polygonMesh->polygons = outputPolygons;
02582 }
02583 else
02584 {
02585 rtabmap::util3d::appendMesh(*mergedClouds, polygonMesh->polygons, *cloudWithNormals, outputPolygons);
02586 }
02587 }
02588 else
02589 {
02590
02591 unsigned int polygonSize = outputPolygons.front().vertices.size();
02592 textureMesh->tex_polygons[oi].resize(outputPolygons.size());
02593 textureMesh->tex_coordinates[oi].resize(outputPolygons.size() * polygonSize);
02594 for(unsigned int j=0; j<outputPolygons.size(); ++j)
02595 {
02596 pcl::Vertices vertices = outputPolygons[j];
02597 UASSERT(polygonSize == vertices.vertices.size());
02598 for(unsigned int k=0; k<vertices.vertices.size(); ++k)
02599 {
02600
02601 UASSERT(vertices.vertices[k] < denseToOrganizedIndices.size());
02602 int originalVertex = denseToOrganizedIndices[vertices.vertices[k]];
02603 textureMesh->tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
02604 float(originalVertex % cloud->width) / float(cloud->width),
02605 float(cloud->height - originalVertex / cloud->width) / float(cloud->height));
02606
02607 vertices.vertices[k] += polygonsStep;
02608 }
02609 textureMesh->tex_polygons[oi][j] = vertices;
02610
02611 }
02612 polygonsStep += outputCloud->size();
02613
02614 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second);
02615 if(mergedClouds->size() == 0)
02616 {
02617 *mergedClouds = *transformedCloud;
02618 }
02619 else
02620 {
02621 *mergedClouds += *transformedCloud;
02622 }
02623
02624 textureMesh->tex_materials[oi].tex_illum = 1;
02625 textureMesh->tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
02626 textureMesh->tex_materials[oi].tex_file = uNumber2Str(iter->first);
02627 ++oi;
02628 }
02629 }
02630 else
02631 {
02632 UERROR("Mesh not found for mesh %d", iter->first);
02633 }
02634
02635 if(progressionStatus_.isCanceled())
02636 {
02637 if(blockRendering)
02638 {
02639 renderingMutex_.unlock();
02640 }
02641 exporting_ = false;
02642 return false;
02643 }
02644 progressionStatus_.increment();
02645 }
02646 if(textureSize == 0)
02647 {
02648 if(mergedClouds->size())
02649 {
02650 pcl::toPCLPointCloud2(*mergedClouds, polygonMesh->cloud);
02651 }
02652 else
02653 {
02654 polygonMesh->polygons.clear();
02655 }
02656 }
02657 else
02658 {
02659 textureMesh->tex_materials.resize(oi);
02660 textureMesh->tex_polygons.resize(oi);
02661
02662 if(mergedClouds->size())
02663 {
02664 pcl::toPCLPointCloud2(*mergedClouds, textureMesh->cloud);
02665 }
02666 }
02667 }
02668
02669
02670
02671 if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size())
02672 {
02673 LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size());
02674 globalTextures = rtabmap::util3d::mergeTextures(
02675 *textureMesh,
02676 std::map<int, cv::Mat>(),
02677 std::map<int, std::vector<rtabmap::CameraModel> >(),
02678 rtabmap_->getMemory(),
02679 0,
02680 textureSize,
02681 textureCount,
02682 vertexToPixels,
02683 true, 10.0f, true ,true, 0, 0, 0, false,
02684 &progressionStatus_);
02685 }
02686 if(progressionStatus_.isCanceled())
02687 {
02688 if(blockRendering)
02689 {
02690 renderingMutex_.unlock();
02691 }
02692 exporting_ = false;
02693 return false;
02694 }
02695
02696 progressionStatus_.increment();
02697 }
02698 if(totalPolygons)
02699 {
02700 if(textureSize == 0)
02701 {
02702 UASSERT((int)polygonMesh->polygons.size() == totalPolygons);
02703 if(polygonMesh->polygons.size())
02704 {
02705
02706 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
02707 pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
02708 cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false));
02709 std::vector<std::vector<std::vector<unsigned int> > > polygons(1);
02710 polygons[0].resize(polygonMesh->polygons.size());
02711 for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
02712 {
02713 polygons[0][p] = polygonMesh->polygons[p].vertices;
02714 }
02715 boost::mutex::scoped_lock lock(rtabmapMutex_);
02716
02717 rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
02718 success = true;
02719 }
02720 }
02721 else if(textureMesh->tex_materials.size())
02722 {
02723 pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
02724 pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
02725 cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false));
02726
02727
02728 std::vector<std::vector<std::vector<unsigned int> > > polygons(textureMesh->tex_polygons.size());
02729 for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
02730 {
02731 polygons[t].resize(textureMesh->tex_polygons[t].size());
02732 for(unsigned int p=0; p<textureMesh->tex_polygons[t].size(); ++p)
02733 {
02734 polygons[t][p] = textureMesh->tex_polygons[t][p].vertices;
02735 }
02736 }
02737 boost::mutex::scoped_lock lock(rtabmapMutex_);
02738 rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons, textureMesh->tex_coordinates, globalTextures);
02739 success = true;
02740 }
02741 else
02742 {
02743 UERROR("Failed exporting texture mesh! There are no textures!");
02744 }
02745 }
02746 else
02747 {
02748 UERROR("Failed exporting mesh! There are no polygons!");
02749 }
02750 }
02751 else
02752 {
02753 pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
02754 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
02755 iter!= poses.end();
02756 ++iter)
02757 {
02758 std::map<int, Mesh>::iterator jter=createdMeshes_.find(iter->first);
02759 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02760 pcl::IndicesPtr indices(new std::vector<int>);
02761 float gains[3];
02762 gains[0] = gains[1] = gains[2] = 1.0f;
02763 if(regenerateCloud)
02764 {
02765 if(jter != createdMeshes_.end())
02766 {
02767 gains[0] = jter->second.gains[0];
02768 gains[1] = jter->second.gains[1];
02769 gains[2] = jter->second.gains[2];
02770 }
02771 rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
02772 if(!data.imageRaw().empty() && !data.depthRaw().empty())
02773 {
02774
02775 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
02776 }
02777 }
02778 else
02779 {
02780 if(jter != createdMeshes_.end())
02781 {
02782 cloud = jter->second.cloud;
02783 indices = jter->second.indices;
02784 gains[0] = jter->second.gains[0];
02785 gains[1] = jter->second.gains[1];
02786 gains[2] = jter->second.gains[2];
02787 }
02788 else
02789 {
02790 rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true);
02791 if(!data.imageRaw().empty() && !data.depthRaw().empty())
02792 {
02793 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation_, maxCloudDepth_, minCloudDepth_, indices.get());
02794 }
02795 }
02796 }
02797 if(cloud->size() && indices->size())
02798 {
02799
02800 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
02801 if(cloudVoxelSize > 0.0f)
02802 {
02803 transformedCloud = rtabmap::util3d::voxelize(cloud, indices, cloudVoxelSize);
02804 transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
02805 }
02806 else
02807 {
02808
02809
02810 pcl::copyPointCloud(*cloud, *indices, *transformedCloud);
02811 transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
02812 }
02813
02814 if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f)
02815 {
02816
02817 for(unsigned int i=0; i<transformedCloud->size(); ++i)
02818 {
02819 pcl::PointXYZRGB & pt = transformedCloud->at(i);
02820
02821 pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
02822 pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
02823 pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
02824
02825 }
02826 }
02827
02828 if(mergedClouds->size() == 0)
02829 {
02830 *mergedClouds = *transformedCloud;
02831 }
02832 else
02833 {
02834 *mergedClouds += *transformedCloud;
02835 }
02836 }
02837
02838 if(progressionStatus_.isCanceled())
02839 {
02840 if(blockRendering)
02841 {
02842 renderingMutex_.unlock();
02843 }
02844 exporting_ = false;
02845 return false;
02846 }
02847 progressionStatus_.increment();
02848 }
02849
02850 if(mergedClouds->size())
02851 {
02852 if(cloudVoxelSize > 0.0f)
02853 {
02854 mergedClouds = rtabmap::util3d::voxelize(mergedClouds, cloudVoxelSize);
02855 }
02856
02857
02858 {
02859 cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds));
02860 boost::mutex::scoped_lock lock(rtabmapMutex_);
02861 rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
02862 success = true;
02863 }
02864 }
02865 else
02866 {
02867 UERROR("Merged cloud is empty!");
02868 }
02869 }
02870
02871 progressionStatus_.finish();
02872
02873 if(blockRendering)
02874 {
02875 renderingMutex_.unlock();
02876 }
02877 }
02878 catch (std::exception & e)
02879 {
02880 UERROR("Out of memory! %s", e.what());
02881
02882 if(blockRendering)
02883 {
02884 renderingMutex_.unlock();
02885 }
02886
02887 success = false;
02888 }
02889 exporting_ = false;
02890
02891 optRefId_ = 0;
02892 if(optRefPose_)
02893 {
02894 delete optRefPose_;
02895 optRefPose_ = 0;
02896 }
02897 if(success && poses.size())
02898 {
02899
02900
02901 optRefId_ = poses.rbegin()->first;
02902 optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
02903 }
02904
02905 return success;
02906 }
02907
02908 bool RTABMapApp::postExportation(bool visualize)
02909 {
02910 LOGI("postExportation(visualize=%d)", visualize?1:0);
02911 optMesh_.reset(new pcl::TextureMesh);
02912 optTexture_ = cv::Mat();
02913 exportedMeshUpdated_ = false;
02914
02915 if(visualize)
02916 {
02917 visualizingMesh_ = false;
02918 cv::Mat cloudMat;
02919 std::vector<std::vector<std::vector<unsigned int> > > polygons;
02920 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
02921 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
02922 #else
02923 std::vector<std::vector<Eigen::Vector2f> > texCoords;
02924 #endif
02925 cv::Mat textures;
02926 if(rtabmap_ && rtabmap_->getMemory())
02927 {
02928 cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
02929 if(!cloudMat.empty())
02930 {
02931 LOGI("postExportation: Found optimized mesh! Visualizing it.");
02932 optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
02933 optTexture_ = textures;
02934
02935 boost::mutex::scoped_lock lock(renderingMutex_);
02936 visualizingMesh_ = true;
02937 exportedMeshUpdated_ = true;
02938 }
02939 else
02940 {
02941 LOGI("postExportation: No optimized mesh found.");
02942 }
02943 }
02944 }
02945 else if(visualizingMesh_)
02946 {
02947 rtabmapMutex_.lock();
02948 if(!rtabmap_->getLocalOptimizedPoses().empty())
02949 {
02950 rtabmap::Statistics stats;
02951 stats.setPoses(rtabmap_->getLocalOptimizedPoses());
02952 rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
02953 }
02954 rtabmapMutex_.unlock();
02955
02956 visualizingMesh_ = false;
02957 }
02958
02959 return visualizingMesh_;
02960 }
02961
02962 bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::string & name)
02963 {
02964 LOGI("writeExportedMesh: dir=%s name=%s", directory.c_str(), name.c_str());
02965 exporting_ = true;
02966
02967 bool success = false;
02968
02969 pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
02970 pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
02971 cv::Mat cloudMat;
02972 std::vector<std::vector<std::vector<unsigned int> > > polygons;
02973 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
02974 std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
02975 #else
02976 std::vector<std::vector<Eigen::Vector2f> > texCoords;
02977 #endif
02978 cv::Mat textures;
02979 if(rtabmap_ && rtabmap_->getMemory())
02980 {
02981 cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
02982 if(!cloudMat.empty())
02983 {
02984 LOGI("writeExportedMesh: Found optimized mesh!");
02985 if(textures.empty())
02986 {
02987 polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<unsigned int> >());
02988 }
02989 else
02990 {
02991 textureMesh = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, false);
02992 }
02993 }
02994 else
02995 {
02996 LOGI("writeExportedMesh: No optimized mesh found.");
02997 }
02998 }
02999
03000 if(polygonMesh->cloud.data.size())
03001 {
03002
03003 std::string filePath = directory + UDirectory::separator() + name + ".ply";
03004 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());
03005 success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0;
03006 if(success)
03007 {
03008 LOGI("Saved ply to %s!", filePath.c_str());
03009 }
03010 else
03011 {
03012 UERROR("Failed saving ply to %s!", filePath.c_str());
03013 }
03014 }
03015 else if(textureMesh->cloud.data.size())
03016 {
03017
03018 LOGD("Saving texture(s) (%d)", textures.empty()?0:textures.cols/textures.rows);
03019 UASSERT(textures.empty() || textures.cols % textures.rows == 0);
03020 UASSERT((int)textureMesh->tex_materials.size() == textures.cols/textures.rows);
03021 for(unsigned int i=0; i<textureMesh->tex_materials.size(); ++i)
03022 {
03023 std::string baseNameNum = name;
03024 if(textureMesh->tex_materials.size()>1)
03025 {
03026 baseNameNum+=uNumber2Str(i);
03027 }
03028 std::string fullPath = directory+UDirectory::separator()+baseNameNum+".jpg";
03029 textureMesh->tex_materials[i].tex_file = baseNameNum+".jpg";
03030 LOGI("Saving texture to %s.", fullPath.c_str());
03031 success = cv::imwrite(fullPath, textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
03032 if(!success)
03033 {
03034 LOGI("Failed saving %s!", fullPath.c_str());
03035 }
03036 else
03037 {
03038 LOGI("Saved %s.", fullPath.c_str());
03039 }
03040 }
03041
03042 if(success)
03043 {
03044
03045 pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
03046 pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
03047 cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform(1,0,0,0, 0,0,1,0, 0,-1,0,0));
03048 pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
03049 std::string filePath = directory + UDirectory::separator() + name + ".obj";
03050 int totalPolygons = 0;
03051 for(unsigned int i=0;i<textureMesh->tex_polygons.size(); ++i)
03052 {
03053 totalPolygons += textureMesh->tex_polygons[i].size();
03054 }
03055 LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
03056 success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
03057
03058 if(success)
03059 {
03060 LOGI("Saved obj to %s!", filePath.c_str());
03061 }
03062 else
03063 {
03064 UERROR("Failed saving obj to %s!", filePath.c_str());
03065 }
03066 }
03067 }
03068 exporting_ = false;
03069 return success;
03070 }
03071
03072 int RTABMapApp::postProcessing(int approach)
03073 {
03074 postProcessing_ = true;
03075 LOGI("postProcessing(%d)", approach);
03076 int returnedValue = 0;
03077 if(rtabmap_)
03078 {
03079 std::map<int, rtabmap::Transform> poses;
03080 std::multimap<int, rtabmap::Link> links;
03081
03082
03083 if(approach == -1 || approach == 2)
03084 {
03085 if(approach == -1)
03086 {
03087 progressionStatus_.reset(6);
03088 }
03089 returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, approach==-1?&progressionStatus_:0);
03090 if(approach == -1 && progressionStatus_.isCanceled())
03091 {
03092 postProcessing_ = false;
03093 return -1;
03094 }
03095 }
03096
03097
03098 if(returnedValue >=0)
03099 {
03100 if (approach == 1)
03101 {
03102 if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
03103 {
03104 std::map<int, rtabmap::Signature> signatures;
03105 rtabmap_->getGraph(poses, links, true, true, &signatures);
03106
03107 rtabmap::ParametersMap param;
03108 param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
03109 param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
03110 rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
03111 poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
03112 delete sba;
03113 }
03114 else
03115 {
03116 UERROR("g2o not available!");
03117 }
03118 }
03119 else if(approach!=4 && approach!=5 && approach != 7)
03120 {
03121
03122 rtabmap_->getGraph(poses, links, true, true);
03123 }
03124 }
03125
03126 if(poses.size())
03127 {
03128 boost::mutex::scoped_lock lock(rtabmapMutex_);
03129 rtabmap::Statistics stats = rtabmap_->getStatistics();
03130 stats.setPoses(poses);
03131 stats.setConstraints(links);
03132
03133 LOGI("PostProcessing, sending rtabmap event to update graph...");
03134 rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
03135
03136 rtabmap_->setOptimizedPoses(poses);
03137 }
03138 else if(approach!=4 && approach!=5 && approach != 7)
03139 {
03140 returnedValue = -1;
03141 }
03142
03143 if(returnedValue >=0)
03144 {
03145 boost::mutex::scoped_lock lock(renderingMutex_);
03146
03147 if(approach == -1 || approach == 4)
03148 {
03149 filterPolygonsOnNextRender_ = true;
03150 }
03151
03152
03153 if(approach == -1 || approach == 5 || approach == 6)
03154 {
03155 gainCompensationOnNextRender_ = approach == 6 ? 2 : 1;
03156 }
03157
03158
03159 if(approach == 7)
03160 {
03161 bilateralFilteringOnNextRender_ = true;
03162 }
03163 }
03164 }
03165
03166 postProcessing_ = false;
03167 return returnedValue;
03168 }
03169
03170 bool RTABMapApp::handleEvent(UEvent * event)
03171 {
03172 if(camera_ && camera_->isRunning())
03173 {
03174
03175 if(event->getClassName().compare("OdometryEvent") == 0)
03176 {
03177 LOGI("Received OdometryEvent!");
03178 if(odomMutex_.try_lock())
03179 {
03180 odomEvents_.clear();
03181 if(camera_->isRunning())
03182 {
03183 odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
03184 }
03185 odomMutex_.unlock();
03186 }
03187 }
03188 if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
03189 event->getClassName().compare("RtabmapEvent") == 0)
03190 {
03191 LOGI("Received RtabmapEvent event!");
03192 if(camera_->isRunning())
03193 {
03194 if(visualizingMesh_)
03195 {
03196 boost::mutex::scoped_lock lock(visLocalizationMutex_);
03197 visLocalizationEvents_.push_back((rtabmap::RtabmapEvent*)event);
03198 }
03199 else
03200 {
03201 boost::mutex::scoped_lock lock(rtabmapMutex_);
03202 rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
03203 }
03204 return true;
03205 }
03206 }
03207 }
03208
03209 if(event->getClassName().compare("PoseEvent") == 0)
03210 {
03211 if(poseMutex_.try_lock())
03212 {
03213 poseEvents_.clear();
03214 poseEvents_.push_back(((rtabmap::PoseEvent*)event)->pose());
03215 poseMutex_.unlock();
03216 }
03217 }
03218
03219 if(event->getClassName().compare("CameraTangoEvent") == 0)
03220 {
03221 rtabmap::CameraTangoEvent * tangoEvent = (rtabmap::CameraTangoEvent*)event;
03222
03223
03224 bool success = false;
03225 if(jvm && RTABMapActivity)
03226 {
03227 JNIEnv *env = 0;
03228 jint rs = jvm->AttachCurrentThread(&env, NULL);
03229 if(rs == JNI_OK && env)
03230 {
03231 jclass clazz = env->GetObjectClass(RTABMapActivity);
03232 if(clazz)
03233 {
03234 jmethodID methodID = env->GetMethodID(clazz, "tangoEventCallback", "(ILjava/lang/String;Ljava/lang/String;)V" );
03235 if(methodID)
03236 {
03237 env->CallVoidMethod(RTABMapActivity, methodID,
03238 tangoEvent->type(),
03239 env->NewStringUTF(tangoEvent->key().c_str()),
03240 env->NewStringUTF(tangoEvent->value().c_str()));
03241 success = true;
03242 }
03243 }
03244 }
03245 jvm->DetachCurrentThread();
03246 }
03247 if(!success)
03248 {
03249 UERROR("Failed to call RTABMapActivity::tangoEventCallback");
03250 }
03251 }
03252
03253 if(event->getClassName().compare("RtabmapEventInit") == 0)
03254 {
03255 LOGI("Received RtabmapEventInit!");
03256 status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
03257 status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
03258
03259
03260 bool success = false;
03261 if(jvm && RTABMapActivity)
03262 {
03263 JNIEnv *env = 0;
03264 jint rs = jvm->AttachCurrentThread(&env, NULL);
03265 if(rs == JNI_OK && env)
03266 {
03267 jclass clazz = env->GetObjectClass(RTABMapActivity);
03268 if(clazz)
03269 {
03270 jmethodID methodID = env->GetMethodID(clazz, "rtabmapInitEventCallback", "(ILjava/lang/String;)V" );
03271 if(methodID)
03272 {
03273 env->CallVoidMethod(RTABMapActivity, methodID,
03274 status_.first,
03275 env->NewStringUTF(status_.second.c_str()));
03276 success = true;
03277 }
03278 }
03279 }
03280 jvm->DetachCurrentThread();
03281 }
03282 if(!success)
03283 {
03284 UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
03285 }
03286 }
03287
03288 if(event->getClassName().compare("PostRenderEvent") == 0)
03289 {
03290 LOGI("Received PostRenderEvent!");
03291
03292 int loopClosureId = 0;
03293 int featuresExtracted = 0;
03294 if(((PostRenderEvent*)event)->getRtabmapEvent())
03295 {
03296 const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getRtabmapEvent()->getStats();
03297 loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
03298 featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
03299
03300 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryWorking_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f)));
03301 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryShort_time_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f)));
03302 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kKeypointDictionary_size(), uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f)));
03303 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kTimingTotal(), uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f)));
03304 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_id(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f)));
03305 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDatabase_memory_used(), uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f)));
03306 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopVisual_inliers(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f)));
03307 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopVisual_matches(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_matches(), 0.0f)));
03308 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopRejectedHypothesis(), uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f)));
03309 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f)));
03310 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error_ratio(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error_ratio(), 0.0f)));
03311 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f)));
03312 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
03313 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
03314 uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
03315 }
03316
03317
03318 int nodes = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
03319 uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
03320 int words = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
03321 float updateTime = uValue(bufferedStatsData_, rtabmap::Statistics::kTimingTotal(), 0.0f);
03322 int highestHypId = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
03323 int databaseMemoryUsed = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
03324 int inliers = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
03325 int matches = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_matches(), 0.0f);
03326 int rejected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
03327 float optimizationMaxError = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f);
03328 float optimizationMaxErrorRatio = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error_ratio(), 0.0f);
03329 float rehearsalValue = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f);
03330 float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
03331 float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
03332 int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
03333 rtabmap::Transform currentPose = main_scene_.GetCameraPose();
03334 float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
03335 if(!currentPose.isNull())
03336 {
03337 currentPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
03338 }
03339
03340
03341 UINFO("Send statistics to GUI");
03342 bool success = false;
03343 if(jvm && RTABMapActivity)
03344 {
03345 JNIEnv *env = 0;
03346 jint rs = jvm->AttachCurrentThread(&env, NULL);
03347 if(rs == JNI_OK && env)
03348 {
03349 jclass clazz = env->GetObjectClass(RTABMapActivity);
03350 if(clazz)
03351 {
03352 jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIFFFFFF)V" );
03353 if(methodID)
03354 {
03355 env->CallVoidMethod(RTABMapActivity, methodID,
03356 nodes,
03357 words,
03358 totalPoints_,
03359 totalPolygons_,
03360 updateTime,
03361 loopClosureId,
03362 highestHypId,
03363 databaseMemoryUsed,
03364 inliers,
03365 matches,
03366 featuresExtracted,
03367 hypothesis,
03368 lastDrawnCloudsCount_,
03369 renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
03370 rejected,
03371 rehearsalValue,
03372 optimizationMaxError,
03373 optimizationMaxErrorRatio,
03374 distanceTravelled,
03375 fastMovement,
03376 x,
03377 y,
03378 z,
03379 roll,
03380 pitch,
03381 yaw);
03382 success = true;
03383 }
03384 }
03385 }
03386 jvm->DetachCurrentThread();
03387 }
03388 if(!success)
03389 {
03390 UERROR("Failed to call RTABMapActivity::updateStatsCallback");
03391 }
03392 renderingTime_ = 0.0f;
03393 }
03394 return false;
03395 }
03396