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

DatabaseViewer: ask saving changes to database on Exit

Browse Source 
New parameter: Mem/InitWMWithAllNodes

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1680 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-09-21 21:14:17 +00:00
parent a98f819821
commit c4408920f6
7 changed files with 252 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
corelib/include/rtabmap/core/Parameters.h
View File

@@ -157,7 +157,8 @@ class RTABMAP_EXP Parameters
RTABMAP_PARAM(Mem, RecentWmRatio, float, 0.2, "Ratio of locations after the last loop closure in WM that cannot be transferred.");
RTABMAP_PARAM(Mem, RehearsalIdUpdatedToNewOne, bool, false, "On merge, update to new id. When false, no copy.");
RTABMAP_PARAM(Mem, GenerateIds, bool, true, "True=Generate location Ids, False=use input image ids.");
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.")
RTABMAP_PARAM(Mem, BadSignaturesIgnored, bool, false, "Bad signatures are ignored.");
RTABMAP_PARAM(Mem, InitWMWithAllNodes, bool, false, "Initialize the Working Memory with all nodes in Long-Term Memory. When false, it is initialized with nodes of the previous session.")
// KeypointMemory (Keypoint-based)
RTABMAP_PARAM(Kp, PublishKeypoints, bool, true, "Publishing keypoints.");

21
corelib/src/Memory.cpp
View File

