From 5d9522c9019c921c9aa78ce46c66a01dfa18c340 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Sun, 3 Apr 2016 21:35:30 -0400 Subject: [PATCH] Tango #57: Global optimization / Post-processing on pause --- app/android/jni/CameraTango.cpp | 1 - app/android/jni/RTABMapApp.cpp | 72 ++++- app/android/jni/RTABMapApp.h | 2 + app/android/jni/jni_interface.cpp | 9 + .../com/introlab/rtabmap/RTABMapActivity.java | 66 ++++- .../src/com/introlab/rtabmap/RTABMapLib.java | 1 + corelib/include/rtabmap/core/DBDriver.h | 2 + corelib/include/rtabmap/core/Memory.h | 6 +- corelib/include/rtabmap/core/Rtabmap.h | 2 + corelib/src/DBDriver.cpp | 25 ++ corelib/src/DBDriverSqlite3.cpp | 152 ++++++++++ corelib/src/DBDriverSqlite3.h | 1 + corelib/src/Memory.cpp | 280 +++++++++--------- corelib/src/OptimizerG2O.cpp | 2 +- corelib/src/Rtabmap.cpp | 166 ++++++++++- 15 files changed, 621 insertions(+), 166 deletions(-) diff --git a/app/android/jni/CameraTango.cpp b/app/android/jni/CameraTango.cpp index ed4a03a4..bef7fd25 100644 --- a/app/android/jni/CameraTango.cpp +++ b/app/android/jni/CameraTango.cpp @@ -379,7 +379,6 @@ void CameraTango::poseReceived(const Transform & pose) void CameraTango::tangoEventReceived(int type, const char * key, const char * value) { - LOGE("Tango event: %s:%s", key, value); this->post(new CameraTangoEvent(type, key, value)); } diff --git a/app/android/jni/RTABMapApp.cpp b/app/android/jni/RTABMapApp.cpp index 4ded0967..6be2342d 100644 --- a/app/android/jni/RTABMapApp.cpp +++ b/app/android/jni/RTABMapApp.cpp @@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include #include +#include #include #include #include @@ -68,13 +69,14 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kBRIEFBytes(), std::string("64"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemBinDataKept(), uBool2Str(!trajectoryMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemNotLinkedNodesKept(), std::string("false"))); - parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), uNumber2Str(graphOptimization_?rtabmap::Parameters::defaultOptimizerIterations():0))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), graphOptimization_?"10":"0")); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), uBool2Str(!localizationMode_))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapMaxRetrieved(), uBool2Str(!localizationMode_))); 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) parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeFromGraphEnd(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), std::string("true"))); parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisMinInliers(), std::string("15"))); + parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // PnP return parameters; } @@ -82,6 +84,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters() RTABMapApp::RTABMapApp() : camera_(0), rtabmapThread_(0), + rtabmap_(0), logHandler_(0), mapCloudShown_(true), odomCloudShown_(true), @@ -138,6 +141,7 @@ int RTABMapApp::TangoInitialize(JNIEnv* env, jobject caller_activity) rtabmapThread_->close(false); delete rtabmapThread_; rtabmapThread_ = 0; + rtabmap_ = 0; } if(logHandler_ == 0) @@ -168,26 +172,42 @@ void RTABMapApp::openDatabase(const std::string & databasePath) rtabmapThread_->close(false); delete rtabmapThread_; rtabmapThread_ = 0; + rtabmap_ = 0; } //Rtabmap - rtabmap::Rtabmap * rtabmap = new rtabmap::Rtabmap(); + rtabmap_ = new rtabmap::Rtabmap(); rtabmap::ParametersMap parameters = getRtabmapParameters(); - rtabmap->init(parameters, databasePath); - rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap); + rtabmap_->init(parameters, databasePath); + rtabmapThread_ = new rtabmap::RtabmapThread(rtabmap_); // Generate all meshes std::map signatures; std::map poses; std::multimap links; - rtabmap->get3DMap( + rtabmap_->get3DMap( signatures, poses, links, true, true); + if(poses.size() > 1 && + rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O)) + { + rtabmap::ParametersMap param; + param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10")); + rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param); + poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures); + delete sba; + + if(poses.size()) + { + rtabmap_->setOptimizedPoses(poses); + } + } + clearSceneOnNextRender_ = true; rtabmap::Statistics stats; stats.setSignatures(signatures); @@ -793,6 +813,48 @@ bool RTABMapApp::exportMesh(const std::string & filePath) return success; } +int RTABMapApp::postProcessing(bool graphOptimizationOnly) +{ + int detectedLoopClosures = 0; + if(rtabmap_) + { + std::map poses; + std::multimap links; + if(graphOptimizationOnly) + { + rtabmap_->getGraph(poses, links, true, true); + } + else + { + detectedLoopClosures = rtabmap_->detectMoreLoopClosures(); + + std::map signatures; + rtabmap_->getGraph(poses, links, false, true, &signatures); + + if(rtabmap::Optimizer::isAvailable(rtabmap::Optimizer::kTypeG2O)) + { + rtabmap::ParametersMap param; + param.insert(rtabmap::ParametersPair(rtabmap::Parameters::kOptimizerIterations(), "10")); + rtabmap::Optimizer * sba = rtabmap::Optimizer::create(rtabmap::Optimizer::kTypeG2O, param); + poses = sba->optimizeBA(poses.rbegin()->first, poses, links, signatures); + delete sba; + } + } + + if(poses.size()) + { + boost::mutex::scoped_lock lock(rtabmapMutex_); + rtabmap::Statistics stats; + stats.setPoses(poses); + stats.setConstraints(links); + rtabmapEvents_.push_back(stats); + + rtabmap_->setOptimizedPoses(poses); + } + } + return detectedLoopClosures; +} + void RTABMapApp::handleEvent(UEvent * event) { if(camera_ && camera_->isRunning()) diff --git a/app/android/jni/RTABMapApp.h b/app/android/jni/RTABMapApp.h index 0cf5258b..f25a471d 100644 --- a/app/android/jni/RTABMapApp.h +++ b/app/android/jni/RTABMapApp.h @@ -128,6 +128,7 @@ class RTABMapApp : public UEventsHandler { void resetMapping(); void save(); bool exportMesh(const std::string & filePath); + int postProcessing(bool graphOptimizationOnly); protected: virtual void handleEvent(UEvent * event); @@ -138,6 +139,7 @@ class RTABMapApp : public UEventsHandler { private: rtabmap::CameraTango * camera_; rtabmap::RtabmapThread * rtabmapThread_; + rtabmap::Rtabmap * rtabmap_; LogHandler * logHandler_; bool mapCloudShown_; diff --git a/app/android/jni/jni_interface.cpp b/app/android/jni/jni_interface.cpp index 4e2a057e..95135f3a 100644 --- a/app/android/jni/jni_interface.cpp +++ b/app/android/jni/jni_interface.cpp @@ -212,6 +212,15 @@ Java_com_introlab_rtabmap_RTABMapLib_exportMesh( return app.exportMesh(filePathC); } +JNIEXPORT int JNICALL +Java_com_introlab_rtabmap_RTABMapLib_postProcessing( + JNIEnv* env, jobject, bool graphOptimizationOnly) +{ + return app.postProcessing(graphOptimizationOnly); +} + + + #ifdef __cplusplus } #endif diff --git a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java index 54e94305..d480dc18 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapActivity.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapActivity.java @@ -509,13 +509,14 @@ public class RTABMapActivity extends Activity implements OnClickListener { final String key, final String value) { - Log.i(TAG, String.format("tangoEventCallback()")); - - runOnUiThread(new Runnable() { - public void run() { - tangoEventUI(type, key, value); - } - }); + if(mItemPause != null && !mItemPause.isChecked()) + { + runOnUiThread(new Runnable() { + public void run() { + tangoEventUI(type, key, value); + } + }); + } } private boolean CheckTangoCoreVersion(int minVersion) { @@ -574,6 +575,57 @@ public class RTABMapActivity extends Activity implements OnClickListener { { RTABMapLib.setPausedMapping(true); ((TextView)findViewById(R.id.status)).setText("Paused"); + + // Post-processing + new AlertDialog.Builder(getActivity()) + .setTitle("Post-Processing") + .setMessage("Do you want to detect more loop closures and do a bundle adjustement? Otherwise, a simple global graph optimization is done.") + .setPositiveButton("Yes", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int which) { + dialog.dismiss(); + mProgressDialog.setTitle("Post-Processing"); + mProgressDialog.setMessage(String.format("Please wait while optimizing...")); + mProgressDialog.show(); + + Thread workingThread = new Thread(new Runnable() { + public void run() { + final int loopDetected = RTABMapLib.postProcessing(false); + runOnUiThread(new Runnable() { + public void run() { + mProgressDialog.dismiss(); + if(loopDetected > 0) + { + Toast.makeText(getActivity(), String.format("%d new loop closure(s) added.", loopDetected), Toast.LENGTH_SHORT).show(); + } + } + }); + } + }); + workingThread.start(); + } + }) + .setNegativeButton("No", new DialogInterface.OnClickListener() { + public void onClick(DialogInterface dialog, int which) { + dialog.dismiss(); + mProgressDialog.setTitle("Post-Processing"); + mProgressDialog.setMessage(String.format("Please wait while optimizing...")); + mProgressDialog.show(); + + Thread workingThread = new Thread(new Runnable() { + public void run() { + RTABMapLib.postProcessing(true); + runOnUiThread(new Runnable() { + public void run() { + mProgressDialog.dismiss(); + } + }); + } + }); + workingThread.start(); + } + }) + .show(); + } else { diff --git a/app/android/src/com/introlab/rtabmap/RTABMapLib.java b/app/android/src/com/introlab/rtabmap/RTABMapLib.java index b881f8cd..628e7847 100644 --- a/app/android/src/com/introlab/rtabmap/RTABMapLib.java +++ b/app/android/src/com/introlab/rtabmap/RTABMapLib.java @@ -59,6 +59,7 @@ public class RTABMapLib public static native void resetMapping(); public static native void save(); public static native boolean exportMesh(String filePath); + public static native int postProcessing(boolean graphOptimizationOnly); public static native String getStatus(); public static native int getTotalNodes(); diff --git a/corelib/include/rtabmap/core/DBDriver.h b/corelib/include/rtabmap/core/DBDriver.h index 38e74b4f..e9cc5427 100644 --- a/corelib/include/rtabmap/core/DBDriver.h +++ b/corelib/include/rtabmap/core/DBDriver.h @@ -119,6 +119,7 @@ public: // Specific queries... void loadNodeData(std::list & signatures) const; void getNodeData(int signatureId, SensorData & data) const; + bool getCalibration(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const; bool getNodeInfo(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const; void loadLinks(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; void getWeight(int signatureId, int & weight) const; @@ -169,6 +170,7 @@ private: virtual void loadLinksQuery(int signatureId, std::map & links, Link::Type type = Link::kUndef) const = 0; virtual void loadNodeDataQuery(std::list & signatures) const = 0; + virtual bool getCalibrationQuery(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const = 0; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const = 0; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures) const = 0; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const = 0; diff --git a/corelib/include/rtabmap/core/Memory.h b/corelib/include/rtabmap/core/Memory.h index b3cb627f..9997bc36 100644 --- a/corelib/include/rtabmap/core/Memory.h +++ b/corelib/include/rtabmap/core/Memory.h @@ -147,11 +147,14 @@ public: Transform & groundTruth, bool lookInDatabase = false) const; cv::Mat getImageCompressed(int signatureId) const; - SensorData getNodeData(int nodeId, bool uncompressedData = false, bool keepLoadedDataInMemory = true); + SensorData getNodeData(int nodeId, bool uncompressedData = false) const; void getNodeWords(int nodeId, std::multimap & words, std::multimap & words3, std::multimap & wordsDescriptors); + void getNodeCalibration(int nodeId, + std::vector & models, + StereoCameraModel & stereoModel); SensorData getSignatureDataConst(int locationId) const; std::set getAllSignatureIds() const; bool memoryChanged() const {return _memoryChanged;} @@ -182,6 +185,7 @@ public: std::multimap & links, bool lookInDatabase = false); + Transform computeTransform(Signature & fromS, Signature & toS, Transform guess, RegistrationInfo * info = 0) const; Transform computeTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0); Transform computeIcpTransform(int fromId, int toId, Transform guess, RegistrationInfo * info = 0); Transform computeIcpTransformMulti( diff --git a/corelib/include/rtabmap/core/Rtabmap.h b/corelib/include/rtabmap/core/Rtabmap.h index c4c65ad9..d73bf264 100644 --- a/corelib/include/rtabmap/core/Rtabmap.h +++ b/corelib/include/rtabmap/core/Rtabmap.h @@ -115,6 +115,7 @@ public: const ParametersMap & getParameters() const {return _parameters;} void setWorkingDirectory(std::string path); void rejectLoopClosure(int oldId, int newId); + void setOptimizedPoses(const std::map & poses); void get3DMap(std::map & signatures, std::map & poses, std::multimap & constraints, @@ -125,6 +126,7 @@ public: bool optimized, bool global, std::map * signatures = 0); + int detectMoreLoopClosures(float clusterRadius = 0.5f, float clusterAngle = M_PI/6.0f, int iterations = 1); int getPathStatus() const {return _pathStatus;} // -1=failed 0=idle/executing 1=success void clearPath(int status); // -1=failed 0=idle/executing 1=success diff --git a/corelib/src/DBDriver.cpp b/corelib/src/DBDriver.cpp index d5a28446..91d4789d 100644 --- a/corelib/src/DBDriver.cpp +++ b/corelib/src/DBDriver.cpp @@ -546,6 +546,31 @@ void DBDriver::getNodeData( } } +bool DBDriver::getCalibration( + int signatureId, + std::vector & models, + StereoCameraModel & stereoModel) const +{ + bool found = false; + // look in the trash + _trashesMutex.lock(); + if(uContains(_trashSignatures, signatureId)) + { + models = _trashSignatures.at(signatureId)->sensorData().cameraModels(); + stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel(); + found = true; + } + _trashesMutex.unlock(); + + if(!found) + { + _dbSafeAccessMutex.lock(); + found = this->getCalibrationQuery(signatureId, models, stereoModel); + _dbSafeAccessMutex.unlock(); + } + return found; +} + bool DBDriver::getNodeInfo( int signatureId, Transform & pose, diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp index 3e986f70..20cbdc8c 100644 --- a/corelib/src/DBDriverSqlite3.cpp +++ b/corelib/src/DBDriverSqlite3.cpp @@ -1009,6 +1009,158 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list & signatures) con } } +bool DBDriverSqlite3::getCalibrationQuery( + int signatureId, + std::vector & models, + StereoCameraModel & stereoModel) const +{ + bool found = false; + if(_ppDb && signatureId) + { + int rc = SQLITE_OK; + sqlite3_stmt * ppStmt = 0; + std::stringstream query; + + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + query << "SELECT calibration " + << "FROM Data " + << "WHERE id = " << signatureId + <<";"; + } + else if(uStrNumCmp(_version, "0.7.0") >= 0) + { + query << "SELECT local_transform, fx, fy, cx, cy " + << "FROM Depth " + << "WHERE id = " << signatureId + <<";"; + } + else + { + query << "SELECT local_transform, constant " + << "FROM Depth " + << "WHERE id = " << signatureId + <<";"; + } + + rc = sqlite3_prepare_v2(_ppDb, query.str().c_str(), -1, &ppStmt, 0); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + const void * data = 0; + int dataSize = 0; + Transform localTransform; + StereoCameraModel stereoModel; + std::vector models; + + // Process the result if one + rc = sqlite3_step(ppStmt); + if(rc == SQLITE_ROW) + { + found = true; + int index = 0; + + // calibration + if(uStrNumCmp(_version, "0.10.0") >= 0) + { + data = sqlite3_column_blob(ppStmt, index); + dataSize = sqlite3_column_bytes(ppStmt, index++); + // multi-cameras [fx,fy,cx,cy,[width,height],local_transform, ... ,fx,fy,cx,cy,[width,height],local_transform] (4or6+12)*float * numCameras + // stereo [fx, fy, cx, cy, baseline, local_transform] (5+12)*float + if(dataSize > 0 && data) + { + float * dataFloat = (float*)data; + if((unsigned int)dataSize % (4+localTransform.size())*sizeof(float) == 0) + { + int cameraCount = dataSize / ((4+localTransform.size())*sizeof(float)); + UDEBUG("Loading calibration for %d cameras (%d bytes)", cameraCount, dataSize); + int max = cameraCount*(4+localTransform.size()); + for(int i=0; i= 0) + { + double fx = sqlite3_column_double(ppStmt, index++); + double fyOrBaseline = sqlite3_column_double(ppStmt, index++); + double cx = sqlite3_column_double(ppStmt, index++); + double cy = sqlite3_column_double(ppStmt, index++); + if(fyOrBaseline < 1.0) + { + //it is a baseline + stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform); + } + else + { + models.push_back(CameraModel(fx, fyOrBaseline, cx, cy, localTransform)); + } + } + else + { + float depthConstant = sqlite3_column_double(ppStmt, index++); + float fx = 1.0f/depthConstant; + float fy = 1.0f/depthConstant; + float cx = 0.0f; + float cy = 0.0f; + models.push_back(CameraModel(fx, fy, cx, cy, localTransform)); + } + + rc = sqlite3_step(ppStmt); // next result... + } + UASSERT_MSG(rc == SQLITE_DONE, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + + // Finalize (delete) the statement + rc = sqlite3_finalize(ppStmt); + UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str()); + } + return found; +} + bool DBDriverSqlite3::getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, diff --git a/corelib/src/DBDriverSqlite3.h b/corelib/src/DBDriverSqlite3.h index a5f1f099..6617f9d2 100644 --- a/corelib/src/DBDriverSqlite3.h +++ b/corelib/src/DBDriverSqlite3.h @@ -83,6 +83,7 @@ private: virtual void loadLinksQuery(int signatureId, std::map & links, Link::Type type = Link::kUndef) const; virtual void loadNodeDataQuery(std::list & signatures) const; + virtual bool getCalibrationQuery(int signatureId, std::vector & models, StereoCameraModel & stereoModel) const; virtual bool getNodeInfoQuery(int signatureId, Transform & pose, int & mapId, int & weight, std::string & label, double & stamp, Transform & groundTruthPose) const; virtual void getAllNodeIdsQuery(std::set & ids, bool ignoreChildren, bool ignoreBadSignatures) const; virtual void getAllLinksQuery(std::multimap & links, bool ignoreNullLinks) const; diff --git a/corelib/src/Memory.cpp b/corelib/src/Memory.cpp index 3844bc5b..36b0f140 100644 --- a/corelib/src/Memory.cpp +++ b/corelib/src/Memory.cpp @@ -2066,113 +2066,7 @@ Transform Memory::computeTransform( if(fromS && toS) { - // make sure we have all data needed - // load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null) - if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS->sensorData().imageCompressed().empty()) || - (_registrationPipeline->isScanRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().laserScanCompressed().empty()) || - (_registrationPipeline->isUserDataRequired() && fromS->sensorData().imageCompressed().empty() && fromS->sensorData().userDataCompressed().empty())) - { - getNodeData(fromS->id()); - } - if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS->sensorData().imageCompressed().empty()) || - (_registrationPipeline->isScanRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().laserScanCompressed().empty()) || - (_registrationPipeline->isUserDataRequired() && toS->sensorData().imageCompressed().empty() && toS->sensorData().userDataCompressed().empty())) - { - getNodeData(toS->id()); - } - // uncompress only what we need - cv::Mat imgBuf, depthBuf, laserBuf, userBuf; - fromS->sensorData().uncompressData( - (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0, - (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0, - _registrationPipeline->isScanRequired()?&laserBuf:0, - _registrationPipeline->isUserDataRequired()?&userBuf:0); - toS->sensorData().uncompressData( - (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0, - (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0, - _registrationPipeline->isScanRequired()?&laserBuf:0, - _registrationPipeline->isUserDataRequired()?&userBuf:0); - - - // compute transform fromId -> toId - std::vector inliersV; - if(_reextractLoopClosureFeatures || (fromS->getWords().size() && toS->getWords().size())) - { - Signature tmpFrom = *fromS; - Signature tmpTo = *toS; - - // make a guess fast with known correspondences (if there are) - RegistrationVis regVis(parameters_); - if(tmpFrom.getWords().size() && - tmpTo.getWords().size() && - tmpFrom.getWords3().size() && - tmpTo.getWords3().size()) - { - UDEBUG(""); - // Remove descriptors, this will avoid recomputation of the correspondences in regVis - tmpFrom.setWordsDescriptors(std::multimap()); - tmpTo.setWordsDescriptors(std::multimap()); - guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); - // set back descriptors - tmpFrom.setWordsDescriptors(fromS->getWordsDescriptors()); - tmpTo.setWordsDescriptors(toS->getWordsDescriptors()); - } - - if(_reextractLoopClosureFeatures) - { - UDEBUG(""); - tmpFrom.setWords(std::multimap()); - tmpFrom.setWords3(std::multimap()); - tmpFrom.setWordsDescriptors(std::multimap()); - tmpFrom.sensorData().setFeatures(std::vector(), cv::Mat()); - tmpTo.setWords(std::multimap()); - tmpTo.setWords3(std::multimap()); - tmpTo.setWordsDescriptors(std::multimap()); - tmpTo.sensorData().setFeatures(std::vector(), cv::Mat()); - } - - if(guess.isNull()) - { - if(!_registrationPipeline->isImageRequired()) - { - UDEBUG(""); - // no visual in the pipeline, make visual registration for guess - guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); - } - else - { - UDEBUG(""); - guess.setIdentity(); - } - } - - if(!guess.isNull()) - { - UDEBUG(""); - transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info); - - if(!transform.isNull()) - { - UDEBUG(""); - // verify if it is a 180 degree transform, well verify > 90 - float x,y,z, roll,pitch,yaw; - transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); - if(fabs(roll) > CV_PI/2 || - fabs(pitch) > CV_PI/2 || - fabs(yaw) > CV_PI/2) - { - transform.setNull(); - std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)", - roll, pitch, yaw); - UINFO(msg.c_str()); - if(info) - { - info->rejectedMsg = msg; - } - } - } - } - } + return computeTransform(*fromS, *toS, guess, info); } else { @@ -2186,6 +2080,125 @@ Transform Memory::computeTransform( return transform; } +// compute transform fromId -> toId +Transform Memory::computeTransform( + Signature & fromS, + Signature & toS, + Transform guess, + RegistrationInfo * info) const +{ + Transform transform; + + // make sure we have all data needed + // load binary data from database if not in RAM (if image is already here, scan and userData should be or they are null) + if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && fromS.sensorData().imageCompressed().empty()) || + (_registrationPipeline->isScanRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().laserScanCompressed().empty()) || + (_registrationPipeline->isUserDataRequired() && fromS.sensorData().imageCompressed().empty() && fromS.sensorData().userDataCompressed().empty())) + { + fromS.sensorData() = getNodeData(fromS.id()); + } + if((_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired() && toS.sensorData().imageCompressed().empty()) || + (_registrationPipeline->isScanRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().laserScanCompressed().empty()) || + (_registrationPipeline->isUserDataRequired() && toS.sensorData().imageCompressed().empty() && toS.sensorData().userDataCompressed().empty())) + { + toS.sensorData() = getNodeData(toS.id()); + } + // uncompress only what we need + cv::Mat imgBuf, depthBuf, laserBuf, userBuf; + fromS.sensorData().uncompressData( + (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0, + (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0, + _registrationPipeline->isScanRequired()?&laserBuf:0, + _registrationPipeline->isUserDataRequired()?&userBuf:0); + toS.sensorData().uncompressData( + (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&imgBuf:0, + (_reextractLoopClosureFeatures && _registrationPipeline->isImageRequired())?&depthBuf:0, + _registrationPipeline->isScanRequired()?&laserBuf:0, + _registrationPipeline->isUserDataRequired()?&userBuf:0); + + + // compute transform fromId -> toId + std::vector inliersV; + if(_reextractLoopClosureFeatures || (fromS.getWords().size() && toS.getWords().size())) + { + Signature tmpFrom = fromS; + Signature tmpTo = toS; + + // make a guess fast with known correspondences (if there are) + RegistrationVis regVis(parameters_); + if(tmpFrom.getWords().size() && + tmpTo.getWords().size() && + tmpFrom.getWords3().size() && + tmpTo.getWords3().size()) + { + UDEBUG(""); + // Remove descriptors, this will avoid recomputation of the correspondences in regVis + tmpFrom.setWordsDescriptors(std::multimap()); + tmpTo.setWordsDescriptors(std::multimap()); + guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); + // set back descriptors + tmpFrom.setWordsDescriptors(fromS.getWordsDescriptors()); + tmpTo.setWordsDescriptors(toS.getWordsDescriptors()); + } + + if(_reextractLoopClosureFeatures) + { + UDEBUG(""); + tmpFrom.setWords(std::multimap()); + tmpFrom.setWords3(std::multimap()); + tmpFrom.setWordsDescriptors(std::multimap()); + tmpFrom.sensorData().setFeatures(std::vector(), cv::Mat()); + tmpTo.setWords(std::multimap()); + tmpTo.setWords3(std::multimap()); + tmpTo.setWordsDescriptors(std::multimap()); + tmpTo.sensorData().setFeatures(std::vector(), cv::Mat()); + } + + if(guess.isNull()) + { + if(!_registrationPipeline->isImageRequired()) + { + UDEBUG(""); + // no visual in the pipeline, make visual registration for guess + guess = regVis.computeTransformation(tmpFrom, tmpTo, guess, info); + } + else + { + UDEBUG(""); + guess.setIdentity(); + } + } + + if(!guess.isNull()) + { + UDEBUG(""); + transform = _registrationPipeline->computeTransformation(tmpFrom, tmpTo, guess, info); + + if(!transform.isNull()) + { + UDEBUG(""); + // verify if it is a 180 degree transform, well verify > 90 + float x,y,z, roll,pitch,yaw; + transform.getTranslationAndEulerAngles(x,y,z, roll,pitch,yaw); + if(fabs(roll) > CV_PI/2 || + fabs(pitch) > CV_PI/2 || + fabs(yaw) > CV_PI/2) + { + transform.setNull(); + std::string msg = uFormat("Too large rotation detected! (roll=%f, pitch=%f, yaw=%f)", + roll, pitch, yaw); + UINFO(msg.c_str()); + if(info) + { + info->rejectedMsg = msg; + } + } + } + } + } + return transform; +} + // compute transform fromId -> toId Transform Memory::computeIcpTransform( int fromId, @@ -2868,45 +2881,24 @@ cv::Mat Memory::getImageCompressed(int signatureId) const return image; } -SensorData Memory::getNodeData(int nodeId, bool uncompressedData, bool keepLoadedDataInMemory) +SensorData Memory::getNodeData(int nodeId, bool uncompressedData) const { UDEBUG("nodeId=%d", nodeId); SensorData r; Signature * s = this->_getSignature(nodeId); if(s && !s->sensorData().imageCompressed().empty()) { - if(keepLoadedDataInMemory && uncompressedData) - { - s->sensorData().uncompressData(); - } r = s->sensorData(); - if(!keepLoadedDataInMemory && uncompressedData) - { - r.uncompressData(); - } } else if(_dbDriver) { // load from database - if(s && keepLoadedDataInMemory) - { - std::list signatures; - signatures.push_back(s); - _dbDriver->loadNodeData(signatures); - if(uncompressedData) - { - s->sensorData().uncompressData(); - } - r = s->sensorData(); - } - else - { - _dbDriver->getNodeData(nodeId, r); - if(uncompressedData) - { - r.uncompressData(); - } - } + _dbDriver->getNodeData(nodeId, r); + } + + if(uncompressedData) + { + r.uncompressData(); } return r; @@ -2951,6 +2943,24 @@ void Memory::getNodeWords(int nodeId, } } +void Memory::getNodeCalibration(int nodeId, + std::vector & models, + StereoCameraModel & stereoModel) +{ + UDEBUG("nodeId=%d", nodeId); + Signature * s = this->_getSignature(nodeId); + if(s) + { + models = s->sensorData().cameraModels(); + stereoModel = s->sensorData().stereoCameraModel(); + } + else if(_dbDriver) + { + // load from database + _dbDriver->getCalibration(nodeId, models, stereoModel); + } +} + SensorData Memory::getSignatureDataConst(int locationId) const { UDEBUG(""); diff --git a/corelib/src/OptimizerG2O.cpp b/corelib/src/OptimizerG2O.cpp index a4242c34..93ce0f6c 100644 --- a/corelib/src/OptimizerG2O.cpp +++ b/corelib/src/OptimizerG2O.cpp @@ -752,7 +752,7 @@ std::map OptimizerG2O::optimizeBA( UASSERT(optimizer.verifyInformationMatrices()); - UINFO("g2o optimizing begin (max iterations=%d, robustKernel=%d)", iterations(), robustKernel?1:0); + UINFO("g2o optimizing begin (max iterations=%d, epsilon=%f robustKernel=%d)", iterations(), this->epsilon(), robustKernel?1:0); int it = 0; UTimer timer; diff --git a/corelib/src/Rtabmap.cpp b/corelib/src/Rtabmap.cpp index 6b4ce551..8cbea0a9 100644 --- a/corelib/src/Rtabmap.cpp +++ b/corelib/src/Rtabmap.cpp @@ -1639,27 +1639,29 @@ bool Rtabmap::process( ++iter) { const Signature * s = _memory->getSignature(iter->second); - UASSERT(s!=0); - // If there is a change of direction, better to be retrieving - // ALL nearest signatures than only newest neighbors - const std::map & links = s->getLinks(); - for(std::map::const_reverse_iterator jter=links.rbegin(); - jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; - ++jter) + if(s!=0) { - if(_memory->getSignature(jter->first) == 0) + // If there is a change of direction, better to be retrieving + // ALL nearest signatures than only newest neighbors + const std::map & links = s->getLinks(); + for(std::map::const_reverse_iterator jter=links.rbegin(); + jter!=links.rend() && retrievalLocalIds.size() < _maxLocalRetrieved; + ++jter) { - UINFO("retrieval of node %d on local map", jter->first); - retrievalLocalIds.push_back(jter->first); + if(_memory->getSignature(jter->first) == 0) + { + UINFO("retrieval of node %d on local map", jter->first); + retrievalLocalIds.push_back(jter->first); + } } - } - if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized) - { - if(immunizedLocations.insert(s->id()).second) + if(!_memory->isInSTM(s->id()) && immunizedLocally < maxLocalLocationsImmunized) { - ++immunizedLocally; + if(immunizedLocations.insert(s->id()).second) + { + ++immunizedLocally; + } + UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); } - UDEBUG("local node %d (%f m) immunized=1", iter->second, iter->first); } } // well, if the maximum retrieved is not reached, look for neighbors in database @@ -2685,6 +2687,11 @@ void Rtabmap::rejectLoopClosure(int oldId, int newId) } } +void Rtabmap::setOptimizedPoses(const std::map & poses) +{ + _optimizedPoses = poses; +} + void Rtabmap::dumpData() const { UDEBUG(""); @@ -3236,6 +3243,20 @@ void Rtabmap::getGraph( label, odomPose, groundTruth))); + + std::multimap words; + std::multimap words3; + std::multimap wordsDescriptors; + _memory->getNodeWords(iter->first, words, words3, wordsDescriptors); + signatures->at(iter->first).setWords(words); + signatures->at(iter->first).setWords3(words3); + signatures->at(iter->first).setWordsDescriptors(wordsDescriptors); + + std::vector models; + StereoCameraModel stereoModel; + _memory->getNodeCalibration(iter->first, models, stereoModel); + signatures->at(iter->first).sensorData().setCameraModels(models); + signatures->at(iter->first).sensorData().setStereoCameraModel(stereoModel); } } } @@ -3249,6 +3270,119 @@ void Rtabmap::getGraph( } } +int Rtabmap::detectMoreLoopClosures(float clusterRadius, float clusterAngle, int iterations) +{ + UASSERT(iterations>0); + + if(_graphOptimizer->iterations() <= 0) + { + UERROR("Cannot detect more loop closures if graph optimization iterations = 0"); + return 0; + } + if(!_rgbdSlamMode) + { + UERROR("Detecting more loop closures can be done only in RGBD-SLAM mode."); + return 0; + } + + std::list loopClosuresAdded; + std::multimap checkedLoopClosures; + + std::map poses; + std::multimap links; + this->getGraph(poses, links, true, true); + + for(int n=0; n clusters = graph::radiusPosesClustering( + poses, + clusterRadius, + clusterAngle); + + UINFO("Looking for more loop closures, clustering poses... found %d clusters.", (int)clusters.size()); + + int i=0; + std::set addedLinks; + for(std::multimap::iterator iter=clusters.begin(); iter!= clusters.end(); ++iter, ++i) + { + int from = iter->first; + int to = iter->second; + if(iter->first < iter->second) + { + from = iter->second; + to = iter->first; + } + + bool alreadyChecked = false; + for(std::multimap::iterator jter = checkedLoopClosures.lower_bound(from); + !alreadyChecked && jter!=checkedLoopClosures.end() && jter->first == from; + ++jter) + { + if(to == jter->second) + { + alreadyChecked = true; + } + } + + if(!alreadyChecked) + { + // only add new links and one per cluster per iteration + if(addedLinks.find(from) == addedLinks.end() && + rtabmap::graph::findLink(links, from, to) == links.end()) + { + checkedLoopClosures.insert(std::make_pair(from, to)); + + RegistrationInfo info; + Transform t = _memory->computeTransform(from, to, Transform(), &info); + + if(!t.isNull()) + { + UINFO("Added new loop closure between %d and %d.", from, to); + addedLinks.insert(from); + addedLinks.insert(to); + links.insert(std::make_pair(from, Link(from, to, Link::kUserClosure, t, info.variance, info.variance))); + loopClosuresAdded.push_back(Link(from, to, Link::kUserClosure, t, info.variance, info.variance)); + UINFO("Detected loop closure %d->%d! (%d/%d)", from, to, i+1, (int)clusters.size()); + } + } + } + } + UINFO("Iteration %d/%d: Detected %d loop closures!", n+1, iterations, (int)addedLinks.size()/2); + if(addedLinks.size() == 0) + { + break; + } + + if(n+1 < iterations) + { + UINFO("Optimizing graph with new links (%d nodes, %d constraints)...", + (int)poses.size(), (int)links.size()); + int fromId = _optimizeFromGraphEnd?poses.rbegin()->first:poses.begin()->first; + poses = _graphOptimizer->optimize(fromId, poses, links, 0); + if(poses.size() == 0) + { + UERROR("Optimization failed! Rejecting all loop closures..."); + loopClosuresAdded.clear(); + break; + } + UINFO("Optimizing graph with new links... done!"); + } + } + UINFO("Total added %d loop closures.", (int)loopClosuresAdded.size()); + + if(loopClosuresAdded.size()) + { + for(std::list::iterator iter=loopClosuresAdded.begin(); iter!=loopClosuresAdded.end(); ++iter) + { + _memory->addLink(*iter); + } + } + return (int)loopClosuresAdded.size(); +} + void Rtabmap::clearPath(int status) { UINFO("status=%d", status);