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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jan 23 2023 03:37:30