@@ -110,6 +110,8 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
UDEBUG("");
this->parseParameters(parameters);
bool loadAllNodesInWM = Parameters::defaultMemInitWMWithAllNodes();
Parameters::parse(parameters, Parameters::kMemInitWMWithAllNodes(), loadAllNodesInWM);
if(postInitEvents) UEventsManager::post(new RtabmapEventInit("Clearing memory..."));
DBDriver * tmpDriver = 0;
@@ -152,9 +154,21 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Connecting to database ") + dbUrl + ", done!"));
// Load the last working memory...
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures...")));
std::list<Signature*> dbSignatures;
_dbDriver->loadLastNodes(dbSignatures);
if(loadAllNodesInWM)
{
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading all nodes to WM...")));
std::set<int> ids;
_dbDriver->getAllNodeIds(ids);
_dbDriver->loadSignatures(std::list<int>(ids.begin(), ids.end()), dbSignatures);
}
else
{
// load previous session working memory
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last nodes to WM...")));
_dbDriver->loadLastNodes(dbSignatures);
}
for(std::list<Signature*>::reverse_iterator iter=dbSignatures.rbegin(); iter!=dbSignatures.rend(); ++iter)
{
// ignore bad signatures
@@ -175,7 +189,7 @@ bool Memory::init(const std::string & dbUrl, bool dbOverwritten, const Parameter
delete *iter;
}
}
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading last signatures, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
if(postInitEvents) UEventsManager::post(new RtabmapEventInit(std::string("Loading nodes to WM, done! (") + uNumber2Str(int(_workingMem.size() + _stMem.size())) + " loaded)"));
// Assign the last signature
if(_stMem.size()>0)
@@ -2130,6 +2144,7 @@ bool Memory::addLoopClosureLink(int oldId, int newId, const Transform & transfor
Signature * newS = _getSignature(newId);
if(oldS && newS)
{
_memoryChanged = true;
const std::map<int, Transform> & oldLoopclosureIds = oldS->getLoopClosureIds();
if(oldLoopclosureIds.size() && oldLoopclosureIds.find(newS->id()) != oldLoopclosureIds.end())
{

1
guilib/include/rtabmap/gui/DatabaseViewer.h
View File

@@ -64,6 +64,7 @@ public:
protected:
virtual void resizeEvent(QResizeEvent* anEvent);
virtual void closeEvent(QCloseEvent* event);
private slots:
void openDatabase();

199
guilib/src/DatabaseViewer.cpp
View File

@@ -32,6 +32,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtGui/QFileDialog>
#include <QtGui/QInputDialog>
#include <QtGui/QGraphicsLineItem>
#include <QtGui/QCloseEvent>
#include <QtCore/QBuffer>
#include <QtCore/QTextStream>
#include <rtabmap/utilite/ULogger.h>
@@ -186,6 +187,7 @@ bool DatabaseViewer::openDatabase(const QString & path)
rtabmap::ParametersMap parameters;
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kDbSqlite3InMemory(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemIncrementalMemory(), "false"));
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kMemInitWMWithAllNodes(), "true"));
// use BruteForce dictionary because we don't know which type of descriptors are saved in database
parameters.insert(rtabmap::ParametersPair(rtabmap::Parameters::kKpNNStrategy(), "3"));
@@ -209,6 +211,65 @@ bool DatabaseViewer::openDatabase(const QString & path)
return false;
}
void DatabaseViewer::closeEvent(QCloseEvent* event)
{
if(linksAdded_.size() || linksRefined_.size() || linksRemoved_.size())
{
QMessageBox::StandardButton button = QMessageBox::question(this,
tr("Links modified"),
tr("Some links are modified (%1 added, %2 refined, %3 removed), do you want to save them?")
.arg(linksAdded_.size()).arg(linksRefined_.size()).arg(linksRemoved_.size()),
QMessageBox::Cancel | QMessageBox::Yes | QMessageBox::No,
QMessageBox::Cancel);
if(button == QMessageBox::Yes)
{
// Added links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksAdded_.begin(); iter!=linksAdded_.end(); ++iter)
{
std::multimap<int, rtabmap::Link>::iterator refinedIter = this->findLink(linksRefined_, iter->second.from(), iter->second.to());
if(refinedIter != linksRefined_.end())
{
memory_->addLoopClosureLink(refinedIter->second.to(), refinedIter->second.from(), refinedIter->second.transform(), true);
}
else
{
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
}
}
//Refined links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRefined_.begin(); iter!=linksRefined_.end(); ++iter)
{
if(!containsLink(linksAdded_, iter->second.from(), iter->second.to()))
{
memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
memory_->addLoopClosureLink(iter->second.to(), iter->second.from(), iter->second.transform(), true);
}
}
// Rejected links
for(std::multimap<int, rtabmap::Link>::iterator iter=linksRemoved_.begin(); iter!=linksRemoved_.end(); ++iter)
{
memory_->rejectLoopClosure(iter->second.to(), iter->second.from());
}
}
if(button == QMessageBox::Yes || button == QMessageBox::No)
{
event->accept();
}
else
{
event->ignore();
}
}
else
{
event->accept();
}
}
void DatabaseViewer::resizeEvent(QResizeEvent* anEvent)
{
ui_->graphicsView_A->fitInView(ui_->graphicsView_A->sceneRect(), Qt::KeepAspectRatio);
@@ -1523,68 +1584,105 @@ void DatabaseViewer::refineConstraint(int from, int to)
return;
}
bool hasConverged = false;
double fitness = 0.0f;
Transform transform;
float fxA, fyA, cxA, cyA;
float fxB, fyB, cxB, cyB;
rtabmap::Transform localTransformA, localTransformB;
std::vector<unsigned char> imageBytesA, depthBytesA, depth2dBytesA;
memory_->getImageDepth(currentLink.from(), imageBytesA, depthBytesA, depth2dBytesA, fxA, fyA, cxA, cyA, localTransformA);
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
std::vector<unsigned char> imageBytesB, depthBytesB, depth2dBytesB;
memory_->getImageDepth(currentLink.to(), imageBytesB, depthBytesB, depth2dBytesB, fxB, fyB, cxB, cyB, localTransformB);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
fxA, fyA, cxA, cyA,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
localTransformA);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
fxB, fyB, cxB, cyB,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * localTransformB);
bool hasConverged = false;
double fitness;
Transform transform;
if(ui_->checkBox_icp_p2plane->isChecked())
if(ui_->checkBox_icp_2d->isChecked())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
//2D
cv::Mat oldDepth2D = util3d::uncompressData(depth2dBytesA);
cv::Mat newDepth2D = util3d::uncompressData(depth2dBytesB);
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
if(!oldDepth2D.empty() && !newDepth2D.empty())
{
UWARN("removed nan normals...");
}
// 2D
pcl::PointCloud<pcl::PointXYZ>::Ptr oldCloud = util3d::cvMat2Cloud(oldDepth2D);
pcl::PointCloud<pcl::PointXYZ>::Ptr newCloud = util3d::cvMat2Cloud(newDepth2D, currentLink.transform());
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
//voxelize
if(ui_->doubleSpinBox_icp_voxel->value() > 0.0f)
{
oldCloud = util3d::voxelize(oldCloud, ui_->doubleSpinBox_icp_voxel->value());
newCloud = util3d::voxelize(newCloud, ui_->doubleSpinBox_icp_voxel->value());
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_decimation->value(),
hasConverged,
fitness);
if(newCloud->size() && oldCloud->size())
{
transform = util3d::icp2D(newCloud,
oldCloud,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
}
}
}
else
{
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_decimation->value(),
hasConverged,
fitness);
//3D
cv::Mat depthA = rtabmap::util3d::uncompressImage(depthBytesA);
cv::Mat depthB = rtabmap::util3d::uncompressImage(depthBytesB);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA = util3d::getICPReadyCloud(depthA,
fxA, fyA, cxA, cyA,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
localTransformA);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB = util3d::getICPReadyCloud(depthB,
fxB, fyB, cxB, cyB,
ui_->spinBox_icp_decimation->value(),
ui_->doubleSpinBox_icp_maxDepth->value(),
ui_->doubleSpinBox_icp_voxel->value(),
0, // no sampling
currentLink.transform() * localTransformB);
if(ui_->checkBox_icp_p2plane->isChecked())
{
pcl::PointCloud<pcl::PointNormal>::Ptr cloudANormals = util3d::computeNormals(cloudA, ui_->spinBox_icp_normalKSearch->value());
pcl::PointCloud<pcl::PointNormal>::Ptr cloudBNormals = util3d::computeNormals(cloudB, ui_->spinBox_icp_normalKSearch->value());
cloudANormals = util3d::removeNaNNormalsFromPointCloud(cloudANormals);
if(cloudA->size() != cloudANormals->size())
{
UWARN("removed nan normals...");
}
cloudBNormals = util3d::removeNaNNormalsFromPointCloud(cloudBNormals);
if(cloudB->size() != cloudBNormals->size())
{
UWARN("removed nan normals...");
}
transform = util3d::icpPointToPlane(cloudBNormals,
cloudANormals,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
}
else
{
transform = util3d::icp(cloudB,
cloudA,
ui_->doubleSpinBox_icp_maxCorrespDistance->value(),
ui_->spinBox_icp_iteration->value(),
hasConverged,
fitness);
}
}
if(hasConverged && !transform.isNull())
@@ -1707,6 +1805,14 @@ bool DatabaseViewer::addConstraint(int from, int to, bool silent)
}
else
{
if(ui_->checkBox_visual_2d->isChecked())
{
// We are 2D here, make sure the guess has only YAW rotation
float x,y,z,r,p,yaw;
t.getTranslationAndEulerAngles(x,y,z, r,p,yaw);
t = util3d::transformFromEigen3f(pcl::getTransformation(x,y,0, 0, 0, yaw));
}
// transform is valid, make a link
linksAdded_.insert(std::make_pair(from, Link(from, to, t, Link::kUserClosure)));
updateSlider = true;
@@ -1755,6 +1861,11 @@ void DatabaseViewer::resetConstraint()
{
this->updateConstraintView(iter->second);
}
iter = findLink(linksAdded_, from, to);
if(iter != linksAdded_.end())
{
this->updateConstraintView(iter->second);
}
}
void DatabaseViewer::rejectConstraint()

