00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include <tango-gl/conversions.h>
00029
00030 #include "RTABMapApp.h"
00031
00032 #include <rtabmap/core/Rtabmap.h>
00033 #include <rtabmap/core/util3d.h>
00034 #include <rtabmap/core/util3d_transforms.h>
00035 #include <rtabmap/core/util3d_filtering.h>
00036 #include <rtabmap/core/Graph.h>
00037 #include <rtabmap/utilite/UEventsManager.h>
00038 #include <rtabmap/utilite/UStl.h>
00039 #include <rtabmap/utilite/UDirectory.h>
00040 #include <rtabmap/utilite/UFile.h>
00041 #include <opencv2/opencv_modules.hpp>
00042 #include <rtabmap/core/util3d_surface.h>
00043 #include <rtabmap/utilite/UConversion.h>
00044 #include <rtabmap/utilite/UTimer.h>
00045 #include <rtabmap/core/ParamEvent.h>
00046 #include <rtabmap/core/Compression.h>
00047 #include <rtabmap/core/Optimizer.h>
00048 #include <rtabmap/core/VWDictionary.h>
00049 #include <rtabmap/core/Memory.h>
00050 #include <pcl/filters/extract_indices.h>
00051 #include <pcl/io/ply_io.h>
00052 #include <pcl/io/obj_io.h>
00053
00054 const int kVersionStringLength = 128;
00055
00056 static JavaVM *jvm;
00057 static jobject RTABMapActivity = 0;
00058
00059 namespace {
00060 constexpr int kTangoCoreMinimumVersion = 9377;
00061 }
00062
00063 rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
00064 {
00065 rtabmap::ParametersMap parameters;
00066
00067 parameters.insert(mappingParameters_.begin(), mappingParameters_.end());
00068
00069 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpDetectorStrategy(), std::string("6")));
00070 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kGFTTQualityLevel(), std::string("0.0001")));
00071 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
00072 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kFASTThreshold(), std::string("1")));
00073 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64")));
00074 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
00075 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
00076 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false")));
00077 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0"));
00078 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
00079 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_)));
00080 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpMaxDepth(), std::string("10")));
00081 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true")));
00082 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true")));
00083 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15")));
00084 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0")));
00085 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.05")));
00086
00087 return parameters;
00088 }
00089
00090 RTABMapApp::RTABMapApp() :
00091 camera_(0),
00092 rtabmapThread_(0),
00093 rtabmap_(0),
00094 logHandler_(0),
00095 odomCloudShown_(true),
00096 graphOptimization_(true),
00097 nodesFiltering_(false),
00098 localizationMode_(false),
00099 trajectoryMode_(false),
00100 autoExposure_(false),
00101 fullResolution_(false),
00102 maxCloudDepth_(0.0),
00103 meshTrianglePix_(1),
00104 meshAngleToleranceDeg_(10.0),
00105 clearSceneOnNextRender_(false),
00106 totalPoints_(0),
00107 totalPolygons_(0),
00108 lastDrawnCloudsCount_(0),
00109 renderingFPS_(0.0f)
00110
00111 {
00112
00113 }
00114
00115 RTABMapApp::~RTABMapApp() {
00116 if(camera_)
00117 {
00118 delete camera_;
00119 }
00120 if(rtabmapThread_)
00121 {
00122 rtabmapThread_->close(false);
00123 delete rtabmapThread_;
00124 }
00125 if(logHandler_)
00126 {
00127 delete logHandler_;
00128 }
00129 }
00130
00131 void RTABMapApp::onCreate(JNIEnv* env, jobject caller_activity)
00132 {
00133 env->GetJavaVM(&jvm);
00134 RTABMapActivity = env->NewGlobalRef(caller_activity);
00135
00136 LOGI("RTABMapApp::onCreate()");
00137 createdMeshes_.clear();
00138 rawPoses_.clear();
00139 clearSceneOnNextRender_ = true;
00140 totalPoints_ = 0;
00141 totalPolygons_ = 0;
00142 lastDrawnCloudsCount_ = 0;
00143 renderingFPS_ = 0.0f;
00144
00145 if(camera_)
00146 {
00147 delete camera_;
00148 }
00149 if(rtabmapThread_)
00150 {
00151 rtabmapThread_->close(false);
00152 delete rtabmapThread_;
00153 rtabmapThread_ = 0;
00154 rtabmap_ = 0;
00155 }
00156
00157 if(logHandler_ == 0)
00158 {
00159 logHandler_ = new LogHandler();
00160 }
00161 ULogger::setEventLevel(ULogger::kInfo);
00162 ULogger::setPrintThreadId(true);
00163
00164 this->registerToEventsManager();
00165
00166 camera_ = new rtabmap::CameraTango(fullResolution_?1:2, autoExposure_);
00167 }
00168
00169 void RTABMapApp::openDatabase(const std::string & databasePath)
00170 {
00171 this->unregisterFromEventsManager();
00172 status_.first = rtabmap::RtabmapEventInit::kInitializing;
00173 rtabmapMutex_.lock();
00174 if(rtabmapThread_)
00175 {
00176 rtabmapThread_->close(false);
00177 delete rtabmapThread_;
00178 rtabmapThread_ = 0;
00179 rtabmap_ = 0;
00180 }
00181
00182
00183 rtabmap_ = new rtabmap::Rtabmap();
00184 rtabmap::ParametersMap parameters = getRtabmapParameters();
00185
00186 rtabmap_->init(parameters, databasePath);
00187 rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
00188
00189
00190 std::map<int, rtabmap::Signature> signatures;
00191 std::map<int, rtabmap::Transform> poses;
00192 std::multimap<int, rtabmap::Link> links;
00193 rtabmap_->get3DMap(
00194 signatures,
00195 poses,
00196 links,
00197 true,
00198 true);
00199
00200 clearSceneOnNextRender_ = true;
00201 rtabmap::Statistics stats;
00202 stats.setSignatures(signatures);
00203 stats.addStatistic(rtabmap::Statistics::kMemoryWorking_memory_size(), (float)rtabmap_->getWMSize());
00204 stats.addStatistic(rtabmap::Statistics::kKeypointDictionary_size(), (float)rtabmap_->getMemory()->getVWDictionary()->getVisualWords().size());
00205 stats.addStatistic(rtabmap::Statistics::kMemoryDatabase_memory_used(), (float)rtabmap_->getMemory()->getDatabaseMemoryUsed());
00206 stats.setPoses(poses);
00207 stats.setConstraints(links);
00208 rtabmapEvents_.push_back(stats);
00209
00210
00211 LOGI("Start rtabmap thread");
00212 this->registerToEventsManager();
00213 rtabmapThread_->registerToEventsManager();
00214 rtabmapThread_->start();
00215
00216 status_.first = rtabmap::RtabmapEventInit::kInitialized;
00217 status_.second = "";
00218 rtabmapMutex_.unlock();
00219 }
00220
00221 bool RTABMapApp::onTangoServiceConnected(JNIEnv* env, jobject iBinder)
00222 {
00223 LOGW("onTangoServiceConnected()");
00224 if(camera_)
00225 {
00226 camera_->join(true);
00227
00228 if (TangoService_setBinder(env, iBinder) != TANGO_SUCCESS) {
00229 LOGE("TangoHandler::ConnectTango, TangoService_setBinder error");
00230 return false;
00231 }
00232
00233 if(camera_->init())
00234 {
00235 LOGI("Start camera thread");
00236 camera_->start();
00237 return true;
00238 }
00239 LOGE("Failed camera initialization!");
00240 }
00241 return false;
00242 }
00243
00244 void RTABMapApp::onPause()
00245 {
00246 LOGW("onPause()");
00247 if(camera_)
00248 {
00249 camera_->join(true);
00250 camera_->close();
00251 }
00252 }
00253
00254
00255 void RTABMapApp::TangoResetMotionTracking() {
00256 TangoService_resetMotionTracking();
00257 }
00258
00259
00260 void RTABMapApp::InitializeGLContent()
00261 {
00262 UINFO("");
00263 main_scene_.InitGLContent();
00264 }
00265
00266
00267 void RTABMapApp::SetViewPort(int width, int height)
00268 {
00269 UINFO("");
00270 main_scene_.SetupViewPort(width, height);
00271 }
00272
00273 class PostRenderEvent : public UEvent
00274 {
00275 public:
00276 PostRenderEvent(const rtabmap::Statistics & stats) :
00277 stats_(stats)
00278 {
00279
00280 }
00281 virtual std::string getClassName() const {return "PostRenderEvent";}
00282 const rtabmap::Statistics & getStats() const {return stats_;}
00283 private:
00284 rtabmap::Statistics stats_;
00285 };
00286
00287
00288 int RTABMapApp::Render()
00289 {
00290
00291 std::list<rtabmap::Statistics> rtabmapEvents;
00292 {
00293 boost::mutex::scoped_lock lock(rtabmapMutex_);
00294 rtabmapEvents = rtabmapEvents_;
00295 rtabmapEvents_.clear();
00296 }
00297
00298 if(clearSceneOnNextRender_)
00299 {
00300 odomMutex_.lock();
00301 odomEvents_.clear();
00302 odomMutex_.unlock();
00303
00304 poseMutex_.lock();
00305 poseEvents_.clear();
00306 poseMutex_.unlock();
00307
00308 main_scene_.clear();
00309 clearSceneOnNextRender_ = false;
00310 createdMeshes_.clear();
00311 rawPoses_.clear();
00312 totalPoints_ = 0;
00313 totalPolygons_ = 0;
00314 lastDrawnCloudsCount_ = 0;
00315 renderingFPS_ = 0.0f;
00316 }
00317
00318
00319 rtabmap::Transform pose;
00320 {
00321 boost::mutex::scoped_lock lock(poseMutex_);
00322 if(poseEvents_.size())
00323 {
00324 pose = poseEvents_.back();
00325 poseEvents_.clear();
00326 }
00327 }
00328 if(!pose.isNull())
00329 {
00330
00331 main_scene_.SetCameraPose(pose);
00332 }
00333
00334 bool notifyDataLoaded = false;
00335 if(rtabmapEvents.size())
00336 {
00337 LOGI("Process rtabmap events");
00338
00339
00340 std::map<int, rtabmap::SensorData> bufferedSensorData;
00341 if(!trajectoryMode_)
00342 {
00343 for(std::list<rtabmap::Statistics>::iterator iter=rtabmapEvents.begin(); iter!=rtabmapEvents.end(); ++iter)
00344 {
00345 for(std::map<int, rtabmap::Signature>::const_iterator jter=iter->getSignatures().begin(); jter!=iter->getSignatures().end(); ++jter)
00346 {
00347 if(!jter->second.sensorData().imageRaw().empty() &&
00348 !jter->second.sensorData().depthRaw().empty())
00349 {
00350 uInsert(bufferedSensorData, std::make_pair(jter->first, jter->second.sensorData()));
00351 uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
00352 }
00353 else if(!jter->second.sensorData().imageCompressed().empty() &&
00354 !jter->second.sensorData().depthOrRightCompressed().empty())
00355 {
00356
00357 rtabmap::SensorData data = jter->second.sensorData();
00358 cv::Mat tmpA,tmpB;
00359 data.uncompressData(&tmpA, &tmpB);
00360 uInsert(bufferedSensorData, std::make_pair(jter->first, data));
00361 uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
00362 notifyDataLoaded = true;
00363 }
00364 }
00365 }
00366 }
00367
00368 std::map<int, rtabmap::Transform> poses = rtabmapEvents.back().poses();
00369
00370
00371 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00372 {
00373 if(!graphOptimization_)
00374 {
00375 std::map<int, rtabmap::Transform>::iterator jter = rawPoses_.find(iter->first);
00376 if(jter != rawPoses_.end())
00377 {
00378 iter->second = opengl_world_T_rtabmap_world*jter->second;
00379 }
00380 }
00381 else
00382 {
00383 iter->second = opengl_world_T_rtabmap_world*iter->second;
00384 }
00385 }
00386
00387 const std::multimap<int, rtabmap::Link> & links = rtabmapEvents.back().constraints();
00388 if(poses.size())
00389 {
00390
00391 main_scene_.updateGraph(poses, links);
00392
00393
00394 boost::mutex::scoped_lock lock(meshesMutex_);
00395 std::set<std::string> strIds;
00396 for(std::map<int, rtabmap::Transform>::iterator iter=poses.begin(); iter!=poses.end(); ++iter)
00397 {
00398 int id = iter->first;
00399 if(!iter->second.isNull())
00400 {
00401 if(main_scene_.hasCloud(id))
00402 {
00403
00404 main_scene_.setCloudPose(id, iter->second);
00405 main_scene_.setCloudVisible(id, true);
00406 std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(id);
00407 if(meshIter!=createdMeshes_.end())
00408 {
00409 meshIter->second.pose = iter->second;
00410 }
00411 else
00412 {
00413 UERROR("Not found mesh %d !?!?", id);
00414 }
00415 }
00416 else if(uContains(bufferedSensorData, id))
00417 {
00418 rtabmap::SensorData & data = bufferedSensorData.at(id);
00419 if(!data.imageRaw().empty() && !data.depthRaw().empty())
00420 {
00421
00422 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00423 pcl::IndicesPtr indices(new std::vector<int>);
00424 LOGI("Creating node cloud %d (image size=%dx%d)", id, data.imageRaw().cols, data.imageRaw().rows);
00425 cloud = rtabmap::util3d::cloudRGBFromSensorData(data, data.imageRaw().rows/data.depthRaw().rows, maxCloudDepth_, 0, indices.get());
00426
00427 if(cloud->size() && indices->size())
00428 {
00429 UTimer time;
00430
00431
00432 pcl::PointCloud<pcl::PointXYZRGB>::Ptr output(new pcl::PointCloud<pcl::PointXYZRGB>);
00433 pcl::ExtractIndices<pcl::PointXYZRGB> filter;
00434 filter.setIndices(indices);
00435 filter.setKeepOrganized(true);
00436 filter.setInputCloud(cloud);
00437 filter.filter(*output);
00438
00439 std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(output, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
00440 pcl::PointCloud<pcl::PointXYZRGB>::Ptr outputCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00441 std::vector<pcl::Vertices> outputPolygons;
00442
00443 outputCloud = output;
00444 outputPolygons = polygons;
00445
00446 LOGI("Creating mesh, %d polygons (%fs)", (int)outputPolygons.size(), time.ticks());
00447
00448 if(outputCloud->size() && outputPolygons.size())
00449 {
00450 totalPolygons_ += outputPolygons.size();
00451
00452 main_scene_.addCloud(id, outputCloud, outputPolygons, iter->second, data.imageRaw());
00453
00454
00455
00456 std::pair<std::map<int, Mesh>::iterator, bool> inserted = createdMeshes_.insert(std::make_pair(id, Mesh()));
00457 UASSERT(inserted.second);
00458 inserted.first->second.cloud = outputCloud;
00459 inserted.first->second.polygons = outputPolygons;
00460 inserted.first->second.pose = iter->second;
00461 inserted.first->second.texture = data.imageCompressed();
00462 }
00463 else
00464 {
00465 LOGE("Not mesh could be created for node %d", id);
00466 }
00467 }
00468 totalPoints_+=indices->size();
00469 }
00470 }
00471 }
00472 }
00473 }
00474
00475
00476 if(poses.size() > 2)
00477 {
00478 if(nodesFiltering_)
00479 {
00480 for(std::multimap<int, rtabmap::Link>::const_iterator iter=links.begin(); iter!=links.end(); ++iter)
00481 {
00482 if(iter->second.type() != rtabmap::Link::kNeighbor)
00483 {
00484 int oldId = iter->second.to()>iter->second.from()?iter->second.from():iter->second.to();
00485 poses.erase(oldId);
00486 }
00487 }
00488 }
00489 }
00490
00491
00492 std::set<int> addedClouds = main_scene_.getAddedClouds();
00493 for(std::set<int>::const_iterator iter=addedClouds.begin();
00494 iter!=addedClouds.end();
00495 ++iter)
00496 {
00497 if(*iter > 0 && poses.find(*iter) == poses.end())
00498 {
00499 main_scene_.setCloudVisible(*iter, false);
00500 }
00501 }
00502 }
00503 else
00504 {
00505 rtabmap::OdometryEvent event;
00506 bool set = false;
00507 {
00508 boost::mutex::scoped_lock lock(odomMutex_);
00509 if(odomEvents_.size())
00510 {
00511 LOGI("Process odom events");
00512 event = odomEvents_.back();
00513 odomEvents_.clear();
00514 set = true;
00515 }
00516 }
00517
00518 main_scene_.setCloudVisible(-1, odomCloudShown_ && !trajectoryMode_);
00519
00520
00521 if(set && !event.pose().isNull())
00522 {
00523 if(odomCloudShown_ && !trajectoryMode_)
00524 {
00525 if(!event.data().imageRaw().empty() && !event.data().depthRaw().empty())
00526 {
00527 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
00528 cloud = rtabmap::util3d::cloudRGBFromSensorData(event.data(), event.data().imageRaw().rows/event.data().depthRaw().rows, maxCloudDepth_);
00529 if(cloud->size())
00530 {
00531 LOGI("Created odom cloud (rgb=%dx%d depth=%dx%d cloud=%dx%d)",
00532 event.data().imageRaw().cols, event.data().imageRaw().rows,
00533 event.data().depthRaw().cols, event.data().depthRaw().rows,
00534 (int)cloud->width, (int)cloud->height);
00535 std::vector<pcl::Vertices> polygons = rtabmap::util3d::organizedFastMesh(cloud, meshAngleToleranceDeg_*M_PI/180.0, false, meshTrianglePix_);
00536 main_scene_.addCloud(-1, cloud, polygons, opengl_world_T_rtabmap_world*event.pose(), event.data().imageRaw());
00537 main_scene_.setCloudVisible(-1, true);
00538 }
00539 else
00540 {
00541 LOGE("Generated cloud is empty!");
00542 }
00543 }
00544 else
00545 {
00546 LOGE("Odom data images are empty!");
00547 }
00548 }
00549 }
00550 }
00551
00552 UTimer fpsTime;
00553 lastDrawnCloudsCount_ = main_scene_.Render();
00554 renderingFPS_ = 1.0/fpsTime.elapsed();
00555
00556 if(rtabmapEvents.size())
00557 {
00558
00559 LOGI("Posting PostRenderEvent!");
00560 UEventsManager::post(new PostRenderEvent(rtabmapEvents.back()));
00561 }
00562
00563 return notifyDataLoaded?1:0;
00564 }
00565
00566 void RTABMapApp::SetCameraType(
00567 tango_gl::GestureCamera::CameraType camera_type) {
00568 main_scene_.SetCameraType(camera_type);
00569 }
00570
00571 void RTABMapApp::OnTouchEvent(int touch_count,
00572 tango_gl::GestureCamera::TouchEvent event,
00573 float x0, float y0, float x1, float y1) {
00574 main_scene_.OnTouchEvent(touch_count, event, x0, y0, x1, y1);
00575 }
00576
00577 void RTABMapApp::setPausedMapping(bool paused)
00578 {
00579 if(camera_)
00580 {
00581 if(paused)
00582 {
00583 LOGW("Pause!");
00584 camera_->kill();
00585 }
00586 else
00587 {
00588 LOGW("Resume!");
00589 camera_->start();
00590 }
00591 }
00592 }
00593 void RTABMapApp::setMapCloudShown(bool shown)
00594 {
00595 main_scene_.setMapRendering(shown);
00596 }
00597 void RTABMapApp::setOdomCloudShown(bool shown)
00598 {
00599 odomCloudShown_ = shown;
00600 main_scene_.setTraceVisible(shown);
00601 }
00602 void RTABMapApp::setMeshRendering(bool enabled)
00603 {
00604 main_scene_.setMeshRendering(enabled);
00605 }
00606 void RTABMapApp::setLocalizationMode(bool enabled)
00607 {
00608 localizationMode_ = enabled;
00609 this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_)));
00610 }
00611 void RTABMapApp::setTrajectoryMode(bool enabled)
00612 {
00613 if(trajectoryMode_ != enabled)
00614 {
00615 main_scene_.SetCameraType(enabled?tango_gl::GestureCamera::kTopDown:tango_gl::GestureCamera::kThirdPersonFollow);
00616 }
00617 trajectoryMode_ = enabled;
00618 this->post(new rtabmap::ParamEvent(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_)));
00619 }
00620
00621 void RTABMapApp::setGraphOptimization(bool enabled)
00622 {
00623 graphOptimization_ = enabled;
00624 if(!camera_->isRunning())
00625 {
00626 std::map<int, rtabmap::Transform> poses;
00627 std::multimap<int, rtabmap::Link> links;
00628 rtabmap_->getGraph(poses, links, true, true);
00629 if(poses.size())
00630 {
00631 boost::mutex::scoped_lock lock(rtabmapMutex_);
00632 rtabmap::Statistics stats = rtabmap_->getStatistics();
00633 stats.setPoses(poses);
00634 stats.setConstraints(links);
00635 rtabmapEvents_.push_back(stats);
00636
00637 rtabmap_->setOptimizedPoses(poses);
00638 }
00639 }
00640 }
00641 void RTABMapApp::setNodesFiltering(bool enabled)
00642 {
00643 nodesFiltering_ = enabled;
00644 setGraphOptimization(graphOptimization_);
00645 }
00646 void RTABMapApp::setGraphVisible(bool visible)
00647 {
00648 main_scene_.setGraphVisible(visible);
00649 }
00650
00651 void RTABMapApp::setAutoExposure(bool enabled)
00652 {
00653 if(autoExposure_ != enabled)
00654 {
00655 autoExposure_ = enabled;
00656 if(camera_)
00657 {
00658 camera_->setAutoExposure(autoExposure_);
00659 }
00660 }
00661 }
00662
00663 void RTABMapApp::setFullResolution(bool enabled)
00664 {
00665 if(fullResolution_ != enabled)
00666 {
00667 fullResolution_ = enabled;
00668 if(camera_)
00669 {
00670 camera_->setDecimation(fullResolution_?1:2);
00671 }
00672 rtabmap::ParametersMap parameters;
00673 parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemImagePreDecimation(), std::string(fullResolution_?"2":"1")));
00674 this->post(new rtabmap::ParamEvent(parameters));
00675 }
00676 }
00677
00678 void RTABMapApp::setMaxCloudDepth(float value)
00679 {
00680 maxCloudDepth_ = value;
00681 }
00682
00683 void RTABMapApp::setMeshAngleTolerance(float value)
00684 {
00685 meshAngleToleranceDeg_ = value;
00686 }
00687
00688 void RTABMapApp::setMeshTriangleSize(int value)
00689 {
00690 meshTrianglePix_ = value;
00691 }
00692
00693 int RTABMapApp::setMappingParameter(const std::string & key, const std::string & value)
00694 {
00695 if(rtabmap::Parameters::getDefaultParameters().find(key) != rtabmap::Parameters::getDefaultParameters().end())
00696 {
00697 LOGI(uFormat("Setting param \"%s\" to \"\"", key.c_str(), value.c_str()).c_str());
00698 uInsert(mappingParameters_, rtabmap::ParametersPair(key, value));
00699 UEventsManager::post(new rtabmap::ParamEvent(mappingParameters_));
00700 return 0;
00701 }
00702 else
00703 {
00704 LOGE(uFormat("Key \"%s\" doesn't exist!", key.c_str()).c_str());
00705 return -1;
00706 }
00707 }
00708
00709 void RTABMapApp::resetMapping()
00710 {
00711 LOGW("Reset!");
00712 status_.first = rtabmap::RtabmapEventInit::kInitializing;
00713 status_.second = "";
00714
00715 clearSceneOnNextRender_ = true;
00716
00717 UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
00718 }
00719
00720 void RTABMapApp::save()
00721 {
00722 UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdClose));
00723 }
00724
00725 bool RTABMapApp::exportMesh(const std::string & filePath)
00726 {
00727 bool success = false;
00728
00729
00730 if(UFile::getExtension(filePath).compare("obj") == 0)
00731 {
00732 pcl::TextureMesh textureMesh;
00733 std::vector<cv::Mat> textures;
00734 int totalPolygons = 0;
00735 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00736 {
00737 boost::mutex::scoped_lock lock(meshesMutex_);
00738
00739 textureMesh.tex_materials.resize(createdMeshes_.size());
00740 textureMesh.tex_polygons.resize(createdMeshes_.size());
00741 textureMesh.tex_coordinates.resize(createdMeshes_.size());
00742 textures.resize(createdMeshes_.size());
00743
00744 int polygonsStep = 0;
00745 int oi = 0;
00746 for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
00747 iter!= createdMeshes_.end();
00748 ++iter)
00749 {
00750 UASSERT(!iter->second.cloud->is_dense);
00751
00752 if(!iter->second.texture.empty() &&
00753 iter->second.cloud->size() &&
00754 iter->second.polygons.size())
00755 {
00756
00757 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
00758 cloudWithNormals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
00759
00760
00761 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00762 std::vector<pcl::Vertices> densePolygons;
00763 std::map<int, int> newToOldIndices;
00764 newToOldIndices = rtabmap::util3d::filterNotUsedVerticesFromMesh(
00765 *cloudWithNormals,
00766 iter->second.polygons,
00767 *denseCloud,
00768 densePolygons);
00769
00770
00771 UASSERT(densePolygons.size());
00772 unsigned int polygonSize = densePolygons.front().vertices.size();
00773 textureMesh.tex_polygons[oi].resize(densePolygons.size());
00774 textureMesh.tex_coordinates[oi].resize(densePolygons.size() * polygonSize);
00775 for(unsigned int j=0; j<densePolygons.size(); ++j)
00776 {
00777 pcl::Vertices vertices = densePolygons[j];
00778 UASSERT(polygonSize == vertices.vertices.size());
00779 for(unsigned int k=0; k<vertices.vertices.size(); ++k)
00780 {
00781
00782 std::map<int, int>::iterator jter = newToOldIndices.find(vertices.vertices[k]);
00783 textureMesh.tex_coordinates[oi][j*vertices.vertices.size()+k] = Eigen::Vector2f(
00784 float(jter->second % iter->second.cloud->width) / float(iter->second.cloud->width),
00785 float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height));
00786
00787 vertices.vertices[k] += polygonsStep;
00788 }
00789 textureMesh.tex_polygons[oi][j] = vertices;
00790
00791 }
00792 totalPolygons += densePolygons.size();
00793 polygonsStep += denseCloud->size();
00794
00795 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
00796 if(mergedClouds->size() == 0)
00797 {
00798 *mergedClouds = *transformedCloud;
00799 }
00800 else
00801 {
00802 *mergedClouds += *transformedCloud;
00803 }
00804
00805 textures[oi] = iter->second.texture;
00806 textureMesh.tex_materials[oi].tex_illum = 1;
00807 textureMesh.tex_materials[oi].tex_name = uFormat("material_%d", iter->first);
00808 ++oi;
00809 }
00810 else
00811 {
00812 UERROR("Texture not set for mesh %d", iter->first);
00813 }
00814 }
00815 textureMesh.tex_materials.resize(oi);
00816 textureMesh.tex_polygons.resize(oi);
00817 textures.resize(oi);
00818
00819 if(textures.size())
00820 {
00821 pcl::toPCLPointCloud2(*mergedClouds, textureMesh.cloud);
00822
00823 std::string textureDirectory = uSplit(filePath, '.').front();
00824 UINFO("Saving %d textures to %s.", textures.size(), textureDirectory.c_str());
00825 UDirectory::makeDir(textureDirectory);
00826 for(unsigned int i=0;i<textures.size(); ++i)
00827 {
00828 cv::Mat rawImage = rtabmap::uncompressImage(textures[i]);
00829 std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
00830 cv::imwrite(texFile, rawImage);
00831
00832 UINFO("Saved %s (%d bytes).", texFile.c_str(), rawImage.total()*rawImage.channels());
00833
00834
00835 textureMesh.tex_materials[i].tex_file = uSplit(UFile::getName(filePath), '.').front()+"/"+textureMesh.tex_materials[i].tex_name+".png";
00836 }
00837
00838 UINFO("Saving obj (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), totalPolygons, filePath.c_str());
00839 success = pcl::io::saveOBJFile(filePath, textureMesh) == 0;
00840 if(success)
00841 {
00842 UINFO("Saved obj to %s!", filePath.c_str());
00843 }
00844 else
00845 {
00846 UERROR("Failed saving obj to %s!", filePath.c_str());
00847 }
00848 }
00849 }
00850 }
00851 else
00852 {
00853 pcl::PointCloud<pcl::PointXYZRGB>::Ptr mergedClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
00854 std::vector<pcl::Vertices> mergedPolygons;
00855
00856 {
00857 boost::mutex::scoped_lock lock(meshesMutex_);
00858
00859 for(std::map<int, Mesh>::iterator iter=createdMeshes_.begin();
00860 iter!= createdMeshes_.end();
00861 ++iter)
00862 {
00863 pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00864 std::vector<pcl::Vertices> densePolygons;
00865
00866 rtabmap::util3d::filterNotUsedVerticesFromMesh(
00867 *iter->second.cloud,
00868 iter->second.polygons,
00869 *denseCloud,
00870 densePolygons);
00871
00872 pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformedCloud = rtabmap::util3d::transformPointCloud(denseCloud, iter->second.pose);
00873 if(mergedClouds->size() == 0)
00874 {
00875 *mergedClouds = *transformedCloud;
00876 mergedPolygons = densePolygons;
00877 }
00878 else
00879 {
00880 rtabmap::util3d::appendMesh(*mergedClouds, mergedPolygons, *transformedCloud, densePolygons);
00881 }
00882 }
00883 }
00884
00885 if(mergedClouds->size() && mergedPolygons.size())
00886 {
00887 pcl::PolygonMesh mesh;
00888 pcl::toPCLPointCloud2(*mergedClouds, mesh.cloud);
00889 mesh.polygons = mergedPolygons;
00890
00891 UINFO("Saving ply (%d vertices, %d polygons) to %s.", (int)mergedClouds->size(), (int)mergedPolygons.size(), filePath.c_str());
00892 success = pcl::io::savePLYFileBinary(filePath, mesh) == 0;
00893 if(success)
00894 {
00895 UINFO("Saved ply to %s!", filePath.c_str());
00896 }
00897 else
00898 {
00899 UERROR("Failed saving ply to %s!", filePath.c_str());
00900 }
00901 }
00902 }
00903 return success;
00904 }
00905
00906 int RTABMapApp::postProcessing(int approach)
00907 {
00908 int returnedValue = 0;
00909 if(rtabmap_)
00910 {
00911 std::map<int, rtabmap::Transform> poses;
00912 std::multimap<int, rtabmap::Link> links;
00913 if(approach == 2 || approach == 0)
00914 {
00915 if(approach == 2)
00916 {
00917
00918 returnedValue = rtabmap_->detectMoreLoopClosures();
00919 }
00920
00921 if(returnedValue >= 0)
00922 {
00923
00924 rtabmap_->getGraph(poses, links, true, true);
00925 }
00926 }
00927 else if (approach == 1)
00928 {
00929 if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O))
00930 {
00931 std::map<int, rtabmap::Signature> signatures;
00932 rtabmap_->getGraph(poses, links, false, true, &signatures);
00933
00934 rtabmap::ParametersMap param;
00935 param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "30"));
00936 param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerEpsilon(), "0"));
00937 rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param);
00938 poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures);
00939 delete sba;
00940 }
00941 else
00942 {
00943 LOGE("g2o not available!");
00944 }
00945 }
00946 else
00947 {
00948 LOGE("Invalid approach %d (should be 0 (graph optimization), 1 (sba) or 2 (detect more loop closures))", approach);
00949 returnedValue = -1;
00950 }
00951
00952 if(poses.size())
00953 {
00954 boost::mutex::scoped_lock lock(rtabmapMutex_);
00955 rtabmap::Statistics stats = rtabmap_->getStatistics();
00956 stats.setPoses(poses);
00957 stats.setConstraints(links);
00958 rtabmapEvents_.push_back(stats);
00959
00960 rtabmap_->setOptimizedPoses(poses);
00961 }
00962 else
00963 {
00964 returnedValue = -1;
00965 }
00966 }
00967 return returnedValue;
00968 }
00969
00970 void RTABMapApp::handleEvent(UEvent * event)
00971 {
00972 if(camera_ && camera_->isRunning())
00973 {
00974
00975 if(event->getClassName().compare("OdometryEvent") == 0)
00976 {
00977 LOGI("Received OdometryEvent!");
00978 if(odomMutex_.try_lock())
00979 {
00980 odomEvents_.clear();
00981 if(camera_->isRunning())
00982 {
00983 odomEvents_.push_back(*((rtabmap::OdometryEvent*)(event)));
00984 }
00985 odomMutex_.unlock();
00986 }
00987 }
00988 if(status_.first == rtabmap::RtabmapEventInit::kInitialized &&
00989 event->getClassName().compare("RtabmapEvent") == 0)
00990 {
00991 LOGI("Received RtabmapEvent!");
00992 if(camera_->isRunning())
00993 {
00994 boost::mutex::scoped_lock lock(rtabmapMutex_);
00995 rtabmapEvents_.push_back(((rtabmap::RtabmapEvent*)event)->getStats());
00996 }
00997 }
00998 }
00999
01000 if(event->getClassName().compare("PoseEvent") == 0)
01001 {
01002 if(poseMutex_.try_lock())
01003 {
01004 poseEvents_.clear();
01005 poseEvents_.push_back(((rtabmap::PoseEvent*)event)->pose());
01006 poseMutex_.unlock();
01007 }
01008 }
01009
01010 if(event->getClassName().compare("CameraTangoEvent") == 0)
01011 {
01012 rtabmap::CameraTangoEvent * tangoEvent = (rtabmap::CameraTangoEvent*)event;
01013
01014
01015 bool success = false;
01016 if(jvm && RTABMapActivity)
01017 {
01018 JNIEnv *env = 0;
01019 jint rs = jvm->AttachCurrentThread(&env, NULL);
01020 if(rs == JNI_OK && env)
01021 {
01022 jclass clazz = env->GetObjectClass(RTABMapActivity);
01023 if(clazz)
01024 {
01025 jmethodID methodID = env->GetMethodID(clazz, "tangoEventCallback", "(ILjava/lang/String;Ljava/lang/String;)V" );
01026 if(methodID)
01027 {
01028 env->CallVoidMethod(RTABMapActivity, methodID,
01029 tangoEvent->type(),
01030 env->NewStringUTF(tangoEvent->key().c_str()),
01031 env->NewStringUTF(tangoEvent->value().c_str()));
01032 success = true;
01033 }
01034 }
01035 }
01036 jvm->DetachCurrentThread();
01037 }
01038 if(!success)
01039 {
01040 UERROR("Failed to call RTABMapActivity::tangoEventCallback");
01041 }
01042 }
01043
01044 if(event->getClassName().compare("RtabmapEventInit") == 0)
01045 {
01046 LOGI("Received RtabmapEventInit!");
01047 status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
01048 status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
01049
01050 if(status_.first == rtabmap::RtabmapEventInit::kClosed)
01051 {
01052 clearSceneOnNextRender_ = true;
01053 }
01054
01055
01056 bool success = false;
01057 if(jvm && RTABMapActivity)
01058 {
01059 JNIEnv *env = 0;
01060 jint rs = jvm->AttachCurrentThread(&env, NULL);
01061 if(rs == JNI_OK && env)
01062 {
01063 jclass clazz = env->GetObjectClass(RTABMapActivity);
01064 if(clazz)
01065 {
01066 jmethodID methodID = env->GetMethodID(clazz, "rtabmapInitEventCallback", "(ILjava/lang/String;)V" );
01067 if(methodID)
01068 {
01069 env->CallVoidMethod(RTABMapActivity, methodID,
01070 status_.first,
01071 env->NewStringUTF(status_.second.c_str()));
01072 success = true;
01073 }
01074 }
01075 }
01076 jvm->DetachCurrentThread();
01077 }
01078 if(!success)
01079 {
01080 UERROR("Failed to call RTABMapActivity::rtabmapInitEventsCallback");
01081 }
01082 }
01083
01084 if(event->getClassName().compare("PostRenderEvent") == 0)
01085 {
01086 LOGI("Received PostRenderEvent!");
01087 const rtabmap::Statistics & stats = ((PostRenderEvent*)event)->getStats();
01088 int nodes = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryWorking_memory_size(), 0.0f) +
01089 uValue(stats.data(), rtabmap::Statistics::kMemoryShort_time_memory_size(), 0.0f);
01090 int words = (int)uValue(stats.data(), rtabmap::Statistics::kKeypointDictionary_size(), 0.0f);
01091 float updateTime = uValue(stats.data(), rtabmap::Statistics::kTimingTotal(), 0.0f);
01092 int loopClosureId = stats.loopClosureId()>0?stats.loopClosureId():stats.proximityDetectionId()>0?stats.proximityDetectionId():0;
01093 int highestHypId = (int)uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_id(), 0.0f);
01094 int databaseMemoryUsed = (int)uValue(stats.data(), rtabmap::Statistics::kMemoryDatabase_memory_used(), 0.0f);
01095 int inliers = (int)uValue(stats.data(), rtabmap::Statistics::kLoopVisual_inliers(), 0.0f);
01096 int rejected = (int)uValue(stats.data(), rtabmap::Statistics::kLoopRejectedHypothesis(), 0.0f);
01097 int featuresExtracted = stats.getSignatures().size()?stats.getSignatures().rbegin()->second.getWords().size():0;
01098 float hypothesis = uValue(stats.data(), rtabmap::Statistics::kLoopHighest_hypothesis_value(), 0.0f);
01099
01100
01101 UINFO("Send statistics to GUI");
01102 bool success = false;
01103 if(jvm && RTABMapActivity)
01104 {
01105 JNIEnv *env = 0;
01106 jint rs = jvm->AttachCurrentThread(&env, NULL);
01107 if(rs == JNI_OK && env)
01108 {
01109 jclass clazz = env->GetObjectClass(RTABMapActivity);
01110 if(clazz)
01111 {
01112 jmethodID methodID = env->GetMethodID(clazz, "updateStatsCallback", "(IIIIFIIIIIFIFI)V" );
01113 if(methodID)
01114 {
01115 env->CallVoidMethod(RTABMapActivity, methodID,
01116 nodes,
01117 words,
01118 totalPoints_,
01119 totalPolygons_,
01120 updateTime,
01121 loopClosureId,
01122 highestHypId,
01123 databaseMemoryUsed,
01124 inliers,
01125 featuresExtracted,
01126 hypothesis,
01127 lastDrawnCloudsCount_,
01128 renderingFPS_,
01129 rejected);
01130 success = true;
01131 }
01132 }
01133 }
01134 jvm->DetachCurrentThread();
01135 }
01136 if(!success)
01137 {
01138 UERROR("Failed to call RTABMapActivity::updateStatsCallback");
01139 }
01140 }
01141 }
01142