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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Sun Dec 1 2024 03:42:51