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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Apr 28 2025 02:45:59