i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tango #57: Global optimization / Post-processing on pause

Browse Source
This commit is contained in:
matlabbe
2016-04-03 21:35:30 -04:00
parent 30e52b785a
commit 5d9522c901
15 changed files with 621 additions and 166 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
app/android/jni/CameraTango.cpp
View File

@@ -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));
}

72
app/android/jni/RTABMapApp.cpp
View File

@@ -44,6 +44,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/core/ParamEvent.h>
#include <rtabmap/core/Compression.h>
#include <rtabmap/core/Optimizer.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/ply_io.h>
#include <pcl/io/obj_io.h>
@@ -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<int, rtabmap::Signature> signatures;
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> 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<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
if(graphOptimizationOnly)
{
rtabmap_->getGraph(poses, links, true, true);
}
else
{
detectedLoopClosures = rtabmap_->detectMoreLoopClosures();
std::map<int, rtabmap::Signature> 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())

2
app/android/jni/RTABMapApp.h
View File

@@ -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_;

9
app/android/jni/jni_interface.cpp
View File

@@ -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

66
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
View File

@@ -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
{

1
app/android/src/com/introlab/rtabmap/RTABMapLib.java
View File

@@ -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();