RTABMapApp.cpp
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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 }  // anonymous namespace.
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"))); // GFTT/BRIEF
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"))); // to avoid extracting features in invalid depth (as we compute transformation directly from the words)
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"))); // 0=3D-3D 1=PnP
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(); // to ignore published init events when closing rtabmap
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         //Rtabmap
00183         rtabmap_ = new rtabmap::Rtabmap();
00184         rtabmap::ParametersMap parameters = getRtabmapParameters();
00185 
00186         rtabmap_->init(parameters, databasePath);
00187         rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_);
00188 
00189         // Generate all meshes
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         // Start threads
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 // OpenGL thread
00260 void RTABMapApp::InitializeGLContent()
00261 {
00262         UINFO("");
00263         main_scene_.InitGLContent();
00264 }
00265 
00266 // OpenGL thread
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 // OpenGL thread
00288 int RTABMapApp::Render()
00289 {
00290         // should be before clearSceneOnNextRender_ in case openDatabase is called
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         // Process events
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                 // update camera pose?
00331                 main_scene_.SetCameraPose(pose);
00332         }
00333 
00334         bool notifyDataLoaded = false;
00335         if(rtabmapEvents.size())
00336         {
00337                 LOGI("Process rtabmap events");
00338 
00339                 // update buffered signatures
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                                                 // uncompress
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                 // Transform pose in OpenGL world
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                         //update graph
00391                         main_scene_.updateGraph(poses, links);
00392 
00393                         // update clouds
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                                                 //just update pose
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                                                         // Voxelize and filter depending on the previous cloud?
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                                                                 // pcl::organizedFastMesh doesn't take indices, so set to NaN points we don't need to mesh
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                                                                         // protect createdMeshes_ used also by exportMesh() method
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                 //filter poses?
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                 //update cloud visibility
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                 //just process the last one
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         // send statistics to GUI
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_); // this will resend the graph if paused
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         //Assemble the meshes
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                                         // OBJ format requires normals
00757                                         pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
00758                                         cloudWithNormals = rtabmap::util3d::computeNormals(iter->second.cloud, 20);
00759 
00760                                         // create dense cloud
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                                         // polygons
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                                                         //uv
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),   // u
00785                                                                         float(iter->second.cloud->height - jter->second / iter->second.cloud->width) / float(iter->second.cloud->height)); // v
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                                         // relative path
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                                 // detect more loop closures
00918                                 returnedValue = rtabmap_->detectMoreLoopClosures();
00919                         }
00920 
00921                         if(returnedValue >= 0)
00922                         {
00923                                 // simple graph optmimization
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                 // called from events manager thread, so protect the data
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                 // Call JAVA callback with tango event msg
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                 // Call JAVA callback with init msg
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                 // Call JAVA callback with some stats
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 


rtabmap
Author(s): Mathieu Labbe
autogenerated on Sat Jul 23 2016 11:44:17