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

rtabmap: refining neighbor links uses computeIcpTransform() instead of computeTransform() (to ignore Visual registration, which adds more errors most of the time). When database is in RAM, we can close under a different name than the one used when the database was loaded.

Browse Source 
Tango: Added Append Mode option (only start a new map after being localized, default true) and the old map is still shown for convenience until localization. Increased minInliers to 25. Increased ICP correspondenceRatio to 0.5. Decreased time to open a database. Saving doesn't close the database anymore and overwrite only if we save with the same name. Removed ICP refining from standard optimization.
This commit is contained in:
matlabbe
2016-11-21 17:09:04 -05:00
parent 4c559a3191
commit 14908e3a11
19 changed files with 380 additions and 296 deletions
Download Patch File Download Diff File Expand all files Collapse all files
app/android/AndroidManifest.xml.in
+1 -1
View File
@@ -14,7 +14,7 @@
<uses-library android:name="com.projecttango.libtango_device2" android:required="true" />
<!-- This is the platform API where NativeActivity was introduced. -->
<uses-sdk android:minSdkVersion="17" />
<uses-sdk android:minSdkVersion="18" />
<!-- This .apk has no Java code itself, so set hasCode to false. -->
<application
app/android/jni/RTABMapApp.cpp
+105 -69
View File
@@ -78,6 +78,7 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapTimeThr(), std::string("800")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishLikelihood(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapPublishPdf(), std::string("false")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
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(), graphOptimization_?"10":"0"));
@@ -87,21 +88,21 @@ rtabmap::ParametersMap RTABMapApp::getRtabmapParameters()
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::kVisMinInliers(), std::string("25")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kVisEstimationType(), std::string("0"))); // 0=3D-3D 1=PnP
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDOptimizeMaxError(), std::string("0.1")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityPathMaxNeighbors(), std::string("0"))); // disable scan matching to merged nodes
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDProximityBySpace(), std::string("false"))); // just keep loop closure detection
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRGBDNeighborLinkRefining(), uBool2Str(driftCorrection_)));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"1":"0")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string(driftCorrection_?"2":"0")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpPointToPlane(), std::string("true")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("6")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemLaserScanNormalK(), std::string("10")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpIterations(), std::string("10")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpEpsilon(), std::string("0.001")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxRotation(), std::string("0.17"))); // 10 degrees
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxTranslation(), std::string("0.05")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.3")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpCorrespondenceRatio(), std::string("0.5")));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kIcpMaxCorrespondenceDistance(), std::string("0.05")));
parameters.insert(*rtabmap::Parameters::getDefaultParameters().find(rtabmap::Parameters::kKpMaxFeatures()));
@@ -130,6 +131,7 @@ RTABMapApp::RTABMapApp() :
trajectoryMode_(false),
autoExposure_(true),
fullResolution_(false),
appendMode_(true),
maxCloudDepth_(0.0),
meshTrianglePix_(1),
meshAngleToleranceDeg_(15.0),
@@ -481,8 +483,15 @@ int RTABMapApp::Render()
{
// uncompress
rtabmap::SensorData data = jter->second.sensorData();
cv::Mat tmpA,tmpB;
data.uncompressData(&tmpA, &tmpB);
cv::Mat tmpA,depth;
data.uncompressData(&tmpA, &depth);
// do post-processing bilateral filtering now
UTimer t;
depth = rtabmap::util2d::fastBilateralFiltering(depth, 2.0f, 0.075f);
data.setDepthOrRightRaw(depth);
LOGI("Bilateral filtering of %d, time=%fs", jter->first, t.ticks());
uInsert(bufferedSensorData, std::make_pair(jter->first, data));
uInsert(rawPoses_, std::make_pair(jter->first, jter->second.getPose()));
@@ -583,9 +592,18 @@ int RTABMapApp::Render()
inserted.first->second.texture = data.imageRaw();
inserted.first->second.cameraModel = data.cameraModels()[0];
main_scene_.addMesh(id, inserted.first->second, iter->second);
if(notifyDataLoaded)
{
// gain compensation is done, so don't compress the texture yet
inserted.first->second.texture = data.imageRaw(); // keep raw
// mesh will be added in gain compensation below
}
else
{
main_scene_.addMesh(id, inserted.first->second, iter->second);
inserted.first->second.texture = data.imageCompressed(); // keep compressed
}
inserted.first->second.texture = data.imageCompressed(); // keep compressed
}
else
{
@@ -615,18 +633,21 @@ int RTABMapApp::Render()
}
}
//update cloud visibility
std::set<int> addedClouds = main_scene_.getAddedClouds();
for(std::set<int>::const_iterator iter=addedClouds.begin();
iter!=addedClouds.end();
++iter)
if(poses.size())
{
if(*iter > 0 && poses.find(*iter) == poses.end())
//update cloud visibility
std::set<int> addedClouds = main_scene_.getAddedClouds();
for(std::set<int>::const_iterator iter=addedClouds.begin();
iter!=addedClouds.end();
++iter)
{
main_scene_.setCloudVisible(*iter, false);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.visible = true;
if(*iter > 0 && poses.find(*iter) == poses.end())
{
main_scene_.setCloudVisible(*iter, false);
std::map<int, Mesh>::iterator meshIter = createdMeshes_.find(*iter);
UASSERT(meshIter!=createdMeshes_.end());
meshIter->second.visible = true;
}
}
}
}
@@ -667,42 +688,49 @@ int RTABMapApp::Render()
if(notifyDataLoaded || gainCompensationOnNextRender_>0)
{
UTimer tGainCompensation;
LOGI("Gain compensation...");
boost::mutex::scoped_lock lock(meshesMutex_);
if(createdMeshes_.size() > 1)
rtabmap::GainCompensator compensator;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
rtabmap::GainCompensator compensator;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > clouds;
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
clouds.insert(std::make_pair(iter->first, iter->second.cloud));
}
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, false, true);
if(gainCompensationOnNextRender_ == 2)
{
// full compensation
links.clear();
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
{
clouds.insert(std::make_pair(iter->first, iter->second.cloud));
}
std::map<int, rtabmap::Transform> poses;
std::multimap<int, rtabmap::Link> links;
rtabmap_->getGraph(poses, links, false, true);
if(gainCompensationOnNextRender_ == 2)
{
// full compensation
links.clear();
for(std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator iter=clouds.begin(); iter!=clouds.end(); ++iter)
int from = iter->first;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator jter = iter;
++jter;
for(;jter!=clouds.end(); ++jter)
{
int from = iter->first;
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr>::const_iterator jter = iter;
++jter;
for(;jter!=clouds.end(); ++jter)
{
int to = jter->first;
links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to))));
}
int to = jter->first;
links.insert(std::make_pair(from, rtabmap::Link(from, to, rtabmap::Link::kUserClosure, poses.at(from).inverse()*poses.at(to))));
}
}
}
if(clouds.size() > 1 && links.size())
{
compensator.feed(clouds, links);
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
LOGI("Gain compensation... compute gain: links=%d, time=%fs", (int)links.size(), tGainCompensation.ticks());
}
for(std::map<int, Mesh>::iterator iter = createdMeshes_.begin(); iter!=createdMeshes_.end(); ++iter)
{
cv::Mat compressedImage = iter->second.texture;
iter->second.texture = compressedImage.rows == 1 ? rtabmap::uncompressImage(compressedImage) : compressedImage;
if(!iter->second.cloud->empty())
{
cv::Mat compressedImage = iter->second.texture;
iter->second.texture = rtabmap::uncompressImage(compressedImage);
if(!iter->second.cloud->empty())
if(clouds.size() > 1 && links.size())
{
compensator.apply(iter->first, iter->second.cloud);
if(!iter->second.texture.empty())
@@ -710,22 +738,21 @@ int RTABMapApp::Render()
compensator.apply(iter->first, iter->second.texture);
}
}
// If we do bilateral filtering, do it right now as the texture is uncompressed
if((notifyDataLoaded || bilateralFilteringOnNextRender_) &&
iter->second.cloud->size() && smoothMesh(iter->first, iter->second))
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world*iter->second.pose);
}
else
{
main_scene_.updateMesh(iter->first, iter->second);
}
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
}
bilateralFilteringOnNextRender_ = false;
if(notifyDataLoaded)
{
main_scene_.addMesh(iter->first, iter->second, opengl_world_T_rtabmap_world* poses.at(iter->first));
}
else
{
main_scene_.updateMesh(iter->first, iter->second);
}
iter->second.texture = rtabmap::compressImage2(iter->second.texture, ".jpg");
}
LOGI("Gain compensation... applying gain: meshes=%d, time=%fs", (int)createdMeshes_.size(), tGainCompensation.ticks());
gainCompensationOnNextRender_ = 0;
notifyDataLoaded = true;
}
@@ -929,6 +956,17 @@ void RTABMapApp::setFullResolution(bool enabled)
}
}
void RTABMapApp::setAppendMode(bool enabled)
{
if(appendMode_ != enabled)
{
appendMode_ = enabled;
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRtabmapStartNewMapOnLoopClosure(), uBool2Str(appendMode_)));
this->post(new rtabmap::ParamEvent(parameters));
}
}
void RTABMapApp::setDataRecorderMode(bool enabled)
{
if(dataRecorderMode_ != enabled)
@@ -979,9 +1017,12 @@ void RTABMapApp::resetMapping()
UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdResetMemory));
}
void RTABMapApp::save()
void RTABMapApp::save(const std::string & databasePath)
{
UEventsManager::post(new rtabmap::RtabmapEventCmd(rtabmap::RtabmapEventCmd::kCmdClose));
rtabmapThread_->join(true);
rtabmap_->close(true, databasePath);
rtabmap_->init(getRtabmapParameters(), databasePath);
rtabmapThread_->start();
}
bool RTABMapApp::exportMesh(const std::string & filePath)
@@ -1079,7 +1120,7 @@ bool RTABMapApp::exportMesh(const std::string & filePath)
UDirectory::makeDir(textureDirectory);
for(unsigned int i=0;i<textures.size(); ++i)
{
cv::Mat rawImage = rtabmap::uncompressImage(textures[i]);
cv::Mat rawImage = textures[i].rows>1?textures[i]:rtabmap::uncompressImage(textures[i]);
std::string texFile = textureDirectory+"/"+textureMesh.tex_materials[i].tex_name+".png";
cv::imwrite(texFile, rawImage);
@@ -1161,11 +1202,11 @@ int RTABMapApp::postProcessing(int approach)
if(approach == -1 || approach == 2)
{
// detect more loop closures
returnedValue = rtabmap_->detectMoreLoopClosures(0.5f, M_PI/6.0f, approach == -1?3:1);
returnedValue = rtabmap_->detectMoreLoopClosures(1.0f, M_PI/6.0f, approach == -1?5:1);
}
// ICP refining
if(returnedValue >=0 && ((approach == -1 && !driftCorrection_) || approach == 3))
if(returnedValue >=0 && approach == 3)
{
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kRegStrategy(), std::string("1"))); // ICP
@@ -1228,7 +1269,7 @@ int RTABMapApp::postProcessing(int approach)
{
boost::mutex::scoped_lock lock(renderingMutex_);
// filter polygons
if(approach == -1 || approach == 4)
if(approach == 4)
{
filterPolygonsOnNextRender_ = true;
}
@@ -1330,11 +1371,6 @@ void RTABMapApp::handleEvent(UEvent * event)
status_.first = ((rtabmap::RtabmapEventInit*)event)->getStatus();
status_.second = ((rtabmap::RtabmapEventInit*)event)->getInfo();
if(status_.first == rtabmap::RtabmapEventInit::kClosed)
{
clearSceneOnNextRender_ = true;
}
// Call JAVA callback with init msg
bool success = false;
if(jvm && RTABMapActivity)
app/android/jni/RTABMapApp.h
+3 -1
View File
@@ -122,6 +122,7 @@ class RTABMapApp : public UEventsHandler {
void setGridVisible(bool visible);
void setAutoExposure(bool enabled);
void setFullResolution(bool enabled);
void setAppendMode(bool enabled);
void setDataRecorderMode(bool enabled);
void setMaxCloudDepth(float value);
void setMeshAngleTolerance(float value);
@@ -129,7 +130,7 @@ class RTABMapApp : public UEventsHandler {
int setMappingParameter(const std::string & key, const std::string & value);
void resetMapping();
void save();
void save(const std::string & databasePath);
bool exportMesh(const std::string & filePath);
int postProcessing(int approach);
@@ -154,6 +155,7 @@ class RTABMapApp : public UEventsHandler {
bool trajectoryMode_;
bool autoExposure_;
bool fullResolution_;
bool appendMode_;
float maxCloudDepth_;
int meshTrianglePix_;
float meshAngleToleranceDeg_;
app/android/jni/jni_interface.cpp
+10 -2
View File
@@ -192,6 +192,12 @@ Java_com_introlab_rtabmap_RTABMapLib_setFullResolution(
return app.setFullResolution(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setAppendMode(
JNIEnv*, jobject, bool enabled)
{
return app.setAppendMode(enabled);
}
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_setDataRecorderMode(
JNIEnv*, jobject, bool enabled)
{
@@ -234,9 +240,11 @@ Java_com_introlab_rtabmap_RTABMapLib_resetMapping(
JNIEXPORT void JNICALL
Java_com_introlab_rtabmap_RTABMapLib_save(
JNIEnv* env, jobject)
JNIEnv* env, jobject, jstring databasePath)
{
return app.save();
std::string databasePathC;
GetJStringContent(env,databasePath,databasePathC);
return app.save(databasePathC);
}
JNIEXPORT bool JNICALL
app/android/res/menu/optionmenu.xml
+1
View File
@@ -65,6 +65,7 @@
<item android:id="@+id/localization_mode" android:checked="false" android:title="Localization Mode" />
<item android:id="@+id/trajectory_mode" android:checked="false" android:title="Trajectory Mode" />
<item android:id="@+id/graph_optimization" android:checked="true" android:title="Optimized Graph" />
<item android:id="@+id/append" android:checked="true" android:title="Append Mode" />
<item android:id="@+id/nodes_filtering" android:checked="false" android:title="Nodes Filtering" />
<item android:id="@+id/drift_correction" android:checked="false" android:title="Drift Correction" />
<item android:id="@+id/resolution" android:checked="false" android:title="HD Mode" />
app/android/src/com/introlab/rtabmap/RTABMapActivity.java
+198 -180
View File
@@ -87,7 +87,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
private String mOpenedDatabasePath = "";
private String mTempDatabasePath = "";
private String mNewDatabasePath = "";
private String mWorkingDirectory = "";
private int mMaxDepthIndex = 5;
@@ -179,7 +178,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
mOpenedDatabasePath = "";
mTempDatabasePath = "";
mNewDatabasePath = "";
mWorkingDirectory = "";
mTotalLoopClosures = 0;
@@ -436,108 +434,6 @@ public class RTABMapActivity extends Activity implements OnClickListener {
((TextView)findViewById(R.id.status)).setText(
status == 1 && msg.isEmpty()?mItemPause!=null&&mItemPause.isChecked()?"Paused":mItemLocalizationMode!=null&&mItemLocalizationMode.isChecked()?"Localization":"Mapping":msg);
/*0=kInitializing,
1=kInitialized,
2=kClosing,
3=kClosed,
4=kInfo,
5=kError*/
if(status == 3)
{
msg = "";
if(!mNewDatabasePath.isEmpty())
{
boolean removed = true;
File outputFile = new File(mNewDatabasePath);
if(outputFile.exists())
{
removed = outputFile.delete();
}
if(removed)
{
File tempFile = new File(mTempDatabasePath);
if(tempFile.renameTo(outputFile))
{
msg = String.format("Database saved to \"%s\". Tip: You can open a saved database with \"Open\".", mNewDatabasePath);
Intent intent = new Intent(this, RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
PendingIntent pIntent = PendingIntent.getActivity(this, (int) System.currentTimeMillis(), intent, 0);
// build notification
// the addAction re-use the same intent to keep the example short
Notification n = new Notification.Builder(this)
.setContentTitle(getString(R.string.app_name))
.setContentText(mNewDatabasePath + " saved!")
.setSmallIcon(R.drawable.ic_launcher)
.setContentIntent(pIntent)
.setAutoCancel(true).build();
NotificationManager notificationManager =
(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
notificationManager.notify(0, n);
}
else
{
msg = String.format("Failed to rename temporary database from \"%s\" to \"%s\".",
mTempDatabasePath, mNewDatabasePath);
}
}
else
{
msg = String.format("Failed to overwrite the database \"%s\". The temporary database is still correctly saved at \"%s\".",
mNewDatabasePath, mTempDatabasePath);
}
}
else if(!mOpenedDatabasePath.isEmpty())
{
msg = String.format("Database \"%s\" updated.", mOpenedDatabasePath);
}
if(!msg.isEmpty())
{
mToast.makeText(this, msg, mToast.LENGTH_LONG).show();
}
mOpenedDatabasePath = "";
mNewDatabasePath = "";
//restart a new scan by default
RTABMapLib.openDatabase(mTempDatabasePath);
((TextView)findViewById(R.id.points)).setText(String.valueOf(0));
((TextView)findViewById(R.id.polygons)).setText(String.valueOf(0));
((TextView)findViewById(R.id.nodes)).setText(String.valueOf(0));
((TextView)findViewById(R.id.words)).setText(String.valueOf(0));
((TextView)findViewById(R.id.memory)).setText(String.valueOf(Debug.getNativeHeapAllocatedSize()/(1024*1024)));
((TextView)findViewById(R.id.db_size)).setText(String.valueOf(0));
((TextView)findViewById(R.id.inliers)).setText(String.valueOf(0));
((TextView)findViewById(R.id.features)).setText(String.valueOf(0));
((TextView)findViewById(R.id.update_time)).setText(String.valueOf(0));
((TextView)findViewById(R.id.hypothesis)).setText(String.valueOf(0));
((TextView)findViewById(R.id.fps)).setText(String.valueOf(0));
mTotalLoopClosures = 0;
((TextView)findViewById(R.id.total_loop)).setText(String.valueOf(mTotalLoopClosures));
if(mItemSave!=null)
{
if(mItemPause.isChecked())
{
mItemPause.setChecked(false);
mItemOpen.setEnabled(false);
mItemPostProcessing.setEnabled(false);
mItemSave.setEnabled(false);
mItemExport.setEnabled(false);
mItemDataRecorderMode.setEnabled(false);
RTABMapLib.setPausedMapping(false); // resume mapping
}
}
mProgressDialog.dismiss();
}
}
//called from jni
@@ -938,6 +834,11 @@ public class RTABMapActivity extends Activity implements OnClickListener {
item.setChecked(!item.isChecked());
RTABMapLib.setFullResolution(item.isChecked());
}
else if(itemId == R.id.append)
{
item.setChecked(!item.isChecked());
RTABMapLib.setAppendMode(item.isChecked());
}
else if(itemId == R.id.max_depth)
{
// get double
@@ -1102,90 +1003,161 @@ public class RTABMapActivity extends Activity implements OnClickListener {
}
else if (itemId == R.id.save)
{
if(mOpenedDatabasePath.isEmpty())
{
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("RTAB-Map Database Name (*.db):");
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
AlertDialog.Builder builder = new AlertDialog.Builder(this);
builder.setTitle("RTAB-Map Database Name (*.db):");
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
if(mOpenedDatabasePath.isEmpty())
{
String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
input.setText(timeStamp);
input.setSelectAllOnFocus(true);
input.selectAll();
builder.setView(input);
builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which)
}
else
{
File f = new File(mOpenedDatabasePath);
String name = f.getName();
input.setText(name.substring(0,name.lastIndexOf(".")));
}
input.setSelectAllOnFocus(true);
input.selectAll();
builder.setView(input);
builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@Override
public void onClick(DialogInterface dialog, int which)
{
final String fileName = input.getText().toString();
dialog.dismiss();
if(!fileName.isEmpty())
{
final String fileName = input.getText().toString();
dialog.dismiss();
if(!fileName.isEmpty())
File newFile = new File(mWorkingDirectory + fileName + ".db");
if(newFile.exists())
{
File newFile = new File(mWorkingDirectory + fileName + ".db");
if(newFile.exists())
{
new AlertDialog.Builder(getActivity())
.setTitle("File Already Exists")
.setMessage("Do you want to overwrite the existing file?")
.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
mNewDatabasePath = mWorkingDirectory + fileName + ".db";
new AlertDialog.Builder(getActivity())
.setTitle("File Already Exists")
.setMessage("Do you want to overwrite the existing file?")
.setPositiveButton("Yes", new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
mProgressDialog.setTitle("Saving");
mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
mProgressDialog.show();
final String newDatabasePath = mWorkingDirectory + fileName + ".db";
mProgressDialog.setTitle("Saving");
if(mOpenedDatabasePath.equals(newDatabasePath))
{
mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", newDatabasePath));
}
else
{
mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
}
mProgressDialog.show();
RTABMapLib.save(); // send save event
//disable gui actions
mItemSave.setEnabled(false);
mItemOpen.setEnabled(false);
mItemPostProcessing.setEnabled(false);
mItemExport.setEnabled(false);
mItemDataRecorderMode.setEnabled(false);
}
})
.setNegativeButton("No", new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
}
})
.show();
}
else
{
mNewDatabasePath = mWorkingDirectory + fileName + ".db";
Thread saveThread = new Thread(new Runnable() {
public void run() {
RTABMapLib.save(newDatabasePath); // save
runOnUiThread(new Runnable() {
public void run() {
if(mOpenedDatabasePath.equals(newDatabasePath))
{
mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
}
else
{
mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
mProgressDialog.setTitle("Saving");
mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", mNewDatabasePath));
mProgressDialog.show();
Intent intent = new Intent(getActivity(), RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
RTABMapLib.save(); // send save event
//disable gui actions
mItemSave.setEnabled(false);
mItemOpen.setEnabled(false);
mItemPostProcessing.setEnabled(false);
mItemExport.setEnabled(false);
mItemDataRecorderMode.setEnabled(false);
}
// build notification
// the addAction re-use the same intent to keep the example short
Notification n = new Notification.Builder(getActivity())
.setContentTitle(getString(R.string.app_name))
.setContentText(newDatabasePath + " saved!")
.setSmallIcon(R.drawable.ic_launcher)
.setContentIntent(pIntent)
.setAutoCancel(true).build();
NotificationManager notificationManager =
(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
notificationManager.notify(0, n);
}
mProgressDialog.dismiss();
}
});
}
});
saveThread.start();
mOpenedDatabasePath = newDatabasePath;
}
})
.setNegativeButton("No", new DialogInterface.OnClickListener() {
public void onClick(DialogInterface dialog, int which) {
dialog.dismiss();
}
})
.show();
}
else
{
final String newDatabasePath = mWorkingDirectory + fileName + ".db";
mProgressDialog.setTitle("Saving");
if(mOpenedDatabasePath.equals(newDatabasePath))
{
mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
}
else
{
mProgressDialog.setMessage(String.format("Please wait while saving \"%s\"...", newDatabasePath));
}
mProgressDialog.show();
Thread saveThread = new Thread(new Runnable() {
public void run() {
RTABMapLib.save(newDatabasePath); // save
runOnUiThread(new Runnable() {
public void run() {
if(mOpenedDatabasePath.equals(newDatabasePath))
{
mToast.makeText(getActivity(), String.format("Database \"%s\" updated.", newDatabasePath), mToast.LENGTH_LONG).show();
}
else
{
mToast.makeText(getActivity(), String.format("Database saved to \"%s\".", newDatabasePath), mToast.LENGTH_LONG).show();
Intent intent = new Intent(getActivity(), RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
// build notification
// the addAction re-use the same intent to keep the example short
Notification n = new Notification.Builder(getActivity())
.setContentTitle(getString(R.string.app_name))
.setContentText(newDatabasePath + " saved!")
.setSmallIcon(R.drawable.ic_launcher)
.setContentIntent(pIntent)
.setAutoCancel(true).build();
NotificationManager notificationManager =
(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
notificationManager.notify(0, n);
}
mProgressDialog.dismiss();
}
});
}
});
saveThread.start();
mOpenedDatabasePath = newDatabasePath;
}
}
});
builder.show();
}
else
{
mProgressDialog.setTitle("Saving");
mProgressDialog.setMessage(String.format("Please wait while updating \"%s\"...", mOpenedDatabasePath));
mProgressDialog.show();
RTABMapLib.save(); // send save event
//disable gui actions
mItemSave.setEnabled(false);
mItemOpen.setEnabled(false);
mItemPostProcessing.setEnabled(false);
mItemExport.setEnabled(false);
mItemDataRecorderMode.setEnabled(false);
}
}
});
builder.show();
}
else if(itemId == R.id.reset)
{
@@ -1276,8 +1248,17 @@ public class RTABMapActivity extends Activity implements OnClickListener {
final EditText input = new EditText(this);
input.setInputType(InputType.TYPE_CLASS_TEXT);
builder.setView(input);
String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
input.setText(timeStamp);
if(mOpenedDatabasePath.isEmpty())
{
String timeStamp = new SimpleDateFormat("yyMMdd-hhmmss").format(new Date());
input.setText(timeStamp);
}
else
{
File f = new File(mOpenedDatabasePath);
String name = f.getName();
input.setText(name.substring(0,name.lastIndexOf(".")));
}
input.setSelectAllOnFocus(true);
input.selectAll();
builder.setPositiveButton("OK", new DialogInterface.OnClickListener() {
@@ -1333,6 +1314,24 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{
mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
}
Intent intent = new Intent(getActivity(), RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
// build notification
// the addAction re-use the same intent to keep the example short
Notification n = new Notification.Builder(getActivity())
.setContentTitle(getString(R.string.app_name))
.setContentText(path + " exported!")
.setSmallIcon(R.drawable.ic_launcher)
.setContentIntent(pIntent)
.setAutoCancel(true).build();
NotificationManager notificationManager =
(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
notificationManager.notify(0, n);
}
else
{
@@ -1376,6 +1375,25 @@ public class RTABMapActivity extends Activity implements OnClickListener {
{
mToast.makeText(getActivity(), String.format("Mesh \"%s\" successfully exported!", path), mToast.LENGTH_LONG).show();
}
Intent intent = new Intent(getActivity(), RTABMapActivity.class);
// use System.currentTimeMillis() to have a unique ID for the pending intent
PendingIntent pIntent = PendingIntent.getActivity(getActivity(), (int) System.currentTimeMillis(), intent, 0);
// build notification
// the addAction re-use the same intent to keep the example short
Notification n = new Notification.Builder(getActivity())
.setContentTitle(getString(R.string.app_name))
.setContentText(path + " exported!")
.setSmallIcon(R.drawable.ic_launcher)
.setContentIntent(pIntent)
.setAutoCancel(true).build();
NotificationManager notificationManager =
(NotificationManager) getSystemService(NOTIFICATION_SERVICE);
notificationManager.notify(0, n);
}
else
{
app/android/src/com/introlab/rtabmap/RTABMapLib.java
+2 -1
View File
@@ -68,6 +68,7 @@ public class RTABMapLib
public static native void setGridVisible(boolean visible);
public static native void setAutoExposure(boolean enabled);
public static native void setFullResolution(boolean enabled);
public static native void setAppendMode(boolean enabled);
public static native void setDataRecorderMode(boolean enabled);
public static native void setMaxCloudDepth(float value);
public static native void setMeshAngleTolerance(float value);
@@ -75,7 +76,7 @@ public class RTABMapLib
public static native int setMappingParameter(String key, String value);
public static native void resetMapping();
public static native void save();
public static native void save(String outputDatabasePath);
public static native boolean exportMesh(String filePath);
public static native int postProcessing(int approach);
corelib/include/rtabmap/core/DBDriver.h
+2 -2
View File
@@ -102,7 +102,7 @@ public:
// Mutex-protected methods of abstract versions below
bool openConnection(const std::string & url, bool overwritten = false);
void closeConnection(bool save = true);
void closeConnection(bool save = true, const std::string & outputUrl = "");
bool isConnected() const;
long getMemoryUsed() const; // In bytes
std::string getDatabaseVersion() const;
@@ -146,7 +146,7 @@ protected:
private:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false) = 0;
virtual void disconnectDatabaseQuery(bool save = true) = 0;
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "") = 0;
virtual bool isConnectedQuery() const = 0;
virtual long getMemoryUsedQuery() const = 0; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const = 0;
corelib/include/rtabmap/core/Memory.h
+1 -1
View File
@@ -80,7 +80,7 @@ public:
bool dbOverwritten = false,
const ParametersMap & parameters = ParametersMap(),
bool postInitClosingEvents = false);
void close(bool databaseSaved = true, bool postInitClosingEvents = false);
void close(bool databaseSaved = true, bool postInitClosingEvents = false, const std::string & ouputDatabasePath = "");
std::map<int, float> computeLikelihood(const Signature * signature,
const std::list<int> & ids);
int incrementMapId(std::map<int, int> * reducedIds = 0);
corelib/include/rtabmap/core/Rtabmap.h
+8 -1
View File
@@ -67,7 +67,14 @@ public:
void init(const ParametersMap & parameters, const std::string & databasePath = "");
void init(const std::string & configFile = "", const std::string & databasePath = "");
void close(bool databaseSaved = true);
/**
* Close rtabmap. This will delete rtabmap object if set.
* @param databaseSaved true=database saved, false=database discarded.
* @param databasePath output database file name, ignored if
* Db/Sqlite3InMemory=false (opened database is
* then overwritten).
*/
void close(bool databaseSaved = true, const std::string & ouputDatabasePath = "");
const std::string & getWorkingDir() const {return _wDir;}
bool isRGBDMode() const { return _rgbdSlamMode; }
corelib/include/rtabmap/core/RtabmapEvent.h
+1 -1
View File
@@ -61,7 +61,7 @@ public:
enum Cmd {
kCmdInit, // params: [string] database path + ParametersMap
kCmdResetMemory,
kCmdClose, // params: [bool] database saved (default true)
kCmdClose, // params: [bool] database saved (default true), [string] output database path (empty=use same database to save, only work when Db/Sqlite3InMemory=true)
kCmdDumpMemory,
kCmdDumpPrediction,
kCmdGenerateDOTGraph, // params: [bool] global, [string] path, if global=false: [int] id, [int] margin
corelib/include/rtabmap/core/RtabmapThread.h
+8 -2
View File
@@ -82,8 +82,14 @@ public:
void setDataBufferSize(unsigned int bufferSize);
void createIntermediateNodes(bool enabled);
// this will delete rtabmap object if set
void close(bool databaseSaved);
/**
* Close rtabmap. This will delete rtabmap object if set.
* @param databaseSaved true=database saved, false=database discarded.
* @param databasePath output database file name, ignored if
* Db/Sqlite3InMemory=false (opened database is
* then overwritten).
*/
void close(bool databaseSaved, const std::string & databasePath = "");
protected:
virtual void handleEvent(UEvent * anEvent);
corelib/src/DBDriver.cpp
+2 -2
View File
@@ -61,7 +61,7 @@ void DBDriver::parseParameters(const ParametersMap & parameters)
{
}
void DBDriver::closeConnection(bool save)
void DBDriver::closeConnection(bool save, const std::string & outputUrl)
{
UDEBUG("isRunning=%d", this->isRunning());
this->join(true);
@@ -78,7 +78,7 @@ void DBDriver::closeConnection(bool save)
_trashesMutex.unlock();
}
_dbSafeAccessMutex.lock();
this->disconnectDatabaseQuery(save);
this->disconnectDatabaseQuery(save, outputUrl);
_dbSafeAccessMutex.unlock();
UDEBUG("");
}
corelib/src/DBDriverSqlite3.cpp
+15 -3
View File
@@ -390,7 +390,7 @@ bool DBDriverSqlite3::connectDatabaseQuery(const std::string & url, bool overwri
return true;
}
void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & outputUrl)
{
UDEBUG("");
if(_ppDb)
@@ -411,11 +411,23 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save)
{
UTimer timer;
timer.start();
UINFO("Saving database to %s ...", this->getUrl().c_str());
rc = loadOrSaveDb(_ppDb, this->getUrl(), 1); // Save memory to file
std::string outputFile = this->getUrl();
if(!outputUrl.empty())
{
outputFile = outputUrl;
}
UINFO("Saving database to %s ...", outputFile.c_str());
rc = loadOrSaveDb(_ppDb, outputFile, 1); // Save memory to file
UASSERT_MSG(rc == SQLITE_OK, uFormat("DB error (%s): %s", _version.c_str(), sqlite3_errmsg(_ppDb)).c_str());
ULOGGER_DEBUG("Saving DB time = %fs", timer.ticks());
}
else if(save && !outputUrl.empty() && outputUrl.compare(this->getUrl()) != 0)
{
UWARN("Output database path (%s) is different than the opened database "
"path (%s). Exporting to a different path is only available "
"when database is in memory (%s=true). Opened database path is overwritten.",
outputUrl.c_str(), this->getUrl().c_str(), Parameters::kDbSqlite3InMemory().c_str());
}
// Then close (delete) the database connection
UINFO("Disconnecting database %s...", this->getUrl().c_str());
corelib/src/DBDriverSqlite3.h
+1 -1
View File
@@ -49,7 +49,7 @@ public:
private:
virtual bool connectDatabaseQuery(const std::string & url, bool overwritten = false);
virtual void disconnectDatabaseQuery(bool save = true);
virtual void disconnectDatabaseQuery(bool save = true, const std::string & outputUrl = "");
virtual bool isConnectedQuery() const;
virtual long getMemoryUsedQuery() const; // In bytes
virtual bool getDatabaseVersionQuery(std::string & version) const;
corelib/src/Memory.cpp
+2 -2
View File
@@ -304,7 +304,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
return success;
}
void Memory::close(bool databaseSaved, bool postInitClosingEvents)
void Memory::close(bool databaseSaved, bool postInitClosingEvents, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d, postInitClosingEvents=%d", databaseSaved?1:0, postInitClosingEvents?1:0);
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(RtabmapEventInit::kClosing));
@@ -342,7 +342,7 @@ void Memory::close(bool databaseSaved, bool postInitClosingEvents)
_dbDriver->emptyTrashes();
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Saving memory, done!"));
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit(uFormat("Closing database \"%s\"...", _dbDriver->getUrl().c_str())));
_dbDriver->closeConnection();
_dbDriver->closeConnection(true, ouputDatabasePath);
delete _dbDriver;
_dbDriver = 0;
if(postInitClosingEvents) UEventsManager::post(new RtabmapEventInit("Closing database, done!"));
corelib/src/Rtabmap.cpp
+3 -3
View File
@@ -320,7 +320,7 @@ void Rtabmap::init(const std::string & configFile, const std::string & databaseP
this->init(param, databasePath);
}
void Rtabmap::close(bool databaseSaved)
void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
{
UINFO("databaseSaved=%d", databaseSaved?1:0);
_highestHypothesis = std::make_pair(0,0.0f);
@@ -354,7 +354,7 @@ void Rtabmap::close(bool databaseSaved)
}
if(_memory)
{
_memory->close(databaseSaved, true);
_memory->close(databaseSaved, true, ouputDatabasePath);
delete _memory;
_memory = 0;
}
@@ -1024,7 +1024,7 @@ bool Rtabmap::process(
{
UINFO("Odometry refining: guess = %s", guess.prettyPrint().c_str());
RegistrationInfo info;
Transform t = _memory->computeTransform(oldId, signature->id(), guess, &info);
Transform t = _memory->computeIcpTransform(oldId, signature->id(), guess, &info);
if(!t.isNull())
{
UINFO("Odometry refining: update neighbor link (%d->%d, variance=%f) from %s to %s",
corelib/src/RtabmapThread.cpp
+4 -3
View File
@@ -117,12 +117,12 @@ void RtabmapThread::createIntermediateNodes(bool enabled)
_createIntermediateNodes = enabled;
}
void RtabmapThread::close(bool databaseSaved)
void RtabmapThread::close(bool databaseSaved, const std::string & ouputDatabasePath)
{
this->join(true);
if(_rtabmap)
{
_rtabmap->close(databaseSaved);
_rtabmap->close(databaseSaved, ouputDatabasePath);
delete _rtabmap;
_rtabmap = 0;
}
@@ -241,7 +241,7 @@ void RtabmapThread::mainLoop()
UWARN("Closing... %d data still buffered! They will be cleared.", (int)_dataBuffer.size());
this->clearBufferedData();
}
_rtabmap->close(uStr2Bool(parameters.at("saved")));
_rtabmap->close(uStr2Bool(parameters.at("saved")), parameters.at("outputPath"));
break;
case kStateDumpingMemory:
_rtabmap->dumpData();
@@ -409,6 +409,7 @@ void RtabmapThread::handleEvent(UEvent* event)
UASSERT(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().isBool());
ParametersMap param;
param.insert(ParametersPair("saved", uBool2Str(rtabmapEvent->value1().isUndef() || rtabmapEvent->value1().toBool())));
param.insert(ParametersPair("outputPath", rtabmapEvent->value2().toStr()));
pushNewState(kStateClose, param);
}
else if(cmd == RtabmapEventCmd::kCmdResetMemory)
corelib/src/util2d.cpp
+9 -17
View File
@@ -1742,15 +1742,15 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
cv::Mat output = depth.clone();
cv::Mat output = cv::Mat::zeros(depth.size(), CV_32FC1);
float base_max = -std::numeric_limits<float>::max ();
float base_min = std::numeric_limits<float>::max ();
bool found_finite = false;
for (int x = 0; x < output.cols; ++x)
for (int y = 0; y < output.rows; ++y)
for (int x = 0; x < depth.cols; ++x)
for (int y = 0; y < depth.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y, x):float(output.at<unsigned short>(y, x))/1000.0f;
float z = depth.type()==CV_32FC1?depth.at<float>(y, x):float(depth.at<unsigned short>(y, x))/1000.0f;
if (z > 0.0f && uIsFinite(z))
{
if (base_max < z)
@@ -1783,7 +1783,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
for (int y = 0; y < depth.rows; ++y)
{
float v = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
float v = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
if((v > 0 && uIsFinite(v)))
{
float z = v - base_min;
@@ -1832,7 +1832,7 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
for (int x = 0; x < depth.cols; ++x)
for (int y = 0; y < depth.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
float z = depth.type()==CV_32FC1?depth.at<float>(y,x):float(depth.at<unsigned short>(y,x))/1000.0f;
if(z > 0 && uIsFinite(z))
{
z -= base_min;
@@ -1844,19 +1844,11 @@ cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR
{
v = 0.0f;
}
if(depth.type()==CV_32FC1)
if(depth.type()==CV_16UC1 && v>65.5350f)
{
output.at<float>(y,x) = v;
}
else
{
v*=1000.0f;
if(v>65535.0f)
{
v = 65535.0f;
}
output.at<unsigned short>(y,x) = v;
v = 65.5350f;
}
output.at<float>(y,x) = v;
}
}