2
guilib/src/PreferencesDialog.cpp
View File

@@ -102,6 +102,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
// remove BruteForceGPU option
_ui->comboBox_dictionary_strategy->removeItem(4);
_ui->odom_bin_nn->removeItem(4);
_ui->reextract_nn->removeItem(4);
}
#if PCL_VERSION_COMPARE(<, 1, 7, 2)
@@ -307,6 +308,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_RehearsalIdUpdatedToNewOne->setObjectName(Parameters::kMemRehearsalIdUpdatedToNewOne().c_str());
_ui->general_checkBox_generateIds->setObjectName(Parameters::kMemGenerateIds().c_str());
_ui->general_checkBox_badSignaturesIgnored->setObjectName(Parameters::kMemBadSignaturesIgnored().c_str());
_ui->general_checkBox_initWMWithAllNodes->setObjectName(Parameters::kMemInitWMWithAllNodes().c_str());
// Database
_ui->checkBox_dbInMemory->setObjectName(Parameters::kDbSqlite3InMemory().c_str());

69
guilib/src/ui/DatabaseViewer.ui
View File

@@ -545,6 +545,13 @@
<layout class="QVBoxLayout" name="verticalLayout_11">
<item>
<layout class="QGridLayout" name="gridLayout_4" columnminimumwidth="1,0">
<item row="4" column="0">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QLabel" name="label_14">
<property name="text">
@@ -637,13 +644,6 @@
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_15">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_icp_iteration">
<property name="minimum">
@@ -694,6 +694,23 @@
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_27">
<property name="text">
<string>2D icp</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QCheckBox" name="checkBox_icp_2d">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>
@@ -737,14 +754,14 @@
</property>
</widget>
</item>
<item row="1" column="0">
<item row="2" column="0">
<widget class="QLabel" name="label_25">
<property name="text">
<string>SURF hessian threshold</string>
</property>
</widget>
</item>
<item row="1" column="1">
<item row="2" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_hessian">
<property name="suffix">
<string/>
@@ -763,14 +780,14 @@
</property>
</widget>
</item>
<item row="2" column="0">
<item row="3" column="0">
<widget class="QLabel" name="label_26">
<property name="text">
<string>NNDR</string>
</property>
</widget>
</item>
<item row="2" column="1">
<item row="3" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_nndr">
<property name="suffix">
<string> m</string>
@@ -792,14 +809,14 @@
</property>
</widget>
</item>
<item row="3" column="0">
<item row="4" column="0">
<widget class="QLabel" name="label_21">
<property name="text">
<string>Min correspondences</string>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="QSpinBox" name="spinBox_visual_minCorrespondences">
<property name="minimum">
<number>3</number>
@@ -812,14 +829,14 @@
</property>
</widget>
</item>
<item row="4" column="0">
<item row="5" column="0">
<widget class="QLabel" name="label_22">
<property name="text">
<string>Max correspondence distance</string>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxCorrespDistance">
<property name="suffix">
<string> m</string>
@@ -838,14 +855,14 @@
</property>
</widget>
</item>
<item row="5" column="0">
<item row="6" column="0">
<widget class="QLabel" name="label_23">
<property name="text">
<string>Iteration</string>
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QSpinBox" name="spinBox_visual_iteration">
<property name="minimum">
<number>1</number>
@@ -858,14 +875,14 @@
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QLabel" name="label_19">
<property name="text">
<string>Max feature depth</string>
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" column="1">
<widget class="QDoubleSpinBox" name="doubleSpinBox_visual_maxDepth">
<property name="suffix">
<string> m</string>
@@ -881,6 +898,20 @@
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_28">
<property name="text">
<string>2D transform (x,y,yaw)</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QCheckBox" name="checkBox_visual_2d">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>

28
guilib/src/ui/preferencesDialog.ui
View File

@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>19</number>
<number>7</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29">
@@ -2790,6 +2790,16 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_retrieved_3">
<property name="text">
@@ -2800,13 +2810,23 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QCheckBox" name="general_checkBox_badSignaturesIgnored">
<item row="5" column="1">
<widget class="QLabel" name="label_retrieved_4">
<property name="text">
<string>Initialize the Woking Memory with all nodes from Long-Term memory, instead of only nodes of the last session. This may be useful in localization mode, where less processing time is required than in SLAM mode, so more nodes can be kept in Working Memory.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>true</bool>
<bool>false</bool>
</property>
</widget>
</item>