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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00