Update version 0.10.11:

-Added parameter "Mem/ReduceGraph" with new link type (NeighborMerged)
-DatabaseViewer: add Info panel, fixed empty extracted database when "RGB images" is not checked
-ConsoleWidget: added buffer size and refresh rate options
-DBDriver: new database info getters
-Graph: added reduceGraph() method, added Dijsktra computePath() with links only
-Updated default parameters: RGBD/LocalLoopDetectionSpace=true, RGBD/LinearUpdate=0.1 and RGBD/AngularUpdate=0.1
-Goals are saved with label instead of ID if the target node has a label
-DataRecorder: fixed saving incrementally the database (instead of only at the end)
-MainWindow: When waypoints are used, resend the current goal (1 sec delay) if it has failed setting it
This commit is contained in:
matlabbe
2015-10-24 17:03:44 -04:00
parent 957498b5a3
commit 463a5d973c
32 changed files with 1820 additions and 314 deletions

View File

@@ -58,6 +58,7 @@ signals:
private slots:
void flushConsole();
void updateTextEditBufferSize();
protected:
virtual void handleEvent(UEvent * anEvent);

View File

@@ -83,6 +83,7 @@ public:
const QColor & getLocalLoopClosureColor() const {return _loopClosureLocalColor;}
const QColor & getUserLoopClosureColor() const {return _loopClosureUserColor;}
const QColor & getVirtualLoopClosureColor() const {return _loopClosureVirtualColor;}
const QColor & getNeighborMergedColor() const {return _neighborMergedColor;}
const QColor & getRejectedLoopClosureColor() const {return _loopClosureRejectedColor;}
const QColor & getLocalPathColor() const {return _localPathColor;}
const QColor & getGlobalPathColor() const {return _globalPathColor;}
@@ -106,6 +107,7 @@ public:
void setLocalLoopClosureColor(const QColor & color);
void setUserLoopClosureColor(const QColor & color);
void setVirtualLoopClosureColor(const QColor & color);
void setNeighborMergedColor(const QColor & color);
void setRejectedLoopClosureColor(const QColor & color);
void setLocalPathColor(const QColor & color);
void setGlobalPathColor(const QColor & color);
@@ -137,6 +139,7 @@ private:
QColor _loopClosureLocalColor;
QColor _loopClosureUserColor;
QColor _loopClosureVirtualColor;
QColor _neighborMergedColor;
QColor _loopClosureRejectedColor;
QColor _localPathColor;
QColor _globalPathColor;

View File

@@ -33,9 +33,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtGui/QTextCursor>
#include <QtCore/QTimer>
#define OLD_TIME 1000 //ms
#define MAXIMUM_ITEMS 100
namespace rtabmap {
ConsoleWidget::ConsoleWidget(QWidget * parent) :
@@ -44,7 +41,7 @@ ConsoleWidget::ConsoleWidget(QWidget * parent) :
_ui = new Ui_consoleWidget();
_ui->setupUi(this);
UEventsManager::addHandler(this);
_ui->textEdit->document()->setMaximumBlockCount(MAXIMUM_ITEMS);
_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
_textCursor = new QTextCursor(_ui->textEdit->document());
_ui->textEdit->setFontPointSize(10);
QPalette p(_ui->textEdit->palette());
@@ -55,6 +52,7 @@ ConsoleWidget::ConsoleWidget(QWidget * parent) :
_time.start();
_timer.setSingleShot(true);
connect(_ui->pushButton_clear, SIGNAL(clicked()), _ui->textEdit, SLOT(clear()));
connect(_ui->spinBox_lines, SIGNAL(valueChanged(int)), this, SLOT(updateTextEditBufferSize()));
connect(this, SIGNAL(msgReceived(const QString &, int)), this, SLOT(appendMsg(const QString &, int)));
connect(&_timer, SIGNAL(timeout()), this, SLOT(flushConsole()));
}
@@ -72,13 +70,13 @@ void ConsoleWidget::handleEvent(UEvent * anEvent)
ULogEvent * logEvent = (ULogEvent*)anEvent;
_msgListMutex.lock();
_msgList.append(QPair<QString, int>(logEvent->getMsg().c_str(), logEvent->getCode()));
if(_msgList.size()>MAXIMUM_ITEMS)
while(_ui->spinBox_lines->value()>0 && _msgList.size()>_ui->spinBox_lines->value())
{
_msgList.pop_front();
}
_msgListMutex.unlock();
if(_time.restart() < OLD_TIME)
if(_ui->spinBox_time->value()>0 && _time.restart() < _ui->spinBox_time->value())
{
if(logEvent->getCode() == ULogger::kFatal)
{
@@ -86,7 +84,7 @@ void ConsoleWidget::handleEvent(UEvent * anEvent)
}
else
{
QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, OLD_TIME));
QMetaObject::invokeMethod(&_timer, "start", Q_ARG(int, _ui->spinBox_time->value()));
}
}
else
@@ -153,4 +151,9 @@ void ConsoleWidget::flushConsole()
_ui->textEdit->setTextCursor(cursor);
}
void ConsoleWidget::updateTextEditBufferSize()
{
_ui->textEdit->document()->setMaximumBlockCount(_ui->spinBox_lines->value());
}
}

