RTABMapApp.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include <tango-gl/conversions.h>
29 
30 #include "RTABMapApp.h"
31 #ifdef __ANDROID__
32 #include "CameraAvailability.h"
33 #endif
34 #ifdef RTABMAP_TANGO
35 #include "CameraTango.h"
36 #endif
37 #ifdef RTABMAP_ARCORE
38 #include "CameraARCore.h"
39 #include <media/NdkImage.h>
40 #endif
41 #ifdef RTABMAP_ARENGINE
42 #include "CameraAREngine.h"
43 #endif
44 
45 #include <rtabmap/core/Rtabmap.h>
46 #include <rtabmap/core/util2d.h>
47 #include <rtabmap/core/util3d.h>
48 #include <rtabmap/core/util3d_transforms.h>
49 #include <rtabmap/core/util3d_filtering.h>
50 #include <rtabmap/core/util3d_surface.h>
51 #include <rtabmap/core/Graph.h>
52 #include <rtabmap/utilite/UEventsManager.h>
53 #include <rtabmap/utilite/UStl.h>
54 #include <rtabmap/utilite/UDirectory.h>
55 #include <rtabmap/utilite/UFile.h>
56 #include <opencv2/opencv_modules.hpp>
57 #include <rtabmap/core/util3d_surface.h>
58 #include <rtabmap/utilite/UConversion.h>
59 #include <rtabmap/utilite/UTimer.h>
60 #include <rtabmap/core/ParamEvent.h>
61 #include <rtabmap/core/Compression.h>
62 #include <rtabmap/core/Optimizer.h>
63 #include <rtabmap/core/VWDictionary.h>
64 #include <rtabmap/core/Memory.h>
65 #include <rtabmap/core/GainCompensator.h>
66 #include <rtabmap/core/DBDriver.h>
67 #include <rtabmap/core/Recovery.h>
68 #include <rtabmap/core/lidar/LidarVLP16.h>
69 #include <pcl/common/common.h>
70 #include <pcl/filters/extract_indices.h>
71 #include <pcl/io/ply_io.h>
72 #include <pcl/io/obj_io.h>
73 #include <pcl/surface/poisson.h>
74 #include <pcl/surface/vtk_smoothing/vtk_mesh_quadric_decimation.h>
75 
76 
77 #define LOW_RES_PIX 2
78 //#define DEBUG_RENDERING_PERFORMANCE
79 
80 const int g_optMeshId = -100;
81 
82 #ifdef __ANDROID__
83 static JavaVM *jvm;
84 static jobject RTABMapActivity = 0;
85 #endif
86 
87 #ifdef __ANDROID__
88 #ifndef DISABLE_LOG
89 //ref: https://codelab.wordpress.com/2014/11/03/how-to-use-standard-output-streams-for-logging-in-android-apps/
90 static int pfd[2];
91 static pthread_t thr;
92 static void *thread_func(void*)
93 {
94  ssize_t rdsz;
95  char buf[128];
96  while((rdsz = read(pfd[0], buf, sizeof buf - 1)) > 0) {
97  if(buf[rdsz - 1] == '\n') --rdsz;
98  buf[rdsz] = 0; /* add null-terminator */
99  __android_log_write(ANDROID_LOG_DEBUG, LOG_TAG, buf);
100  }
101  return 0;
102 }
103 
104 int start_logger()
105 {
106  /* make stdout line-buffered and stderr unbuffered */
107  setvbuf(stdout, 0, _IOLBF, 0);
108  setvbuf(stderr, 0, _IONBF, 0);
109 
110  /* create the pipe and redirect stdout and stderr */
111  pipe(pfd);
112  dup2(pfd[1], 1);
113  dup2(pfd[1], 2);
114 
115  /* spawn the logging thread */
116  if(pthread_create(&thr, 0, thread_func, 0) == -1)
117  return -1;
118  pthread_detach(thr);
119  return 0;
120 }
121 #endif
122 #endif
123 
124 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
125 {
126  rtabmap::ParametersMap parameters;
127 
128  parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
129 
130  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("200")));
131  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
132  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(cameraColor_&&fullResolution_?"2":"1")));
133  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
134  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
135  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
136  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
137  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
138  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAggressiveLoopThr(), "0.0"));
139  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
140  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
141  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
142  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), "1"));
143  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDMaxLocalRetrieved(), "0"));
144  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemCompressionParallelized(), std::string("false")));
145  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpParallelized(), std::string("false")));
146  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
147  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("25")));
148  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
149  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
150  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDLinearUpdate(), std::string("0.05")));
151  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDAngularUpdate(), std::string("0.05")));
152  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMarkerLength(), std::string("0.0")));
153 
154  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemUseOdomGravity(), "true"));
155  if(parameters.find(rtabmap::Parameters::kOptimizerStrategy()) != parameters.end())
156  {
157  if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("2") == 0) // GTSAM
158  {
159  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
160  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
161  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0.2"));
162  }
163  else if(parameters.at(rtabmap::Parameters::kOptimizerStrategy()).compare("1") == 0) // g2o
164  {
165  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.0"));
166  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10"));
167  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0.2"));
168  }
169  else // TORO
170  {
171  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0.00001"));
172  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "100"));
173  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerGravitySigma(), "0"));
174  }
175  }
176 
177  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true")));
178  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("0")));
179  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10")));
180  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
181  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
182  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
183  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.49")));
184  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
185 
186  parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
187  parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemRehearsalSimilarity()));
188  parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kMemMapLabelsAdded()));
189  if(dataRecorderMode_)
190  {
191  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxFeatures(), std::string("-1")));
192  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemRehearsalSimilarity(), std::string("1.0"))); // deactivate rehearsal
193  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemMapLabelsAdded(), "false")); // don't create map labels
194  uInsert(parameters, rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("true")));
195  }
196 
197  return parameters;
198 }
199 
200 #ifdef __ANDROID__
201 RTABMapApp::RTABMapApp(JNIEnv* env, jobject caller_activity) :
202 #else //__APPLE__
203 RTABMapApp::RTABMapApp() :
204 #endif
205  cameraDriver_(0),
206  camera_(0),
207  sensorCaptureThread_(0),
208  rtabmapThread_(0),
209  rtabmap_(0),
210  logHandler_(0),
211  odomCloudShown_(true),
212  graphOptimization_(true),
213  nodesFiltering_(false),
214  localizationMode_(false),
215  trajectoryMode_(false),
216  rawScanSaved_(false),
217  smoothing_(true),
218  depthFromMotion_(false),
219  cameraColor_(true),
220  fullResolution_(false),
221  appendMode_(true),
222  useExternalLidar_(false),
223  maxCloudDepth_(2.5),
224  minCloudDepth_(0.0),
225  cloudDensityLevel_(1),
226  meshTrianglePix_(2),
227  meshAngleToleranceDeg_(20.0),
228  meshDecimationFactor_(0),
229  clusterRatio_(0.1),
230  maxGainRadius_(0.02f),
231  renderingTextureDecimation_(4),
232  backgroundColor_(0.2f),
233  depthConfidence_(2),
234  dataRecorderMode_(false),
235  clearSceneOnNextRender_(false),
236  openingDatabase_(false),
237  exporting_(false),
238  postProcessing_(false),
239  filterPolygonsOnNextRender_(false),
240  gainCompensationOnNextRender_(0),
241  bilateralFilteringOnNextRender_(false),
242  takeScreenshotOnNextRender_(false),
243  cameraJustInitialized_(false),
244  totalPoints_(0),
245  totalPolygons_(0),
246  lastDrawnCloudsCount_(0),
247  renderingTime_(0.0f),
248  lastPostRenderEventTime_(0.0),
249  lastPoseEventTime_(0.0),
250  visualizingMesh_(false),
251  exportedMeshUpdated_(false),
252  optMesh_(new pcl::TextureMesh),
253  optRefId_(0),
254  optRefPose_(0),
255  mapToOdom_(rtabmap::Transform::getIdentity())
256 
257 {
258  mappingParameters_.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), "5")); // GFTT/FREAK
259 
260 #ifdef __ANDROID__
261  env->GetJavaVM(&jvm);
262  RTABMapActivity = env->NewGlobalRef(caller_activity);
263 #endif
264 
265  LOGI("RTABMapApp::RTABMapApp()");
266  createdMeshes_.clear();
267  rawPoses_.clear();
268  clearSceneOnNextRender_ = true;
269  openingDatabase_ = false;
270  exporting_ = false;
271  postProcessing_=false;
272  totalPoints_ = 0;
273  totalPolygons_ = 0;
274  lastDrawnCloudsCount_ = 0;
275  renderingTime_ = 0.0f;
276  lastPostRenderEventTime_ = 0.0;
277  lastPoseEventTime_ = 0.0;
278  bufferedStatsData_.clear();
279 #ifdef __ANDROID__
280  progressionStatus_.setJavaObjects(jvm, RTABMapActivity);
281 #endif
282  main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
283 
284  logHandler_ = new rtabmap::LogHandler();
285 
286  this->registerToEventsManager();
287  LOGI("RTABMapApp::RTABMapApp() end");
288 
289 #ifdef __ANDROID__
290 #ifndef DISABLE_LOG
291  start_logger();
292 #endif
293 #endif
294 }
295 
296 #ifndef __ANDROID__ // __APPLE__
297 void RTABMapApp::setupSwiftCallbacks(void * classPtr,
298  void(*progressCallback)(void *, int, int),
299  void(*initCallback)(void *, int, const char*),
300  void(*statsUpdatedCallback)(void *,
301  int, int, int, int,
302  float,
303  int, int, int, int, int ,int,
304  float,
305  int,
306  float,
307  int,
308  float, float, float, float,
309  int, int,
310  float, float, float, float, float, float))
311 {
312  swiftClassPtr_ = classPtr;
313  progressionStatus_.setSwiftCallback(classPtr, progressCallback);
314  swiftInitCallback = initCallback;
315  swiftStatsUpdatedCallback = statsUpdatedCallback;
316 }
317 #endif
318 
319 RTABMapApp::~RTABMapApp() {
320  LOGI("~RTABMapApp() begin");
321  stopCamera();
322  if(rtabmapThread_)
323  {
324  rtabmapThread_->close(false);
325  }
326  delete rtabmapThread_;
327  delete logHandler_;
328  delete optRefPose_;
329  {
330  boost::mutex::scoped_lock lock(rtabmapMutex_);
331  if(rtabmapEvents_.size())
332  {
333  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
334  {
335  delete *iter;
336  }
337  }
338  rtabmapEvents_.clear();
339  }
340  LOGI("~RTABMapApp() end");
341 }
342 
343 void RTABMapApp::setScreenRotation(int displayRotation, int cameraRotation)
344 {
345  rtabmap::ScreenRotation rotation = rtabmap::GetAndroidRotationFromColorCameraToDisplay(displayRotation, cameraRotation);
346  //LOGI("Set orientation: display=%d camera=%d -> %d", displayRotation, cameraRotation, (int)rotation);
347  main_scene_.setScreenRotation(rotation);
348 
349  boost::mutex::scoped_lock lock(cameraMutex_);
350  if(camera_)
351  {
352  camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
353  }
354 }
355 
356 int RTABMapApp::openDatabase(const std::string & databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
357 {
358  LOGW("Opening database %s (inMemory=%d, optimize=%d, clearDatabase=%d)", databasePath.c_str(), databaseInMemory?1:0, optimize?1:0, clearDatabase?1:0);
359  this->unregisterFromEventsManager(); // to ignore published init events when closing rtabmap
360  status_.first = rtabmap::RtabmapEventInit::kInitializing;
361  rtabmapMutex_.lock();
362  if(rtabmapEvents_.size())
363  {
364  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents_.begin(); iter!=rtabmapEvents_.end(); ++iter)
365  {
366  delete *iter;
367  }
368  }
369  rtabmapEvents_.clear();
370  openingDatabase_ = true;
371  bool restartThread = false;
372  if(rtabmapThread_)
373  {
374  restartThread = rtabmapThread_->isRunning();
375 
376  rtabmapThread_->close(false);
377  delete rtabmapThread_;
378  rtabmapThread_ = 0;
379  rtabmap_ = 0;
380  }
381 
382  totalPoints_ = 0;
383  totalPolygons_ = 0;
384  lastDrawnCloudsCount_ = 0;
385  renderingTime_ = 0.0f;
386  lastPostRenderEventTime_ = 0.0;
387  lastPoseEventTime_ = 0.0;
388  bufferedStatsData_.clear();
389 
390  this->registerToEventsManager();
391 
392  int status = 0;
393 
394  // Open visualization while we load (if there is an optimized mesh saved in database)
395  optMesh_.reset(new pcl::TextureMesh);
396  optTexture_ = cv::Mat();
397  optRefId_ = 0;
398  if(optRefPose_)
399  {
400  delete optRefPose_;
401  optRefPose_ = 0;
402  }
403  cv::Mat cloudMat;
404  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
405 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
406  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
407 #else
408  std::vector<std::vector<Eigen::Vector2f> > texCoords;
409 #endif
410  cv::Mat textures;
411  if(!databasePath.empty() && UFile::exists(databasePath) && !clearDatabase)
412  {
413  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud/mesh..."));
414  rtabmap::DBDriver * driver = rtabmap::DBDriver::create();
415  if(driver->openConnection(databasePath))
416  {
417  cloudMat = driver->loadOptimizedMesh(&polygons, &texCoords, &textures);
418  if(!cloudMat.empty())
419  {
420  LOGI("Open: Found optimized mesh! Visualizing it.");
421  optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
422  optTexture_ = textures;
423  if(!optTexture_.empty())
424  {
425  LOGI("Open: Texture mesh: %dx%d.", optTexture_.cols, optTexture_.rows);
426  status=3;
427  }
428  else if(optMesh_->tex_polygons.size())
429  {
430  LOGI("Open: Polygon mesh");
431  status=2;
432  }
433  else if(!optMesh_->cloud.data.empty())
434  {
435  LOGI("Open: Point cloud");
436  status=1;
437  }
438  }
439  else
440  {
441  LOGI("Open: No optimized mesh found.");
442  }
443  delete driver;
444  }
445  }
446 
447  if(status > 0)
448  {
449  if(status==1)
450  {
451  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized cloud...done!"));
452  }
453  else if(status==2)
454  {
455  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized mesh...done!"));
456  }
457  else
458  {
459  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading optimized texture mesh...done!"));
460  }
461  boost::mutex::scoped_lock lockRender(renderingMutex_);
462  visualizingMesh_ = true;
463  exportedMeshUpdated_ = true;
464  }
465 
466  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading database..."));
467  if(clearDatabase)
468  {
469  LOGI("Erasing database \"%s\"...", databasePath.c_str());
470  UFile::erase(databasePath);
471  }
472 
473  //Rtabmap
474  mapToOdom_.setIdentity();
475  rtabmap_ = new rtabmap::Rtabmap();
476  rtabmap::ParametersMap parameters = getRtabmapParameters();
477 
478  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), uBool2Str(databaseInMemory)));
479  LOGI("Initializing database...");
480  rtabmap_->init(parameters, databasePath);
481  rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
482  if(parameters.find(rtabmap::Parameters::kRtabmapDetectionRate()) != parameters.end())
483  {
484  rtabmapThread_->setDetectorRate(uStr2Float(parameters.at(rtabmap::Parameters::kRtabmapDetectionRate())));
485  }
486 
487  // Generate all meshes
488  std::map<int, rtabmap::Signature> signatures;
489  std::map<int, rtabmap::Transform> poses;
490  std::multimap<int, rtabmap::Link> links;
491  LOGI("Loading full map from database...");
492  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Loading data from database..."));
493  rtabmap_->getGraph(
494  poses,
495  links,
496  true,
497  false, // Make sure poses are the same than optimized mesh (in case we switched RGBD/OptimizedFromGraphEnd)
498  &signatures,
499  true,
500  true,
501  true,
502  true);
503 
504  if(signatures.size() && poses.empty())
505  {
506  LOGE("Failed to optimize the graph!");
507  status = -1;
508  }
509 
510  {
511  LOGI("Creating the meshes (%d)....", (int)poses.size());
512  boost::mutex::scoped_lock lock(meshesMutex_);
513  createdMeshes_.clear();
514  int i=0;
515  UTimer addTime;
516  rawPoses_.clear();
517  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end() && status>=0; ++iter)
518  {
519  try
520  {
521  int id = iter->first;
522  if(!iter->second.isNull())
523  {
524  if(uContains(signatures, id))
525  {
526  UTimer timer;
527  rtabmap::SensorData data = signatures.at(id).sensorData();
528  rawPoses_.insert(std::make_pair(id, signatures.at(id).getPose()));
529 
530  cv::Mat tmpA, depth;
531  data.uncompressData(&tmpA, &depth);
532 
533  if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
534  {
535  rtabmap::LaserScan scan;
536  data.uncompressData(0, 0, &scan);
537  }
538 
539  if((!data.imageRaw().empty() && !data.depthRaw().empty()) || !data.laserScanRaw().isEmpty())
540  {
541  // Voxelize and filter depending on the previous cloud?
542  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
543  pcl::IndicesPtr indices(new std::vector<int>);
544  if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
545  {
546  int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
547 
548  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
549  }
550  else
551  {
552  //scan
553  cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
554  indices->resize(cloud->size());
555  for(unsigned int i=0; i<cloud->size(); ++i)
556  {
557  indices->at(i) = i;
558  }
559  }
560 
561  if(cloud->size() && indices->size())
562  {
563  std::vector<pcl::Vertices> polygons;
564  std::vector<pcl::Vertices> polygonsLowRes;
565 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
566  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
567 #else
568  std::vector<Eigen::Vector2f> texCoords;
569 #endif
570  if(cloud->isOrganized() && main_scene_.isMeshRendering() && main_scene_.isMapRendering())
571  {
572  polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
573 #ifndef DISABLE_VTK
574  if(meshDecimationFactor_ > 0.0f && !polygons.empty())
575  {
576  pcl::PolygonMesh::Ptr tmpMesh(new pcl::PolygonMesh);
577  pcl::toPCLPointCloud2(*cloud, tmpMesh->cloud);
578  tmpMesh->polygons = polygons;
579  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGB>(tmpMesh, meshDecimationFactor_, 0, cloud, 0);
580  if(!tmpMesh->polygons.empty())
581  {
582  if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
583  {
584  std::map<int, rtabmap::Transform> cameraPoses;
585  std::map<int, rtabmap::CameraModel> cameraModels;
586  cameraPoses.insert(std::make_pair(0, rtabmap::Transform::getIdentity()));
587  cameraModels.insert(std::make_pair(0, data.cameraModels()[0]));
588  pcl::TextureMesh::Ptr textureMesh = rtabmap::util3d::createTextureMesh(
589  tmpMesh,
590  cameraPoses,
591  cameraModels,
592  std::map<int, cv::Mat>());
593  pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
594  polygons = textureMesh->tex_polygons[0];
595  texCoords = textureMesh->tex_coordinates[0];
596  }
597  else
598  {
599  pcl::fromPCLPointCloud2(tmpMesh->cloud, *cloud);
600  polygons = tmpMesh->polygons;
601  }
602 
603  indices->resize(cloud->size());
604  for(unsigned int i=0; i<cloud->size(); ++i)
605  {
606  indices->at(i) = i;
607  }
608  }
609  else
610  {
611  LOGE("Mesh decimation factor is too high (%f), returning full mesh (id=%d).", meshDecimationFactor_, data.id());
612  polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
613  }
614 #ifdef DEBUG_RENDERING_PERFORMANCE
615  LOGW("Mesh simplication, %d polygons, %d points (%fs)", (int)polygons.size(), (int)cloud->size(), timer.ticks());
616 #endif
617  }
618  else
619 #endif
620  {
621  polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
622  }
623  }
624 
625  std::pair<std::map<int, rtabmap::Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, rtabmap::Mesh()));
626  UASSERT(inserted.second);
627  inserted.first->second.cloud = cloud;
628  inserted.first->second.indices = indices;
629  inserted.first->second.polygons = polygons;
630  inserted.first->second.polygonsLowRes = polygonsLowRes;
631  inserted.first->second.visible = true;
632  inserted.first->second.cameraModel = data.cameraModels()[0];
633  inserted.first->second.gains[0] = 1.0;
634  inserted.first->second.gains[1] = 1.0;
635  inserted.first->second.gains[2] = 1.0;
636  if((cloud->isOrganized() || !texCoords.empty()) && main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
637  {
638  inserted.first->second.texCoords = texCoords;
639  if(renderingTextureDecimation_>1)
640  {
641  cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
642  cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
643  }
644  else
645  {
646  inserted.first->second.texture = data.imageRaw();
647  }
648  }
649  LOGI("Created cloud %d (%fs, %d points)", id, timer.ticks(), (int)cloud->size());
650  }
651  else
652  {
653  UWARN("Cloud %d is empty", id);
654  }
655  }
656  else if(!data.depthOrRightCompressed().empty() || !data.laserScanCompressed().isEmpty())
657  {
658  UERROR("Failed to uncompress data! (rgb=%d, depth=%d, scan=%d)", data.imageCompressed().cols, data.depthOrRightCompressed().cols, data.laserScanCompressed().size());
659  status=-2;
660  }
661  }
662  else
663  {
664  UWARN("Data for node %d not found", id);
665  }
666  }
667  else
668  {
669  UWARN("Pose %d is null !?", id);
670  }
671  ++i;
672  if(addTime.elapsed() >= 4.0f)
673  {
674  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, uFormat("Created clouds %d/%d", i, (int)poses.size())));
675  addTime.restart();
676  }
677  }
678  catch(const UException & e)
679  {
680  UERROR("Exception! msg=\"%s\"", e.what());
681  status = -2;
682  }
683  catch (const cv::Exception & e)
684  {
685  UERROR("Exception! msg=\"%s\"", e.what());
686  status = -2;
687  }
688  catch (const std::exception & e)
689  {
690  UERROR("Exception! msg=\"%s\"", e.what());
691  status = -2;
692  }
693  }
694  if(status < 0)
695  {
696  createdMeshes_.clear();
697  rawPoses_.clear();
698  }
699  else
700  {
701  LOGI("Created %d meshes...", (int)createdMeshes_.size());
702  }
703  }
704 
705 
706 
707  if(optimize && status>=0)
708  {
709  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Visual optimization..."));
710  gainCompensation();
711 
712  LOGI("Polygon filtering...");
713  boost::mutex::scoped_lock lock(meshesMutex_);
714  UTimer time;
715  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
716  {
717  if(iter->second.polygons.size())
718  {
719  // filter polygons
720  iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size());
721  }
722  }
723  }
724 
725  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInfo, "Updating scene..."));
726  LOGI("Open: add rtabmap event to update the scene");
727  rtabmap::Statistics stats;
728  stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
729  stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
730  stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
731  stats.setPoses(poses);
732  stats.setConstraints(links);
733  rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
734 
735  rtabmap_->setOptimizedPoses(poses, links);
736 
737  // for optimized mesh
738  if(poses.size())
739  {
740  // just take the last as reference
741  optRefId_ = poses.rbegin()->first;
742  optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
743  }
744 
745  {
746  boost::mutex::scoped_lock lock(cameraMutex_);
747  if(camera_)
748  {
749  camera_->resetOrigin();
750  }
751  }
752 
753  UEventsManager::post(new rtabmap::RtabmapEventInit(rtabmap::RtabmapEventInit::kInitialized, ""));
754 
755  if(restartThread)
756  {
757  rtabmapThread_->registerToEventsManager();
758  rtabmapThread_->start();
759  }
760 
761  rtabmapMutex_.unlock();
762 
763  boost::mutex::scoped_lock lockRender(renderingMutex_);
764  if(poses.empty() || status>0)
765  {
766  openingDatabase_ = false;
767  }
768 
769  clearSceneOnNextRender_ = status<=0;
770 
771  return status;
772 }
773 
774 int RTABMapApp::updateMeshDecimation(int width, int height)
775 {
776  int meshDecimation = 1;
777  if(cloudDensityLevel_ == 3) // very low
778  {
779  if((height >= 480 || width >= 480) && width % 20 == 0 && height % 20 == 0)
780  {
781  meshDecimation = 20;
782  }
783  else if(width % 15 == 0 && height % 15 == 0)
784  {
785  meshDecimation = 15;
786  }
787  else if(width % 10 == 0 && height % 10 == 0)
788  {
789  meshDecimation = 10;
790  }
791  else if(width % 8 == 0 && height % 8 == 0)
792  {
793  meshDecimation = 8;
794  }
795  else
796  {
797  UERROR("Could not set decimation to high (size=%dx%d)", width, height);
798  }
799  }
800  else if(cloudDensityLevel_ == 2) // low
801  {
802  if((height >= 480 || width >= 480) && width % 10 == 0 && height % 10 == 0)
803  {
804  meshDecimation = 10;
805  }
806  else if(width % 5 == 0 && height % 5 == 0)
807  {
808  meshDecimation = 5;
809  }
810  else if(width % 4 == 0 && height % 4 == 0)
811  {
812  meshDecimation = 4;
813  }
814  else
815  {
816  UERROR("Could not set decimation to medium (size=%dx%d)", width, height);
817  }
818  }
819  else if(cloudDensityLevel_ == 1) // high
820  {
821  if((height >= 480 || width >= 480) && width % 5 == 0 && height % 5 == 0)
822  {
823  meshDecimation = 5;
824  }
825  else if(width % 3 == 0 && width % 3 == 0)
826  {
827  meshDecimation = 3;
828  }
829  else if(width % 2 == 0 && width % 2 == 0)
830  {
831  meshDecimation = 2;
832  }
833  else
834  {
835  UERROR("Could not set decimation to low (size=%dx%d)", width, height);
836  }
837  }
838  // else maximum
839  LOGI("Set decimation to %d (image=%dx%d, density level=%d)", meshDecimation, width, height, cloudDensityLevel_);
840  return meshDecimation;
841 }
842 
843 bool RTABMapApp::isBuiltWith(int cameraDriver) const
844 {
845  if(cameraDriver == 0)
846  {
847 #ifdef RTABMAP_TANGO
848  return true;
849 #else
850  return false;
851 #endif
852  }
853 
854  if(cameraDriver == 1)
855  {
856 #ifdef RTABMAP_ARCORE
857  return true;
858 #else
859  return false;
860 #endif
861  }
862 
863  if(cameraDriver == 2)
864  {
865 #ifdef RTABMAP_ARENGINE
866  return true;
867 #else
868  return false;
869 #endif
870  }
871  return false;
872 }
873 
874 #ifdef __ANDROID__
875 bool RTABMapApp::startCamera(JNIEnv* env, jobject iBinder, jobject context, jobject activity, int driver)
876 #else // __APPLE__
877 bool RTABMapApp::startCamera()
878 #endif
879 {
880  stopCamera();
881 
882  //ccapp = new computer_vision::ComputerVisionApplication();
883  //ccapp->OnResume(env, context, activity);
884  //return true;
885 #ifdef __ANDROID__
886  cameraDriver_ = driver;
887 #else // __APPLE__
888  cameraDriver_ = 3;
889 #endif
890  LOGW("startCamera() camera driver=%d", cameraDriver_);
891  boost::mutex::scoped_lock lock(cameraMutex_);
892 
893  if(cameraDriver_ == 0) // Tango
894  {
895 #ifdef RTABMAP_TANGO
896  camera_ = new rtabmap::CameraTango(cameraColor_, !cameraColor_ || fullResolution_?1:2, rawScanSaved_, smoothing_);
897 
898  if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
899  UERROR("TangoHandler::ConnectTango, TangoService_setBinder error");
900  delete camera_;
901  camera_ = 0;
902  return false;
903  }
904 #else
905  UERROR("RTAB-Map is not built with Tango support!");
906 #endif
907  }
908  else if(cameraDriver_ == 1)
909  {
910 #ifdef RTABMAP_ARCORE
911  camera_ = new rtabmap::CameraARCore(env, context, activity, depthFromMotion_, smoothing_);
912 #else
913  UERROR("RTAB-Map is not built with ARCore support!");
914 #endif
915  }
916  else if(cameraDriver_ == 2)
917  {
918 #ifdef RTABMAP_ARENGINE
919  camera_ = new rtabmap::CameraAREngine(env, context, activity, smoothing_);
920 #else
921  UERROR("RTAB-Map is not built with AREngine support!");
922 #endif
923  }
924  else if(cameraDriver_ == 3)
925  {
926  camera_ = new rtabmap::CameraMobile(smoothing_);
927  }
928 
929  if(camera_ == 0)
930  {
931  UERROR("Unknown or not supported camera driver! %d", cameraDriver_);
932  return false;
933  }
934 
935  if(camera_->init())
936  {
937  camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
938 
939  //update mesh decimation based on camera calibration
940  LOGI("Cloud density level %d", cloudDensityLevel_);
941 
942  LOGI("Start camera thread");
943  cameraJustInitialized_ = true;
944  if(useExternalLidar_)
945  {
946  rtabmap::LidarVLP16 * lidar = new rtabmap::LidarVLP16(boost::asio::ip::address_v4::from_string("192.168.1.201"), 2368, true);
947  lidar->init();
948  camera_->setImageRate(0); // if lidar, to get close camera synchronization
949  sensorCaptureThread_ = new rtabmap::SensorCaptureThread(lidar, camera_, camera_, rtabmap::Transform::getIdentity());
950  sensorCaptureThread_->setScanParameters(false, 1, 0.0f, 0.0f, 0.0f, 0, 0.0f, 0.0f, true);
951  }
952  else
953  {
954  sensorCaptureThread_ = new rtabmap::SensorCaptureThread(camera_);
955  }
956  sensorCaptureThread_->start();
957  return true;
958  }
959  UERROR("Failed camera initialization!");
960  return false;
961 }
962 
963 void RTABMapApp::stopCamera()
964 {
965  LOGI("stopCamera()");
966  {
967  boost::mutex::scoped_lock lock(cameraMutex_);
968  if(sensorCaptureThread_!=0)
969  {
970  sensorCaptureThread_->join(true);
971  delete sensorCaptureThread_; // camera_ is closed and deleted inside
972  sensorCaptureThread_ = 0;
973  camera_ = 0;
974  }
975  }
976  {
977  boost::mutex::scoped_lock lock(renderingMutex_);
978  delete main_scene_.background_renderer_;
979  main_scene_.background_renderer_ = 0;
980  }
981 }
982 
983 std::vector<pcl::Vertices> RTABMapApp::filterOrganizedPolygons(
984  const std::vector<pcl::Vertices> & polygons,
985  int cloudSize) const
986 {
987  std::vector<int> vertexToCluster(cloudSize, 0);
988  std::map<int, std::list<int> > clusters;
989  int lastClusterID = 0;
990 
991  for(unsigned int i=0; i<polygons.size(); ++i)
992  {
993  int clusterID = 0;
994  for(unsigned int j=0;j<polygons[i].vertices.size(); ++j)
995  {
996  if(vertexToCluster[polygons[i].vertices[j]]>0)
997  {
998  clusterID = vertexToCluster[polygons[i].vertices[j]];
999  break;
1000  }
1001  }
1002  if(clusterID>0)
1003  {
1004  clusters.at(clusterID).push_back(i);
1005  }
1006  else
1007  {
1008  clusterID = ++lastClusterID;
1009  std::list<int> polygons;
1010  polygons.push_back(i);
1011  clusters.insert(std::make_pair(clusterID, polygons));
1012  }
1013  for(unsigned int j=0;j<polygons[i].vertices.size(); ++j)
1014  {
1015  vertexToCluster[polygons[i].vertices[j]] = clusterID;
1016  }
1017  }
1018 
1019  unsigned int biggestClusterSize = 0;
1020  for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
1021  {
1022  //LOGD("cluster %d = %d", iter->first, (int)iter->second.size());
1023 
1024  if(iter->second.size() > biggestClusterSize)
1025  {
1026  biggestClusterSize = iter->second.size();
1027  }
1028  }
1029  unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_);
1030  //LOGI("Biggest cluster %d -> minClusterSize(ratio=%f)=%d",
1031  // biggestClusterSize, clusterRatio_, (int)minClusterSize);
1032 
1033  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
1034  int oi = 0;
1035  for(std::map<int, std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
1036  {
1037  if(iter->second.size() >= minClusterSize)
1038  {
1039  for(std::list<int>::iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
1040  {
1041  filteredPolygons[oi++] = polygons[*jter];
1042  }
1043  }
1044  }
1045  filteredPolygons.resize(oi);
1046  return filteredPolygons;
1047 }
1048 
1049 
1050 std::vector<pcl::Vertices> RTABMapApp::filterPolygons(
1051  const std::vector<pcl::Vertices> & polygons,
1052  int cloudSize) const
1053 {
1054  // filter polygons
1055  std::vector<std::set<int> > neighbors;
1056  std::vector<std::set<int> > vertexToPolygons;
1057  rtabmap::util3d::createPolygonIndexes(
1058  polygons,
1059  cloudSize,
1060  neighbors,
1061  vertexToPolygons);
1062  std::list<std::list<int> > clusters = rtabmap::util3d::clusterPolygons(neighbors);
1063 
1064  unsigned int biggestClusterSize = 0;
1065  for(std::list<std::list<int> >::iterator iter=clusters.begin(); iter!=clusters.end(); ++iter)
1066  {
1067  if(iter->size() > biggestClusterSize)
1068  {
1069  biggestClusterSize = iter->size();
1070  }
1071  }
1072  unsigned int minClusterSize = (unsigned int)(float(biggestClusterSize)*clusterRatio_);
1073  LOGI("Biggest cluster = %d -> minClusterSize(ratio=%f)=%d",
1074  biggestClusterSize, clusterRatio_, (int)minClusterSize);
1075 
1076  std::vector<pcl::Vertices> filteredPolygons(polygons.size());
1077  int oi=0;
1078  for(std::list<std::list<int> >::iterator jter=clusters.begin(); jter!=clusters.end(); ++jter)
1079  {
1080  if(jter->size() >= minClusterSize)
1081  {
1082  for(std::list<int>::iterator kter=jter->begin(); kter!=jter->end(); ++kter)
1083  {
1084  filteredPolygons[oi++] = polygons.at(*kter);
1085  }
1086  }
1087  }
1088  filteredPolygons.resize(oi);
1089  return filteredPolygons;
1090 }
1091 
1092 // OpenGL thread
1093 void RTABMapApp::InitializeGLContent()
1094 {
1095  UINFO("");
1096  main_scene_.InitGLContent();
1097 
1098  float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
1099  main_scene_.setGridColor(v, v, v);
1100 }
1101 
1102 // OpenGL thread
1103 void RTABMapApp::SetViewPort(int width, int height)
1104 {
1105  main_scene_.SetupViewPort(width, height);
1106  boost::mutex::scoped_lock lock(cameraMutex_);
1107  if(camera_)
1108  {
1109  camera_->setScreenRotationAndSize(main_scene_.getScreenRotation(), main_scene_.getViewPortWidth(), main_scene_.getViewPortHeight());
1110  }
1111 }
1112 
1113 class PostRenderEvent : public UEvent
1114 {
1115 public:
1116  PostRenderEvent(rtabmap::RtabmapEvent * event = 0) :
1117  rtabmapEvent_(event)
1118  {
1119  }
1120  ~PostRenderEvent()
1121  {
1122  if(rtabmapEvent_!=0)
1123  {
1124  delete rtabmapEvent_;
1125  }
1126  }
1127  virtual std::string getClassName() const {return "PostRenderEvent";}
1128  const rtabmap::RtabmapEvent * getRtabmapEvent() const {return rtabmapEvent_;}
1129 private:
1130  rtabmap::RtabmapEvent * rtabmapEvent_;
1131 };
1132 
1133 // OpenGL thread
1134 bool RTABMapApp::smoothMesh(int id, rtabmap::Mesh & mesh)
1135 {
1136  UTimer t;
1137  // reconstruct depth image
1138  UASSERT(mesh.indices.get() && mesh.indices->size());
1139  cv::Mat depth = cv::Mat::zeros(mesh.cloud->height, mesh.cloud->width, CV_32FC1);
1140  rtabmap::Transform localTransformInv = mesh.cameraModel.localTransform().inverse();
1141  for(unsigned int i=0; i<mesh.indices->size(); ++i)
1142  {
1143  int index = mesh.indices->at(i);
1144  // FastBilateralFilter works in camera frame
1145  if(mesh.cloud->at(index).x > 0)
1146  {
1147  pcl::PointXYZRGB pt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
1148  depth.at<float>(index) = pt.z;
1149  }
1150  }
1151 
1152  depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
1153  LOGI("smoothMesh() Bilateral filtering of %d, time=%fs", id, t.ticks());
1154 
1155  if(!depth.empty() && mesh.indices->size())
1156  {
1157  pcl::IndicesPtr newIndices(new std::vector<int>(mesh.indices->size()));
1158  int oi = 0;
1159  for(unsigned int i=0; i<mesh.indices->size(); ++i)
1160  {
1161  int index = mesh.indices->at(i);
1162 
1163  pcl::PointXYZRGB & pt = mesh.cloud->at(index);
1164  pcl::PointXYZRGB newPt = rtabmap::util3d::transformPoint(mesh.cloud->at(index), localTransformInv);
1165  if(depth.at<float>(index) > 0)
1166  {
1167  newPt.z = depth.at<float>(index);
1168  newPt = rtabmap::util3d::transformPoint(newPt, mesh.cameraModel.localTransform());
1169  newIndices->at(oi++) = index;
1170  }
1171  else
1172  {
1173  newPt.x = newPt.y = newPt.z = std::numeric_limits<float>::quiet_NaN();
1174  }
1175  pt.x = newPt.x;
1176  pt.y = newPt.y;
1177  pt.z = newPt.z;
1178  }
1179  newIndices->resize(oi);
1180  mesh.indices = newIndices;
1181 
1182  //reconstruct the mesh with smoothed surfaces
1183  std::vector<pcl::Vertices> polygons;
1184  if(main_scene_.isMeshRendering())
1185  {
1186  polygons = rtabmap::util3d::organizedFastMesh(mesh.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
1187  }
1188  LOGI("smoothMesh() Reconstructing the mesh of %d, time=%fs", id, t.ticks());
1189  mesh.polygons = polygons;
1190  }
1191  else
1192  {
1193  UERROR("smoothMesh() Failed to smooth surface %d", id);
1194  return false;
1195  }
1196  return true;
1197 }
1198 
1199 void RTABMapApp::gainCompensation(bool full)
1200 {
1201  UTimer tGainCompensation;
1202  LOGI("Gain compensation...");
1203  boost::mutex::scoped_lock lock(meshesMutex_);
1204 
1205  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
1206  std::map<int, pcl::IndicesPtr> indices;
1207  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
1208  {
1209  clouds.insert(std::make_pair(iter->first, iter->second.cloud));
1210  indices.insert(std::make_pair(iter->first, iter->second.indices));
1211  }
1212  std::map<int, rtabmap::Transform> poses;
1213  std::multimap<int, rtabmap::Link> links;
1214  rtabmap_->getGraph(poses, links, true, true);
1215  if(full)
1216  {
1217  // full compensation
1218  links.clear();
1219  for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
1220  {
1221  int from = iter->first;
1222  std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator jter = iter;
1223  ++jter;
1224  for(;jter!=clouds.end(); ++jter)
1225  {
1226  int to = jter->first;
1227  links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to))));
1228  }
1229  }
1230  }
1231 
1232  UASSERT(maxGainRadius_>0.0f);
1233  rtabmap::GainCompensator compensator(maxGainRadius_, 0.0f, 0.01f, 1.0f);
1234  if(clouds.size() > 1 && links.size())
1235  {
1236  compensator.feed(clouds, indices, links);
1237  LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
1238  }
1239 
1240  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
1241  {
1242  if(!iter->second.cloud->empty())
1243  {
1244  if(clouds.size() > 1 && links.size())
1245  {
1246  compensator.getGain(iter->first, &iter->second.gains[0], &iter->second.gains[1], &iter->second.gains[2]);
1247  LOGI("%d mesh has gain %f,%f,%f", iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]);
1248  }
1249  }
1250  }
1251  LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
1252 }
1253 
1254 // OpenGL thread
1255 int RTABMapApp::Render()
1256 {
1257  std::list<rtabmap::RtabmapEvent*> rtabmapEvents;
1258  try
1259  {
1260  if(sensorCaptureThread_ == 0)
1261  {
1262  // We are not doing continous drawing, just measure single draw
1263  fpsTime_.restart();
1264  }
1265 
1266 #ifdef DEBUG_RENDERING_PERFORMANCE
1267  UTimer time;
1268 #endif
1269  boost::mutex::scoped_lock lock(renderingMutex_);
1270 
1271  bool notifyDataLoaded = false;
1272  bool notifyCameraStarted = false;
1273 
1274  if(clearSceneOnNextRender_)
1275  {
1276  visualizingMesh_ = false;
1277  }
1278 
1279  // ARCore and AREngine capture should be done in opengl thread!
1280  const float* uvsTransformed = 0;
1281  glm::mat4 arProjectionMatrix(0);
1282  glm::mat4 arViewMatrix(0);
1283  rtabmap::Mesh occlusionMesh;
1284 
1285  {
1286  boost::mutex::scoped_lock lock(cameraMutex_);
1287  if(camera_!=0)
1288  {
1289  if(cameraDriver_ <= 2)
1290  {
1291  camera_->updateOnRender();
1292  }
1293 #ifdef DEBUG_RENDERING_PERFORMANCE
1294  LOGW("Camera updateOnRender %fs", time.ticks());
1295 #endif
1296  if(main_scene_.background_renderer_ == 0 && camera_->getTextureId() != 0)
1297  {
1298  main_scene_.background_renderer_ = new BackgroundRenderer();
1299  main_scene_.background_renderer_->InitializeGlContent(((rtabmap::CameraMobile*)camera_)->getTextureId(), cameraDriver_ <= 2);
1300  }
1301  if(camera_->uvsInitialized())
1302  {
1303  uvsTransformed = ((rtabmap::CameraMobile*)camera_)->uvsTransformed();
1304  ((rtabmap::CameraMobile*)camera_)->getVPMatrices(arViewMatrix, arProjectionMatrix);
1305  if(graphOptimization_ && !mapToOdom_.isIdentity())
1306  {
1307  rtabmap::Transform mapCorrection = rtabmap::opengl_world_T_rtabmap_world * mapToOdom_ *rtabmap::rtabmap_world_T_opengl_world;
1308  arViewMatrix = glm::inverse(rtabmap::glmFromTransform(mapCorrection)*glm::inverse(arViewMatrix));
1309  }
1310  }
1311  if(!visualizingMesh_ && main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson)
1312  {
1313  rtabmap::CameraModel occlusionModel;
1314  cv::Mat occlusionImage = ((rtabmap::CameraMobile*)camera_)->getOcclusionImage(&occlusionModel);
1315 
1316  if(occlusionModel.isValidForProjection())
1317  {
1318  pcl::IndicesPtr indices(new std::vector<int>);
1319  int meshDecimation = updateMeshDecimation(occlusionImage.cols, occlusionImage.rows);
1320  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = rtabmap::util3d::cloudFromDepth(occlusionImage, occlusionModel, meshDecimation, 0, 0, indices.get());
1321  cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*occlusionModel.localTransform());
1322  occlusionMesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>());
1323  pcl::copyPointCloud(*cloud, *occlusionMesh.cloud);
1324  occlusionMesh.indices = indices;
1325  occlusionMesh.polygons = rtabmap::util3d::organizedFastMesh(cloud, 1.0*M_PI/180.0, false, meshTrianglePix_);
1326  }
1327  else if(!occlusionImage.empty())
1328  {
1329  UERROR("invalid occlusionModel: %f %f %f %f %dx%d", occlusionModel.fx(), occlusionModel.fy(), occlusionModel.cx(), occlusionModel.cy(), occlusionModel.imageWidth(), occlusionModel.imageHeight());
1330  }
1331  }
1332 #ifdef DEBUG_RENDERING_PERFORMANCE
1333  LOGW("Update background and occlusion mesh %fs", time.ticks());
1334 #endif
1335  }
1336  }
1337 
1338  // process only pose events in visualization mode
1339  rtabmap::Transform pose;
1340  {
1341  boost::mutex::scoped_lock lock(poseMutex_);
1342  if(poseEvents_.size())
1343  {
1344  pose = poseEvents_.back();
1345  poseEvents_.clear();
1346  }
1347  }
1348 
1349  rtabmap::SensorEvent sensorEvent;
1350  {
1351  boost::mutex::scoped_lock lock(sensorMutex_);
1352  if(sensorEvents_.size())
1353  {
1354  LOGI("Process sensor events");
1355  sensorEvent = sensorEvents_.back();
1356  sensorEvents_.clear();
1357  if(cameraJustInitialized_)
1358  {
1359  notifyCameraStarted = true;
1360  cameraJustInitialized_ = false;
1361  }
1362  }
1363  }
1364 
1365  if(!pose.isNull())
1366  {
1367  // update camera pose?
1368  if(graphOptimization_ && !mapToOdom_.isIdentity())
1369  {
1370  main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*pose*rtabmap::optical_T_opengl);
1371  }
1372  else
1373  {
1374  main_scene_.SetCameraPose(rtabmap::opengl_world_T_rtabmap_world*pose*rtabmap::optical_T_opengl);
1375  }
1376  if(sensorCaptureThread_!=0 && cameraJustInitialized_)
1377  {
1378  notifyCameraStarted = true;
1379  cameraJustInitialized_ = false;
1380  }
1381  lastPoseEventTime_ = UTimer::now();
1382  }
1383 
1384  if(visualizingMesh_)
1385  {
1386  if(exportedMeshUpdated_)
1387  {
1388  main_scene_.clear();
1389  exportedMeshUpdated_ = false;
1390  }
1391  if(!main_scene_.hasCloud(g_optMeshId))
1392  {
1393  LOGI("Adding optimized mesh to opengl (%d points, %d polygons, %d tex_coords, materials=%d texture=%dx%d)...",
1394  optMesh_->cloud.point_step==0?0:(int)optMesh_->cloud.data.size()/optMesh_->cloud.point_step,
1395  optMesh_->tex_polygons.size()!=1?0:(int)optMesh_->tex_polygons[0].size(),
1396  optMesh_->tex_coordinates.size()!=1?0:(int)optMesh_->tex_coordinates[0].size(),
1397  (int)optMesh_->tex_materials.size(),
1398  optTexture_.cols, optTexture_.rows);
1399  if(optMesh_->tex_polygons.size() && optMesh_->tex_polygons[0].size())
1400  {
1401  rtabmap::Mesh mesh;
1402  mesh.gains[0] = mesh.gains[1] = mesh.gains[2] = 1.0;
1403  mesh.cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
1404  mesh.normals.reset(new pcl::PointCloud<pcl::Normal>);
1405  pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.cloud);
1406  pcl::fromPCLPointCloud2(optMesh_->cloud, *mesh.normals);
1407  mesh.polygons = optMesh_->tex_polygons[0];
1408  mesh.pose.setIdentity();
1409  if(optMesh_->tex_coordinates.size())
1410  {
1411  mesh.texCoords = optMesh_->tex_coordinates[0];
1412  mesh.texture = optTexture_;
1413  }
1414  main_scene_.addMesh(g_optMeshId, mesh, rtabmap::opengl_world_T_rtabmap_world, true);
1415  }
1416  else
1417  {
1418  pcl::IndicesPtr indices(new std::vector<int>); // null
1419  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
1420  pcl::fromPCLPointCloud2(optMesh_->cloud, *cloud);
1421  main_scene_.addCloud(g_optMeshId, cloud, indices, rtabmap::opengl_world_T_rtabmap_world);
1422  }
1423  }
1424 
1425  if(!openingDatabase_)
1426  {
1427  rtabmapMutex_.lock();
1428  rtabmapEvents = rtabmapEvents_;
1429  rtabmapEvents_.clear();
1430  rtabmapMutex_.unlock();
1431 
1432  if(rtabmapEvents.size())
1433  {
1434  const rtabmap::Statistics & stats = rtabmapEvents.back()->getStats();
1435  if(!stats.mapCorrection().isNull())
1436  {
1437  mapToOdom_ = stats.mapCorrection();
1438  }
1439 
1440  std::map<int, rtabmap::Transform>::const_iterator iter = stats.poses().find(optRefId_);
1441  if(iter != stats.poses().end() && !iter->second.isNull() && optRefPose_)
1442  {
1443  // adjust opt mesh pose
1444  main_scene_.setCloudPose(g_optMeshId, rtabmap::opengl_world_T_rtabmap_world * iter->second * (*optRefPose_).inverse());
1445  }
1446  int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
1447  int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
1448  int proximityClosureId = int(uValue(stats.data(), rtabmap::Statistics::kProximitySpace_last_detection_id(), 0.0f));
1449  int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
1450  int landmark = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
1451  if(rtabmapThread_ && rtabmapThread_->isRunning() && loopClosure>0)
1452  {
1453  main_scene_.setBackgroundColor(0, 0.5f, 0); // green
1454  }
1455  else if(rtabmapThread_ && rtabmapThread_->isRunning() && proximityClosureId>0)
1456  {
1457  main_scene_.setBackgroundColor(0.5f, 0.5f, 0); // yellow
1458  }
1459  else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
1460  {
1461  if(rejected)
1462  {
1463  main_scene_.setBackgroundColor(0.5, 0.325f, 0); // dark orange
1464  }
1465  else
1466  {
1467  main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
1468  }
1469  }
1470  else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
1471  {
1472  main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
1473  }
1474  else if(rtabmapThread_ && rtabmapThread_->isRunning() && fastMovement)
1475  {
1476  main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
1477  }
1478  else
1479  {
1480  main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
1481  }
1482 
1483  // Update markers
1484  for(std::map<int, rtabmap::Transform>::const_iterator iter=stats.poses().begin();
1485  iter!=stats.poses().end() && iter->first<0;
1486  ++iter)
1487  {
1488  int id = iter->first;
1489  if(main_scene_.hasMarker(id))
1490  {
1491  //just update pose
1492  main_scene_.setMarkerPose(id, rtabmap::opengl_world_T_rtabmap_world*iter->second);
1493  }
1494  else
1495  {
1496  main_scene_.addMarker(id, rtabmap::opengl_world_T_rtabmap_world*iter->second);
1497  }
1498  }
1499  }
1500  }
1501 
1502  //backup state
1503  bool isMeshRendering = main_scene_.isMeshRendering();
1504  bool isTextureRendering = main_scene_.isMeshTexturing();
1505 
1506  main_scene_.setMeshRendering(main_scene_.hasMesh(g_optMeshId), main_scene_.hasTexture(g_optMeshId));
1507 
1508  main_scene_.setFrustumVisible(camera_!=0);
1509  lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix);
1510  double fpsTime = fpsTime_.ticks();
1511  if(renderingTime_ < fpsTime)
1512  {
1513  renderingTime_ = fpsTime;
1514  }
1515 
1516  // revert state
1517  main_scene_.setMeshRendering(isMeshRendering, isTextureRendering);
1518 
1519  if(rtabmapEvents.size())
1520  {
1521  // send statistics to GUI
1522  if(rtabmapEvents.back()->getStats().refImageId()>0 ||
1523  !rtabmapEvents.back()->getStats().data().empty())
1524  {
1525  UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
1526  rtabmapEvents.pop_back();
1527  }
1528 
1529  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
1530  {
1531  delete *iter;
1532  }
1533  rtabmapEvents.clear();
1534  lastPostRenderEventTime_ = UTimer::now();
1535  }
1536  }
1537  else
1538  {
1539  if(main_scene_.hasCloud(g_optMeshId))
1540  {
1541  main_scene_.clear();
1542  optMesh_.reset(new pcl::TextureMesh);
1543  optTexture_ = cv::Mat();
1544  }
1545 
1546  // should be before clearSceneOnNextRender_ in case database is reset
1547  if(!openingDatabase_)
1548  {
1549  rtabmapMutex_.lock();
1550  rtabmapEvents = rtabmapEvents_;
1551  rtabmapEvents_.clear();
1552  rtabmapMutex_.unlock();
1553 
1554  if(!clearSceneOnNextRender_ && rtabmapEvents.size())
1555  {
1556  boost::mutex::scoped_lock lockMesh(meshesMutex_);
1557  if(createdMeshes_.size())
1558  {
1559  if(rtabmapEvents.front()->getStats().refImageId()>0 && rtabmapEvents.front()->getStats().refImageId() < createdMeshes_.rbegin()->first)
1560  {
1561  LOGI("Detected new database! new=%d old=%d", rtabmapEvents.front()->getStats().refImageId(), createdMeshes_.rbegin()->first);
1562  clearSceneOnNextRender_ = true;
1563  }
1564  }
1565  }
1566 #ifdef DEBUG_RENDERING_PERFORMANCE
1567  if(rtabmapEvents.size())
1568  {
1569  LOGW("begin and getting rtabmap events %fs", time.ticks());
1570  }
1571 #endif
1572  }
1573 
1574  if(clearSceneOnNextRender_)
1575  {
1576  LOGI("Clearing all rendering data...");
1577  sensorMutex_.lock();
1578  sensorEvents_.clear();
1579  sensorMutex_.unlock();
1580 
1581  poseMutex_.lock();
1582  poseEvents_.clear();
1583  poseMutex_.unlock();
1584 
1585  main_scene_.clear();
1586  clearSceneOnNextRender_ = false;
1587  if(!openingDatabase_)
1588  {
1589  boost::mutex::scoped_lock lock(meshesMutex_);
1590  LOGI("Clearing meshes...");
1591  createdMeshes_.clear();
1592  rawPoses_.clear();
1593  }
1594  else
1595  {
1596  notifyDataLoaded = true;
1597  }
1598  totalPoints_ = 0;
1599  totalPolygons_ = 0;
1600  lastDrawnCloudsCount_ = 0;
1601  renderingTime_ = 0.0f;
1602  lastPostRenderEventTime_ = 0.0;
1603  lastPoseEventTime_ = 0.0;
1604  bufferedStatsData_.clear();
1605  }
1606 
1607  // Did we lose OpenGL context? If so, recreate the context;
1608  std::set<int> added = main_scene_.getAddedClouds();
1609  added.erase(-1);
1610  if(!openingDatabase_)
1611  {
1612  boost::mutex::scoped_lock lock(meshesMutex_);
1613  unsigned int meshes = (unsigned int)createdMeshes_.size();
1614  if(added.size() != meshes)
1615  {
1616  LOGI("added (%d) != meshes (%d)", (int)added.size(), meshes);
1617  boost::mutex::scoped_lock lockRtabmap(rtabmapMutex_);
1618  UASSERT(rtabmap_!=0);
1619  for(std::map<int, rtabmap::Mesh>::iterator iter=createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
1620  {
1621  if(!main_scene_.hasCloud(iter->first) && !iter->second.pose.isNull())
1622  {
1623  LOGI("Re-add mesh %d to OpenGL context", iter->first);
1624  if(iter->second.cloud->isOrganized() && main_scene_.isMeshRendering() && iter->second.polygons.size() == 0)
1625  {
1626  iter->second.polygons = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
1627  iter->second.polygonsLowRes = rtabmap::util3d::organizedFastMesh(iter->second.cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
1628  }
1629 
1630  if(iter->second.cloud->isOrganized() && main_scene_.isMeshTexturing())
1631  {
1632  cv::Mat textureRaw;
1633  textureRaw = rtabmap::uncompressImage(rtabmap_->getMemory()->getImageCompressed(iter->first));
1634  if(!textureRaw.empty())
1635  {
1636  if(renderingTextureDecimation_ > 1)
1637  {
1638  cv::Size reducedSize(textureRaw.cols/renderingTextureDecimation_, textureRaw.rows/renderingTextureDecimation_);
1639  LOGD("resize image from %dx%d to %dx%d", textureRaw.cols, textureRaw.rows, reducedSize.width, reducedSize.height);
1640  cv::resize(textureRaw, iter->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
1641  }
1642  else
1643  {
1644  iter->second.texture = textureRaw;
1645  }
1646  }
1647  }
1648  main_scene_.addMesh(iter->first, iter->second, rtabmap::opengl_world_T_rtabmap_world*iter->second.pose, true);
1649  main_scene_.setCloudVisible(iter->first, iter->second.visible);
1650 
1651  iter->second.texture = cv::Mat(); // don't keep textures in memory
1652  }
1653  }
1654  }
1655  }
1656  else if(notifyDataLoaded)
1657  {
1658  rtabmapMutex_.lock();
1659  rtabmapEvents = rtabmapEvents_;
1660  rtabmapEvents_.clear();
1661  rtabmapMutex_.unlock();
1662  openingDatabase_ = false;
1663  }
1664 
1665  if(rtabmapEvents.size())
1666  {
1667 #ifdef DEBUG_RENDERING_PERFORMANCE
1668  LOGW("Process rtabmap events %fs", time.ticks());
1669 #else
1670  LOGI("Process rtabmap events");
1671 #endif
1672 
1673  // update buffered signatures
1674  std::map<int, rtabmap::SensorData> bufferedSensorData;
1675  if(!dataRecorderMode_)
1676  {
1677  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
1678  {
1679  const rtabmap::Statistics & stats = (*iter)->getStats();
1680 
1681  // Don't create mesh for the last node added if rehearsal happened or if discarded (small movement)
1682  int smallMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemorySmall_movement(), 0.0f);
1683  int fastMovement = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
1684  int rehearsalMerged = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_merged(), 0.0f);
1685  if(!localizationMode_ && stats.getLastSignatureData().id() > 0 &&
1686  smallMovement == 0 && rehearsalMerged == 0 && fastMovement == 0)
1687  {
1688  int id = stats.getLastSignatureData().id();
1689  const rtabmap::Signature & s = stats.getLastSignatureData();
1690 
1691  if(!trajectoryMode_ &&
1692  ((!s.sensorData().imageRaw().empty() && !s.sensorData().depthRaw().empty()) ||
1693  !s.sensorData().laserScanRaw().isEmpty()))
1694  {
1695  uInsert(bufferedSensorData, std::make_pair(id, s.sensorData()));
1696  }
1697 
1698  uInsert(rawPoses_, std::make_pair(id, s.getPose()));
1699  }
1700 
1701  int loopClosure = (int)uValue(stats.data(), rtabmap::Statistics::kLoopAccepted_hypothesis_id(), 0.0f);
1702  int proximityClosureId = int(uValue(stats.data(), rtabmap::Statistics::kProximitySpace_last_detection_id(), 0.0f));
1703  int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
1704  int landmark = (int)uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
1705  if(rtabmapThread_ && rtabmapThread_->isRunning() && loopClosure>0)
1706  {
1707  main_scene_.setBackgroundColor(0, 0.5f, 0); // green
1708  }
1709  else if(rtabmapThread_ && rtabmapThread_->isRunning() && proximityClosureId>0)
1710  {
1711  main_scene_.setBackgroundColor(0.5f, 0.5f, 0); // yellow
1712  }
1713  else if(rtabmapThread_ && rtabmapThread_->isRunning() && landmark!=0)
1714  {
1715  main_scene_.setBackgroundColor(1, 0.65f, 0); // orange
1716  }
1717  else if(rtabmapThread_ && rtabmapThread_->isRunning() && rejected>0)
1718  {
1719  main_scene_.setBackgroundColor(0, 0.2f, 0); // dark green
1720  }
1721  else if(rtabmapThread_ && rtabmapThread_->isRunning() && rehearsalMerged>0)
1722  {
1723  main_scene_.setBackgroundColor(0, 0, 0.2f); // blue
1724  }
1725  else if(rtabmapThread_ && rtabmapThread_->isRunning() && fastMovement)
1726  {
1727  main_scene_.setBackgroundColor(0.2f, 0, 0.2f); // dark magenta
1728  }
1729  else
1730  {
1731  main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
1732  }
1733  }
1734  }
1735 
1736 #ifdef DEBUG_RENDERING_PERFORMANCE
1737  LOGW("Looking for data to load (%d) %fs", (int)bufferedSensorData.size(), time.ticks());
1738 #endif
1739 
1740  std::map<int, rtabmap::Transform> posesWithMarkers = rtabmapEvents.back()->getStats().poses();
1741  if(!rtabmapEvents.back()->getStats().mapCorrection().isNull())
1742  {
1743  mapToOdom_ = rtabmapEvents.back()->getStats().mapCorrection();
1744  }
1745 
1746  // Transform pose in OpenGL world
1747  for(std::map<int, rtabmap::Transform>::iterator iter=posesWithMarkers.begin(); iter!=posesWithMarkers.end(); ++iter)
1748  {
1749  if(!graphOptimization_)
1750  {
1751  std::map<int, rtabmap::Transform>::iterator jter = rawPoses_.find(iter->first);
1752  if(jter != rawPoses_.end())
1753  {
1754  iter->second = rtabmap::opengl_world_T_rtabmap_world*jter->second;
1755  }
1756  }
1757  else
1758  {
1759  iter->second = rtabmap::opengl_world_T_rtabmap_world*iter->second;
1760  }
1761  }
1762 
1763  std::map<int, rtabmap::Transform> poses(posesWithMarkers.lower_bound(0), posesWithMarkers.end());
1764  const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back()->getStats().constraints();
1765  if(poses.size())
1766  {
1767  //update graph
1768  main_scene_.updateGraph(poses, links);
1769 
1770 #ifdef DEBUG_RENDERING_PERFORMANCE
1771  LOGW("Update graph: %fs", time.ticks());
1772 #endif
1773 
1774  // update clouds
1775  boost::mutex::scoped_lock lock(meshesMutex_);
1776  std::set<std::string> strIds;
1777  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
1778  {
1779  int id = iter->first;
1780  if(!iter->second.isNull())
1781  {
1782  if(main_scene_.hasCloud(id))
1783  {
1784  //just update pose
1785  main_scene_.setCloudPose(id, iter->second);
1786  main_scene_.setCloudVisible(id, true);
1787  std::map<int, rtabmap::Mesh>::iterator meshIter = createdMeshes_.find(id);
1788  UASSERT(meshIter!=createdMeshes_.end());
1789  meshIter->second.pose = rtabmap::opengl_world_T_rtabmap_world.inverse()*iter->second;
1790  meshIter->second.visible = true;
1791  }
1792  else
1793  {
1794  if(createdMeshes_.find(id) == createdMeshes_.end() &&
1795  bufferedSensorData.find(id) != bufferedSensorData.end())
1796  {
1797  rtabmap::SensorData data = bufferedSensorData.at(id);
1798 
1799  cv::Mat tmpA, depth;
1800  data.uncompressData(&tmpA, &depth);
1801  if(!(!data.imageRaw().empty() && !data.depthRaw().empty()) && !data.laserScanCompressed().isEmpty())
1802  {
1803  rtabmap::LaserScan scan;
1804  data.uncompressData(0, 0, &scan);
1805  }
1806 #ifdef DEBUG_RENDERING_PERFORMANCE
1807  LOGW("Decompressing data: %fs", time.ticks());
1808 #endif
1809 
1810  if((!data.imageRaw().empty() && !data.depthRaw().empty()) || !data.laserScanRaw().isEmpty())
1811  {
1812  // Voxelize and filter depending on the previous cloud?
1813  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
1814  pcl::IndicesPtr indices(new std::vector<int>);
1815  if(!data.imageRaw().empty() && !data.depthRaw().empty() && (!useExternalLidar_ || data.laserScanRaw().isEmpty()))
1816  {
1817  int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
1818  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
1819  }
1820  else
1821  {
1822  //scan
1823  cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
1824  indices->resize(cloud->size());
1825  for(unsigned int i=0; i<cloud->size(); ++i)
1826  {
1827  indices->at(i) = i;
1828  }
1829  }
1830 #ifdef DEBUG_RENDERING_PERFORMANCE
1831  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());
1832 #endif
1833  if(cloud->size() && indices->size())
1834  {
1835  std::vector<pcl::Vertices> polygons;
1836  std::vector<pcl::Vertices> polygonsLowRes;
1837 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
1838  std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > texCoords;
1839 #else
1840  std::vector<Eigen::Vector2f> texCoords;
1841 #endif
1842  pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
1843  if(cloud->isOrganized() && main_scene_.isMeshRendering() && main_scene_.isMapRendering())
1844  {
1845  polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
1846 #ifdef DEBUG_RENDERING_PERFORMANCE
1847  LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
1848 #endif
1849 #ifndef DISABLE_VTK
1850  if(meshDecimationFactor_ > 0.0f && !polygons.empty())
1851  {
1852  pcl::PolygonMesh::Ptr tmpMesh(new pcl::PolygonMesh);
1853  pcl::toPCLPointCloud2(*cloud, tmpMesh->cloud);
1854  tmpMesh->polygons = polygons;
1855  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGB>(tmpMesh, meshDecimationFactor_, 0, cloud, 0);
1856 
1857  if(!tmpMesh->polygons.empty())
1858  {
1859  if(main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
1860  {
1861  std::map<int, rtabmap::Transform> cameraPoses;
1862  std::map<int, rtabmap::CameraModel> cameraModels;
1863  cameraPoses.insert(std::make_pair(0, rtabmap::Transform::getIdentity()));
1864  cameraModels.insert(std::make_pair(0, data.cameraModels()[0]));
1865  pcl::TextureMesh::Ptr textureMesh = rtabmap::util3d::createTextureMesh(
1866  tmpMesh,
1867  cameraPoses,
1868  cameraModels,
1869  std::map<int, cv::Mat>());
1870  pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
1871  polygons = textureMesh->tex_polygons[0];
1872  texCoords = textureMesh->tex_coordinates[0];
1873  }
1874  else
1875  {
1876  pcl::fromPCLPointCloud2(tmpMesh->cloud, *cloud);
1877  polygons = tmpMesh->polygons;
1878  }
1879 
1880  indices->resize(cloud->size());
1881  for(unsigned int i=0; i<cloud->size(); ++i)
1882  {
1883  indices->at(i) = i;
1884  }
1885  }
1886  else
1887  {
1888  LOGE("Mesh decimation factor is too high (%f), returning full mesh (id=%d).", meshDecimationFactor_, data.id());
1889  polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
1890 #ifdef DEBUG_RENDERING_PERFORMANCE
1891  LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
1892 #endif
1893  }
1894 #ifdef DEBUG_RENDERING_PERFORMANCE
1895  LOGW("Mesh simplication, %d polygons, %d points (%fs)", (int)polygons.size(), (int)cloud->size(), time.ticks());
1896 #endif
1897  }
1898  else
1899 #endif
1900  {
1901  polygonsLowRes = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_+LOW_RES_PIX);
1902 #ifdef DEBUG_RENDERING_PERFORMANCE
1903  LOGW("Creating mesh, %d polygons (%fs)", (int)polygons.size(), time.ticks());
1904 #endif
1905  }
1906  }
1907 
1908  std::pair<std::map<int, rtabmap::Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, rtabmap::Mesh()));
1909  UASSERT(inserted.second);
1910  inserted.first->second.cloud = cloud;
1911  inserted.first->second.indices = indices;
1912  inserted.first->second.polygons = polygons;
1913  inserted.first->second.polygonsLowRes = polygonsLowRes;
1914  inserted.first->second.visible = true;
1915  inserted.first->second.cameraModel = data.cameraModels()[0];
1916  inserted.first->second.gains[0] = 1.0;
1917  inserted.first->second.gains[1] = 1.0;
1918  inserted.first->second.gains[2] = 1.0;
1919  if((cloud->isOrganized() || !texCoords.empty()) && main_scene_.isMeshTexturing() && main_scene_.isMapRendering())
1920  {
1921  inserted.first->second.texCoords = texCoords;
1922  if(renderingTextureDecimation_ > 1)
1923  {
1924  cv::Size reducedSize(data.imageRaw().cols/renderingTextureDecimation_, data.imageRaw().rows/renderingTextureDecimation_);
1925  cv::resize(data.imageRaw(), inserted.first->second.texture, reducedSize, 0, 0, cv::INTER_LINEAR);
1926 #ifdef DEBUG_RENDERING_PERFORMANCE
1927  LOGW("resize image from %dx%d to %dx%d (%fs)", data.imageRaw().cols, data.imageRaw().rows, reducedSize.width, reducedSize.height, time.ticks());
1928 #endif
1929  }
1930  else
1931  {
1932  inserted.first->second.texture = data.imageRaw();
1933  }
1934  }
1935  }
1936  }
1937  }
1938  if(createdMeshes_.find(id) != createdMeshes_.end())
1939  {
1940  rtabmap::Mesh & mesh = createdMeshes_.at(id);
1941  totalPoints_+=mesh.indices->size();
1942  totalPolygons_ += mesh.polygons.size();
1943  mesh.pose = rtabmap::opengl_world_T_rtabmap_world.inverse()*iter->second;
1944  main_scene_.addMesh(id, mesh, iter->second, true);
1945 #ifdef DEBUG_RENDERING_PERFORMANCE
1946  LOGW("Adding mesh to scene: %fs", time.ticks());
1947 #endif
1948  mesh.texture = cv::Mat(); // don't keep textures in memory
1949  }
1950  }
1951  }
1952  }
1953  }
1954 
1955  //filter poses?
1956  if(poses.size() > 2)
1957  {
1958  if(nodesFiltering_)
1959  {
1960  for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
1961  {
1962  if(iter->second.type() != rtabmap::Link::kNeighbor)
1963  {
1964  int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
1965  poses.erase(oldId);
1966  }
1967  }
1968  }
1969  }
1970 
1971  if(!poses.empty())
1972  {
1973  //update cloud visibility
1974  boost::mutex::scoped_lock lock(meshesMutex_);
1975  std::set<int> addedClouds = main_scene_.getAddedClouds();
1976  for(std::set<int>::const_iterator iter=addedClouds.begin();
1977  iter!=addedClouds.end();
1978  ++iter)
1979  {
1980  if(*iter > 0 && poses.find(*iter) == poses.end())
1981  {
1982  main_scene_.setCloudVisible(*iter, false);
1983  std::map<int, rtabmap::Mesh>::iterator meshIter = createdMeshes_.find(*iter);
1984  UASSERT(meshIter!=createdMeshes_.end());
1985  meshIter->second.visible = false;
1986  }
1987  }
1988  }
1989 
1990  // Update markers
1991  std::set<int> addedMarkers = main_scene_.getAddedMarkers();
1992  for(std::set<int>::const_iterator iter=addedMarkers.begin();
1993  iter!=addedMarkers.end();
1994  ++iter)
1995  {
1996  if(posesWithMarkers.find(*iter) == posesWithMarkers.end())
1997  {
1998  main_scene_.removeMarker(*iter);
1999  }
2000  }
2001  for(std::map<int, rtabmap::Transform>::const_iterator iter=posesWithMarkers.begin();
2002  iter!=posesWithMarkers.end() && iter->first<0;
2003  ++iter)
2004  {
2005  int id = iter->first;
2006  if(main_scene_.hasMarker(id))
2007  {
2008  //just update pose
2009  main_scene_.setMarkerPose(id, iter->second);
2010  }
2011  else
2012  {
2013  main_scene_.addMarker(id, iter->second);
2014  }
2015  }
2016  }
2017  else
2018  {
2019  main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_ && sensorCaptureThread_!=0);
2020 
2021  //just process the last one
2022  if(!sensorEvent.info().odomPose.isNull())
2023  {
2024  if(odomCloudShown_ && !trajectoryMode_)
2025  {
2026  if((!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty()) || !sensorEvent.data().laserScanRaw().isEmpty())
2027  {
2028  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
2029  pcl::IndicesPtr indices(new std::vector<int>);
2030  if(!sensorEvent.data().imageRaw().empty() && !sensorEvent.data().depthRaw().empty() && (!useExternalLidar_ || sensorEvent.data().laserScanRaw().isEmpty()))
2031  {
2032  int meshDecimation = updateMeshDecimation(sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows);
2033  cloud = rtabmap::util3d::cloudRGBFromSensorData(sensorEvent.data(), meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
2034  }
2035  else
2036  {
2037  //scan
2038  cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(sensorEvent.data().laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), sensorEvent.data().laserScanRaw().localTransform(), 255, 255, 255);
2039  indices->resize(cloud->size());
2040  for(unsigned int i=0; i<cloud->size(); ++i)
2041  {
2042  indices->at(i) = i;
2043  }
2044  }
2045 
2046  if(cloud->size() && indices->size())
2047  {
2048  LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
2049  sensorEvent.data().imageRaw().cols, sensorEvent.data().imageRaw().rows,
2050  sensorEvent.data().depthRaw().cols, sensorEvent.data().depthRaw().rows,
2051  (int)cloud->width, (int)cloud->height);
2052  main_scene_.addCloud(-1, cloud, indices, rtabmap::opengl_world_T_rtabmap_world*mapToOdom_*sensorEvent.info().odomPose);
2053  main_scene_.setCloudVisible(-1, true);
2054  }
2055  else
2056  {
2057  UERROR("Generated cloud is empty!");
2058  }
2059  }
2060  else
2061  {
2062  UWARN("Odom data images/scans are empty!");
2063  }
2064  }
2065  }
2066  }
2067 
2068  if(gainCompensationOnNextRender_>0)
2069  {
2070  gainCompensation(gainCompensationOnNextRender_==2);
2071  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
2072  {
2073  main_scene_.updateGains(iter->first, iter->second.gains[0], iter->second.gains[1], iter->second.gains[2]);
2074  }
2075  gainCompensationOnNextRender_ = 0;
2076  notifyDataLoaded = true;
2077  }
2078 
2079  if(bilateralFilteringOnNextRender_)
2080  {
2081  LOGI("Bilateral filtering...");
2082  bilateralFilteringOnNextRender_ = false;
2083  boost::mutex::scoped_lock lock(meshesMutex_);
2084  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
2085  {
2086  if(iter->second.cloud->size() && iter->second.indices->size())
2087  {
2088  if(smoothMesh(iter->first, iter->second))
2089  {
2090  main_scene_.updateMesh(iter->first, iter->second);
2091  }
2092  }
2093  }
2094  notifyDataLoaded = true;
2095  }
2096 
2097  if(filterPolygonsOnNextRender_ && clusterRatio_>0.0f)
2098  {
2099  LOGI("Polygon filtering...");
2100  filterPolygonsOnNextRender_ = false;
2101  boost::mutex::scoped_lock lock(meshesMutex_);
2102  UTimer time;
2103  for(std::map<int, rtabmap::Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
2104  {
2105  if(iter->second.polygons.size())
2106  {
2107  // filter polygons
2108  iter->second.polygons = filterOrganizedPolygons(iter->second.polygons, iter->second.cloud->size());
2109  main_scene_.updateCloudPolygons(iter->first, iter->second.polygons);
2110  }
2111  }
2112  notifyDataLoaded = true;
2113  }
2114 
2115  main_scene_.setFrustumVisible(camera_!=0);
2116  lastDrawnCloudsCount_ = main_scene_.Render(uvsTransformed, arViewMatrix, arProjectionMatrix, occlusionMesh, true);
2117  double fpsTime = fpsTime_.ticks();
2118  if(renderingTime_ < fpsTime)
2119  {
2120  renderingTime_ = fpsTime;
2121  }
2122 
2123  if(rtabmapEvents.size())
2124  {
2125  // send statistics to GUI
2126  LOGI("New data added to map, rendering time: %fs", renderingTime_);
2127  if(rtabmapEvents.back()->getStats().refImageId()>0 ||
2128  !rtabmapEvents.back()->getStats().data().empty())
2129  {
2130  UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
2131  rtabmapEvents.pop_back();
2132  }
2133 
2134  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
2135  {
2136  delete *iter;
2137  }
2138  rtabmapEvents.clear();
2139 
2140  lastPostRenderEventTime_ = UTimer::now();
2141 
2142  if(sensorCaptureThread_!=0 && lastPoseEventTime_>0.0 && UTimer::now()-lastPoseEventTime_ > 1.0)
2143  {
2144  UERROR("TangoPoseEventNotReceived");
2145  UEventsManager::post(new rtabmap::CameraInfoEvent(10, "TangoPoseEventNotReceived", uNumber2Str(UTimer::now()-lastPoseEventTime_, 6)));
2146  }
2147  }
2148  }
2149 
2150  if(takeScreenshotOnNextRender_)
2151  {
2152  takeScreenshotOnNextRender_ = false;
2153  int w = main_scene_.getViewPortWidth();
2154  int h = main_scene_.getViewPortHeight();
2155  cv::Mat image(h, w, CV_8UC4);
2156  glReadPixels(0, 0, w, h, GL_RGBA, GL_UNSIGNED_BYTE, image.data);
2157  cv::flip(image, image, 0);
2158  cv::cvtColor(image, image, cv::COLOR_RGBA2BGRA);
2159  cv::Mat roi;
2160  if(w>h)
2161  {
2162  int offset = (w-h)/2;
2163  roi = image(cv::Range::all(), cv::Range(offset,offset+h));
2164  }
2165  else
2166  {
2167  int offset = (h-w)/2;
2168  roi = image(cv::Range(offset,offset+w), cv::Range::all());
2169  }
2170  rtabmapMutex_.lock();
2171  LOGI("Saving screenshot %dx%d...", roi.cols, roi.rows);
2172  rtabmap_->getMemory()->savePreviewImage(roi);
2173  rtabmapMutex_.unlock();
2174  screenshotReady_.release();
2175  }
2176 
2177  if((rtabmapThread_==0 || !rtabmapThread_->isRunning()) && lastPostRenderEventTime_ > 0.0)
2178  {
2179  double interval = UTimer::now() - lastPostRenderEventTime_;
2180  double updateInterval = 1.0;
2181  if(!openingDatabase_ && rtabmapThread_)
2182  {
2183  boost::mutex::scoped_lock lock(rtabmapMutex_);
2184  if(rtabmapThread_ && rtabmapThread_->getDetectorRate()>0.0f)
2185  {
2186  updateInterval = 1.0f/rtabmapThread_->getDetectorRate();
2187  }
2188  }
2189 
2190  if(interval >= updateInterval)
2191  {
2192  if(!openingDatabase_)
2193  {
2194  // don't send event when we are opening the database (init events already sent)
2195  UEventsManager::post(new PostRenderEvent());
2196  }
2197  lastPostRenderEventTime_ = UTimer::now();
2198  }
2199  }
2200 
2201  return notifyDataLoaded||notifyCameraStarted?1:0;
2202  }
2203  catch(const UException & e)
2204  {
2205  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
2206  {
2207  delete *iter;
2208  }
2209  rtabmapEvents.clear();
2210  UERROR("Exception! msg=\"%s\"", e.what());
2211  return -2;
2212  }
2213  catch(const cv::Exception & e)
2214  {
2215  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
2216  {
2217  delete *iter;
2218  }
2219  rtabmapEvents.clear();
2220  UERROR("Exception! msg=\"%s\"", e.what());
2221  return -1;
2222  }
2223  catch(const std::exception & e)
2224  {
2225  for(std::list<rtabmap::RtabmapEvent*>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
2226  {
2227  delete *iter;
2228  }
2229  rtabmapEvents.clear();
2230  UERROR("Exception! msg=\"%s\"", e.what());
2231  return -2;
2232  }
2233 }
2234 
2235 void RTABMapApp::SetCameraType(
2236  tango_gl::GestureCamera::CameraType camera_type) {
2237  main_scene_.SetCameraType(camera_type);
2238 }
2239 
2240 void RTABMapApp::OnTouchEvent(int touch_count,
2241  tango_gl::GestureCamera::TouchEvent event,
2242  float x0, float y0, float x1, float y1) {
2243  main_scene_.OnTouchEvent(touch_count, event, x0, y0, x1, y1);
2244 }
2245 
2246 void RTABMapApp::setPausedMapping(bool paused)
2247 {
2248  {
2249  boost::mutex::scoped_lock lock(renderingMutex_);
2250  main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
2251  }
2252 
2253  if(rtabmapThread_)
2254  {
2255  if(rtabmapThread_->isRunning() && paused)
2256  {
2257  LOGW("Pause!");
2258  rtabmapThread_->unregisterFromEventsManager();
2259  rtabmapThread_->join(true);
2260  }
2261  else if(!rtabmapThread_->isRunning() && !paused)
2262  {
2263  LOGW("Resume!");
2264  rtabmap_->triggerNewMap();
2265  rtabmap_->parseParameters(getRtabmapParameters());
2266  rtabmapThread_->registerToEventsManager();
2267  rtabmapThread_->start();
2268  }
2269  }
2270 }
2271 void RTABMapApp::setOnlineBlending(bool enabled)
2272 {
2273  main_scene_.setBlending(enabled);
2274 }
2275 void RTABMapApp::setMapCloudShown(bool shown)
2276 {
2277  main_scene_.setMapRendering(shown);
2278 }
2279 void RTABMapApp::setOdomCloudShown(bool shown)
2280 {
2281  odomCloudShown_ = shown;
2282  main_scene_.setTraceVisible(shown);
2283 }
2284 void RTABMapApp::setMeshRendering(bool enabled, bool withTexture)
2285 {
2286  main_scene_.setMeshRendering(enabled, withTexture);
2287 }
2288 void RTABMapApp::setPointSize(float value)
2289 {
2290  main_scene_.setPointSize(value);
2291 }
2292 void RTABMapApp::setFOV(float angle)
2293 {
2294  main_scene_.setFOV(angle);
2295 }
2296 void RTABMapApp::setOrthoCropFactor(float value)
2297 {
2298  main_scene_.setOrthoCropFactor(value);
2299 }
2300 void RTABMapApp::setGridRotation(float value)
2301 {
2302  main_scene_.setGridRotation(value);
2303 }
2304 void RTABMapApp::setLighting(bool enabled)
2305 {
2306  main_scene_.setLighting(enabled);
2307 }
2308 void RTABMapApp::setBackfaceCulling(bool enabled)
2309 {
2310  main_scene_.setBackfaceCulling(enabled);
2311 }
2312 void RTABMapApp::setWireframe(bool enabled)
2313 {
2314  main_scene_.setWireframe(enabled);
2315 }
2316 
2317 void RTABMapApp::setLocalizationMode(bool enabled)
2318 {
2319  localizationMode_ = enabled;
2320  rtabmap::ParametersMap parameters;
2321  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
2322  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
2323  this->post(new rtabmap::ParamEvent(parameters));
2324 }
2325 void RTABMapApp::setTrajectoryMode(bool enabled)
2326 {
2327  trajectoryMode_ = enabled;
2328  this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
2329 }
2330 
2331 void RTABMapApp::setGraphOptimization(bool enabled)
2332 {
2333  graphOptimization_ = enabled;
2334  if((sensorCaptureThread_ == 0) && rtabmap_ && rtabmap_->getMemory()->getLastWorkingSignature()!=0)
2335  {
2336  std::map<int, rtabmap::Transform> poses;
2337  std::multimap<int, rtabmap::Link> links;
2338  rtabmap_->getGraph(poses, links, true, true);
2339  if(poses.size())
2340  {
2341  boost::mutex::scoped_lock lock(rtabmapMutex_);
2342  rtabmap::Statistics stats = rtabmap_->getStatistics();
2343  stats.setPoses(poses);
2344  stats.setConstraints(links);
2345 
2346  LOGI("Send rtabmap event to update graph...");
2347  rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
2348 
2349  rtabmap_->setOptimizedPoses(poses, links);
2350  }
2351  }
2352 }
2353 void RTABMapApp::setNodesFiltering(bool enabled)
2354 {
2355  nodesFiltering_ = enabled;
2356  setGraphOptimization(graphOptimization_); // this will resend the graph if paused
2357 }
2358 void RTABMapApp::setGraphVisible(bool visible)
2359 {
2360  main_scene_.setGraphVisible(visible);
2361  main_scene_.setTraceVisible(visible);
2362  setGraphOptimization(graphOptimization_); // this will republish the graph
2363 }
2364 void RTABMapApp::setGridVisible(bool visible)
2365 {
2366  main_scene_.setGridVisible(visible);
2367 }
2368 
2369 void RTABMapApp::setRawScanSaved(bool enabled)
2370 {
2371  if(rawScanSaved_ != enabled)
2372  {
2373  rawScanSaved_ = enabled;
2374  }
2375 }
2376 
2377 void RTABMapApp::setCameraColor(bool enabled)
2378 {
2379  if(cameraColor_ != enabled)
2380  {
2381  cameraColor_ = enabled;
2382  }
2383 }
2384 
2385 void RTABMapApp::setFullResolution(bool enabled)
2386 {
2387  if(fullResolution_ != enabled)
2388  {
2389  fullResolution_ = enabled;
2390  }
2391 }
2392 
2393 void RTABMapApp::setSmoothing(bool enabled)
2394 {
2395  if(smoothing_ != enabled)
2396  {
2397  smoothing_ = enabled;
2398  }
2399 }
2400 
2401 void RTABMapApp::setDepthFromMotion(bool enabled)
2402 {
2403  if(depthFromMotion_ != enabled)
2404  {
2405  depthFromMotion_ = enabled;
2406  }
2407 }
2408 
2409 void RTABMapApp::setAppendMode(bool enabled)
2410 {
2411  if(appendMode_ != enabled)
2412  {
2413  appendMode_ = enabled;
2414  rtabmap::ParametersMap parameters;
2415  parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(!localizationMode_ && appendMode_)));
2416  this->post(new rtabmap::ParamEvent(parameters));
2417  }
2418 }
2419 
2420 void RTABMapApp::setDataRecorderMode(bool enabled)
2421 {
2422  if(dataRecorderMode_ != enabled)
2423  {
2424  dataRecorderMode_ = enabled; // parameters will be set when resuming (we assume we are paused)
2425  if(localizationMode_ && enabled)
2426  {
2427  localizationMode_ = false;
2428  }
2429  }
2430 }
2431 
2432 void RTABMapApp::setMaxCloudDepth(float value)
2433 {
2434  maxCloudDepth_ = value;
2435 }
2436 
2437 void RTABMapApp::setMinCloudDepth(float value)
2438 {
2439  minCloudDepth_ = value;
2440 }
2441 
2442 void RTABMapApp::setCloudDensityLevel(int value)
2443 {
2444  cloudDensityLevel_ = value;
2445 }
2446 
2447 void RTABMapApp::setMeshAngleTolerance(float value)
2448 {
2449  meshAngleToleranceDeg_ = value;
2450 }
2451 
2452 void RTABMapApp::setMeshDecimationFactor(float value)
2453 {
2454  meshDecimationFactor_ = value;
2455 }
2456 
2457 void RTABMapApp::setMeshTriangleSize(int value)
2458 {
2459  meshTrianglePix_ = value;
2460 }
2461 
2462 void RTABMapApp::setClusterRatio(float value)
2463 {
2464  clusterRatio_ = value;
2465 }
2466 
2467 void RTABMapApp::setMaxGainRadius(float value)
2468 {
2469  maxGainRadius_ = value;
2470 }
2471 
2472 void RTABMapApp::setRenderingTextureDecimation(int value)
2473 {
2474  UASSERT(value>=1);
2475  renderingTextureDecimation_ = value;
2476 }
2477 
2478 void RTABMapApp::setBackgroundColor(float gray)
2479 {
2480  backgroundColor_ = gray;
2481  float v = backgroundColor_ == 0.5f?0.4f:1.0f-backgroundColor_;
2482  main_scene_.setGridColor(v, v, v);
2483  main_scene_.setBackgroundColor(backgroundColor_, backgroundColor_, backgroundColor_);
2484 }
2485 
2486 void RTABMapApp::setDepthConfidence(int value)
2487 {
2488  depthConfidence_ = value;
2489  if(depthConfidence_>2)
2490  {
2491  depthConfidence_ = 2;
2492  }
2493 }
2494 
2495 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
2496 {
2497  std::string compatibleKey = key;
2498 
2499  // Backward compatibility
2500  std::map<std::string, std::pair<bool, std::string> >::const_iterator iter=rtabmap::Parameters::getRemovedParameters().find(key);
2501  if(iter != rtabmap::Parameters::getRemovedParameters().end())
2502  {
2503  if(iter->second.first)
2504  {
2505  // can be migrated
2506  compatibleKey = iter->second.second;
2507  LOGW("Parameter name changed: \"%s\" -> \"%s\". Please update the code accordingly. Value \"%s\" is still set to the new parameter name.",
2508  iter->first.c_str(), iter->second.second.c_str(), value.c_str());
2509  }
2510  else
2511  {
2512  if(iter->second.second.empty())
2513  {
2514  UERROR("Parameter \"%s\" doesn't exist anymore!",
2515  iter->first.c_str());
2516  }
2517  else
2518  {
2519  UERROR("Parameter \"%s\" doesn't exist anymore! You may look at this similar parameter: \"%s\"",
2520  iter->first.c_str(), iter->second.second.c_str());
2521  }
2522  }
2523  }
2524 
2525  if(rtabmap::Parameters::getDefaultParameters().find(compatibleKey) != rtabmap::Parameters::getDefaultParameters().end())
2526  {
2527  LOGI("%s", uFormat("Setting param \"%s\" to \"%s\"", compatibleKey.c_str(), value.c_str()).c_str());
2528  uInsert(mappingParameters_, rtabmap::ParametersPair(compatibleKey, value));
2529  UEventsManager::post(new rtabmap::ParamEvent(this->getRtabmapParameters()));
2530  return 0;
2531  }
2532  else
2533  {
2534  UERROR(uFormat("Key \"%s\" doesn't exist!", compatibleKey.c_str()).c_str());
2535  return -1;
2536  }
2537 }
2538 
2539 void RTABMapApp::setGPS(const rtabmap::GPS & gps)
2540 {
2541  boost::mutex::scoped_lock lock(cameraMutex_);
2542  if(camera_!=0)
2543  {
2544  camera_->setGPS(gps);
2545  }
2546 }
2547 
2548 void RTABMapApp::addEnvSensor(int type, float value)
2549 {
2550  boost::mutex::scoped_lock lock(cameraMutex_);
2551  if(camera_!=0)
2552  {
2553  camera_->addEnvSensor(type, value);
2554  }
2555 }
2556 
2557 void RTABMapApp::save(const std::string & databasePath)
2558 {
2559  LOGI("Saving database to %s", databasePath.c_str());
2560  rtabmapThread_->unregisterFromEventsManager();
2561  rtabmapThread_->join(true);
2562 
2563  LOGI("Taking screenshot...");
2564  takeScreenshotOnNextRender_ = true;
2565  if(!screenshotReady_.acquire(1, 2000))
2566  {
2567  UERROR("Failed to take a screenshot after 2 sec!");
2568  }
2569 
2570  // save mapping parameters in the database
2571  bool appendModeBackup = appendMode_;
2572  if(appendMode_)
2573  {
2574  appendMode_ = false;
2575  }
2576 
2577  bool dataRecorderModeBackup = dataRecorderMode_;
2578  if(dataRecorderMode_)
2579  {
2580  dataRecorderMode_ = false;
2581  }
2582 
2583  bool localizationModeBackup = localizationMode_;
2584  if(localizationMode_)
2585  {
2586  localizationMode_ = false;
2587  }
2588 
2589  if(appendModeBackup || dataRecorderModeBackup || localizationModeBackup)
2590  {
2591  rtabmap::ParametersMap parameters = getRtabmapParameters();
2592  rtabmap_->parseParameters(parameters);
2593  appendMode_ = appendModeBackup;
2594  dataRecorderMode_ = dataRecorderModeBackup;
2595  localizationMode_ = localizationModeBackup;
2596  }
2597 
2598  std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
2599  std::multimap<int, rtabmap::Link> links = rtabmap_->getLocalConstraints();
2600  rtabmap_->close(true, databasePath);
2601  rtabmap_->init(getRtabmapParameters(), dataRecorderMode_?"":databasePath);
2602  rtabmap_->setOptimizedPoses(poses, links);
2603  if(dataRecorderMode_)
2604  {
2605  clearSceneOnNextRender_ = true;
2606  }
2607 }
2608 
2609 bool RTABMapApp::recover(const std::string & from, const std::string & to)
2610 {
2611  std::string errorMsg;
2612  if(!databaseRecovery(from, false, &errorMsg, &progressionStatus_))
2613  {
2614  LOGE("Recovery Error: %s", errorMsg.c_str());
2615  return false;
2616  }
2617  else
2618  {
2619  LOGI("Renaming %s to %s", from.c_str(), to.c_str());
2620  if(UFile::rename(from, to) != 0)
2621  {
2622  LOGE("Failed renaming %s to %s", from.c_str(), to.c_str());
2623  return false;
2624  }
2625  return true;
2626  }
2627 }
2628 
2629 void RTABMapApp::cancelProcessing()
2630 {
2631  UWARN("Processing canceled!");
2632  progressionStatus_.setCanceled(true);
2633 }
2634 
2635 bool RTABMapApp::exportMesh(
2636  float cloudVoxelSize,
2637  bool regenerateCloud,
2638  bool meshing,
2639  int textureSize,
2640  int textureCount,
2641  int normalK,
2642  bool optimized,
2643  float optimizedVoxelSize,
2644  int optimizedDepth,
2645  int optimizedMaxPolygons,
2646  float optimizedColorRadius,
2647  bool optimizedCleanWhitePolygons,
2648  int optimizedMinClusterSize,
2649  float optimizedMaxTextureDistance,
2650  int optimizedMinTextureClusterSize,
2651  bool blockRendering)
2652 {
2653  // make sure createdMeshes_ is not modified while exporting! We don't
2654  // lock the meshesMutex_ because we want to continue rendering.
2655 
2656  std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
2657  if(poses.empty())
2658  {
2659  // look if we just triggered new map without localizing afterward (pause/resume in append Mode)
2660  std::multimap<int, rtabmap::Link> links;
2661  rtabmap_->getGraph(
2662  poses,
2663  links,
2664  true,
2665  false);
2666  if(poses.empty())
2667  {
2668  UERROR("Empty optimized poses!");
2669  return false;
2670  }
2671  rtabmap_->setOptimizedPoses(poses, links);
2672  }
2673 
2674  if(blockRendering)
2675  {
2676  renderingMutex_.lock();
2677  main_scene_.clear();
2678  }
2679 
2680  exporting_ = true;
2681 
2682  bool success = false;
2683 
2684  try
2685  {
2686  int totalSteps = 0;
2687  totalSteps+=poses.size(); // assemble
2688  if(meshing)
2689  {
2690  if(optimized)
2691  {
2692  totalSteps += poses.size(); // meshing
2693  if(textureSize > 0)
2694  {
2695  totalSteps += 1; // gain
2696  totalSteps += 1; // blending
2697 
2698  if(optimizedMaxPolygons > 0)
2699  {
2700  totalSteps += 1; // decimation
2701  }
2702  }
2703 
2704  totalSteps += 1; // texture/coloring
2705 
2706  if(textureSize > 0)
2707  {
2708  totalSteps+=poses.size()+1; // texture cameras + apply polygons
2709  }
2710  }
2711  if(textureSize>0)
2712  {
2713  totalSteps += poses.size()+1; // uncompress and merge textures
2714  }
2715  }
2716  totalSteps += 1; // save file
2717 
2718  progressionStatus_.reset(totalSteps);
2719 
2720  //Assemble the meshes
2721  if(meshing) // Mesh or Texture Mesh
2722  {
2723  pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
2724  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
2725  std::vector<std::map<int, pcl::PointXY> > vertexToPixels;
2726  cv::Mat globalTextures;
2727  int totalPolygons = 0;
2728  {
2729  if(optimized)
2730  {
2731  std::map<int, rtabmap::Transform> cameraPoses;
2732  std::map<int, rtabmap::CameraModel> cameraModels;
2733  std::map<int, cv::Mat> cameraDepths;
2734 
2735  UTimer timer;
2736  LOGI("Assemble clouds (%d)...", (int)poses.size());
2737 #ifndef DISABLE_LOG
2738  int cloudCount=0;
2739 #endif
2740  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2741  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
2742  iter!= poses.end();
2743  ++iter)
2744  {
2745  std::map<int, rtabmap::Mesh>::iterator jter = createdMeshes_.find(iter->first);
2746  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
2747  pcl::IndicesPtr indices(new std::vector<int>);
2748  rtabmap::CameraModel model;
2749  cv::Mat depth;
2750  float gains[3];
2751  gains[0] = gains[1] = gains[2] = 1.0f;
2752  if(jter != createdMeshes_.end() && (jter->second.polygons.empty() || meshDecimationFactor_ == 0.0f))
2753  {
2754  cloud = jter->second.cloud;
2755  indices = jter->second.indices;
2756  model = jter->second.cameraModel;
2757  gains[0] = jter->second.gains[0];
2758  gains[1] = jter->second.gains[1];
2759  gains[2] = jter->second.gains[2];
2760 
2761  rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
2762  data.uncompressData(0, &depth);
2763  }
2764  else
2765  {
2766  rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
2767  data.uncompressData();
2768  if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
2769  {
2770  int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
2771  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
2772  model = data.cameraModels()[0];
2773  depth = data.depthRaw();
2774  }
2775  }
2776  if(cloud->size() && indices->size() && model.isValidForProjection())
2777  {
2778  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
2779  if(optimizedVoxelSize > 0.0f)
2780  {
2781  transformedCloud = rtabmap::util3d::voxelize(cloud, indices, optimizedVoxelSize);
2782  transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
2783  }
2784  else
2785  {
2786  // it looks like that using only transformPointCloud with indices
2787  // flushes the colors, so we should extract points before... maybe a too old PCL version
2788  pcl::copyPointCloud(*cloud, *indices, *transformedCloud);
2789  transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
2790  }
2791 
2792  Eigen::Vector3f viewpoint( iter->second.x(), iter->second.y(), iter->second.z());
2793  pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(transformedCloud, normalK, 0.0f, viewpoint);
2794 
2795  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2796  pcl::concatenateFields(*transformedCloud, *normals, *cloudWithNormals);
2797 
2798  if(textureSize == 0 && (gains[0] != 1.0 || gains[1] != 1.0 || gains[2] != 1.0))
2799  {
2800  for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
2801  {
2802  pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
2803  pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
2804  pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
2805  pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
2806  }
2807  }
2808 
2809 
2810  if(mergedClouds->size() == 0)
2811  {
2812  *mergedClouds = *cloudWithNormals;
2813  }
2814  else
2815  {
2816  *mergedClouds += *cloudWithNormals;
2817  }
2818 
2819  cameraPoses.insert(std::make_pair(iter->first, iter->second));
2820  cameraModels.insert(std::make_pair(iter->first, model));
2821  if(!depth.empty())
2822  {
2823  cameraDepths.insert(std::make_pair(iter->first, depth));
2824  }
2825 
2826  LOGI("Assembled %d points (%d/%d total=%d)", (int)cloudWithNormals->size(), ++cloudCount, (int)poses.size(), (int)mergedClouds->size());
2827  }
2828  else
2829  {
2830  UERROR("Cloud %d not found or empty", iter->first);
2831  }
2832 
2833  if(progressionStatus_.isCanceled())
2834  {
2835  if(blockRendering)
2836  {
2837  renderingMutex_.unlock();
2838  }
2839  exporting_ = false;
2840  return false;
2841  }
2842  progressionStatus_.increment();
2843  }
2844  LOGI("Assembled clouds (%d)... done! %fs (total points=%d)", (int)cameraPoses.size(), timer.ticks(), (int)mergedClouds->size());
2845 
2846  if(mergedClouds->size()>=3)
2847  {
2848  if(optimizedDepth == 0)
2849  {
2850  Eigen::Vector4f min,max;
2851  pcl::getMinMax3D(*mergedClouds, min, max);
2852  float mapLength = uMax3(max[0]-min[0], max[1]-min[1], max[2]-min[2]);
2853  optimizedDepth = 12;
2854  for(int i=6; i<12; ++i)
2855  {
2856  if(mapLength/float(1<<i) < 0.03f)
2857  {
2858  optimizedDepth = i;
2859  break;
2860  }
2861  }
2862  LOGI("optimizedDepth=%d (map length=%f)", optimizedDepth, mapLength);
2863  }
2864 
2865  // Mesh reconstruction
2866  LOGI("Mesh reconstruction...");
2867  pcl::PolygonMesh::Ptr mesh(new pcl::PolygonMesh);
2868  pcl::Poisson<pcl::PointXYZRGBNormal> poisson;
2869  poisson.setDepth(optimizedDepth);
2870  poisson.setInputCloud(mergedClouds);
2871  poisson.reconstruct(*mesh);
2872  LOGI("Mesh reconstruction... done! %fs (%d polygons)", timer.ticks(), (int)mesh->polygons.size());
2873 
2874  if(progressionStatus_.isCanceled())
2875  {
2876  if(blockRendering)
2877  {
2878  renderingMutex_.unlock();
2879  }
2880  exporting_ = false;
2881  return false;
2882  }
2883 
2884  progressionStatus_.increment(poses.size());
2885 
2886  if(mesh->polygons.size())
2887  {
2888  totalPolygons=(int)mesh->polygons.size();
2889 
2890  if(optimizedMaxPolygons > 0 && optimizedMaxPolygons < (int)mesh->polygons.size())
2891  {
2892 #ifndef DISABLE_VTK
2893  unsigned int count = mesh->polygons.size();
2894  float factor = 1.0f-float(optimizedMaxPolygons)/float(count);
2895  LOGI("Mesh decimation (max polygons %d/%d -> factor=%f)...", optimizedMaxPolygons, (int)count, factor);
2896 
2897  progressionStatus_.setMax(progressionStatus_.getMax() + optimizedMaxPolygons/10000);
2898 
2899  pcl::PolygonMesh::Ptr output(new pcl::PolygonMesh);
2900  pcl::MeshQuadricDecimationVTK mqd;
2901  mqd.setTargetReductionFactor(factor);
2902  mqd.setInputMesh(mesh);
2903  mqd.process (*output);
2904  mesh = output;
2905 
2906  //mesh = rtabmap::util3d::meshDecimation(mesh, decimationFactor);
2907  // use direct instantiation above to this fix some linker errors on android like:
2908  // pcl::MeshQuadricDecimationVTK::performProcessing(pcl::PolygonMesh&): error: undefined reference to 'vtkQuadricDecimation::New()'
2909  // pcl::VTKUtils::mesh2vtk(pcl::PolygonMesh const&, vtkSmartPointer<vtkPolyData>&): error: undefined reference to 'vtkFloatArray::New()'
2910 
2911  LOGI("Mesh decimated (factor=%f) from %d to %d polygons (%fs)", factor, count, (int)mesh->polygons.size(), timer.ticks());
2912  if(count < mesh->polygons.size())
2913  {
2914  UWARN("Decimated mesh has more polygons than before!");
2915  }
2916 #else
2917  UWARN("RTAB-Map is not built with PCL-VTK module so mesh decimation cannot be used!");
2918 #endif
2919  }
2920 
2921  if(progressionStatus_.isCanceled())
2922  {
2923  if(blockRendering)
2924  {
2925  renderingMutex_.unlock();
2926  }
2927  exporting_ = false;
2928  return false;
2929  }
2930 
2931  progressionStatus_.increment();
2932 
2933  rtabmap::util3d::denseMeshPostProcessing<pcl::PointXYZRGBNormal>(
2934  mesh,
2935  0.0f,
2936  0,
2937  mergedClouds,
2938  optimizedColorRadius,
2939  textureSize == 0,
2940  optimizedCleanWhitePolygons,
2941  optimizedMinClusterSize);
2942 
2943  if(textureSize>0)
2944  {
2945  LOGI("Texturing... cameraPoses=%d, cameraDepths=%d", (int)cameraPoses.size(), (int)cameraDepths.size());
2946  textureMesh = rtabmap::util3d::createTextureMesh(
2947  mesh,
2948  cameraPoses,
2949  cameraModels,
2950  cameraDepths,
2951  optimizedMaxTextureDistance,
2952  0.0f,
2953  0.0f,
2954  optimizedMinTextureClusterSize,
2955  std::vector<float>(),
2956  &progressionStatus_,
2957  &vertexToPixels);
2958  LOGI("Texturing... done! %fs", timer.ticks());
2959 
2960  if(progressionStatus_.isCanceled())
2961  {
2962  if(blockRendering)
2963  {
2964  renderingMutex_.unlock();
2965  }
2966  exporting_ = false;
2967  return false;
2968  }
2969 
2970  // Remove occluded polygons (polygons with no texture)
2971  if(textureMesh->tex_coordinates.size() && optimizedCleanWhitePolygons)
2972  {
2973  LOGI("Cleanup mesh...");
2974  rtabmap::util3d::cleanTextureMesh(*textureMesh, 0);
2975  LOGI("Cleanup mesh... done! %fs", timer.ticks());
2976  }
2977 
2978  totalPolygons = 0;
2979  for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
2980  {
2981  totalPolygons+=textureMesh->tex_polygons[t].size();
2982  }
2983  }
2984  else
2985  {
2986  totalPolygons = (int)mesh->polygons.size();
2987  polygonMesh = mesh;
2988  }
2989  }
2990  }
2991  else
2992  {
2993  UERROR("Merged cloud too small (%d points) to create polygons!", (int)mergedClouds->size());
2994  }
2995  }
2996  else // organized meshes
2997  {
2998  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
2999 
3000  if(textureSize > 0)
3001  {
3002  textureMesh->tex_materials.resize(poses.size());
3003  textureMesh->tex_polygons.resize(poses.size());
3004  textureMesh->tex_coordinates.resize(poses.size());
3005  }
3006 
3007  int polygonsStep = 0;
3008  int oi = 0;
3009  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
3010  iter!= poses.end();
3011  ++iter)
3012  {
3013  LOGI("Assembling cloud %d (total=%d)...", iter->first, (int)poses.size());
3014 
3015  std::map<int, rtabmap::Mesh>::iterator jter = createdMeshes_.find(iter->first);
3016  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3017  std::vector<pcl::Vertices> polygons;
3018  float gains[3] = {1.0f};
3019  if(jter != createdMeshes_.end())
3020  {
3021  cloud = jter->second.cloud;
3022  polygons= jter->second.polygons;
3023  if(cloud->size() && polygons.size() == 0)
3024  {
3025  polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
3026  }
3027  gains[0] = jter->second.gains[0];
3028  gains[1] = jter->second.gains[1];
3029  gains[2] = jter->second.gains[2];
3030  }
3031  else
3032  {
3033  rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, false, false, false);
3034  data.uncompressData();
3035  if(!data.imageRaw().empty() && !data.depthRaw().empty() && data.cameraModels().size() == 1)
3036  {
3037  int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
3038  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_);
3039  polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
3040  }
3041  }
3042 
3043  if(cloud->size() && polygons.size())
3044  {
3045  // Convert organized to dense cloud
3046  pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3047  std::vector<pcl::Vertices> outputPolygons;
3048  std::vector<int> denseToOrganizedIndices = rtabmap::util3d::filterNaNPointsFromMesh(*cloud, polygons, *outputCloud, outputPolygons);
3049 
3050  pcl::PointCloud<pcl::Normal>::Ptr normals = rtabmap::util3d::computeNormals(outputCloud, normalK);
3051 
3052  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3053  pcl::concatenateFields(*outputCloud, *normals, *cloudWithNormals);
3054 
3055  UASSERT(outputPolygons.size());
3056 
3057  totalPolygons+=outputPolygons.size();
3058 
3059  if(textureSize == 0)
3060  {
3061  // colored mesh
3062  cloudWithNormals = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second);
3063 
3064  if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f)
3065  {
3066  for(unsigned int i=0; i<cloudWithNormals->size(); ++i)
3067  {
3068  pcl::PointXYZRGBNormal & pt = cloudWithNormals->at(i);
3069  pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
3070  pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
3071  pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
3072  }
3073  }
3074 
3075  if(mergedClouds->size() == 0)
3076  {
3077  *mergedClouds = *cloudWithNormals;
3078  polygonMesh->polygons = outputPolygons;
3079  }
3080  else
3081  {
3082  rtabmap::util3d::appendMesh(*mergedClouds, polygonMesh->polygons, *cloudWithNormals, outputPolygons);
3083  }
3084  }
3085  else
3086  {
3087  // texture mesh
3088  size_t polygonSize = outputPolygons.front().vertices.size();
3089  textureMesh->tex_polygons[oi].resize(outputPolygons.size());
3090  textureMesh->tex_coordinates[oi].resize(outputPolygons.size() * polygonSize);
3091  for(unsigned int j=0; j<outputPolygons.size(); ++j)
3092  {
3093  pcl::Vertices vertices = outputPolygons[j];
3094  UASSERT(polygonSize == vertices.vertices.size());
3095  for(unsigned int k=0; k<vertices.vertices.size(); ++k)
3096  {
3097  //uv
3098  UASSERT(vertices.vertices[k] < denseToOrganizedIndices.size());
3099  int originalVertex = denseToOrganizedIndices[vertices.vertices[k]];
3100  textureMesh->tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
3101  float(originalVertex % cloud->width) / float(cloud->width), // u
3102  float(cloud->height - originalVertex / cloud->width) / float(cloud->height)); // v
3103 
3104  vertices.vertices[k] += polygonsStep;
3105  }
3106  textureMesh->tex_polygons[oi][j] = vertices;
3107 
3108  }
3109  polygonsStep += outputCloud->size();
3110 
3111  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(cloudWithNormals, iter->second);
3112  if(mergedClouds->size() == 0)
3113  {
3114  *mergedClouds = *transformedCloud;
3115  }
3116  else
3117  {
3118  *mergedClouds += *transformedCloud;
3119  }
3120 
3121  textureMesh->tex_materials[oi].tex_illum = 1;
3122  textureMesh->tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
3123  textureMesh->tex_materials[oi].tex_file = uNumber2Str(iter->first);
3124  ++oi;
3125  }
3126  }
3127  else
3128  {
3129  UERROR("Mesh not found for mesh %d", iter->first);
3130  }
3131 
3132  if(progressionStatus_.isCanceled())
3133  {
3134  if(blockRendering)
3135  {
3136  renderingMutex_.unlock();
3137  }
3138  exporting_ = false;
3139  return false;
3140  }
3141  progressionStatus_.increment();
3142  }
3143  if(textureSize == 0)
3144  {
3145  if(mergedClouds->size())
3146  {
3147  pcl::toPCLPointCloud2(*mergedClouds, polygonMesh->cloud);
3148  }
3149  else
3150  {
3151  polygonMesh->polygons.clear();
3152  }
3153  }
3154  else
3155  {
3156  textureMesh->tex_materials.resize(oi);
3157  textureMesh->tex_polygons.resize(oi);
3158 
3159  if(mergedClouds->size())
3160  {
3161  pcl::toPCLPointCloud2(*mergedClouds, textureMesh->cloud);
3162  }
3163  }
3164  }
3165 
3166  // end optimized or organized
3167 
3168  if(textureSize>0 && totalPolygons && textureMesh->tex_materials.size())
3169  {
3170  LOGI("Merging %d textures...", (int)textureMesh->tex_materials.size());
3171  globalTextures = rtabmap::util3d::mergeTextures(
3172  *textureMesh,
3173  std::map<int, cv::Mat>(),
3174  std::map<int, std::vector<rtabmap::CameraModel> >(),
3175  rtabmap_->getMemory(),
3176  0,
3177  textureSize,
3178  textureCount,
3179  vertexToPixels,
3180  true, 10.0f, true ,true, 0, 0, 0, false,
3181  &progressionStatus_);
3182  LOGI("Merging %d textures... globalTextures=%dx%d", (int)textureMesh->tex_materials.size(),
3183  globalTextures.cols, globalTextures.rows);
3184  }
3185  if(progressionStatus_.isCanceled())
3186  {
3187  if(blockRendering)
3188  {
3189  renderingMutex_.unlock();
3190  }
3191  exporting_ = false;
3192  return false;
3193  }
3194 
3195  progressionStatus_.increment();
3196  }
3197  if(totalPolygons)
3198  {
3199  if(textureSize == 0)
3200  {
3201  UASSERT((int)polygonMesh->polygons.size() == totalPolygons);
3202  if(polygonMesh->polygons.size())
3203  {
3204  // save in database
3205  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
3206  pcl::fromPCLPointCloud2(polygonMesh->cloud, *cloud);
3207  cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
3208  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(1);
3209  polygons[0].resize(polygonMesh->polygons.size());
3210  for(unsigned int p=0; p<polygonMesh->polygons.size(); ++p)
3211  {
3212  polygons[0][p] = polygonMesh->polygons[p].vertices;
3213  }
3214  boost::mutex::scoped_lock lock(rtabmapMutex_);
3215 
3216  rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons);
3217  success = true;
3218  }
3219  }
3220  else if(textureMesh->tex_materials.size())
3221  {
3222  pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
3223  pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
3224  cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*cloud, rtabmap::Transform(), false).data()); // for database
3225 
3226  // save in database
3227  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons(textureMesh->tex_polygons.size());
3228  for(unsigned int t=0; t<textureMesh->tex_polygons.size(); ++t)
3229  {
3230  polygons[t].resize(textureMesh->tex_polygons[t].size());
3231  for(unsigned int p=0; p<textureMesh->tex_polygons[t].size(); ++p)
3232  {
3233  polygons[t][p] = textureMesh->tex_polygons[t][p].vertices;
3234  }
3235  }
3236  boost::mutex::scoped_lock lock(rtabmapMutex_);
3237  rtabmap_->getMemory()->saveOptimizedMesh(cloudMat, polygons, textureMesh->tex_coordinates, globalTextures);
3238  success = true;
3239  }
3240  else
3241  {
3242  UERROR("Failed exporting texture mesh! There are no textures!");
3243  }
3244  }
3245  else
3246  {
3247  UERROR("Failed exporting mesh! There are no polygons!");
3248  }
3249  }
3250  else // Point cloud
3251  {
3252  pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
3253  for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin();
3254  iter!= poses.end();
3255  ++iter)
3256  {
3257  std::map<int, rtabmap::Mesh>::iterator jter=createdMeshes_.find(iter->first);
3258  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3259  pcl::IndicesPtr indices(new std::vector<int>);
3260  float gains[3];
3261  gains[0] = gains[1] = gains[2] = 1.0f;
3262  if(regenerateCloud)
3263  {
3264  if(jter != createdMeshes_.end())
3265  {
3266  gains[0] = jter->second.gains[0];
3267  gains[1] = jter->second.gains[1];
3268  gains[2] = jter->second.gains[2];
3269  }
3270  rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, true, false, false);
3271  data.uncompressData();
3272  if(!data.imageRaw().empty() && !data.depthRaw().empty())
3273  {
3274  // full resolution
3275  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, 1, maxCloudDepth_, minCloudDepth_, indices.get());
3276  }
3277  else if(!data.laserScanRaw().empty())
3278  {
3279  //scan
3280  cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
3281  indices->resize(cloud->size());
3282  for(unsigned int i=0; i<cloud->size(); ++i)
3283  {
3284  indices->at(i) = i;
3285  }
3286  }
3287  }
3288  else
3289  {
3290  if(jter != createdMeshes_.end())
3291  {
3292  cloud = jter->second.cloud;
3293  indices = jter->second.indices;
3294  gains[0] = jter->second.gains[0];
3295  gains[1] = jter->second.gains[1];
3296  gains[2] = jter->second.gains[2];
3297  }
3298  else
3299  {
3300  rtabmap::SensorData data = rtabmap_->getMemory()->getNodeData(iter->first, true, true, false, false);
3301  data.uncompressData();
3302  if(!data.imageRaw().empty() && !data.depthRaw().empty())
3303  {
3304  int meshDecimation = updateMeshDecimation(data.depthRaw().cols, data.depthRaw().rows);
3305  cloud = rtabmap::util3d::cloudRGBFromSensorData(data, meshDecimation, maxCloudDepth_, minCloudDepth_, indices.get());
3306  }
3307  else if(!data.laserScanRaw().empty())
3308  {
3309  //scan
3310  cloud = rtabmap::util3d::laserScanToPointCloudRGB(rtabmap::util3d::commonFiltering(data.laserScanRaw(), 1, minCloudDepth_, maxCloudDepth_), data.laserScanRaw().localTransform(), 255, 255, 255);
3311  indices->resize(cloud->size());
3312  for(unsigned int i=0; i<cloud->size(); ++i)
3313  {
3314  indices->at(i) = i;
3315  }
3316  }
3317  }
3318  }
3319  if(cloud->size() && indices->size())
3320  {
3321  // Convert organized to dense cloud
3322  pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
3323  if(cloudVoxelSize > 0.0f)
3324  {
3325  transformedCloud = rtabmap::util3d::voxelize(cloud, indices, cloudVoxelSize);
3326  transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
3327  }
3328  else
3329  {
3330  // it looks like that using only transformPointCloud with indices
3331  // flushes the colors, so we should extract points before... maybe a too old PCL version
3332  pcl::copyPointCloud(*cloud, *indices, *transformedCloud);
3333  transformedCloud = rtabmap::util3d::transformPointCloud(transformedCloud, iter->second);
3334  }
3335 
3336  if(gains[0] != 1.0f || gains[1] != 1.0f || gains[2] != 1.0f)
3337  {
3338  //LOGD("cloud %d, gain=%f", iter->first, gain);
3339  for(unsigned int i=0; i<transformedCloud->size(); ++i)
3340  {
3341  pcl::PointXYZRGB & pt = transformedCloud->at(i);
3342  //LOGI("color %d = %d %d %d", i, (int)pt.r, (int)pt.g, (int)pt.b);
3343  pt.r = uchar(std::max(0.0, std::min(255.0, double(pt.r) * gains[0])));
3344  pt.g = uchar(std::max(0.0, std::min(255.0, double(pt.g) * gains[1])));
3345  pt.b = uchar(std::max(0.0, std::min(255.0, double(pt.b) * gains[2])));
3346 
3347  }
3348  }
3349 
3350  if(mergedClouds->size() == 0)
3351  {
3352  *mergedClouds = *transformedCloud;
3353  }
3354  else
3355  {
3356  *mergedClouds += *transformedCloud;
3357  }
3358  }
3359 
3360  if(progressionStatus_.isCanceled())
3361  {
3362  if(blockRendering)
3363  {
3364  renderingMutex_.unlock();
3365  }
3366  exporting_ = false;
3367  return false;
3368  }
3369  progressionStatus_.increment();
3370  }
3371 
3372  if(mergedClouds->size())
3373  {
3374  if(cloudVoxelSize > 0.0f)
3375  {
3376  mergedClouds = rtabmap::util3d::voxelize(mergedClouds, cloudVoxelSize);
3377  }
3378 
3379  // save in database
3380  {
3381  cv::Mat cloudMat = rtabmap::compressData2(rtabmap::util3d::laserScanFromPointCloud(*mergedClouds).data()); // for database
3382  boost::mutex::scoped_lock lock(rtabmapMutex_);
3383  rtabmap_->getMemory()->saveOptimizedMesh(cloudMat);
3384  success = true;
3385  }
3386  }
3387  else
3388  {
3389  UERROR("Merged cloud is empty!");
3390  }
3391  }
3392 
3393  progressionStatus_.finish();
3394 
3395  if(blockRendering)
3396  {
3397  renderingMutex_.unlock();
3398  }
3399  }
3400  catch (std::exception & e)
3401  {
3402  UERROR("Out of memory! %s", e.what());
3403 
3404  if(blockRendering)
3405  {
3406  renderingMutex_.unlock();
3407  }
3408 
3409  success = false;
3410  }
3411  exporting_ = false;
3412 
3413  optRefId_ = 0;
3414  if(optRefPose_)
3415  {
3416  delete optRefPose_;
3417  optRefPose_ = 0;
3418  }
3419  if(success && poses.size())
3420  {
3421  // for optimized mesh
3422  // just take the last as reference
3423  optRefId_ = poses.rbegin()->first;
3424  optRefPose_ = new rtabmap::Transform(poses.rbegin()->second);
3425  }
3426 
3427  return success;
3428 }
3429 
3430 bool RTABMapApp::postExportation(bool visualize)
3431 {
3432  LOGI("postExportation(visualize=%d)", visualize?1:0);
3433  optMesh_.reset(new pcl::TextureMesh);
3434  optTexture_ = cv::Mat();
3435  exportedMeshUpdated_ = false;
3436 
3437  if(visualize)
3438  {
3439  visualizingMesh_ = false;
3440  cv::Mat cloudMat;
3441  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3442 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3443  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3444 #else
3445  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3446 #endif
3447  cv::Mat textures;
3448  if(rtabmap_ && rtabmap_->getMemory())
3449  {
3450  cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
3451  if(!cloudMat.empty())
3452  {
3453  LOGI("postExportation: Found optimized mesh! Visualizing it.");
3454  optMesh_ = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, true);
3455  optTexture_ = textures;
3456 
3457  boost::mutex::scoped_lock lock(renderingMutex_);
3458  visualizingMesh_ = true;
3459  exportedMeshUpdated_ = true;
3460  }
3461  else
3462  {
3463  LOGI("postExportation: No optimized mesh found.");
3464  }
3465  }
3466  }
3467  else if(visualizingMesh_)
3468  {
3469  rtabmapMutex_.lock();
3470  std::map<int, rtabmap::Transform> poses = rtabmap_->getLocalOptimizedPoses();
3471  std::multimap<int, rtabmap::Link> links = rtabmap_->getLocalConstraints();
3472  if(poses.empty())
3473  {
3474  rtabmap_->getGraph(
3475  poses,
3476  links,
3477  true,
3478  true,
3479  0,
3480  false,
3481  false,
3482  false,
3483  false,
3484  false,
3485  false);
3486  }
3487  if(!poses.empty())
3488  {
3489  rtabmap::Statistics stats;
3490  for(std::map<std::string, float>::iterator iter=bufferedStatsData_.begin(); iter!=bufferedStatsData_.end(); ++iter)
3491  {
3492  stats.addStatistic(iter->first, iter->second);
3493  }
3494  stats.setPoses(poses);
3495  stats.setConstraints(links);
3496  rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
3497  }
3498  rtabmapMutex_.unlock();
3499 
3500  visualizingMesh_ = false;
3501  }
3502 
3503  return visualizingMesh_;
3504 }
3505 
3506 bool RTABMapApp::writeExportedMesh(const std::string & directory, const std::string & name)
3507 {
3508  LOGI("writeExportedMesh: dir=%s name=%s", directory.c_str(), name.c_str());
3509  exporting_ = true;
3510 
3511  bool success = false;
3512 
3513  pcl::PolygonMesh::Ptr polygonMesh(new pcl::PolygonMesh);
3514  pcl::TextureMesh::Ptr textureMesh(new pcl::TextureMesh);
3515  cv::Mat cloudMat;
3516  std::vector<std::vector<std::vector<RTABMAP_PCL_INDEX> > > polygons;
3517 #if PCL_VERSION_COMPARE(>=, 1, 8, 0)
3518  std::vector<std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> > > texCoords;
3519 #else
3520  std::vector<std::vector<Eigen::Vector2f> > texCoords;
3521 #endif
3522  cv::Mat textures;
3523  if(rtabmap_ && rtabmap_->getMemory())
3524  {
3525  cloudMat = rtabmap_->getMemory()->loadOptimizedMesh(&polygons, &texCoords, &textures);
3526  if(!cloudMat.empty())
3527  {
3528  LOGI("writeExportedMesh: Found optimized mesh!");
3529  if(textures.empty())
3530  {
3531  polygonMesh = rtabmap::util3d::assemblePolygonMesh(cloudMat, polygons.size() == 1?polygons[0]:std::vector<std::vector<RTABMAP_PCL_INDEX> >());
3532  }
3533  else
3534  {
3535  textureMesh = rtabmap::util3d::assembleTextureMesh(cloudMat, polygons, texCoords, textures, false);
3536  }
3537  }
3538  else
3539  {
3540  LOGI("writeExportedMesh: No optimized mesh found.");
3541  }
3542  }
3543 
3544  if(polygonMesh->cloud.data.size())
3545  {
3546  // Point cloud PLY
3547  std::string filePath = directory + UDirectory::separator() + name + ".ply";
3548  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());
3549  success = pcl::io::savePLYFileBinary(filePath, *polygonMesh) == 0;
3550  if(success)
3551  {
3552  LOGI("Saved ply to %s!", filePath.c_str());
3553  }
3554  else
3555  {
3556  UERROR("Failed saving ply to %s!", filePath.c_str());
3557  }
3558  }
3559  else if(textureMesh->cloud.data.size())
3560  {
3561  // TextureMesh OBJ
3562  LOGD("Saving texture(s) (%d)", textures.empty()?0:textures.cols/textures.rows);
3563  UASSERT(textures.empty() || textures.cols % textures.rows == 0);
3564  UASSERT((int)textureMesh->tex_materials.size() == textures.cols/textures.rows);
3565  for(unsigned int i=0; i<textureMesh->tex_materials.size(); ++i)
3566  {
3567  std::string baseNameNum = name;
3568  if(textureMesh->tex_materials.size()>1)
3569  {
3570  baseNameNum+=uNumber2Str(i);
3571  }
3572  std::string fullPath = directory+UDirectory::separator()+baseNameNum+".jpg";
3573  textureMesh->tex_materials[i].tex_file = baseNameNum+".jpg";
3574  LOGI("Saving texture to %s.", fullPath.c_str());
3575  success = cv::imwrite(fullPath, textures(cv::Range::all(), cv::Range(i*textures.rows, (i+1)*textures.rows)));
3576  if(!success)
3577  {
3578  LOGI("Failed saving %s!", fullPath.c_str());
3579  }
3580  else
3581  {
3582  LOGI("Saved %s.", fullPath.c_str());
3583  }
3584  }
3585 
3586  if(success)
3587  {
3588  // With Sketchfab, the OBJ models are rotated 90 degrees on x axis, so rotate -90 to have model in right position
3589  //pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
3590  //pcl::fromPCLPointCloud2(textureMesh->cloud, *cloud);
3591  //cloud = rtabmap::util3d::transformPointCloud(cloud, rtabmap::Transform(1,0,0,0, 0,0,1,0, 0,-1,0,0));
3592  //pcl::toPCLPointCloud2(*cloud, textureMesh->cloud);
3593  std::string filePath = directory + UDirectory::separator() + name + ".obj";
3594  int totalPolygons = 0;
3595  for(unsigned int i=0;i<textureMesh->tex_polygons.size(); ++i)
3596  {
3597  totalPolygons += textureMesh->tex_polygons[i].size();
3598  }
3599  LOGI("Saving obj (%d vertices, %d polygons) to %s.", (int)textureMesh->cloud.data.size()/textureMesh->cloud.point_step, totalPolygons, filePath.c_str());
3600  success = pcl::io::saveOBJFile(filePath, *textureMesh) == 0;
3601 
3602  if(success)
3603  {
3604  LOGI("Saved obj to %s!", filePath.c_str());
3605  }
3606  else
3607  {
3608  UERROR("Failed saving obj to %s!", filePath.c_str());
3609  }
3610  }
3611  }
3612  exporting_ = false;
3613  return success;
3614 }
3615 
3616 int RTABMapApp::postProcessing(int approach)
3617 {
3618  postProcessing_ = true;
3619  LOGI("postProcessing begin(%d)", approach);
3620  int returnedValue = 0;
3621  if(rtabmap_)
3622  {
3623  std::map<int, rtabmap::Transform> poses;
3624  std::multimap<int, rtabmap::Link> links;
3625 
3626  // detect more loop closures
3627  if(approach == -1 || approach == 2)
3628  {
3629  if(approach == -1)
3630  {
3631  progressionStatus_.reset(6);
3632  }
3633  returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1, true, true, approach==-1?&progressionStatus_:0);
3634  if(approach == -1 && progressionStatus_.isCanceled())
3635  {
3636  postProcessing_ = false;
3637  return -1;
3638  }
3639  }
3640 
3641  // graph optimization
3642  if(returnedValue >=0)
3643  {
3644  if (approach == 1)
3645  {
3646  if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
3647  {
3648  std::map<int, rtabmap::Signature> signatures;
3649  rtabmap_->getGraph(poses, links, true, true, &signatures);
3650 
3651  rtabmap::ParametersMap param;
3652  param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
3653  param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
3654  rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
3655  poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
3656  delete sba;
3657  }
3658  else
3659  {
3660  UERROR("g2o not available!");
3661  }
3662  }
3663  else if(approach!=4 && approach!=5 && approach != 7)
3664  {
3665  // simple graph optmimization
3666  rtabmap_->getGraph(poses, links, true, true);
3667  }
3668  }
3669 
3670  if(poses.size())
3671  {
3672  boost::mutex::scoped_lock lock(rtabmapMutex_);
3673  rtabmap::Statistics stats = rtabmap_->getStatistics();
3674  stats.setPoses(poses);
3675  stats.setConstraints(links);
3676 
3677  LOGI("PostProcessing, sending rtabmap event to update graph...");
3678  rtabmapEvents_.push_back(new rtabmap::RtabmapEvent(stats));
3679 
3680  rtabmap_->setOptimizedPoses(poses, links);
3681  }
3682  else if(approach!=4 && approach!=5 && approach != 7)
3683  {
3684  returnedValue = -1;
3685  }
3686 
3687  if(returnedValue >=0)
3688  {
3689  boost::mutex::scoped_lock lock(renderingMutex_);
3690  // filter polygons
3691  if(approach == -1 || approach == 4)
3692  {
3693  filterPolygonsOnNextRender_ = true;
3694  }
3695 
3696  // gain compensation
3697  if(approach == -1 || approach == 5 || approach == 6)
3698  {
3699  gainCompensationOnNextRender_ = approach == 6 ? 2 : 1; // 2 = full, 1 = fast
3700  }
3701 
3702  // bilateral filtering
3703  if(approach == 7)
3704  {
3705  bilateralFilteringOnNextRender_ = true;
3706  }
3707  }
3708  }
3709 
3710  postProcessing_ = false;
3711  LOGI("postProcessing end(%d) -> %d", approach, returnedValue);
3712  return returnedValue;
3713 }
3714 
3715 void RTABMapApp::postCameraPoseEvent(
3716  float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
3717 {
3718  boost::mutex::scoped_lock lock(cameraMutex_);
3719  if(cameraDriver_ == 3 && camera_)
3720  {
3721  if(qx==0 && qy==0 && qz==0 && qw==0)
3722  {
3723  // Lost! clear buffer
3724  camera_->resetOrigin(); // we are lost, create new session on next valid frame
3725  return;
3726  }
3727  rtabmap::Transform pose(x,y,z,qx,qy,qz,qw);
3728  pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
3729  camera_->poseReceived(pose, stamp);
3730  }
3731 }
3732 
3733 void RTABMapApp::postOdometryEvent(
3734  rtabmap::Transform pose,
3735  float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy,
3736  float depth_fx, float depth_fy, float depth_cx, float depth_cy,
3737  const rtabmap::Transform & rgbFrame,
3738  const rtabmap::Transform & depthFrame,
3739  double stamp,
3740  double depthStamp,
3741  const void * yPlane, const void * uPlane, const void * vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat,
3742  const void * depth, int depthLen, int depthWidth, int depthHeight, int depthFormat,
3743  const void * conf, int confLen, int confWidth, int confHeight, int confFormat,
3744  const float * points, int pointsLen, int pointsChannels,
3745  const rtabmap::Transform & viewMatrix,
3746  float p00, float p11, float p02, float p12, float p22, float p32, float p23,
3747  float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
3748 {
3749 #if defined(RTABMAP_ARCORE) || defined(__APPLE__)
3750  boost::mutex::scoped_lock lock(cameraMutex_);
3751  if(cameraDriver_ == 3 && camera_)
3752  {
3753  if(rgb_fx > 0.0f && rgb_fy > 0.0f && rgb_cx > 0.0f && rgb_cy > 0.0f && stamp > 0.0f && yPlane && vPlane && yPlaneLen == rgbWidth*rgbHeight)
3754  {
3755 #ifndef DISABLE_LOG
3756  //LOGD("rgb format = %d depth format =%d ", rgbFormat, depthFormat);
3757 #endif
3758 #if defined(RTABMAP_ARCORE)
3759  if(rgbFormat == AR_IMAGE_FORMAT_YUV_420_888 &&
3760  (depth==0 || depthFormat == AIMAGE_FORMAT_DEPTH16))
3761 #else //__APPLE__
3762  if(rgbFormat == 875704422 &&
3763  (depth==0 || depthFormat == 1717855600))
3764 #endif
3765  {
3766  cv::Mat outputRGB;
3767 #ifndef DISABLE_LOG
3768  //LOGD("y=%p u=%p v=%p yLen=%d y->v=%ld", yPlane, uPlane, vPlane, yPlaneLen, (long)vPlane-(long)yPlane);
3769 #endif
3770  if((long)vPlane-(long)yPlane != yPlaneLen)
3771  {
3772  // The uv-plane is not concatenated to y plane in memory, so concatenate them
3773  cv::Mat yuv(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1);
3774  memcpy(yuv.data, yPlane, yPlaneLen);
3775  memcpy(yuv.data+yPlaneLen, vPlane, rgbHeight/2*rgbWidth);
3776  cv::cvtColor(yuv, outputRGB, cv::COLOR_YUV2BGR_NV21);
3777  }
3778  else
3779  {
3780 #ifdef __ANDROID__
3781  cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2BGR_NV21);
3782 #else // __APPLE__
3783  cv::cvtColor(cv::Mat(rgbHeight+rgbHeight/2, rgbWidth, CV_8UC1, (void*)yPlane), outputRGB, cv::COLOR_YUV2RGB_NV21);
3784 #endif
3785  }
3786 
3787 
3788  cv::Mat outputDepth;
3789  if(depth && depthHeight>0 && depthWidth>0)
3790  {
3791 #ifndef DISABLE_LOG
3792  //LOGD("depth %dx%d len=%d", depthWidth, depthHeight, depthLen);
3793 #endif
3794  if(depthLen == 4*depthWidth*depthHeight)
3795  {
3796  // IOS
3797  outputDepth = cv::Mat(depthHeight, depthWidth, CV_32FC1, (void*)depth).clone();
3798  if(conf && confWidth == depthWidth && confHeight == depthHeight && confFormat == 1278226488 && depthConfidence_>0)
3799  {
3800  const unsigned char * confPtr = (const unsigned char *)conf;
3801  float * depthPtr = outputDepth.ptr<float>();
3802  int i=0;
3803  for (int y = 0; y < outputDepth.rows; ++y)
3804  {
3805  for (int x = 0; x < outputDepth.cols; ++x)
3806  {
3807  // https://developer.apple.com/documentation/arkit/arconfidencelevel
3808  // 0 = low
3809  // 1 = medium
3810  // 2 = high
3811  if(confPtr[y*outputDepth.cols + x] < depthConfidence_)
3812  {
3813  depthPtr[y*outputDepth.cols + x] = 0.0f;
3814  ++i;
3815  }
3816  }
3817  }
3818  }
3819  }
3820  else if(depthLen == 2*depthWidth*depthHeight)
3821  {
3822  // ANDROID
3823  outputDepth = cv::Mat(depthHeight, depthWidth, CV_16UC1);
3824  uint16_t *dataShort = (uint16_t *)depth;
3825  for (int y = 0; y < outputDepth.rows; ++y)
3826  {
3827  for (int x = 0; x < outputDepth.cols; ++x)
3828  {
3829  uint16_t depthSample = dataShort[y*outputDepth.cols + x];
3830  uint16_t depthRange = (depthSample & 0x1FFF); // first 3 bits are confidence
3831  outputDepth.at<uint16_t>(y,x) = depthRange;
3832  }
3833  }
3834  }
3835  }
3836 
3837  if(!outputRGB.empty())
3838  {
3839  pose = rtabmap::rtabmap_world_T_opengl_world * pose * rtabmap::opengl_world_T_rtabmap_world;
3840 
3841  rtabmap::Transform poseWithOriginOffset = pose;
3842  if(!camera_->getOriginOffset().isNull())
3843  {
3844  poseWithOriginOffset = camera_->getOriginOffset() * pose;
3845  }
3846 
3847  // Registration depth to rgb
3848  if(!outputDepth.empty() && !depthFrame.isNull() && depth_fx!=0 && (rgbFrame != depthFrame || depthStamp!=stamp))
3849  {
3850  UTimer time;
3851  rtabmap::Transform motion = rtabmap::Transform::getIdentity();
3852  if(depthStamp != stamp)
3853  {
3854  // Interpolate pose
3855  rtabmap::Transform poseDepth;
3856  cv::Mat cov;
3857  if(!camera_->getPose(camera_->getStampEpochOffset()+depthStamp, poseDepth, cov, 0.0))
3858  {
3859  UERROR("Could not find pose at depth stamp %f (epoch=%f rgb=%f)!", depthStamp, camera_->getStampEpochOffset()+depthStamp, stamp);
3860  }
3861  else
3862  {
3863 #ifndef DISABLE_LOG
3864  UDEBUG("poseRGB =%s (stamp=%f)", poseWithOriginOffset.prettyPrint().c_str(), stamp);
3865  UDEBUG("poseDepth=%s (stamp=%f)", poseDepth.prettyPrint().c_str(), depthStamp);
3866 #endif
3867  motion = poseWithOriginOffset.inverse()*poseDepth;
3868  // transform in camera frame
3869 #ifndef DISABLE_LOG
3870  UDEBUG("motion=%s", motion.prettyPrint().c_str());
3871 #endif
3872  motion = rtabmap::CameraModel::opticalRotation().inverse() * motion * rtabmap::CameraModel::opticalRotation();
3873 #ifndef DISABLE_LOG
3874  UDEBUG("motion=%s", motion.prettyPrint().c_str());
3875 #endif
3876  }
3877  }
3878  rtabmap::Transform rgbToDepth = motion*rgbFrame.inverse()*depthFrame;
3879  float scale = (float)outputDepth.cols/(float)outputRGB.cols;
3880  cv::Mat colorK = (cv::Mat_<double>(3,3) <<
3881  rgb_fx*scale, 0, rgb_cx*scale,
3882  0, rgb_fy*scale, rgb_cy*scale,
3883  0, 0, 1);
3884  cv::Mat depthK = (cv::Mat_<double>(3,3) <<
3885  depth_fx, 0, depth_cx,
3886  0, depth_fy, depth_cy,
3887  0, 0, 1);
3888  outputDepth = rtabmap::util2d::registerDepth(outputDepth, depthK, outputDepth.size(), colorK, rgbToDepth);
3889 #ifndef DISABLE_LOG
3890  UDEBUG("Depth registration time: %fs", time.elapsed());
3891 #endif
3892  }
3893 
3894  rtabmap::CameraModel model = rtabmap::CameraModel(rgb_fx, rgb_fy, rgb_cx, rgb_cy, camera_->getDeviceTColorCamera(), 0, cv::Size(rgbWidth, rgbHeight));
3895 #ifndef DISABLE_LOG
3896  //LOGI("pointCloudData size=%d", pointsLen);
3897 #endif
3898  if(!fullResolution_)
3899  {
3900  outputRGB = rtabmap::util2d::decimate(outputRGB, 2);
3901  model = model.scaled(1.0/double(2));
3902  }
3903 
3904  std::vector<cv::KeyPoint> kpts;
3905  std::vector<cv::Point3f> kpts3;
3906  rtabmap::LaserScan scan;
3907  if(points && pointsLen>0)
3908  {
3909  cv::Mat pointsMat(1, pointsLen, CV_32FC(pointsChannels), (void*)points);
3910  if(outputDepth.empty())
3911  {
3912  int kptsSize = fullResolution_ ? 12 : 6;
3913  scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB, &kpts, &kpts3, kptsSize);
3914  }
3915  else
3916  {
3917  // We will recompute features if depth is available
3918  scan = rtabmap::CameraMobile::scanFromPointCloudData(pointsMat, pointsLen, pose, model, outputRGB);
3919  }
3920  }
3921 
3922  if(!outputDepth.empty())
3923  {
3924  rtabmap::CameraModel depthModel = model.scaled(float(outputDepth.cols) / float(model.imageWidth()));
3925  depthModel.setLocalTransform(poseWithOriginOffset*model.localTransform());
3926  camera_->setOcclusionImage(outputDepth, depthModel);
3927  }
3928 
3929  rtabmap::SensorData data(scan, outputRGB, outputDepth, model, 0, stamp);
3930  data.setFeatures(kpts, kpts3, cv::Mat());
3931  glm::mat4 projectionMatrix(0);
3932  projectionMatrix[0][0] = p00;
3933  projectionMatrix[1][1] = p11;
3934  projectionMatrix[2][0] = p02;
3935  projectionMatrix[2][1] = p12;
3936  projectionMatrix[2][2] = p22;
3937  projectionMatrix[2][3] = p32;
3938  projectionMatrix[3][2] = p23;
3939  glm::mat4 viewMatrixMat = rtabmap::glmFromTransform(viewMatrix);
3940  float texCoords[8];
3941  texCoords[0] = t0;
3942  texCoords[1] = t1;
3943  texCoords[2] = t2;
3944  texCoords[3] = t3;
3945  texCoords[4] = t4;
3946  texCoords[5] = t5;
3947  texCoords[6] = t6;
3948  texCoords[7] = t7;
3949  camera_->update(data, pose, viewMatrixMat, projectionMatrix, main_scene_.GetCameraType() == tango_gl::GestureCamera::kFirstPerson?texCoords:0);
3950  }
3951  }
3952  }
3953  else
3954  {
3955  UERROR("Missing image information! fx=%f fy=%f cx=%f cy=%f stamp=%f yPlane=%d vPlane=%d yPlaneLen=%d rgbWidth=%d rgbHeight=%d",
3956  rgb_fx, rgb_fy, rgb_cx, rgb_cy, stamp, yPlane?1:0, vPlane?1:0, yPlaneLen, rgbWidth, rgbHeight);
3957  }
3958  }
3959 #else
3960  UERROR("Not built with ARCore or iOS!");
3961 #endif
3962 }
3963 
3964 bool RTABMapApp::handleEvent(UEvent * event)
3965 {
3966  if(sensorCaptureThread_!=0)
3967  {
3968  // called from events manager thread, so protect the data
3969  if(event->getClassName().compare("SensorEvent") == 0)
3970  {
3971  LOGI("Received SensorEvent!");
3972  if(sensorMutex_.try_lock())
3973  {
3974  sensorEvents_.clear();
3975  sensorEvents_.push_back(*((rtabmap::SensorEvent*)(event)));
3976  sensorMutex_.unlock();
3977  }
3978  }
3979  if(event->getClassName().compare("RtabmapEvent") == 0)
3980  {
3981  LOGI("Received RtabmapEvent event! status=%d", status_.first);
3982  if(status_.first == rtabmap::RtabmapEventInit::kInitialized)
3983  {
3984  boost::mutex::scoped_lock lock(rtabmapMutex_);
3985  rtabmapEvents_.push_back((rtabmap::RtabmapEvent*)event);
3986  return true;
3987  }
3988  else
3989  {
3990  LOGW("Received RtabmapEvent event but ignoring it while we are initializing...status=%d", status_.first);
3991  }
3992  }
3993  }
3994 
3995  if(event->getClassName().compare("PoseEvent") == 0)
3996  {
3997  if(poseMutex_.try_lock())
3998  {
3999  poseEvents_.clear();
4000  poseEvents_.push_back(((rtabmap::PoseEvent*)event)->pose());
4001  poseMutex_.unlock();
4002  }
4003  }
4004 
4005  if(event->getClassName().compare("CameraInfoEvent") == 0)
4006  {
4007  rtabmap::CameraInfoEvent * tangoEvent = (rtabmap::CameraInfoEvent*)event;
4008 
4009  // Call JAVA callback with tango event msg
4010  bool success = false;
4011 #ifdef __ANDROID__
4012  if(jvm && RTABMapActivity)
4013  {
4014  JNIEnv *env = 0;
4015  jint rs = jvm->AttachCurrentThread(&env, NULL);
4016  if(rs == JNI_OK && env)
4017  {
4018  jclass clazz = env->GetObjectClass(RTABMapActivity);
4019  if(clazz)
4020  {
4021  jmethodID methodID = env->GetMethodID(clazz, "cameraEventCallback", "(ILjava/lang/String;Ljava/lang/String;)V" );
4022  if(methodID)
4023  {
4024  env->CallVoidMethod(RTABMapActivity, methodID,
4025  tangoEvent->type(),
4026  env->NewStringUTF(tangoEvent->key().c_str()),
4027  env->NewStringUTF(tangoEvent->value().c_str()));
4028  success = true;
4029  }
4030  }
4031  }
4032  jvm->DetachCurrentThread();
4033  }
4034 #endif
4035  if(!success)
4036  {
4037  UERROR("Failed to call RTABMapActivity::tangoEventCallback");
4038  }
4039  }
4040 
4041  if(event->getClassName().compare("RtabmapEventInit") == 0)
4042  {
4043  status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
4044  status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
4045  LOGI("Received RtabmapEventInit! Status=%d info=%s", (int)status_.first, status_.second.c_str());
4046 
4047  // Call JAVA callback with init msg
4048  bool success = false;
4049 #ifdef __ANDROID__
4050  if(jvm && RTABMapActivity)
4051  {
4052  JNIEnv *env = 0;
4053  jint rs = jvm->AttachCurrentThread(&env, NULL);
4054  if(rs == JNI_OK && env)
4055  {
4056  jclass clazz = env->GetObjectClass(RTABMapActivity);
4057  if(clazz)
4058  {
4059  jmethodID methodID = env->GetMethodID(clazz, "rtabmapInitEventCallback", "(ILjava/lang/String;)V" );
4060  if(methodID)
4061  {
4062  env->CallVoidMethod(RTABMapActivity, methodID,
4063  status_.first,
4064  env->NewStringUTF(status_.second.c_str()));
4065  success = true;
4066  }
4067  }
4068  }
4069  jvm->DetachCurrentThread();
4070  }
4071 #else
4072  if(swiftClassPtr_)
4073  {
4074  std::function<void()> actualCallback = [&](){
4075  swiftInitCallback(swiftClassPtr_, status_.first, status_.second.c_str());
4076  };
4077  actualCallback();
4078  success = true;
4079  }
4080 #endif
4081  if(!success)
4082  {
4083  UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
4084  }
4085  }
4086 
4087  if(event->getClassName().compare("PostRenderEvent") == 0)
4088  {
4089  LOGI("Received PostRenderEvent!");
4090 
4091  int loopClosureId = 0;
4092  int featuresExtracted = 0;
4093  if(((PostRenderEvent*)event)->getRtabmapEvent())
4094  {
4095  LOGI("Received PostRenderEvent! has getRtabmapEvent");
4096 
4097  const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getRtabmapEvent()->getStats();
4098  loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
4099  featuresExtracted = stats.getLastSignatureData().getWords().size();
4100 
4101  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryWorking_memory_size(), uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f)));
4102  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)));
4103  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kKeypointDictionary_size(), uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f)));
4104  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kTimingTotal(), uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f)));
4105  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_id(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f)));
4106  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDatabase_memory_used(), uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f)));
4107  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopVisual_inliers(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f)));
4108  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopVisual_matches(), uValue(stats.data(), rtabmap::Statistics::kLoopVisual_matches(), 0.0f)));
4109  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopRejectedHypothesis(), uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f)));
4110  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopOptimization_max_error(), uValue(stats.data(), rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f)));
4111  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)));
4112  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryRehearsal_sim(), uValue(stats.data(), rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f)));
4113  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopHighest_hypothesis_value(), uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f)));
4114  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryDistance_travelled(), uValue(stats.data(), rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f)));
4115  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kMemoryFast_movement(), uValue(stats.data(), rtabmap::Statistics::kMemoryFast_movement(), 0.0f)));
4116  uInsert(bufferedStatsData_, std::make_pair<std::string, float>(rtabmap::Statistics::kLoopLandmark_detected(), uValue(stats.data(), rtabmap::Statistics::kLoopLandmark_detected(), 0.0f)));
4117  }
4118  // else use last data
4119 
4120  int nodes = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
4121  uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
4122  int words = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
4123  float updateTime = uValue(bufferedStatsData_, rtabmap::Statistics::kTimingTotal(), 0.0f);
4124  int highestHypId = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
4125  int databaseMemoryUsed = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
4126  int inliers = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
4127  int matches = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopVisual_matches(), 0.0f);
4128  int rejected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
4129  float optimizationMaxError = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error(), 0.0f);
4130  float optimizationMaxErrorRatio = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopOptimization_max_error_ratio(), 0.0f);
4131  float rehearsalValue = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryRehearsal_sim(), 0.0f);
4132  float hypothesis = uValue(bufferedStatsData_, rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
4133  float distanceTravelled = uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryDistance_travelled(), 0.0f);
4134  int fastMovement = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kMemoryFast_movement(), 0.0f);
4135  int landmarkDetected = (int)uValue(bufferedStatsData_, rtabmap::Statistics::kLoopLandmark_detected(), 0.0f);
4136  rtabmap::Transform currentPose = main_scene_.GetCameraPose();
4137  float x=0.0f,y=0.0f,z=0.0f,roll=0.0f,pitch=0.0f,yaw=0.0f;
4138  if(!currentPose.isNull())
4139  {
4140  currentPose.getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
4141  }
4142 
4143  // Call JAVA callback with some stats
4144  UINFO("Send statistics to GUI");
4145  bool success = false;
4146 #ifdef __ANDROID__
4147  if(jvm && RTABMapActivity)
4148  {
4149  JNIEnv *env = 0;
4150  jint rs = jvm->AttachCurrentThread(&env, NULL);
4151  if(rs == JNI_OK && env)
4152  {
4153  jclass clazz = env->GetObjectClass(RTABMapActivity);
4154  if(clazz)
4155  {
4156  jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIIFIFIFFFFIIFFFFFF)V" );
4157  if(methodID)
4158  {
4159  env->CallVoidMethod(RTABMapActivity, methodID,
4160  nodes,
4161  words,
4162  totalPoints_,
4163  totalPolygons_,
4164  updateTime,
4165  loopClosureId,
4166  highestHypId,
4167  databaseMemoryUsed,
4168  inliers,
4169  matches,
4170  featuresExtracted,
4171  hypothesis,
4172  lastDrawnCloudsCount_,
4173  renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
4174  rejected,
4175  rehearsalValue,
4176  optimizationMaxError,
4177  optimizationMaxErrorRatio,
4178  distanceTravelled,
4179  fastMovement,
4180  landmarkDetected,
4181  x,
4182  y,
4183  z,
4184  roll,
4185  pitch,
4186  yaw);
4187  success = true;
4188  }
4189  }
4190  }
4191  jvm->DetachCurrentThread();
4192  }
4193 #else // __APPLE__
4194  if(swiftClassPtr_)
4195  {
4196  std::function<void()> actualCallback = [&](){
4197  swiftStatsUpdatedCallback(swiftClassPtr_,
4198  nodes,
4199  words,
4200  totalPoints_,
4201  totalPolygons_,
4202  updateTime,
4203  loopClosureId,
4204  highestHypId,
4205  databaseMemoryUsed,
4206  inliers,
4207  matches,
4208  featuresExtracted,
4209  hypothesis,
4210  lastDrawnCloudsCount_,
4211  renderingTime_>0.0f?1.0f/renderingTime_:0.0f,
4212  rejected,
4213  rehearsalValue,
4214  optimizationMaxError,
4215  optimizationMaxErrorRatio,
4216  distanceTravelled,
4217  fastMovement,
4218  landmarkDetected,
4219  x,
4220  y,
4221  z,
4222  roll,
4223  pitch,
4224  yaw);
4225  };
4226  actualCallback();
4227  success = true;
4228  }
4229 #endif
4230  if(!success)
4231  {
4232  UERROR("Failed to call RTABMapActivity::updateStatsCallback");
4233  }
4234  renderingTime_ = 0.0f;
4235  }
4236  return false;
4237 }
4238 
rtabmap::Optimizer::isAvailable
static bool isAvailable(Optimizer::Type type)
Definition: Optimizer.cpp:47
rtabmap::SensorData
Definition: SensorData.h:51
UFile::rename
static int rename(const std::string &oldFilePath, const std::string &newFilePath)
Definition: UFile.cpp:63
RTABMapApp::swiftStatsUpdatedCallback
void(* swiftStatsUpdatedCallback)(void *, int, int, int, int, float, int, int, int, int, int, int, float, int, float, int, float, float, float, float, int, int, float, float, float, float, float, float)
Definition: RTABMapApp.h:301
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
w
RowVector3d w
glm::min
GLM_FUNC_DECL genType min(genType const &x, genType const &y)
int
int
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
rtabmap::CameraModel::fx
double fx() const
Definition: CameraModel.h:102
rtabmap::Memory::getImageCompressed
cv::Mat getImageCompressed(int signatureId) const
Definition: Memory.cpp:4096
RTABMapApp::meshTrianglePix_
int meshTrianglePix_
Definition: RTABMapApp.h:234
RTABMapApp::setMinCloudDepth
void setMinCloudDepth(float value)
Definition: RTABMapApp.cpp:2437
Scene::hasTexture
bool hasTexture(int id) const
Definition: scene.cpp:918
rtabmap::CameraMobile::scanFromPointCloudData
static LaserScan scanFromPointCloudData(const cv::Mat &pointCloudData, int points, const Transform &pose, const CameraModel &model, const cv::Mat &rgb, std::vector< cv::KeyPoint > *kpts=0, std::vector< cv::Point3f > *kpts3D=0, int kptsSize=3)
Definition: CameraMobile.cpp:461
Compression.h
rtabmap::CameraModel::cx
double cx() const
Definition: CameraModel.h:104
rtabmap::ProgressionStatus::setMax
void setMax(int max)
Definition: ProgressionStatus.h:65
RTABMapApp::localizationMode_
bool localizationMode_
Definition: RTABMapApp.h:222
RTABMapApp::mapToOdom_
rtabmap::Transform mapToOdom_
Definition: RTABMapApp.h:280
Scene::isMeshTexturing
bool isMeshTexturing() const
Definition: scene.h:154
pcl
Definition: CameraOpenni.h:47
rtabmap::Memory::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: Memory.cpp:2212
rtabmap::CameraModel::imageWidth
int imageWidth() const
Definition: CameraModel.h:120
rtabmap::GetAndroidRotationFromColorCameraToDisplay
ScreenRotation GetAndroidRotationFromColorCameraToDisplay(ScreenRotation display_rotation, int color_camera_rotation)
Definition: util.h:239
USemaphore::acquire
bool acquire(int n=1, int ms=0)
Definition: USemaphore.h:101
Scene::OnTouchEvent
void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event, float x0, float y0, float x1, float y1)
Definition: scene.cpp:696
rtabmap::CameraInfoEvent::key
const std::string & key() const
Definition: CameraMobile.h:50
Scene::Render
int Render(const float *uvsTransformed=0, glm::mat4 arViewMatrix=glm::mat4(0), glm::mat4 arProjectionMatrix=glm::mat4(0), const rtabmap::Mesh &occlusionMesh=rtabmap::Mesh(), bool mapping=false)
Definition: scene.cpp:385
RTABMapApp::recover
bool recover(const std::string &from, const std::string &to)
Definition: RTABMapApp.cpp:2609
rtabmap::uncompressImage
cv::Mat RTABMAP_CORE_EXPORT uncompressImage(const cv::Mat &bytes)
Definition: Compression.cpp:130
UINFO
#define UINFO(...)
RTABMapApp::getRtabmapParameters
rtabmap::ParametersMap getRtabmapParameters()
Definition: RTABMapApp.cpp:124
name
rtabmap::util3d::createPolygonIndexes
void RTABMAP_CORE_EXPORT createPolygonIndexes(const std::vector< pcl::Vertices > &polygons, int cloudSize, std::vector< std::set< int > > &neighborPolygons, std::vector< std::set< int > > &vertexPolygons)
Given a set of polygons, create two indexes: polygons to neighbor polygons and vertices to polygons.
Definition: util3d_surface.cpp:94
rtabmap::RtabmapEventInit::kInitialized
@ kInitialized
Definition: RtabmapEvent.h:141
rtabmap::Statistics
Definition: Statistics.h:53
Scene::getScreenRotation
rtabmap::ScreenRotation getScreenRotation() const
Definition: scene.h:66
UFile::erase
static int erase(const std::string &filePath)
Definition: UFile.cpp:58
RTABMapApp::swiftInitCallback
void(* swiftInitCallback)(void *, int, const char *)
Definition: RTABMapApp.h:300
RTABMapApp::setSmoothing
void setSmoothing(bool enabled)
Definition: RTABMapApp.cpp:2393
RTABMapApp::setGridRotation
void setGridRotation(float value)
Definition: RTABMapApp.cpp:2300
rtabmap::RtabmapEventInit
Definition: RtabmapEvent.h:135
ParamEvent.h
rtabmap::Optimizer::optimizeBA
virtual std::map< int, Transform > optimizeBA(int rootId, const std::map< int, Transform > &poses, const std::multimap< int, Link > &links, const std::map< int, std::vector< CameraModel > > &models, std::map< int, cv::Point3f > &points3DMap, const std::map< int, std::map< int, FeatureBA > > &wordReferences, std::set< int > *outliers=0)
Definition: Optimizer.cpp:430
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
RTABMapApp::graphOptimization_
bool graphOptimization_
Definition: RTABMapApp.h:220
util3d_surface.h
rtabmap::ProgressionStatus::getMax
int getMax() const
Definition: ProgressionStatus.h:69
RTABMapApp::appendMode_
bool appendMode_
Definition: RTABMapApp.h:229
x1
x1
rtabmap::CameraMobile::getTextureId
GLuint getTextureId()
Definition: CameraMobile.h:118
RTABMapApp::setFullResolution
void setFullResolution(bool enabled)
Definition: RTABMapApp.cpp:2385
ssize_t
Py_ssize_t ssize_t
RTABMapApp.h
RTABMapApp::progressionStatus_
rtabmap::ProgressionStatus progressionStatus_
Definition: RTABMapApp.h:296
Scene::setScreenRotation
void setScreenRotation(rtabmap::ScreenRotation colorCameraToDisplayRotation)
Definition: scene.h:67
RTABMapApp::totalPoints_
int totalPoints_
Definition: RTABMapApp.h:255
UTimer::now
static double now()
Definition: UTimer.cpp:80
RTABMapApp::rawScanSaved_
bool rawScanSaved_
Definition: RTABMapApp.h:224
RTABMapApp::setAppendMode
void setAppendMode(bool enabled)
Definition: RTABMapApp.cpp:2409
timer
s
RealScalar s
RTABMapApp::setGraphOptimization
void setGraphOptimization(bool enabled)
Definition: RTABMapApp.cpp:2331
PostRenderEvent::rtabmapEvent_
rtabmap::RtabmapEvent * rtabmapEvent_
Definition: RTABMapApp.cpp:1130
rtabmap::ParamEvent
Definition: ParamEvent.h:41
rtabmap::VWDictionary::getVisualWords
const std::map< int, VisualWord * > & getVisualWords() const
Definition: VWDictionary.h:97
Scene::getViewPortWidth
int getViewPortWidth() const
Definition: scene.h:63
Scene::clear
void clear()
Definition: scene.cpp:190
UEventsHandler::registerToEventsManager
void registerToEventsManager()
Definition: UEventsHandler.cpp:29
RTABMapApp::setOnlineBlending
void setOnlineBlending(bool enabled)
Definition: RTABMapApp.cpp:2271
rtabmap::util3d::cleanTextureMesh
void RTABMAP_CORE_EXPORT cleanTextureMesh(pcl::TextureMesh &textureMesh, int minClusterSize)
Definition: util3d_surface.cpp:873
rtabmap::CameraMobile::setGPS
void setGPS(const GPS &gps)
Definition: CameraMobile.cpp:206
Scene::setGridColor
void setGridColor(float r, float g, float b)
Definition: scene.cpp:955
RTABMapApp::setScreenRotation
void setScreenRotation(int displayRotation, int cameraRotation)
Definition: RTABMapApp.cpp:343
Scene::setGridVisible
void setGridVisible(bool visible)
Definition: scene.cpp:733
rtflann::uchar
unsigned char uchar
Definition: matrix.h:69
rtabmap::Rtabmap::getMemory
const Memory * getMemory() const
Definition: Rtabmap.h:147
RTABMapApp::setRawScanSaved
void setRawScanSaved(bool enabled)
Definition: RTABMapApp.cpp:2369
rtabmap::DBDriver::openConnection
bool openConnection(const std::string &url, bool overwritten=false)
Definition: DBDriver.cpp:86
rtabmap::CameraMobile::getOriginOffset
const Transform & getOriginOffset() const
Definition: CameraMobile.h:102
RTABMapApp::SetCameraType
void SetCameraType(tango_gl::GestureCamera::CameraType camera_type)
Definition: RTABMapApp.cpp:2235
VWDictionary.h
RTABMapApp::cameraDriver_
int cameraDriver_
Definition: RTABMapApp.h:212
LidarVLP16.h
tango_gl::GestureCamera::kFirstPerson
@ kFirstPerson
Definition: gesture_camera.h:29
h
const double h
rtabmap::Rtabmap::getLocalOptimizedPoses
const std::map< int, Transform > & getLocalOptimizedPoses() const
Definition: Rtabmap.h:143
RTABMapApp::gainCompensation
void gainCompensation(bool full=false)
Definition: RTABMapApp.cpp:1199
rtabmap::CameraModel::cy
double cy() const
Definition: CameraModel.h:105
RTABMapApp::~RTABMapApp
~RTABMapApp()
Definition: RTABMapApp.cpp:319
UDirectory.h
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
RTABMapApp::mappingParameters_
rtabmap::ParametersMap mappingParameters_
Definition: RTABMapApp.h:243
rtabmap::CameraModel
Definition: CameraModel.h:38
RTABMapApp::setLocalizationMode
void setLocalizationMode(bool enabled)
Definition: RTABMapApp.cpp:2317
RTABMapApp::RTABMapApp
RTABMapApp()
Definition: RTABMapApp.cpp:203
RTABMapApp::exporting_
bool exporting_
Definition: RTABMapApp.h:248
RTABMapApp::setCameraColor
void setCameraColor(bool enabled)
Definition: RTABMapApp.cpp:2377
Scene::isMeshRendering
bool isMeshRendering() const
Definition: scene.h:153
PostRenderEvent
Definition: RTABMapApp.cpp:1113
rtabmap::util3d::createTextureMesh
pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT createTextureMesh(const pcl::PolygonMesh::Ptr &mesh, const std::map< int, Transform > &poses, const std::map< int, CameraModel > &cameraModels, const std::map< int, cv::Mat > &cameraDepths, float maxDistance=0.0f, float maxDepthError=0.0f, float maxAngle=0.0f, int minClusterSize=50, const std::vector< float > &roiRatios=std::vector< float >(), const ProgressState *state=0, std::vector< std::map< int, pcl::PointXY > > *vertexToPixels=0, bool distanceToCamPolicy=false)
Definition: util3d_surface.cpp:675
rtabmap::util3d::appendMesh
void RTABMAP_CORE_EXPORT appendMesh(pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudA, std::vector< pcl::Vertices > &polygonsA, const pcl::PointCloud< pcl::PointXYZRGBNormal > &cloudB, const std::vector< pcl::Vertices > &polygonsB)
Definition: util3d_surface.cpp:283
RTABMapApp::logHandler_
rtabmap::LogHandler * logHandler_
Definition: RTABMapApp.h:217
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::Optimizer
Definition: Optimizer.h:61
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
RTABMapApp::setWireframe
void setWireframe(bool enabled)
Definition: RTABMapApp.cpp:2312
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
tango_gl::vertices
static const float vertices[]
Definition: quad.cpp:39
Scene::isMapRendering
bool isMapRendering() const
Definition: scene.h:152
rtabmap::Mesh::normals
pcl::PointCloud< pcl::Normal >::Ptr normals
Definition: util.h:178
count
Index count
rtabmap::ProgressState::setCanceled
void setCanceled(bool canceled)
Definition: ProgressState.h:47
rtabmap::Optimizer::create
static Optimizer * create(const ParametersMap &parameters)
Definition: Optimizer.cpp:72
RTABMapApp::maxCloudDepth_
float maxCloudDepth_
Definition: RTABMapApp.h:231
RTABMapApp::isBuiltWith
bool isBuiltWith(int cameraDriver) const
Definition: RTABMapApp.cpp:843
RTABMapApp::setDataRecorderMode
void setDataRecorderMode(bool enabled)
Definition: RTABMapApp.cpp:2420
end
end
RTABMapApp::sensorEvents_
std::list< rtabmap::SensorEvent > sensorEvents_
Definition: RTABMapApp.h:277
RTABMapApp::postProcessing_
bool postProcessing_
Definition: RTABMapApp.h:249
Scene::addMesh
void addMesh(int id, const rtabmap::Mesh &mesh, const rtabmap::Transform &pose, bool createWireframe=false)
Definition: scene.cpp:816
rtabmap::GPS
Definition: GPS.h:35
RTABMapApp::sensorMutex_
boost::mutex sensorMutex_
Definition: RTABMapApp.h:285
RTABMapApp::setDepthFromMotion
void setDepthFromMotion(bool enabled)
Definition: RTABMapApp.cpp:2401
UDirectory::separator
static std::string separator()
Definition: UDirectory.cpp:391
type
RTABMapApp::setMeshTriangleSize
void setMeshTriangleSize(int value)
Definition: RTABMapApp.cpp:2457
Scene::setWireframe
void setWireframe(bool enabled)
Definition: scene.h:147
rtabmap::CameraMobile::uvsInitialized
bool uvsInitialized() const
Definition: CameraMobile.h:119
RTABMapApp::SetViewPort
void SetViewPort(int width, int height)
Definition: RTABMapApp.cpp:1103
rtabmap::RtabmapThread::close
void close(bool databaseSaved, const std::string &databasePath="")
Definition: RtabmapThread.cpp:118
rtabmap::PoseEvent
Definition: CameraMobile.h:60
LOG_TAG
#define LOG_TAG
Definition: tango-gl/include/tango-gl/util.h:41
rtabmap::Rtabmap::getGraph
void getGraph(std::map< int, Transform > &poses, std::multimap< int, Link > &constraints, bool optimized, bool global, std::map< int, Signature > *signatures=0, bool withImages=false, bool withScan=false, bool withUserData=false, bool withGrid=false, bool withWords=true, bool withGlobalDescriptors=true) const
Definition: Rtabmap.cpp:5479
Scene::updateCloudPolygons
void updateCloudPolygons(int id, const std::vector< pcl::Vertices > &polygons)
Definition: scene.cpp:928
rtabmap::LidarVLP16::init
virtual bool init(const std::string &calibrationFolder=".", const std::string &cameraName="") override
Definition: LidarVLP16.cpp:132
rtabmap::util3d::assembleTextureMesh
pcl::TextureMesh::Ptr RTABMAP_CORE_EXPORT assembleTextureMesh(const cv::Mat &cloudMat, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons, const std::vector< std::vector< Eigen::Vector2f > > &texCoords, cv::Mat &textures, bool mergeTextures=false)
Definition: util3d_surface.cpp:1272
PostRenderEvent::~PostRenderEvent
~PostRenderEvent()
Definition: RTABMapApp.cpp:1120
rtabmap::CameraMobile::addEnvSensor
void addEnvSensor(int type, float value)
Definition: CameraMobile.cpp:211
rtabmap::RtabmapEvent
Definition: RtabmapEvent.h:42
rtabmap::util3d::clusterPolygons
std::list< std::list< int > > RTABMAP_CORE_EXPORT clusterPolygons(const std::vector< std::set< int > > &neighborPolygons, int minClusterSize=0)
Definition: util3d_surface.cpp:131
rtabmap::util2d::registerDepth
cv::Mat RTABMAP_CORE_EXPORT registerDepth(const cv::Mat &depth, const cv::Mat &depthK, const cv::Size &colorSize, const cv::Mat &colorK, const rtabmap::Transform &transform)
Definition: util2d.cpp:1362
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_CORE_EXPORT commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
Scene::SetupViewPort
void SetupViewPort(int w, int h)
Definition: scene.cpp:219
y
Matrix3f y
Scene::getAddedClouds
std::set< int > getAddedClouds() const
Definition: scene.cpp:923
RTABMapApp::bilateralFilteringOnNextRender_
bool bilateralFilteringOnNextRender_
Definition: RTABMapApp.h:252
rtabmap::CameraAREngine
Definition: CameraAREngine.h:47
RTABMapApp::cancelProcessing
void cancelProcessing()
Definition: RTABMapApp.cpp:2629
Scene::getAddedMarkers
std::set< int > getAddedMarkers() const
Definition: scene.cpp:791
true
#define true
Definition: ConvertUTF.c:57
RTABMapApp::setMaxCloudDepth
void setMaxCloudDepth(float value)
Definition: RTABMapApp.cpp:2432
RTABMapApp::setGraphVisible
void setGraphVisible(bool visible)
Definition: RTABMapApp.cpp:2358
iterator
Scene::setBackfaceCulling
void setBackfaceCulling(bool enabled)
Definition: scene.h:146
RTABMapApp::trajectoryMode_
bool trajectoryMode_
Definition: RTABMapApp.h:223
RTABMapApp::stopCamera
void stopCamera()
Definition: RTABMapApp.cpp:963
rtabmap::ProgressionStatus::increment
void increment(int count=1) const
Definition: ProgressionStatus.h:71
rtabmap::RtabmapThread::setDetectorRate
void setDetectorRate(float rate)
Definition: RtabmapThread.cpp:102
RTABMapApp::postProcessing
int postProcessing(int approach)
Definition: RTABMapApp.cpp:3616
rtabmap::RtabmapEventInit::kInfo
@ kInfo
Definition: RtabmapEvent.h:144
PostRenderEvent::PostRenderEvent
PostRenderEvent(rtabmap::RtabmapEvent *event=0)
Definition: RTABMapApp.cpp:1116
util3d.h
RTABMapApp::bufferedStatsData_
std::map< std::string, float > bufferedStatsData_
Definition: RTABMapApp.h:261
rtabmap::LaserScan
Definition: LaserScan.h:37
RTABMapApp::rtabmapEvents_
std::list< rtabmap::RtabmapEvent * > rtabmapEvents_
Definition: RTABMapApp.h:276
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
RTABMapApp
Definition: RTABMapApp.h:53
Scene::setFOV
void setFOV(float angle)
Definition: scene.cpp:669
rtabmap::Memory::getLastWorkingSignature
const Signature * getLastWorkingSignature() const
Definition: Memory.cpp:2557
Scene::setMeshRendering
void setMeshRendering(bool enabled, bool withTexture)
Definition: scene.h:140
rtabmap::Mesh::pose
rtabmap::Transform pose
Definition: util.h:182
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
Scene::removeMarker
void removeMarker(int id)
Definition: scene.cpp:782
UTimer.h
glm::detail::tmat4x4
Definition: type_mat.hpp:47
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
RTABMapApp::exportedMeshUpdated_
bool exportedMeshUpdated_
Definition: RTABMapApp.h:264
rtabmap::LogHandler
Definition: util.h:42
BackgroundRenderer
Definition: background_renderer.h:52
RTABMapApp::setRenderingTextureDecimation
void setRenderingTextureDecimation(int value)
Definition: RTABMapApp.cpp:2472
RTABMapApp::nodesFiltering_
bool nodesFiltering_
Definition: RTABMapApp.h:221
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
conversions.h
Scene::getViewPortHeight
int getViewPortHeight() const
Definition: scene.h:64
RTABMapApp::setPointSize
void setPointSize(float value)
Definition: RTABMapApp.cpp:2288
UException
Definition: UException.h:25
RTABMapApp::openingDatabase_
bool openingDatabase_
Definition: RTABMapApp.h:247
LOGE
#define LOGE(...)
Definition: tango-gl/include/tango-gl/util.h:61
RTABMapApp::setGridVisible
void setGridVisible(bool visible)
Definition: RTABMapApp.cpp:2364
indices
indices
rtabmap::Transform::getTranslationAndEulerAngles
void getTranslationAndEulerAngles(float &x, float &y, float &z, float &roll, float &pitch, float &yaw) const
Definition: Transform.cpp:258
RTABMapApp::main_scene_
Scene main_scene_
Definition: RTABMapApp.h:272
rtabmap::util3d::organizedFastMesh
std::vector< pcl::Vertices > RTABMAP_CORE_EXPORT organizedFastMesh(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, double angleTolerance, bool quad, int trianglePixelSize, const Eigen::Vector3f &viewpoint=Eigen::Vector3f(0, 0, 0))
Definition: util3d_surface.cpp:168
RTABMapApp::clusterRatio_
float clusterRatio_
Definition: RTABMapApp.h:237
rtabmap::Mesh
Definition: util.h:165
RTABMapApp::rtabmap_
rtabmap::Rtabmap * rtabmap_
Definition: RTABMapApp.h:216
optimize
gtsam.ISAM2 optimize(List[GpsMeasurement] gps_measurements, List[ImuMeasurement] imu_measurements, gtsam.noiseModel.Diagonal sigma_init_x, gtsam.noiseModel.Diagonal sigma_init_v, gtsam.noiseModel.Diagonal sigma_init_b, gtsam.noiseModel.Diagonal noise_model_gps, KittiCalibration kitti_calibration, int first_gps_pose, int gps_skip)
Scene::updateGains
void updateGains(int id, float gainR, float gainG, float gainB)
Definition: scene.cpp:946
UEvent
Definition: UEvent.h:57
RTABMapApp::renderingTextureDecimation_
int renderingTextureDecimation_
Definition: RTABMapApp.h:239
RTABMapApp::setCloudDensityLevel
void setCloudDensityLevel(int value)
Definition: RTABMapApp.cpp:2442
GainCompensator.h
RTABMapApp::OnTouchEvent
void OnTouchEvent(int touch_count, tango_gl::GestureCamera::TouchEvent event, float x0, float y0, float x1, float y1)
Definition: RTABMapApp.cpp:2240
tango_gl::GestureCamera::CameraType
CameraType
Definition: gesture_camera.h:28
rtabmap::CameraModel::setLocalTransform
void setLocalTransform(const Transform &transform)
Definition: CameraModel.h:115
rtabmap::Rtabmap::getStatistics
const Statistics & getStatistics() const
Definition: Rtabmap.cpp:849
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
RTABMapApp::clearSceneOnNextRender_
bool clearSceneOnNextRender_
Definition: RTABMapApp.h:246
rtabmap::Mesh::cameraModel
rtabmap::CameraModel cameraModel
Definition: util.h:184
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::SensorCaptureInfo::odomPose
Transform odomPose
Definition: SensorCaptureInfo.h:74
matches
matches
rtabmap::util3d::mergeTextures
cv::Mat RTABMAP_CORE_EXPORT mergeTextures(pcl::TextureMesh &mesh, const std::map< int, cv::Mat > &images, const std::map< int, CameraModel > &calibrations, const Memory *memory=0, const DBDriver *dbDriver=0, int textureSize=4096, int textureCount=1, const std::vector< std::map< int, pcl::PointXY > > &vertexToPixels=std::vector< std::map< int, pcl::PointXY > >(), bool gainCompensation=true, float gainBeta=10.0f, bool gainRGB=true, bool blending=true, int blendingDecimation=0, int brightnessContrastRatioLow=0, int brightnessContrastRatioHigh=0, bool exposureFusion=false, const ProgressState *state=0, unsigned char blankValue=255, std::map< int, std::map< int, cv::Vec4d > > *gains=0, std::map< int, std::map< int, cv::Mat > > *blendingGains=0, std::pair< float, float > *contrastValues=0)
Definition: util3d_surface.cpp:1438
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:896
uContains
bool uContains(const std::list< V > &list, const V &value)
Definition: UStl.h:407
g_optMeshId
const int g_optMeshId
Definition: RTABMapApp.cpp:80
RTABMapApp::handleEvent
virtual bool handleEvent(UEvent *event)
Definition: RTABMapApp.cpp:3964
rtabmap::rtabmap_world_T_opengl_world
static const rtabmap::Transform rtabmap_world_T_opengl_world(0.0f, 0.0f,-1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f)
rtabmap::util3d::transformPointCloud
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT transformPointCloud(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const Transform &transform)
Definition: util3d_transforms.cpp:107
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
uint16_t
::uint16_t uint16_t
RTABMapApp::lastPostRenderEventTime_
double lastPostRenderEventTime_
Definition: RTABMapApp.h:259
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
data
int data[]
util3d_transforms.h
rtabmap::RtabmapThread::getDetectorRate
float getDetectorRate() const
Definition: RtabmapThread.h:71
UEvent::getClassName
virtual std::string getClassName() const =0
scale
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics scale
j
std::ptrdiff_t j
RTABMapApp::fullResolution_
bool fullResolution_
Definition: RTABMapApp.h:228
uNumber2Str
std::string UTILITE_EXPORT uNumber2Str(unsigned int number)
Definition: UConversion.cpp:91
Scene::background_renderer_
BackgroundRenderer * background_renderer_
Definition: scene.h:160
rtabmap::util2d::fastBilateralFiltering
cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1814
stats
bool stats
RTABMapApp::setMeshAngleTolerance
void setMeshAngleTolerance(float value)
Definition: RTABMapApp.cpp:2447
RTABMapApp::cloudDensityLevel_
int cloudDensityLevel_
Definition: RTABMapApp.h:233
UConversion.h
Some conversion functions.
rtabmap::Rtabmap::triggerNewMap
int triggerNewMap()
Definition: Rtabmap.cpp:887
RTABMapApp::poseMutex_
boost::mutex poseMutex_
Definition: RTABMapApp.h:286
rtabmap::Mesh::texture
cv::Mat texture
Definition: util.h:191
RTABMapApp::updateMeshDecimation
int updateMeshDecimation(int width, int height)
Definition: RTABMapApp.cpp:774
RTABMapApp::optRefId_
int optRefId_
Definition: RTABMapApp.h:267
rtabmap::GainCompensator
Definition: GainCompensator.h:44
util3d_filtering.h
uMax3
T uMax3(const T &a, const T &b, const T &c)
Definition: UMath.h:78
RTABMapApp::rtabmapThread_
rtabmap::RtabmapThread * rtabmapThread_
Definition: RTABMapApp.h:215
RTABMapApp::filterOrganizedPolygons
std::vector< pcl::Vertices > filterOrganizedPolygons(const std::vector< pcl::Vertices > &polygons, int cloudSize) const
Definition: RTABMapApp.cpp:983
Scene::setMapRendering
void setMapRendering(bool enabled)
Definition: scene.h:139
RTABMapApp::useExternalLidar_
bool useExternalLidar_
Definition: RTABMapApp.h:230
rtabmap::util3d::laserScanToPointCloudRGB
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT laserScanToPointCloudRGB(const LaserScan &laserScan, const Transform &transform=Transform(), unsigned char r=100, unsigned char g=100, unsigned char b=100)
Definition: util3d.cpp:2322
rtabmap::opengl_world_T_rtabmap_world
static const rtabmap::Transform opengl_world_T_rtabmap_world(0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f)
all
static const Eigen::internal::all_t all
RTABMapApp::setBackfaceCulling
void setBackfaceCulling(bool enabled)
Definition: RTABMapApp.cpp:2308
LOW_RES_PIX
#define LOW_RES_PIX
Definition: RTABMapApp.cpp:77
rtabmap::optical_T_opengl
static const rtabmap::Transform optical_T_opengl(1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, -1.0f, 0.0f)
CameraARCore.h
Rtabmap.h
RTABMapApp::setMaxGainRadius
void setMaxGainRadius(float value)
Definition: RTABMapApp.cpp:2467
Scene::GetCameraPose
rtabmap::Transform GetCameraPose() const
Definition: scene.h:87
x0
x0
RTABMapApp::postOdometryEvent
void postOdometryEvent(rtabmap::Transform pose, float rgb_fx, float rgb_fy, float rgb_cx, float rgb_cy, float depth_fx, float depth_fy, float depth_cx, float depth_cy, const rtabmap::Transform &rgbFrame, const rtabmap::Transform &depthFrame, double stamp, double depthStamp, const void *yPlane, const void *uPlane, const void *vPlane, int yPlaneLen, int rgbWidth, int rgbHeight, int rgbFormat, const void *depth, int depthLen, int depthWidth, int depthHeight, int depthFormat, const void *conf, int confLen, int confWidth, int confHeight, int confFormat, const float *points, int pointsLen, int pointsChannels, const rtabmap::Transform &viewMatrix, float p00, float p11, float p02, float p12, float p22, float p32, float p23, float t0, float t1, float t2, float t3, float t4, float t5, float t6, float t7)
Definition: RTABMapApp.cpp:3733
rtabmap::CameraMobile::update
void update(const SensorData &data, const Transform &pose, const glm::mat4 &viewMatrix, const glm::mat4 &projectionMatrix, const float *texCoord)
Definition: CameraMobile.cpp:216
Scene::addMarker
void addMarker(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:749
rtabmap::CameraModel::isValidForProjection
bool isValidForProjection() const
Definition: CameraModel.h:87
Optimizer.h
rtabmap::Memory::getDatabaseMemoryUsed
int getDatabaseMemoryUsed() const
Definition: Memory.cpp:1707
glm::max
GLM_FUNC_DECL genType max(genType const &x, genType const &y)
tango_gl::GestureCamera::TouchEvent
TouchEvent
Definition: gesture_camera.h:36
RTABMapApp::maxGainRadius_
float maxGainRadius_
Definition: RTABMapApp.h:238
BackgroundRenderer::InitializeGlContent
void InitializeGlContent(GLuint textureId, bool oes)
Definition: background_renderer.cc:134
RTABMapApp::save
void save(const std::string &databasePath)
Definition: RTABMapApp.cpp:2557
RTABMapApp::createdMeshes_
std::map< int, rtabmap::Mesh > createdMeshes_
Definition: RTABMapApp.h:291
RTABMapApp::setMeshDecimationFactor
void setMeshDecimationFactor(float value)
Definition: RTABMapApp.cpp:2452
RTABMapApp::sensorCaptureThread_
rtabmap::SensorCaptureThread * sensorCaptureThread_
Definition: RTABMapApp.h:214
rtabmap::Parameters::getRemovedParameters
static const std::map< std::string, std::pair< bool, std::string > > & getRemovedParameters()
Definition: Parameters.cpp:233
RTABMapApp::visualizingMesh_
bool visualizingMesh_
Definition: RTABMapApp.h:263
Scene::setLighting
void setLighting(bool enabled)
Definition: scene.h:145
rtabmap::CameraTango
Definition: CameraTango.h:46
rtabmap::ProgressState::isCanceled
bool isCanceled() const
Definition: ProgressState.h:51
UEventsHandler::unregisterFromEventsManager
void unregisterFromEventsManager()
Definition: UEventsHandler.cpp:33
Scene::setOrthoCropFactor
void setOrthoCropFactor(float value)
Definition: scene.cpp:673
rtabmap::CameraMobile::getPose
virtual bool getPose(double epochStamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.06)
Definition: CameraMobile.cpp:105
rtabmap::CameraModel::fy
double fy() const
Definition: CameraModel.h:103
UASSERT
#define UASSERT(condition)
z
z
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
UTimer::restart
double restart()
Definition: UTimer.h:94
x
x
UThread::start
void start()
Definition: UThread.cpp:122
RTABMapApp::optMesh_
pcl::TextureMesh::Ptr optMesh_
Definition: RTABMapApp.h:265
p
Point3_ p(2)
Scene::InitGLContent
void InitGLContent()
Definition: scene.cpp:118
Scene::setBlending
void setBlending(bool enabled)
Definition: scene.h:138
RTABMapApp::cameraColor_
bool cameraColor_
Definition: RTABMapApp.h:227
Scene::setBackgroundColor
void setBackgroundColor(float r, float g, float b)
Definition: scene.h:148
rtabmap::Memory::getNodeData
SensorData getNodeData(int locationId, bool images, bool scan, bool userData, bool occupancyGrid) const
Definition: Memory.cpp:4113
DBDriver.h
PostRenderEvent::getRtabmapEvent
const rtabmap::RtabmapEvent * getRtabmapEvent() const
Definition: RTABMapApp.cpp:1128
glm::angle
GLM_FUNC_DECL T angle(detail::tquat< T, P > const &x)
Scene::addCloud
void addCloud(int id, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloud, const pcl::IndicesPtr &indices, const rtabmap::Transform &pose)
Definition: scene.cpp:796
rtabmap::CameraMobile::resetOrigin
void resetOrigin()
Definition: CameraMobile.cpp:95
rtabmap::util3d::meshDecimation
pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT meshDecimation(const pcl::PolygonMesh::Ptr &mesh, float factor)
Definition: util3d_surface.cpp:3761
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::CameraModel::opticalRotation
static Transform opticalRotation()
Definition: CameraModel.h:45
RTABMapApp::writeExportedMesh
bool writeExportedMesh(const std::string &directory, const std::string &name)
Definition: RTABMapApp.cpp:3506
RTABMapApp::setLighting
void setLighting(bool enabled)
Definition: RTABMapApp.cpp:2304
time
#define time
rtabmap::CameraMobile::updateOnRender
void updateOnRender()
Definition: CameraMobile.cpp:283
RTABMapApp::cameraJustInitialized_
bool cameraJustInitialized_
Definition: RTABMapApp.h:254
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
Scene::setCloudVisible
void setCloudVisible(int id, bool visible)
Definition: scene.cpp:899
RTABMapApp::setDepthConfidence
void setDepthConfidence(int value)
Definition: RTABMapApp.cpp:2486
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
RTABMapApp::odomCloudShown_
bool odomCloudShown_
Definition: RTABMapApp.h:219
rtabmap::LidarVLP16
Definition: LidarVLP16.h:45
rtabmap::util3d::filterNaNPointsFromMesh
std::vector< int > RTABMAP_CORE_EXPORT filterNaNPointsFromMesh(const pcl::PointCloud< pcl::PointXYZRGB > &cloud, const std::vector< pcl::Vertices > &polygons, pcl::PointCloud< pcl::PointXYZRGB > &outputCloud, std::vector< pcl::Vertices > &outputPolygons)
Definition: util3d_surface.cpp:415
offset
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size set set xzeroaxis lt lw set x2zeroaxis lt lw set yzeroaxis lt lw set y2zeroaxis lt lw set tics in set ticslevel set tics set mxtics default set mytics default set mx2tics default set my2tics default set xtics border mirror norotate autofreq set ytics border mirror norotate autofreq set ztics border nomirror norotate autofreq set nox2tics set noy2tics set timestamp bottom norotate offset
rtabmap::CameraARCore
Definition: CameraARCore.h:51
rtabmap::Mesh::polygons
std::vector< pcl::Vertices > polygons
Definition: util.h:180
RTABMapApp::setBackgroundColor
void setBackgroundColor(float gray)
Definition: RTABMapApp.cpp:2478
key
const gtsam::Symbol key( 'X', 0)
UWARN
#define UWARN(...)
RTABMapApp::smoothing_
bool smoothing_
Definition: RTABMapApp.h:225
rtabmap::compressData2
cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat &data)
Definition: Compression.cpp:201
rtabmap::Rtabmap::init
void init(const ParametersMap &parameters, const std::string &databasePath="", bool loadDatabaseParameters=false)
Definition: Rtabmap.cpp:320
LOGI
#define LOGI(...)
Definition: tango-gl/include/tango-gl/util.h:53
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
rtabmap::DBDriver
Definition: DBDriver.h:62
rtabmap::CameraMobile::poseReceived
void poseReceived(const Transform &pose, double deviceStamp)
Definition: CameraMobile.cpp:164
Scene::setPointSize
void setPointSize(float size)
Definition: scene.h:141
Scene::SetCameraPose
void SetCameraPose(const rtabmap::Transform &pose)
Definition: scene.cpp:659
rtabmap::Rtabmap::setOptimizedPoses
void setOptimizedPoses(const std::map< int, Transform > &poses, const std::multimap< int, Link > &constraints)
Definition: Rtabmap.cpp:4899
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
y1
CEPHES_EXTERN_EXPORT double y1(double x)
RTABMapApp::setMappingParameter
int setMappingParameter(const std::string &key, const std::string &value)
Definition: RTABMapApp.cpp:2495
nodes
KeyVector nodes
M_PI
#define M_PI
Definition: tango-gl/include/tango-gl/util.h:66
RTABMapApp::setOrthoCropFactor
void setOrthoCropFactor(float value)
Definition: RTABMapApp.cpp:2296
rtabmap::CameraMobile::setScreenRotationAndSize
virtual void setScreenRotationAndSize(ScreenRotation colorCameraToDisplayRotation, int width, int height)
Definition: CameraMobile.h:114
rtabmap::Rtabmap::getWMSize
int getWMSize() const
Definition: Rtabmap.cpp:780
RTABMapApp::filterPolygonsOnNextRender_
bool filterPolygonsOnNextRender_
Definition: RTABMapApp.h:250
qz
RealQZ< MatrixXf > qz(4)
e
Array< double, 1, 3 > e(1./3., 0.5, 2.)
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
RTABMapApp::setClusterRatio
void setClusterRatio(float value)
Definition: RTABMapApp.cpp:2462
Scene::hasCloud
bool hasCloud(int id) const
Definition: scene.cpp:908
RTABMapApp::takeScreenshotOnNextRender_
bool takeScreenshotOnNextRender_
Definition: RTABMapApp.h:253
RTABMapApp::renderingMutex_
boost::mutex renderingMutex_
Definition: RTABMapApp.h:287
rtabmap::util3d::assemblePolygonMesh
pcl::PolygonMesh::Ptr RTABMAP_CORE_EXPORT assemblePolygonMesh(const cv::Mat &cloudMat, const std::vector< std::vector< RTABMAP_PCL_INDEX > > &polygons)
Definition: util3d_surface.cpp:1395
rtabmap::Transform
Definition: Transform.h:41
USemaphore::release
void release(int n=1)
Definition: USemaphore.h:168
Memory.h
RTABMapApp::rtabmapMutex_
boost::mutex rtabmapMutex_
Definition: RTABMapApp.h:283
RTABMapApp::postCameraPoseEvent
void postCameraPoseEvent(float x, float y, float z, float qx, float qy, float qz, float qw, double stamp)
Definition: RTABMapApp.cpp:3715
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
util2d.h
LOGW
#define LOGW(...)
Definition: tango-gl/include/tango-gl/util.h:54
rtabmap::GainCompensator::getGain
double getGain(int id, double *r=0, double *g=0, double *b=0) const
Definition: GainCompensator.cpp:475
full
@ full
Definition: lz4.c:365
RTABMapApp::filterPolygons
std::vector< pcl::Vertices > filterPolygons(const std::vector< pcl::Vertices > &polygons, int cloudSize) const
Definition: RTABMapApp.cpp:1050
glm::rotation
GLM_FUNC_DECL detail::tquat< T, P > rotation(detail::tvec3< T, P > const &orig, detail::tvec3< T, P > const &dest)
Scene::SetCameraType
void SetCameraType(tango_gl::GestureCamera::CameraType camera_type)
Definition: scene.cpp:655
Graph.h
rtabmap::SensorEvent::info
const SensorCaptureInfo & info() const
Definition: SensorEvent.h:81
RTABMapApp::setMapCloudShown
void setMapCloudShown(bool shown)
Definition: RTABMapApp.cpp:2275
y0
CEPHES_EXTERN_EXPORT double y0(double x)
RTABMapApp::renderingTime_
float renderingTime_
Definition: RTABMapApp.h:258
RTABMapApp::postExportation
bool postExportation(bool visualize)
Definition: RTABMapApp.cpp:3430
rtabmap::SensorEvent::data
const SensorData & data() const
Definition: SensorEvent.h:79
rtabmap::CameraMobile::getStampEpochOffset
double getStampEpochOffset() const
Definition: CameraMobile.h:109
RTABMapApp::openDatabase
int openDatabase(const std::string &databasePath, bool databaseInMemory, bool optimize, bool clearDatabase)
Definition: RTABMapApp.cpp:356
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
Scene::updateGraph
void updateGraph(const std::map< int, rtabmap::Transform > &poses, const std::multimap< int, rtabmap::Link > &links)
Definition: scene.cpp:717
iter
iterator iter(handle obj)
LOGD
#define LOGD(...)
Definition: tango-gl/include/tango-gl/util.h:52
rtabmap::DBDriver::loadOptimizedMesh
cv::Mat loadOptimizedMesh(std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > *polygons=0, std::vector< std::vector< Eigen::Vector2f > > *texCoords=0, cv::Mat *textures=0) const
Definition: DBDriver.cpp:1235
rtabmap::Rtabmap::detectMoreLoopClosures
int detectMoreLoopClosures(float clusterRadiusMax=0.5f, float clusterAngle=M_PI/6.0f, int iterations=1, bool intraSession=true, bool interSession=true, const ProgressState *state=0, float clusterRadiusMin=0.0f)
Definition: Rtabmap.cpp:5595
rtabmap::ScreenRotation
ScreenRotation
Definition: util.h:194
RTABMapApp::minCloudDepth_
float minCloudDepth_
Definition: RTABMapApp.h:232
env
rtabmap::Transform::isIdentity
bool isIdentity() const
Definition: Transform.cpp:136
rtabmap::Rtabmap::parseParameters
void parseParameters(const ParametersMap &parameters)
Definition: Rtabmap.cpp:545
rtabmap::GainCompensator::feed
void feed(const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudA, const pcl::PointCloud< pcl::PointXYZRGB >::Ptr &cloudB, const Transform &transformB)
Definition: GainCompensator.cpp:57
Scene::updateMesh
void updateMesh(int id, const rtabmap::Mesh &mesh)
Definition: scene.cpp:937
RTABMapApp::depthFromMotion_
bool depthFromMotion_
Definition: RTABMapApp.h:226
RTABMapApp::gainCompensationOnNextRender_
int gainCompensationOnNextRender_
Definition: RTABMapApp.h:251
UStl.h
Wrappers of STL for convenient functions.
rtabmap::Mesh::indices
pcl::IndicesPtr indices
Definition: util.h:179
RTABMapApp::backgroundColor_
float backgroundColor_
Definition: RTABMapApp.h:240
rtabmap::Memory::getVWDictionary
const VWDictionary * getVWDictionary() const
Definition: Memory.cpp:1246
rtabmap::Rtabmap::close
void close(bool databaseSaved=true, const std::string &ouputDatabasePath="")
Definition: Rtabmap.cpp:463
RTABMapApp::screenshotReady_
USemaphore screenshotReady_
Definition: RTABMapApp.h:289
RTABMapApp::fpsTime_
UTimer fpsTime_
Definition: RTABMapApp.h:274
rtabmap::CameraMobile
Definition: CameraMobile.h:71
rtabmap::CameraInfoEvent
Definition: CameraMobile.h:44
v
Array< int, Dynamic, 1 > v
UDEBUG
#define UDEBUG(...)
RTABMapApp::setFOV
void setFOV(float angle)
Definition: RTABMapApp.cpp:2292
else
else
RTABMapApp::setGPS
void setGPS(const rtabmap::GPS &gps)
Definition: RTABMapApp.cpp:2539
rtabmap::ProgressionStatus::setSwiftCallback
void setSwiftCallback(void *classPtr, void(*callback)(void *, int, int))
Definition: ProgressionStatus.h:49
UEventsManager::post
static void post(UEvent *event, bool async=true, const UEventsSender *sender=0)
Definition: UEventsManager.cpp:54
RTABMapApp::status_
std::pair< rtabmap::RtabmapEventInit::Status, std::string > status_
Definition: RTABMapApp.h:294
UTimer
Definition: UTimer.h:46
rtabmap::CameraModel::imageHeight
int imageHeight() const
Definition: CameraModel.h:121
Scene::setMarkerPose
void setMarkerPose(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:765
RTABMapApp::setupSwiftCallbacks
void setupSwiftCallbacks(void *classPtr, void(*progressCallback)(void *, int, int), void(*initCallback)(void *, int, const char *), void(*statsUpdatedCallback)(void *, int, int, int, int, float, int, int, int, int, int, int, float, int, float, int, float, float, float, float, int, int, float, float, float, float, float, float))
Definition: RTABMapApp.cpp:297
Scene::setTraceVisible
void setTraceVisible(bool visible)
Definition: scene.cpp:738
RTABMapApp::smoothMesh
bool smoothMesh(int id, rtabmap::Mesh &mesh)
Definition: RTABMapApp.cpp:1134
rtabmap::Mesh::cloud
pcl::PointCloud< pcl::PointXYZRGB >::Ptr cloud
Definition: util.h:177
rtabmap::CameraMobile::getDeviceTColorCamera
const Transform & getDeviceTColorCamera() const
Definition: CameraMobile.h:112
float
float
RTABMapApp::totalPolygons_
int totalPolygons_
Definition: RTABMapApp.h:256
Scene::setFrustumVisible
void setFrustumVisible(bool visible)
Definition: scene.cpp:743
rtabmap::Optimizer::kTypeG2O
@ kTypeG2O
Definition: Optimizer.h:67
p22
p22
RTABMapApp::setPausedMapping
void setPausedMapping(bool paused)
Definition: RTABMapApp.cpp:2246
RTABMapApp::optTexture_
cv::Mat optTexture_
Definition: RTABMapApp.h:266
rtabmap::util3d::cloudFromDepth
RTABMAP_DEPRECATED pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT cloudFromDepth(const cv::Mat &imageDepth, float cx, float cy, float fx, float fy, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0)
Definition: util3d.cpp:266
rtabmap::util3d::getMinMax3D
void RTABMAP_CORE_EXPORT getMinMax3D(const cv::Mat &laserScan, cv::Point3f &min, cv::Point3f &max)
Definition: util3d.cpp:2613
rtabmap::Rtabmap
Definition: Rtabmap.h:54
UThread::isRunning
bool isRunning() const
Definition: UThread.cpp:245
RTABMapApp::dataRecorderMode_
bool dataRecorderMode_
Definition: RTABMapApp.h:245
Scene::setGridRotation
void setGridRotation(float angleDeg)
Definition: scene.cpp:677
RTABMapApp::meshAngleToleranceDeg_
float meshAngleToleranceDeg_
Definition: RTABMapApp.h:235
NULL
#define NULL
rtabmap::Rtabmap::getLocalConstraints
const std::multimap< int, Link > & getLocalConstraints() const
Definition: Rtabmap.h:144
false
#define false
Definition: ConvertUTF.c:56
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
RTABMapApp::Render
int Render()
Definition: RTABMapApp.cpp:1255
RTABMapApp::optRefPose_
rtabmap::Transform * optRefPose_
Definition: RTABMapApp.h:268
RTABMapApp::exportMesh
bool exportMesh(float cloudVoxelSize, bool regenerateCloud, bool meshing, int textureSize, int textureCount, int normalK, bool optimized, float optimizedVoxelSize, int optimizedDepth, int optimizedMaxPolygons, float optimizedColorRadius, bool optimizedCleanWhitePolygons, int optimizedMinClusterSize, float optimizedMaxTextureDistance, int optimizedMinTextureClusterSize, bool blockRendering)
Definition: RTABMapApp.cpp:2635
PostRenderEvent::getClassName
virtual std::string getClassName() const
Definition: RTABMapApp.cpp:1127
RTABMapApp::rawPoses_
std::map< int, rtabmap::Transform > rawPoses_
Definition: RTABMapApp.h:292
RTABMapApp::meshDecimationFactor_
float meshDecimationFactor_
Definition: RTABMapApp.h:236
rtabmap::glmFromTransform
glm::mat4 glmFromTransform(const rtabmap::Transform &transform)
Definition: util.h:124
rtabmap::CameraMobile::setOcclusionImage
void setOcclusionImage(const cv::Mat &image, const CameraModel &model)
Definition: CameraMobile.h:124
rtabmap::Memory::saveOptimizedMesh
void saveOptimizedMesh(const cv::Mat &cloud, const std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > > &polygons=std::vector< std::vector< std::vector< RTABMAP_PCL_INDEX > > >(), const std::vector< std::vector< Eigen::Vector2f > > &texCoords=std::vector< std::vector< Eigen::Vector2f > >(), const cv::Mat &textures=cv::Mat()) const
Definition: Memory.cpp:2196
RTABMapApp::depthConfidence_
int depthConfidence_
Definition: RTABMapApp.h:241
Scene::setGraphVisible
void setGraphVisible(bool visible)
Definition: scene.cpp:728
rtabmap::CameraInfoEvent::value
const std::string & value() const
Definition: CameraMobile.h:51
RTABMapApp::startCamera
bool startCamera()
Definition: RTABMapApp.cpp:877
RTABMapApp::poseEvents_
std::list< rtabmap::Transform > poseEvents_
Definition: RTABMapApp.h:278
rtabmap::Memory::savePreviewImage
void savePreviewImage(const cv::Mat &image) const
Definition: Memory.cpp:2123
uStr2Float
float UTILITE_EXPORT uStr2Float(const std::string &str)
Definition: UConversion.cpp:138
Scene::hasMesh
bool hasMesh(int id) const
Definition: scene.cpp:913
rtabmap::ProgressionStatus::finish
void finish()
Definition: ProgressionStatus.h:76
t
Point2 t(10, 10)
UFile.h
rtabmap
Definition: CameraARCore.cpp:35
UFile::exists
bool exists()
Definition: UFile.h:104
RTABMapApp::InitializeGLContent
void InitializeGLContent()
Definition: RTABMapApp.cpp:1093
rtabmap::CameraInfoEvent::type
int type() const
Definition: CameraMobile.h:49
UEventsManager.h
RTABMapApp::setMeshRendering
void setMeshRendering(bool enabled, bool withTexture)
Definition: RTABMapApp.cpp:2284
rtabmap::CameraModel::localTransform
const Transform & localTransform() const
Definition: CameraModel.h:116
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
rtabmap::databaseRecovery
bool RTABMAP_CORE_EXPORT databaseRecovery(const std::string &corruptedDatabase, bool keepCorruptedDatabase=true, std::string *errorMsg=0, ProgressState *progressState=0)
Definition: Recovery.cpp:39
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
UTimer::elapsed
double elapsed()
Definition: UTimer.h:75
CameraTango.h
CameraAREngine.h
rtabmap::RtabmapEventInit::kInitializing
@ kInitializing
Definition: RtabmapEvent.h:140
rs
Definition: CameraRealSense.h:36
value
value
RTABMapApp::setNodesFiltering
void setNodesFiltering(bool enabled)
Definition: RTABMapApp.cpp:2353
RTABMapApp::camera_
rtabmap::CameraMobile * camera_
Definition: RTABMapApp.h:213
i
int i
Scene::GetCameraType
tango_gl::GestureCamera::CameraType GetCameraType() const
Definition: scene.h:84
rtabmap::ProgressionStatus::reset
void reset(int max)
Definition: ProgressionStatus.h:56
Scene::hasMarker
bool hasMarker(int id) const
Definition: scene.cpp:778
conf
RTABMapApp::setOdomCloudShown
void setOdomCloudShown(bool shown)
Definition: RTABMapApp.cpp:2279
RTABMapApp::lastDrawnCloudsCount_
int lastDrawnCloudsCount_
Definition: RTABMapApp.h:257
rtabmap::SensorEvent
Definition: SensorEvent.h:37
rtabmap::Mesh::gains
double gains[3]
Definition: util.h:185
rtabmap::Mesh::texCoords
std::vector< Eigen::Vector2f > texCoords
Definition: util.h:189
RTABMapApp::lastPoseEventTime_
double lastPoseEventTime_
Definition: RTABMapApp.h:260
RTABMapApp::addEnvSensor
void addEnvSensor(int type, float value)
Definition: RTABMapApp.cpp:2548
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
RTABMapApp::meshesMutex_
boost::mutex meshesMutex_
Definition: RTABMapApp.h:284
Scene::setCloudPose
void setCloudPose(int id, const rtabmap::Transform &pose)
Definition: scene.cpp:889
rtabmap::Signature
Definition: Signature.h:48
Recovery.h
rtabmap::RtabmapThread
Definition: RtabmapThread.h:51
uBool2Str
std::string UTILITE_EXPORT uBool2Str(bool boolean)
Definition: UConversion.cpp:156
RTABMapApp::cameraMutex_
boost::mutex cameraMutex_
Definition: RTABMapApp.h:282
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28
RTABMapApp::swiftClassPtr_
void * swiftClassPtr_
Definition: RTABMapApp.h:299
rtabmap::DBDriver::create
static DBDriver * create(const ParametersMap &parameters=ParametersMap())
Definition: DBDriver.cpp:41
RTABMapApp::setTrajectoryMode
void setTrajectoryMode(bool enabled)
Definition: RTABMapApp.cpp:2325

rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:15