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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32