View File

@@ -73,6 +73,7 @@ bool DataRecorder::init(const QString & path, bool recordInRAM)
customParameters.insert(ParametersPair(Parameters::kMemRehearsalSimilarity(), "1.0")); // desactivate rehearsal
customParameters.insert(ParametersPair(Parameters::kKpWordsPerImage(), "-1")); // desactivate keypoints extraction
customParameters.insert(ParametersPair(Parameters::kMemBinDataKept(), "true")); // to keep images
customParameters.insert(ParametersPair(Parameters::kMemBadSignaturesIgnored(), "true")); // make usre memory cleanup is done
if(!recordInRAM)
{
customParameters.insert(ParametersPair(Parameters::kDbSqlite3InMemory(), "false"));
@@ -138,6 +139,7 @@ void DataRecorder::addData(const rtabmap::SensorData & data, const Transform & p
const Signature * s = memory_->getLastWorkingSignature();
totalSizeKB_ += (int)s->sensorData().imageCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().depthOrRightCompressed().total()/1000;
totalSizeKB_ += (int)s->sensorData().laserScanCompressed().total()/1000;
memory_->cleanup();
if(++count_ % 30)

View File

@@ -96,6 +96,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_constraints->setVisible(false);
ui_->dockWidget_graphView->setVisible(false);
ui_->dockWidget_parameters->setVisible(false);
ui_->dockWidget_info->setVisible(false);
ui_->dockWidget_stereoView->setVisible(false);
ui_->dockWidget_view3d->setVisible(false);
@@ -146,6 +147,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->menuView->addAction(ui_->dockWidget_stereoView->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_view3d->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_parameters->toggleViewAction());
ui_->menuView->addAction(ui_->dockWidget_info->toggleViewAction());
connect(ui_->dockWidget_graphView->toggleViewAction(), SIGNAL(triggered()), this, SLOT(updateGraphView()));
connect(ui_->actionQuit, SIGNAL(triggered()), this, SLOT(close()));
@@ -328,6 +330,7 @@ DatabaseViewer::DatabaseViewer(QWidget * parent) :
ui_->dockWidget_stereoView->installEventFilter(this);
ui_->dockWidget_view3d->installEventFilter(this);
ui_->dockWidget_parameters->installEventFilter(this);
ui_->dockWidget_info->installEventFilter(this);
}
DatabaseViewer::~DatabaseViewer()
@@ -828,6 +831,11 @@ void DatabaseViewer::exportDatabase()
progressDialog->setAttribute(Qt::WA_DeleteOnClose);
progressDialog->setMaximumSteps(ids.size());
progressDialog->show();
UINFO("Decompress: rgb=%d depth=%d scan=%d userData=%d",
dialog.isRgbExported()?1:0,
dialog.isDepthExported()?1:0,
dialog.isDepth2dExported()?1:0,
dialog.isUserDataExported()?1:0);
for(int i=0; i<ids.size(); ++i)
{
@@ -835,7 +843,12 @@ void DatabaseViewer::exportDatabase()
SensorData data;
dbDriver_->getNodeData(id, data);
data.uncompressData();
cv::Mat depth, rgb, scan, userData;
data.uncompressDataConst(
!dialog.isRgbExported()?0:&rgb,
!dialog.isDepthExported()?0:&depth,
!dialog.isDepth2dExported()?0:&scan,
!dialog.isUserDataExported()?0:&userData);
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1);
if(dialog.isOdomExported())
{
@@ -851,28 +864,28 @@ void DatabaseViewer::exportDatabase()
if(data.cameraModels().size())
{
sensorData = rtabmap::SensorData(
dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
scan,
dialog.isDepth2dExported()?data.laserScanMaxPts():0,
dialog.isDepth2dExported()?data.laserScanMaxRange():0,
dialog.isRgbExported()?data.imageRaw():cv::Mat(),
dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
rgb,
depth,
data.cameraModels(),
data.id(),
data.stamp(),
dialog.isUserDataExported()?data.userDataRaw():cv::Mat());
userData);
}
else
{
sensorData = rtabmap::SensorData(
dialog.isDepth2dExported()?data.laserScanRaw():cv::Mat(),
scan,
dialog.isDepth2dExported()?data.laserScanMaxPts():0,
dialog.isDepth2dExported()?data.laserScanMaxRange():0,
dialog.isRgbExported()?data.imageRaw():cv::Mat(),
dialog.isDepthExported()?data.depthOrRightRaw():cv::Mat(),
rgb,
depth,
data.stereoCameraModel(),
data.id(),
data.stamp(),
dialog.isUserDataExported()?data.userDataRaw():cv::Mat());
userData);
}
recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
@@ -1029,6 +1042,7 @@ void DatabaseViewer::updateIds()
return;
}
UINFO("Loading all IDs...");
std::set<int> ids;
dbDriver_->getAllNodeIds(ids);
ids_ = QList<int>::fromStdList(std::list<int>(ids.begin(), ids.end()));
@@ -1040,6 +1054,12 @@ void DatabaseViewer::updateIds()
linksRefined_.clear();
linksRemoved_.clear();
ui_->label_optimizeFrom->setText(tr("Optimize from"));
std::multimap<int, Link> links;
dbDriver_->getAllLinks(links, true);
UDEBUG("%d total links loaded", (int)links.size());
double totalOdom = 0.0;
Transform previousPose;
int sessions = ids_.size()?1:0;
for(int i=0; i<ids_.size(); ++i)
{
idToIndex_.insert(ids_[i], i);
@@ -1051,22 +1071,60 @@ void DatabaseViewer::updateIds()
int mapId;
dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s);
mapIds_.insert(std::make_pair(ids_[i], mapId));
poses_.insert(std::make_pair(ids_[i], p));
//links
std::map<int, Link> neighbors;
dbDriver_->loadLinks(ids_[i], neighbors); // including loop closures
for(std::map<int, Link>::iterator jter=neighbors.begin(); jter!=neighbors.end(); ++jter)
if(i>0)
{
if( jter->second.isValid() && // null transform means a rehearsed location
uContains(poses_, jter->first) &&
graph::findLink(links_, ids_[i], jter->first) == links_.end() &&
(jter->second.type() == Link::kNeighbor || jter->first < ids_[i])) //Loop parent to child
if(mapIds_.at(ids_[i-1]) == mapId)
{
links_.insert(std::make_pair(ids_[i], jter->second));
if(!p.isNull() && !previousPose.isNull())
{
totalOdom += p.getDistance(previousPose);
}
}
else
{
++sessions;
}
}
previousPose=p;
//links
bool linksInserted = false;
for(std::map<int, Link>::iterator jter=links.find(ids_[i]); jter!=links.end() && jter->first == ids_[i]; ++jter)
{
if( jter->second.isValid() && // null transform means a rehearsed location
ids.find(jter->second.from()) != ids.end() &&
ids.find(jter->second.to()) != ids.end() &&
graph::findLink(links_, jter->second.from(), jter->second.to()) == links_.end() &&
graph::findLink(links, jter->second.from(), jter->second.to(), false) != links.end() &&
graph::findLink(links, jter->second.to(), jter->second.from(), false) != links.end())
{
links_.insert(std::make_pair(ids_[i], jter->second));
linksInserted = true;
}
}
if(linksInserted)
{
poses_.insert(std::make_pair(ids_[i], p));
}
}
UDEBUG("Update database info...");
ui_->label_odom->setText(tr("%1 m").arg(totalOdom));
ui_->label_sessions->setNum(sessions);
ui_->label_nodes->setText(tr("%1 nodes").arg(ids.size()));
ui_->label_version->setText(dbDriver_->getDatabaseVersion().c_str());
ui_->label_db_size->setText(tr("%1 MB").arg(dbDriver_->getMemoryUsed()/1000000));
ui_->label_words->setText(tr("%1 words").arg(dbDriver_->getTotalDictionarySize()));
ui_->label_global_map->setText(tr("%1 nodes").arg(poses_.size()));
ui_->label_rgb_size->setText(tr("%1 MB").arg(dbDriver_->getImagesMemoryUsed()/1000000));
ui_->label_depth_size->setText(tr("%1 MB").arg(dbDriver_->getDepthImagesMemoryUsed()/1000000));
ui_->label_scan_size->setText(tr("%1 MB").arg(dbDriver_->getLaserScansMemoryUsed()/1000000));
ui_->label_userData_size->setText(tr("%1 bytes").arg(dbDriver_->getUserDataMemoryUsed()));
ui_->label_words_size->setText(tr("%1 bytes").arg(dbDriver_->getWordsMemoryUsed()));
ui_->label_wm->setText(tr("%1 nodes").arg(dbDriver_->getLastNodesSize()));
ui_->label_dictionary->setText(tr("%1 words").arg(dbDriver_->getLastDictionarySize()));
UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size());
if(ids.size())
{
@@ -1094,9 +1152,12 @@ void DatabaseViewer::updateIds()
links_.clear();
}
ui_->spinBox_optimizationsFrom->setRange(ids_.first(), ids_.last());
ui_->spinBox_optimizationsFrom->setValue(ids_.first());
ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(ids_.first()).arg(ids_.last()));
if(poses_.size())
{
ui_->spinBox_optimizationsFrom->setRange(poses_.begin()->first, poses_.rbegin()->first);
ui_->spinBox_optimizationsFrom->setValue(poses_.begin()->first);
ui_->label_optimizeFrom->setText(tr("Optimize from [%1, %2]").arg(poses_.begin()->first).arg(poses_.rbegin()->first));
}
}
}
@@ -1110,7 +1171,8 @@ void DatabaseViewer::updateIds()
{
if(!iter->second.transform().isNull())
{
if(iter->second.type() == rtabmap::Link::kNeighbor)
if(iter->second.type() == rtabmap::Link::kNeighbor ||
iter->second.type() == rtabmap::Link::kNeighborMerged)
{
neighborLinks_.append(iter->second);
}
@@ -1227,7 +1289,8 @@ void DatabaseViewer::generateLocalGraph()
if( !uContains(ids, iter->first))
{
UASSERT(iter->second.type() != Link::kUndef);
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
nextMargin.insert(iter->first);
}
@@ -1338,6 +1401,7 @@ void DatabaseViewer::view3DMap()
QMessageBox::warning(this, tr("Cannot view 3D map"), tr("The database is empty..."));
return;
}
if(graphes_.empty())
{
this->updateGraphView();
@@ -1798,13 +1862,30 @@ void DatabaseViewer::update(int value,
}
// 3d view
if(view3D->isVisible() && !data.depthOrRightRaw().empty())
if(view3D->isVisible() &&
(!data.depthOrRightRaw().empty() ||
!data.laserScanRaw().empty()))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudRGBFromSensorData(data);
if(cloud->size())
if(!data.depthOrRightRaw().empty())
{
view3D->addOrUpdateCloud("0", cloud);
if(!data.imageRaw().empty())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
cloud = util3d::cloudRGBFromSensorData(data);
if(cloud->size())
{
view3D->addOrUpdateCloud("0", cloud);
}
}
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud;
cloud = util3d::cloudFromSensorData(data);
if(cloud->size())
{
view3D->addOrUpdateCloud("0", cloud);
}
}
}
//add scan
@@ -1845,7 +1926,8 @@ void DatabaseViewer::update(int value,
QString strParents, strChildren;
for(std::map<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end(); ++iter)
{
if(iter->second.type() != Link::kNeighbor)
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
if(iter->first < id)
{
@@ -2271,7 +2353,8 @@ void DatabaseViewer::updateConstraintView()
Link link = this->findActiveLink(ids_.at(ui_->horizontalSlider_A->value()), ids_.at(ui_->horizontalSlider_B->value()));
if(link.isValid())
{
if(link.type() == Link::kNeighbor)
if(link.type() == Link::kNeighbor ||
link.type() == Link::kNeighborMerged)
{
this->updateConstraintView(neighborLinks_.at(ui_->horizontalSlider_neighbors->value()), false);
}
@@ -2304,17 +2387,25 @@ void DatabaseViewer::updateConstraintView(
ui_->checkBox_showOptimized->setEnabled(false);
UASSERT(!t.isNull() && dbDriver_);
ui_->label_type->setNum(link.type());
ui_->label_type->setText(tr("%1 (%2)")
.arg(link.type())
.arg(link.type()==Link::kNeighbor?"Neigbor":
link.type()==Link::kNeighbor?"Merged neighbor":
link.type()==Link::kGlobalClosure?":Loop closure":
link.type()==Link::kLocalSpaceClosure?"Space proximity link":
link.type()==Link::kLocalTimeClosure?"Time proximity link":
link.type()==Link::kUserClosure?"User link":
link.type()==Link::kVirtualClosure?"Virtual link":"Undefined"));
ui_->label_variance->setText(QString("%1, %2")
.arg(sqrt(link.rotVariance()))
.arg(sqrt(link.transVariance())));
ui_->label_constraint->setText(QString("%1").arg(t.prettyPrint().c_str()).replace(" ", "\n"));
if(link.type() == Link::kNeighbor &&
if((link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged) &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
{
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(link.type() == Link::kNeighbor)
if(link.type() == Link::kNeighbor || link.type() == Link::kNeighborMerged)
{
std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(link.from());
std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(link.to());
@@ -2395,14 +2486,20 @@ void DatabaseViewer::updateConstraintView(
if(ui_->checkBox_show3Dclouds->isChecked())
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFrom, cloudTo;
cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
if(!dataFrom.imageRaw().empty() && !dataFrom.depthOrRightRaw().empty())
{
cloudFrom=util3d::cloudRGBFromSensorData(dataFrom, 1);
}
if(!dataTo.imageRaw().empty() && !dataTo.depthOrRightRaw().empty())
{
cloudTo=util3d::cloudRGBFromSensorData(dataTo, 1);
}
if(cloudFrom->size())
if(cloudFrom.get() && cloudFrom->size())
{
ui_->constraintsViewer->addOrUpdateCloud("cloud0", cloudFrom, Transform::getIdentity(), Qt::red);
}
if(cloudTo->size())
if(cloudTo.get() && cloudTo->size())
{
cloudTo = rtabmap::util3d::transformPointCloud(cloudTo, t);
ui_->constraintsViewer->addOrUpdateCloud("cloud1", cloudTo, Transform::getIdentity(), Qt::cyan);
@@ -2737,7 +2834,9 @@ void DatabaseViewer::updateConstraintButtons()
{
if(!containsLink(linksRemoved_, from ,to))
{
ui_->pushButton_reject->setEnabled(currentLink.type() != Link::kNeighbor);
ui_->pushButton_reject->setEnabled(
currentLink.type() != Link::kNeighbor &&
currentLink.type() != Link::kNeighborMerged);
}
//check for modified link
@@ -2887,7 +2986,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
const rtabmap::Transform & poseA = jterA->second;
const rtabmap::Transform & poseB = jterB->second;
if(iter->second.type() == rtabmap::Link::kNeighbor)
if(iter->second.type() == rtabmap::Link::kNeighbor ||
iter->second.type() == rtabmap::Link::kNeighborMerged)
{
Eigen::Vector3f vA, vB;
poseA.getTranslation(vA[0], vA[1], vA[2]);
@@ -2901,8 +3001,13 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
void DatabaseViewer::updateGraphView()
{
if(!ui_->graphViewer->isVisible())
{
return;
}
ui_->label_loopClosures->clear();
ui_->label_nodes->clear();
ui_->label_poses->clear();
if(poses_.size())
{
int fromId = ui_->spinBox_optimizationsFrom->value();
@@ -2969,7 +3074,8 @@ void DatabaseViewer::updateGraphView()
std::multimap<int, Link> tmp = links;
for(std::multimap<int, Link>::iterator iter=tmp.begin(); iter!=tmp.end(); ++iter)
{
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor ||
iter->second.type() == Link::kNeighborMerged)
{
Transform poseFrom = uValue(poses, iter->second.from(), Transform());
Transform poseTo = uValue(poses, iter->second.to(), Transform());
@@ -2988,13 +3094,23 @@ void DatabaseViewer::updateGraphView()
}
// filter links
int totalNeighbor = 0;
int totalNeighborMerged = 0;
int totalGlobal = 0;
int totalLocalTime = 0;
int totalLocalSpace = 0;
int totalUser = 0;
for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.type() == Link::kGlobalClosure)
if(iter->second.type() == Link::kNeighbor)
{
++totalNeighbor;
}
else if(iter->second.type() == Link::kNeighborMerged)
{
++totalNeighborMerged;
}
else if(iter->second.type() == Link::kGlobalClosure)
{
if(ui_->checkBox_ignoreGlobalLoop->isChecked())
{
@@ -3032,7 +3148,13 @@ void DatabaseViewer::updateGraphView()
}
++iter;
}
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4)").arg(totalGlobal).arg(totalLocalSpace).arg(totalLocalTime).arg(totalUser));
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6)")
.arg(totalNeighbor)
.arg(totalNeighborMerged)
.arg(totalGlobal)
.arg(totalLocalSpace)
.arg(totalLocalTime)
.arg(totalUser));
graph::Optimizer * optimizer = 0;
if(ui_->comboBox_graphOptimizer->currentIndex() == graph::Optimizer::kTypeGTSAM)
@@ -3064,7 +3186,7 @@ void DatabaseViewer::updateGraphView()
}
std::map<int, rtabmap::Transform> posesOut;
std::multimap<int, rtabmap::Link> linksOut;
UINFO("Get connected graph (%d poses, %d links)", (int)poses.size(), (int)links.size());
UINFO("Get connected graph from %d (%d poses, %d links)", fromId, (int)poses.size(), (int)links.size());
optimizer->getConnectedGraph(
fromId,
poses,
@@ -3079,7 +3201,7 @@ void DatabaseViewer::updateGraphView()
ui_->label_timeOptimization->setNum(double(time.elapsed())/1000.0);
graphes_.push_back(finalPoses);
graphLinks_ = linksOut;
ui_->label_nodes->setNum((int)finalPoses.size());
ui_->label_poses->setNum((int)finalPoses.size());
delete optimizer;
if(posesOut.size() && finalPoses.empty())
{
@@ -3169,12 +3291,12 @@ void DatabaseViewer::refineConstraint(int from, int to, bool silent, bool update
}
Transform t = currentLink.transform();
if(ui_->checkBox_showOptimized->isChecked() &&
currentLink.type() == Link::kNeighbor &&
(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged) &&
graphes_.size() &&
(int)graphes_.size()-1 == ui_->horizontalSlider_iterations->maximum())
{
std::map<int, rtabmap::Transform> & graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(currentLink.type() == Link::kNeighbor)
if(currentLink.type() == Link::kNeighbor || currentLink.type() == Link::kNeighborMerged)
{
std::map<int, rtabmap::Transform>::iterator iterFrom = graph.find(currentLink.from());
std::map<int, rtabmap::Transform>::iterator iterTo = graph.find(currentLink.to());
@@ -3713,7 +3835,7 @@ void DatabaseViewer::rejectConstraint()
iter = rtabmap::graph::findLink(links_, from, to);
if(iter != links_.end())
{
if(iter->second.type() == Link::kNeighbor)
if(iter->second.type() == Link::kNeighbor || iter->second.type() == Link::kNeighborMerged)
{
UWARN("Cannot reject neighbor links (%d->%d)", from, to);
return;
@@ -3814,7 +3936,8 @@ void DatabaseViewer::updateLoopClosuresSlider(int from, int to)
{
if(!iter->second.transform().isNull())
{
if(iter->second.type() != rtabmap::Link::kNeighbor)
if(iter->second.type() != rtabmap::Link::kNeighbor &&
iter->second.type() != rtabmap::Link::kNeighborMerged)
{
if((iter->second.from() == from && iter->second.to() == to) ||
(iter->second.to() == from && iter->second.from() == to))

View File

@@ -179,6 +179,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopClosureLocalColor(Qt::yellow),
_loopClosureUserColor(Qt::red),
_loopClosureVirtualColor(Qt::magenta),
_neighborMergedColor(QColor(255,170,0)),
_loopClosureRejectedColor(Qt::black),
_localPathColor(Qt::cyan),
_globalPathColor(Qt::darkMagenta),
@@ -344,6 +345,10 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
linkItem->setColor(_loopClosureVirtualColor);
}
else if(iter->second.type() == Link::kNeighborMerged)
{
linkItem->setColor(_neighborMergedColor);
}
else if(iter->second.type() == Link::kUserClosure)
{
linkItem->setColor(_loopClosureUserColor);
@@ -381,7 +386,8 @@ void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
{
t = t.inverse();
}
if(iter->second.type() != Link::kNeighbor)
if(iter->second.type() != Link::kNeighbor &&
iter->second.type() != Link::kNeighborMerged)
{
float linearError = uMax3(
fabs(iter->second.transform().x() - t.x()),
@@ -668,6 +674,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("local_color", this->getLocalLoopClosureColor());
settings.setValue("user_color", this->getUserLoopClosureColor());
settings.setValue("virtual_color", this->getVirtualLoopClosureColor());
settings.setValue("neighbor_merged_color", this->getNeighborMergedColor());
settings.setValue("rejected_color", this->getRejectedLoopClosureColor());
settings.setValue("local_path_color", this->getLocalPathColor());
settings.setValue("global_path_color", this->getGlobalPathColor());
@@ -700,6 +707,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setLocalLoopClosureColor(settings.value("local_color", this->getLocalLoopClosureColor()).value<QColor>());
this->setUserLoopClosureColor(settings.value("user_color", this->getUserLoopClosureColor()).value<QColor>());
this->setVirtualLoopClosureColor(settings.value("virtual_color", this->getVirtualLoopClosureColor()).value<QColor>());
this->setNeighborMergedColor(settings.value("neighbor_merged_color", this->getNeighborMergedColor()).value<QColor>());
this->setRejectedLoopClosureColor(settings.value("rejected_color", this->getRejectedLoopClosureColor()).value<QColor>());
this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
@@ -841,6 +849,17 @@ void GraphViewer::setVirtualLoopClosureColor(const QColor & color)
}
}
}
void GraphViewer::setNeighborMergedColor(const QColor & color)
{
_neighborMergedColor = color;
for(QMultiMap<int, LinkItem*>::iterator iter=_linkItems.begin(); iter!=_linkItems.end(); ++iter)
{
if(iter.value()->linkType() == Link::kNeighborMerged)
{
iter.value()->setColor(_neighborMergedColor);
}
}
}
void GraphViewer::setRejectedLoopClosureColor(const QColor & color)
{
_loopClosureRejectedColor = color;
@@ -936,6 +955,7 @@ void GraphViewer::restoreDefaults()
setLocalLoopClosureColor(Qt::yellow);
setUserLoopClosureColor(Qt::red);
setVirtualLoopClosureColor(Qt::magenta);
setNeighborMergedColor(QColor(255,170,0));
setGridMapVisible(true);
}
@@ -978,6 +998,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeLocalLoopColor = menuLink->addAction(tr("Local loop closure"));
QAction * aChangeUserLoopColor = menuLink->addAction(tr("User loop closure"));
QAction * aChangeVirtualLoopColor = menuLink->addAction(tr("Virtual loop closure"));
QAction * aChangeNeighborMergedColor = menuLink->addAction(tr("Neighbor merged"));
QAction * aChangeRejectedLoopColor = menuLink->addAction(tr("Outlier loop closure"));
QAction * aChangeRejectedLoopThr = menuLink->addAction(tr("Set outlier threshold..."));
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
@@ -991,6 +1012,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalLoopColor->setIcon(createIcon(_loopClosureLocalColor));
aChangeUserLoopColor->setIcon(createIcon(_loopClosureUserColor));
aChangeVirtualLoopColor->setIcon(createIcon(_loopClosureVirtualColor));
aChangeNeighborMergedColor->setIcon(createIcon(_neighborMergedColor));
aChangeRejectedLoopColor->setIcon(createIcon(_loopClosureRejectedColor));
aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
@@ -1001,6 +1023,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalLoopColor->setIconVisibleInMenu(true);
aChangeUserLoopColor->setIconVisibleInMenu(true);
aChangeVirtualLoopColor->setIconVisibleInMenu(true);
aChangeNeighborMergedColor->setIconVisibleInMenu(true);
aChangeRejectedLoopColor->setIconVisibleInMenu(true);
aChangeLocalPathColor->setIconVisibleInMenu(true);
aChangeGlobalPathColor->setIconVisibleInMenu(true);
@@ -1149,6 +1172,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeLocalLoopColor ||
r == aChangeUserLoopColor ||
r == aChangeVirtualLoopColor ||
r == aChangeNeighborMergedColor ||
r == aChangeRejectedLoopColor ||
r == aChangeLocalPathColor ||
r == aChangeGlobalPathColor ||
@@ -1180,6 +1204,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _loopClosureVirtualColor;
}
else if(r == aChangeNeighborMergedColor)
{
color = _neighborMergedColor;
}
else if(r == aChangeRejectedLoopColor)
{
color = _loopClosureRejectedColor;
@@ -1232,6 +1260,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setVirtualLoopClosureColor(color);
}
else if(r == aChangeNeighborMergedColor)
{
this->setNeighborMergedColor(color);
}
else if(r == aChangeRejectedLoopColor)
{
this->setRejectedLoopClosureColor(color);

View File

@@ -2229,6 +2229,12 @@ void MainWindow::processRtabmapGlobalPathEvent(const rtabmap::RtabmapGlobalPathE
info->show();
}
}
else if(event.getPoses().empty() && _waypoints.size())
{
// resend the same goal
uSleep(1000);
this->postGoal(_waypoints.at(_waypointsIndex % _waypoints.size()));
}
}
void MainWindow::processRtabmapLabelErrorEvent(int id, const QString & label)

View File

@@ -457,6 +457,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->general_checkBox_keepRawData->setObjectName(Parameters::kMemImageKept().c_str());
_ui->general_checkBox_keepBinaryData->setObjectName(Parameters::kMemBinDataKept().c_str());
_ui->general_checkBox_saveDepth16bits->setObjectName(Parameters::kMemSaveDepth16Format().c_str());
_ui->general_checkBox_reduceGraph->setObjectName(Parameters::kMemReduceGraph().c_str());
_ui->general_checkBox_keepNotLinkedNodes->setObjectName(Parameters::kMemNotLinkedNodesKept().c_str());
_ui->general_spinBox_maxStMemSize->setObjectName(Parameters::kMemSTMSize().c_str());
_ui->doubleSpinBox_similarityThreshold->setObjectName(Parameters::kMemRehearsalSimilarity().c_str());

View File

@@ -50,7 +50,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>197</width>
<width>166</width>
<height>173</height>
</rect>
</property>
@@ -236,7 +236,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>196</width>
<width>165</width>
<height>173</height>
</rect>
</property>
@@ -739,7 +739,7 @@
<item row="5" column="1">
<widget class="QLabel" name="label_52">
<property name="text">
<string>Nodes</string>
<string>Poses</string>
</property>
</widget>
</item>
@@ -753,14 +753,14 @@
<item row="6" column="1">
<widget class="QLabel" name="label_41">
<property name="text">
<string>Loop closures (G, LS, LT, U)</string>
<string>Links (N, NM, G, LS, LT, U)</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_10">
<property name="text">
<string>Total path length (m)</string>
<string>Path length (m)</string>
</property>
</widget>
</item>
@@ -800,7 +800,7 @@
</layout>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_nodes">
<widget class="QLabel" name="label_poses">
<property name="text">
<string/>
</property>
@@ -836,7 +836,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>314</width>
<width>338</width>
<height>303</height>
</rect>
</property>
@@ -1055,7 +1055,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-129</y>
<y>0</y>
<width>351</width>
<height>407</height>
</rect>
@@ -1389,7 +1389,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>333</width>
<width>338</width>
<height>333</height>
</rect>
</property>
@@ -1628,7 +1628,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>276</width>
<width>338</width>
<height>377</height>
</rect>
</property>
@@ -1885,8 +1885,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>201</width>
<height>117</height>
<width>351</width>
<height>176</height>
</rect>
</property>
<attribute name="label">
@@ -1985,7 +1985,7 @@
<rect>
<x>0</x>
<y>0</y>
<width>285</width>
<width>338</width>
<height>309</height>
</rect>
</property>
@@ -2330,6 +2330,242 @@
</layout>
</widget>
</widget>
<widget class="QDockWidget" name="dockWidget_info">
<property name="windowTitle">
<string>Info</string>
</property>
<attribute name="dockWidgetArea">
<number>2</number>
</attribute>
<widget class="QWidget" name="dockWidgetContents_4">
<layout class="QVBoxLayout" name="verticalLayout_12">
<item>
<layout class="QGridLayout" name="gridLayout_15" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_70">
<property name="text">
<string>Database version</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_version">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_69">
<property name="text">
<string>Sessions</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_sessions">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_9">
<property name="text">
<string>Nodes</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_nodes">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_80">
<property name="text">
<string>Dictionary</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_words">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_78">
<property name="text">
<string>Global map</string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_global_map">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_79">
<property name="text">
<string>Working memory</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_wm">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QLabel" name="label_77">
<property name="text">
<string>Last dictionary</string>
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_dictionary">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="7" column="0">
<widget class="QLabel" name="label_74">
<property name="text">
<string>Odometry</string>
</property>
</widget>
</item>
<item row="7" column="1">
<widget class="QLabel" name="label_odom">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item>
<layout class="QGridLayout" name="gridLayout_14" columnstretch="0,1">
<item row="0" column="0">
<widget class="QLabel" name="label_68">
<property name="text">
<string>Database size</string>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_db_size">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QLabel" name="label_72">
<property name="text">
<string>RGB images</string>
</property>
</widget>
</item>
<item row="1" column="1">
<widget class="QLabel" name="label_rgb_size">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QLabel" name="label_73">
<property name="text">
<string>Depth images</string>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_depth_size">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QLabel" name="label_71">
<property name="text">
<string>Laser scans</string>
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_scan_size">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="4" column="0">
<widget class="QLabel" name="label_75">
<property name="text">
<string>User data </string>
</property>
</widget>
</item>
<item row="4" column="1">
<widget class="QLabel" name="label_userData_size">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="5" column="0">
<widget class="QLabel" name="label_76">
<property name="text">
<string>Dictionary</string>
</property>
</widget>
</item>
<item row="5" column="1">
<widget class="QLabel" name="label_words_size">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</item>
<item>
<spacer name="verticalSpacer_7">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
<property name="sizeHint" stdset="0">
<size>
<width>0</width>
<height>0</height>
</size>
</property>
</spacer>
</item>
</layout>
</widget>
</widget>
<action name="actionOpen_database">
<property name="text">
<string>Open database</string>

View File

@@ -25,11 +25,44 @@
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_clear">
<property name="text">
<string>Clear</string>
</property>
</widget>
<layout class="QHBoxLayout" name="horizontalLayout" stretch="0,0,1">
<item>
<widget class="QSpinBox" name="spinBox_lines">
<property name="suffix">
<string> lines</string>
</property>
<property name="maximum">
<number>99999</number>
</property>
<property name="value">
<number>100</number>
</property>
</widget>
</item>
<item>
<widget class="QSpinBox" name="spinBox_time">
<property name="suffix">
<string> ms</string>
</property>
<property name="maximum">
<number>1000</number>
</property>
<property name="singleStep">
<number>100</number>
</property>
<property name="value">
<number>1000</number>
</property>
</widget>
</item>
<item>
<widget class="QPushButton" name="pushButton_clear">
<property name="text">
<string>Clear</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
</widget>

View File

@@ -63,7 +63,7 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-390</y>
<y>0</y>
<width>760</width>
<height>1705</height>
</rect>
@@ -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">
@@ -3950,7 +3950,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="0">
<item row="11" column="0">
<widget class="QDoubleSpinBox" name="general_doubleSpinBox_laserScanVoxel">
<property name="minimumSize">
<size>
@@ -3975,7 +3975,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="0">
<item row="10" column="0">
<widget class="QSpinBox" name="spinBox_imageDecimation">
<property name="minimum">
<number>1</number>
@@ -4017,7 +4017,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<item row="7" 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>
@@ -4115,7 +4115,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="0">
<item row="7" column="0">
<widget class="QCheckBox" name="general_checkBox_initWMWithAllNodes">
<property name="text">
<string/>
@@ -4125,7 +4125,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="1">
<item row="8" column="1">
<widget class="QLabel" name="label_retrieved_5">
<property name="text">
<string>Keep raw sensor data. Only useful to save loop closure computation time when features re-extraction is enabled. Disable to save RAM memory.</string>
@@ -4138,7 +4138,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="7" column="0">
<item row="8" column="0">
<widget class="QCheckBox" name="general_checkBox_keepRawData">
<property name="text">
<string/>
@@ -4148,7 +4148,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="9" column="1">
<item row="10" column="1">
<widget class="QLabel" name="label_retrieved_6">
<property name="text">
<string>Image decimation. This feature can be used to save images in lower resolution (size/decimation).</string>
@@ -4184,7 +4184,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="10" column="1">
<item row="11" column="1">
<widget class="QLabel" name="label_retrieved_8">
<property name="text">
<string>If &gt; 0.0, voxelize laser scans when creating a location. This feature can be used to save laser scans already voxelized.</string>
@@ -4197,7 +4197,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="1">
<item row="9" column="1">
<widget class="QLabel" name="label_retrieved_9">
<property name="text">
<string>Save depth image into 16 bits format to reduce memory used. Warning: values over ~65 meters are ignored (maximum 65535 millimeters).</string>
@@ -4210,7 +4210,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="8" column="0">
<item row="9" column="0">
<widget class="QCheckBox" name="general_checkBox_saveDepth16bits">
<property name="text">
<string/>
@@ -4220,6 +4220,29 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="6" column="1">
<widget class="QLabel" name="label_retrieved_10">
<property name="text">
<string>Reduce graph. Merge nodes when loop closures are added (ignoring those with user data set).</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="6" column="0">
<widget class="QCheckBox" name="general_checkBox_reduceGraph">
<property name="text">
<string/>
</property>
<property name="checked">
<bool>false</bool>
</property>
</widget>
</item>
</layout>
</item>
<item>