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

Added GPS class for convenience, database viewer can view GPS values and export to KML format

Browse Source
This commit is contained in:
matlabbe
2017-09-26 14:13:06 -04:00
parent 8759fda632
commit 9691a4f361
35 changed files with 1150 additions and 381 deletions
Download Patch File Download Diff File Expand all files Collapse all files

254
guilib/src/DatabaseViewer.cpp
View File

@@ -68,6 +68,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "rtabmap/core/RegistrationVis.h"
#include "rtabmap/core/RegistrationIcp.h"
#include "rtabmap/core/OccupancyGrid.h"
#include "rtabmap/core/GeodeticCoords.h"
#include "rtabmap/gui/DataRecorder.h"
#include "ExportCloudsDialog.h"
#include "EditDepthArea.h"
@@ -229,6 +230,9 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
connect(ui_->actionKITTI_format_txt, SIGNAL(triggered()), this , SLOT(exportPosesKITTI()));
connect(ui_->actionTORO_graph, SIGNAL(triggered()), this , SLOT(exportPosesTORO()));
connect(ui_->actionG2o_g2o, SIGNAL(triggered()), this , SLOT(exportPosesG2O()));
connect(ui_->actionPoses_KML, SIGNAL(triggered()), this , SLOT(exportPosesKML()));
connect(ui_->actionGPS_TXT, SIGNAL(triggered()), this , SLOT(exportGPS_TXT()));
connect(ui_->actionGPS_KML, SIGNAL(triggered()), this , SLOT(exportGPS_KML()));
connect(ui_->actionView_3D_map, SIGNAL(triggered()), this, SLOT(view3DMap()));
connect(ui_->actionGenerate_3D_map_pcd, SIGNAL(triggered()), this, SLOT(generate3DMap()));
connect(ui_->actionDetect_more_loop_closures, SIGNAL(triggered()), this, SLOT(detectMoreLoopClosures()));
@@ -250,6 +254,8 @@ DatabaseViewer::DatabaseViewer(const QString & ini, QWidget * parent) :
ui_->pushButton_reject->setEnabled(false);
ui_->menuExport_poses->setEnabled(false);
ui_->menuExport_GPS->setEnabled(false);
ui_->actionPoses_KML->setEnabled(false);
ui_->horizontalSlider_A->setTracking(false);
ui_->horizontalSlider_B->setTracking(false);
@@ -698,6 +704,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
graphLinks_.clear();
poses_.clear();
groundTruthPoses_.clear();
gpsPoses_.clear();
gpsValues_.clear();
mapIds_.clear();
links_.clear();
linksAdded_.clear();
@@ -710,6 +718,8 @@ bool DatabaseViewer::openDatabase(const QString & path)
ui_->graphViewer->clearAll();
occupancyGridViewer_->clear();
ui_->menuExport_poses->setEnabled(false);
ui_->menuExport_GPS->setEnabled(false);
ui_->actionPoses_KML->setEnabled(false);
ui_->checkBox_showOptimized->setEnabled(false);
ui_->toolBox_statistics->clear();
databaseFileName_.clear();
@@ -1011,6 +1021,7 @@ void DatabaseViewer::exportDatabase()
std::map<int, Transform> poses;
std::map<int, double> stamps;
std::map<int, Transform> groundTruths;
std::map<int, GPS> gpsValues;
for(int i=0; i<ids_.size(); i+=1+framesIgnored)
{
Transform odomPose, groundTruth;
@@ -1019,7 +1030,7 @@ void DatabaseViewer::exportDatabase()
std::string label;
double stamp = 0;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
if(dbDriver_->getNodeInfo(ids_[i], odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
{
if(frameRate == 0 ||
@@ -1040,6 +1051,10 @@ void DatabaseViewer::exportDatabase()
poses.insert(std::make_pair(ids_[i], odomPose));
stamps.insert(std::make_pair(ids_[i], stamp));
groundTruths.insert(std::make_pair(ids_[i], groundTruth));
if(gps.stamp() > 0.0)
{
gpsValues.insert(std::make_pair(ids_[i], gps));
}
}
}
if(sessionExported >= 0 && mapId > sessionExported)
@@ -1115,7 +1130,14 @@ void DatabaseViewer::exportDatabase()
stamps.at(id),
userData);
}
sensorData.setGroundTruth(groundTruths.at(id));
if(groundTruths.find(id)!=groundTruths.end())
{
sensorData.setGroundTruth(groundTruths.at(id));
}
if(gpsValues.find(id)!=gpsValues.end())
{
sensorData.setGPS(gpsValues.at(id));
}
recorder.addData(sensorData, dialog.isOdomExported()?poses.at(id):Transform(), covariance);
@@ -1294,8 +1316,12 @@ void DatabaseViewer::updateIds()
mapIds_.clear();
poses_.clear();
groundTruthPoses_.clear();
gpsPoses_.clear();
gpsValues_.clear();
ui_->checkBox_alignPosesWithGroundTruth->setVisible(false);
ui_->label_alignPosesWithGroundTruth->setVisible(false);
ui_->menuExport_GPS->setEnabled(false);
ui_->actionPoses_KML->setEnabled(false);
links_.clear();
linksAdded_.clear();
linksRefined_.clear();
@@ -1325,10 +1351,9 @@ void DatabaseViewer::updateIds()
double s;
int mapId;
std::vector<float> v;
std::vector<double> gps;
GPS gps;
dbDriver_->getNodeInfo(ids_[i], p, mapId, w, l, s, g, v, gps);
mapIds_.insert(std::make_pair(ids_[i], mapId));
if(i>0)
{
if(mapIds_.at(ids_[i-1]) == mapId)
@@ -1392,6 +1417,20 @@ void DatabaseViewer::updateIds()
{
groundTruthPoses_.insert(std::make_pair(ids_[i], g));
}
if(gps.stamp() > 0.0)
{
gpsValues_.insert(std::make_pair(ids_[i], gps));
cv::Point3f p(0.0f,0.0f,0.0f);
if(!gpsPoses_.empty())
{
GeodeticCoords coords = gps.toGeodeticCoords();
GPS originGPS = gpsValues_.begin()->second;
p = coords.toENU_WGS84(originGPS.toGeodeticCoords());
}
Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*180.0/M_PI));
gpsPoses_.insert(std::make_pair(ids_[i], pose));
}
}
if(idsWithoutBad.find(ids_[i]) == idsWithoutBad.end())
@@ -1403,10 +1442,23 @@ void DatabaseViewer::updateIds()
}
}
}
if(!groundTruthPoses_.empty())
if(!groundTruthPoses_.empty() || !gpsPoses_.empty())
{
ui_->checkBox_alignPosesWithGroundTruth->setVisible(true);
ui_->label_alignPosesWithGroundTruth->setVisible(true);
if(!groundTruthPoses_.empty())
{
ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with ground truth"));
}
else
{
ui_->label_alignPosesWithGroundTruth->setText(tr("Align poses with GPS"));
}
}
if(!gpsValues_.empty())
{
ui_->menuExport_GPS->setEnabled(true);
ui_->actionPoses_KML->setEnabled(groundTruthPoses_.empty());
}
UINFO("Loaded %d ids, %d poses and %d links", (int)ids_.size(), (int)poses_.size(), (int)links_.size());
@@ -1434,6 +1486,7 @@ void DatabaseViewer::updateIds()
ui_->textEdit_info->append(tr("WM:\t\t%1 nodes and %2 words").arg(dbDriver_->getLastNodesSize()).arg(dbDriver_->getLastDictionarySize()));
ui_->textEdit_info->append(tr("Global graph:\t%1 poses and %2 links").arg(poses_.size()).arg(links_.size()));
ui_->textEdit_info->append(tr("Ground truth:\t%1 poses").arg(groundTruthPoses_.size()));
ui_->textEdit_info->append(tr("GPS:\t%1 poses").arg(gpsValues_.size()));
ui_->textEdit_info->append("");
long total = 0;
long dbSize = UFile::length(dbDriver_->getUrl());
@@ -1663,6 +1716,10 @@ void DatabaseViewer::exportPosesG2O()
{
exportPoses(4);
}
void DatabaseViewer::exportPosesKML()
{
exportPoses(5);
}
void DatabaseViewer::exportPoses(int format)
{
@@ -1676,6 +1733,104 @@ void DatabaseViewer::exportPoses(int format)
}
}
if(format == 5)
{
if(gpsValues_.empty() || gpsPoses_.empty())
{
QMessageBox::warning(this, tr("Cannot export poses"), tr("No GPS in database?!"));
}
else
{
std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
//align with ground truth for more meaningful results
pcl::PointCloud<pcl::PointXYZ> cloud1, cloud2;
cloud1.resize(graph.size());
cloud2.resize(graph.size());
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=gpsPoses_.begin(); iter!=gpsPoses_.end(); ++iter)
{
std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
if(iter2!=graph.end())
{
if(oi==0)
{
idFirst = iter->first;
}
cloud1[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
cloud2[oi++] = pcl::PointXYZ(iter2->second.x(), iter2->second.y(), iter2->second.z());
}
}
Transform t = Transform::getIdentity();
if(oi>5)
{
cloud1.resize(oi);
cloud2.resize(oi);
t = util3d::transformFromXYZCorrespondencesSVD(cloud2, cloud1);
}
else if(idFirst)
{
t = gpsPoses_.at(idFirst) * graph.at(idFirst).inverse();
}
std::map<int, GPS> values;
GeodeticCoords origin = gpsValues_.begin()->second.toGeodeticCoords();
for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
{
iter->second = t * iter->second;
GeodeticCoords coord;
coord.fromENU_WGS84(cv::Point3d(iter->second.x(), iter->second.y(), iter->second.z()), origin);
double bearing = -(iter->second.theta()*180.0/M_PI-90.0);
if(bearing < 0)
{
bearing += 360;
}
Transform p, g;
int w;
std::string l;
double stamp=0.0;
int mapId;
std::vector<float> v;
GPS gps;
dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps);
values.insert(std::make_pair(iter->first, GPS(stamp, coord.longitude(), coord.latitude(), coord.altitude(), 0, 0)));
}
QString output = pathDatabase_ + QDir::separator() + "poses.kml";
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
output,
tr("Google Earth file (*.kml)"));
if(!path.isEmpty())
{
bool saved = graph::exportGPS(path.toStdString(), values, ui_->graphViewer->getNodeColor().rgba());
if(saved)
{
QMessageBox::information(this,
tr("Export poses..."),
tr("GPS coordinates saved to \"%1\".")
.arg(path));
}
else
{
QMessageBox::information(this,
tr("Export poses..."),
tr("Failed to save GPS coordinates to \"%1\"!")
.arg(path));
}
}
}
return;
}
std::map<int, Transform> optimizedPoses = uValueAt(graphes_, ui_->horizontalSlider_iterations->value());
if(optimizedPoses.size())
@@ -1796,7 +1951,7 @@ void DatabaseViewer::exportPoses(int format)
double stamp=0.0;
int mapId;
std::vector<float> v;
std::vector<double> gps;
GPS gps;
if(dbDriver_->getNodeInfo(iter->first, p, mapId, w, l, stamp, g, v, gps))
{
stamps.insert(std::make_pair(iter->first, stamp));
@@ -1842,6 +1997,48 @@ void DatabaseViewer::exportPoses(int format)
}
}
void DatabaseViewer::exportGPS_TXT()
{
exportGPS(0);
}
void DatabaseViewer::exportGPS_KML()
{
exportGPS(1);
}
void DatabaseViewer::exportGPS(int format)
{
if(!gpsValues_.empty())
{
QString output = pathDatabase_ + QDir::separator() + (format==0?"gps.txt":"gps.kml");
QString path = QFileDialog::getSaveFileName(
this,
tr("Save File"),
output,
format==0?tr("Raw format (*.txt)"):tr("Google Earth file (*.kml)"));
if(!path.isEmpty())
{
bool saved = graph::exportGPS(path.toStdString(), gpsValues_, ui_->graphViewer->getGPSColor().rgba());
if(saved)
{
QMessageBox::information(this,
tr("Export poses..."),
tr("GPS coordinates saved to \"%1\".")
.arg(path));
}
else
{
QMessageBox::information(this,
tr("Export poses..."),
tr("Failed to save GPS coordinates to \"%1\"!")
.arg(path));
}
}
}
}
void DatabaseViewer::generateGraph()
{
if(!dbDriver_)
@@ -1989,7 +2186,7 @@ void DatabaseViewer::regenerateLocalMaps()
double stamp;
QString msg;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
{
Signature s = data;
@@ -2067,7 +2264,7 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
double stamp;
QString msg;
std::vector<float> velocity;
std::vector<double> gps;
GPS gps;
if(dbDriver_->getNodeInfo(data.id(), odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps))
{
Signature s = data;
@@ -2469,7 +2666,7 @@ void DatabaseViewer::update(int value,
std::string l;
double s;
std::vector<float> v;
std::vector<double> gps;
GPS gps;
dbDriver_->getNodeInfo(id, odomPose, mapId, w, l, s, g, v, gps);
weight->setNum(w);
@@ -2482,10 +2679,10 @@ void DatabaseViewer::update(int value,
stamp->setText(QString::number(s, 'f'));
stamp->setToolTip(QDateTime::fromMSecsSinceEpoch(s*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
}
if(gps.size())
if(gps.stamp()>0.0)
{
labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps[0], 'f')).arg(gps[1]).arg(gps[2]).arg(gps[3]).arg(gps[4]).arg(gps[5]));
labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps[0]*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
labelGps->setText(QString("stamp=%1 longitude=%2 latitude=%3 altitude=%4m error=%5m bearing=%6deg").arg(QString::number(gps.stamp(), 'f')).arg(gps.longitude()).arg(gps.latitude()).arg(gps.altitude()).arg(gps.error()).arg(gps.bearing()));
labelGps->setToolTip(QDateTime::fromMSecsSinceEpoch(gps.stamp()*1000.0).toString("dd.MM.yyyy hh:mm:ss.zzz"));
}
if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
{
@@ -3511,7 +3708,7 @@ void DatabaseViewer::updateConstraintView(
double s;
Transform p,g;
std::vector<float> v;
std::vector<double> gps;
GPS gps;
dbDriver_->getNodeInfo(link.from(), p, m, w, l, s, g, v, gps);
if(!p.isNull())
{
@@ -3914,16 +4111,22 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
{
std::map<int, rtabmap::Transform> graph = uValueAt(graphes_, value);
std::map<int, Transform> refPoses = groundTruthPoses_;
if(refPoses.empty())
{
refPoses = gpsPoses_;
}
// Log ground truth statistics (in TUM's RGBD-SLAM format)
if(groundTruthPoses_.size())
if(refPoses.size())
{
// compute KITTI statistics before aligning the poses
float length = graph::computePathLength(graph);
if(groundTruthPoses_.size() == graph.size() && length >= 100.0f)
if(refPoses.size() == graph.size() && length >= 100.0f)
{
float t_err = 0.0f;
float r_err = 0.0f;
graph::calcKittiSequenceErrors(uValues(groundTruthPoses_), uValues(graph), t_err, r_err);
graph::calcKittiSequenceErrors(uValues(refPoses), uValues(graph), t_err, r_err);
UINFO("KITTI t_err = %f %%", t_err);
UINFO("KITTI r_err = %f deg/m", r_err);
ui_->toolBox_statistics->updateStat("GT/kitti_t_err/%", t_err, false);
@@ -3938,7 +4141,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
cloud2.resize(graph.size());
int oi = 0;
int idFirst = 0;
for(std::map<int, Transform>::const_iterator iter=groundTruthPoses_.begin(); iter!=groundTruthPoses_.end(); ++iter)
for(std::map<int, Transform>::const_iterator iter=refPoses.begin(); iter!=refPoses.end(); ++iter)
{
std::map<int, Transform>::iterator iter2 = graph.find(iter->first);
if(iter2!=graph.end())
@@ -3962,7 +4165,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
else if(idFirst)
{
t = groundTruthPoses_.at(idFirst) * graph.at(idFirst).inverse();
t = refPoses.at(idFirst) * graph.at(idFirst).inverse();
}
if(!t.isIdentity())
{
@@ -3987,8 +4190,8 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
int oi=0;
for(std::map<int, Transform>::iterator iter=graph.begin(); iter!=graph.end(); ++iter)
{
std::map<int, Transform>::const_iterator jter = groundTruthPoses_.find(iter->first);
if(jter!=groundTruthPoses_.end())
std::map<int, Transform>::const_iterator jter = refPoses.find(iter->first);
if(jter!=refPoses.end())
{
Eigen::Vector3f vA = iter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
Eigen::Vector3f vB = jter->second.toEigen3f().rotation()*Eigen::Vector3f(1,0,0);
@@ -4142,6 +4345,7 @@ void DatabaseViewer::sliderIterationsValueChanged(int value)
}
ui_->graphViewer->updateGTGraph(groundTruthPoses_);
ui_->graphViewer->updateGPSGraph(gpsPoses_, gpsValues_);
ui_->graphViewer->updateGraph(graph, graphLinks_, mapIds_);
ui_->graphViewer->clearMap();
occupancyGridViewer_->clear();
@@ -4428,6 +4632,7 @@ void DatabaseViewer::updateGraphView()
int totalLocalTime = 0;
int totalLocalSpace = 0;
int totalUser = 0;
int totalPriors = 0;
for(std::multimap<int, rtabmap::Link>::iterator iter=links.begin(); iter!=links.end();)
{
if(iter->second.type() == Link::kNeighbor)
@@ -4474,15 +4679,20 @@ void DatabaseViewer::updateGraphView()
}
++totalUser;
}
else if(iter->second.type() == Link::kPosePrior)
{
++totalPriors;
}
++iter;
}
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6)")
ui_->label_loopClosures->setText(tr("(%1, %2, %3, %4, %5, %6, %7)")
.arg(totalNeighbor)
.arg(totalNeighborMerged)
.arg(totalGlobal)
.arg(totalLocalSpace)
.arg(totalLocalTime)
.arg(totalUser));
.arg(totalUser)
.arg(totalPriors));
Optimizer * optimizer = Optimizer::create(ui_->parameters_toolbox->getParameters());

4
guilib/src/ExportCloudsDialog.cpp
View File

@@ -2089,7 +2089,7 @@ bool ExportCloudsDialog::getExportedClouds(
int m,w;
std::string l;
double s;
std::vector<double> gps;
GPS gps;
_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
}
}
@@ -2122,7 +2122,7 @@ bool ExportCloudsDialog::getExportedClouds(
int m,w;
std::string l;
double s;
std::vector<double> gps;
GPS gps;
_dbDriver->getNodeInfo(jter->first, p, m, w, l, s, gt, velocity, gps);
}
}

244
guilib/src/GraphViewer.cpp
View File

@@ -48,6 +48,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <QtCore/QUrl>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/GeodeticCoords.h>
#include <rtabmap/utilite/UCv2Qt.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/ULogger.h>
@@ -81,6 +82,8 @@ public:
this->setBrush(b);
}
int id() const {return _id;};
int mapId() const {return _mapId;}
const Transform & pose() const {return _pose;}
void setPose(const Transform & pose) {this->setPos(-pose.y()*100.0f,-pose.x()*100.0f); _pose=pose;}
@@ -104,6 +107,29 @@ private:
Transform _pose;
};
class NodeGPSItem: public NodeItem
{
public:
NodeGPSItem(int id, int mapId, const Transform & pose, float radius, const GPS & gps) :
NodeItem(id, mapId, pose, radius),
_gps(gps)
{
}
virtual ~NodeGPSItem() {}
protected:
virtual void hoverEnterEvent ( QGraphicsSceneHoverEvent * event )
{
this->setToolTip(QString("%1 [%2] %3\n"
"longitude=%4 latitude=%5 altitude=%6m error=%7m bearing=%8deg")
.arg(id()).arg(mapId()).arg(pose().prettyPrint().c_str())
.arg(_gps.longitude()).arg(_gps.latitude()).arg(_gps.altitude()).arg(_gps.error()).arg(_gps.bearing()));
this->setScale(2);
QGraphicsEllipseItem::hoverEnterEvent(event);
}
private:
GPS _gps;
};
class LinkItem: public QGraphicsLineItem
{
public:
@@ -195,6 +221,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
_localPathColor(Qt::cyan),
_globalPathColor(Qt::darkMagenta),
_gtPathColor(Qt::gray),
_gpsPathColor(Qt::darkCyan),
_loopIntraSessionColor(Qt::red),
_loopInterSessionColor(Qt::green),
_intraInterSessionColors(false),
@@ -209,7 +236,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
_gridCellSize(0.0f),
_localRadius(0),
_loopClosureOutlierThr(0),
_maxLinkLength(0.02f)
_maxLinkLength(0.02f),
_orientationENU(false)
{
this->setScene(new QGraphicsScene(this));
this->setDragMode(QGraphicsView::ScrollHandDrag);
@@ -268,6 +296,10 @@ GraphViewer::GraphViewer(QWidget * parent) :
_gtGraphRoot->setZValue(2);
_gtGraphRoot->setParentItem(_root);
_gpsGraphRoot = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
_gpsGraphRoot->setZValue(2);
_gpsGraphRoot->setParentItem(_root);
this->restoreDefaults();
this->fitInView(this->sceneRect(), Qt::KeepAspectRatio);
@@ -622,6 +654,135 @@ void GraphViewer::updateGTGraph(const std::map<int, Transform> & poses)
UDEBUG("_gtNodeItems=%d, _gtLinkItems=%d timer=%fs", _gtNodeItems.size(), _gtLinkItems.size(), timer.ticks());
}
void GraphViewer::updateGPSGraph(
const std::map<int, Transform> & poses,
const std::map<int, GPS> & gpsValues)
{
UTimer timer;
bool wasVisible = _gpsGraphRoot->isVisible();
_gpsGraphRoot->show();
bool wasEmpty = _gpsNodeItems.size() == 0 && _gpsNodeItems.size() == 0;
UDEBUG("poses=%d", (int)poses.size());
//Hide nodes and links
for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
{
iter.value()->hide();
iter.value()->setColor(_gpsPathColor); // reset color
}
for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
{
iter.value()->hide();
}
for(std::map<int, Transform>::const_iterator iter=poses.begin(); iter!=poses.end(); ++iter)
{
if(!iter->second.isNull())
{
QMap<int, NodeItem*>::iterator itemIter = _gpsNodeItems.find(iter->first);
if(itemIter != _gpsNodeItems.end())
{
itemIter.value()->setPose(iter->second);
itemIter.value()->show();
}
else
{
// create node item
const Transform & pose = iter->second;
UASSERT(gpsValues.find(iter->first) != gpsValues.end());
NodeItem * item = new NodeGPSItem(iter->first, -1, pose, _nodeRadius, gpsValues.at(iter->first));
this->scene()->addItem(item);
item->setZValue(20);
item->setColor(_gpsPathColor);
item->setParentItem(_gpsGraphRoot);
_gpsNodeItems.insert(iter->first, item);
}
if(iter!=poses.begin())
{
std::map<int, Transform>::const_iterator iterPrevious = iter;
--iterPrevious;
Transform previousPose = iterPrevious->second;
Transform currentPose = iter->second;
LinkItem * linkItem = 0;
QMultiMap<int, LinkItem*>::iterator linkIter = _gpsLinkItems.end();
if(_gpsLinkItems.contains(iterPrevious->first))
{
linkIter = _gpsLinkItems.find(iter->first);
while(linkIter.key() == iterPrevious->first && linkIter != _gpsLinkItems.end())
{
if(linkIter.value()->to() == iter->first)
{
linkIter.value()->setPoses(previousPose, currentPose);
linkIter.value()->show();
linkItem = linkIter.value();
break;
}
++linkIter;
}
}
if(linkItem == 0)
{
//create a link item
linkItem = new LinkItem(iterPrevious->first, iter->first, previousPose, currentPose, Link(), 1);
QPen p = linkItem->pen();
p.setWidthF(_linkWidth*100.0f);
linkItem->setPen(p);
linkItem->setZValue(10);
this->scene()->addItem(linkItem);
linkItem->setParentItem(_gpsGraphRoot);
_gpsLinkItems.insert(iterPrevious->first, linkItem);
}
if(linkItem)
{
linkItem->setColor(_gpsPathColor);
}
}
}
}
//remove not used nodes and links
for(QMap<int, NodeItem*>::iterator iter = _gpsNodeItems.begin(); iter!=_gpsNodeItems.end();)
{
if(!iter.value()->isVisible())
{
delete iter.value();
iter = _gpsNodeItems.erase(iter);
}
else
{
++iter;
}
}
for(QMultiMap<int, LinkItem*>::iterator iter = _gpsLinkItems.begin(); iter!=_gpsLinkItems.end();)
{
if(!iter.value()->isVisible())
{
delete iter.value();
iter = _gpsLinkItems.erase(iter);
}
else
{
++iter;
}
}
if(_gpsNodeItems.size() || _gpsLinkItems.size())
{
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
if(wasEmpty)
{
QRectF rect = this->scene()->itemsBoundingRect();
this->fitInView(rect.adjusted(-rect.width()/2.0f, -rect.height()/2.0f, rect.width()/2.0f, rect.height()/2.0f), Qt::KeepAspectRatio);
}
}
_gpsGraphRoot->setVisible(wasVisible);
UDEBUG("_gpsNodeItems=%d, _gpsLinkItems=%d timer=%fs", _gpsNodeItems.size(), _gpsLinkItems.size(), timer.ticks());
}
void GraphViewer::updateReferentialPosition(const Transform & t)
{
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24()*100.0f, -t.o14()*100.0f);
@@ -820,6 +981,10 @@ void GraphViewer::clearGraph()
_gtNodeItems.clear();
qDeleteAll(_gtLinkItems);
_gtLinkItems.clear();
qDeleteAll(_gpsNodeItems);
_gpsNodeItems.clear();
qDeleteAll(_gpsLinkItems);
_gpsLinkItems.clear();
_referential->resetTransform();
_localRadius->resetTransform();
@@ -867,6 +1032,7 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("local_path_color", this->getLocalPathColor());
settings.setValue("global_path_color", this->getGlobalPathColor());
settings.setValue("gt_color", this->getGTColor());
settings.setValue("gps_color", this->getGPSColor());
settings.setValue("intra_session_color", this->getIntraSessionLoopColor());
settings.setValue("inter_session_color", this->getInterSessionLoopColor());
settings.setValue("intra_inter_session_colors_enabled", this->isIntraInterSessionColorsEnabled());
@@ -880,6 +1046,8 @@ void GraphViewer::saveSettings(QSettings & settings, const QString & group) cons
settings.setValue("global_path_visible", this->isGlobalPathVisible());
settings.setValue("local_path_visible", this->isLocalPathVisible());
settings.setValue("gt_graph_visible", this->isGtGraphVisible());
settings.setValue("gps_graph_visible", this->isGPSGraphVisible());
settings.setValue("orientation_ENU", this->isOrientationENU());
if(!group.isEmpty())
{
settings.endGroup();
@@ -906,6 +1074,7 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setLocalPathColor(settings.value("local_path_color", this->getLocalPathColor()).value<QColor>());
this->setGlobalPathColor(settings.value("global_path_color", this->getGlobalPathColor()).value<QColor>());
this->setGTColor(settings.value("gt_color", this->getGTColor()).value<QColor>());
this->setGPSColor(settings.value("gps_color", this->getGPSColor()).value<QColor>());
this->setIntraSessionLoopColor(settings.value("intra_session_color", this->getIntraSessionLoopColor()).value<QColor>());
this->setInterSessionLoopColor(settings.value("inter_session_color", this->getInterSessionLoopColor()).value<QColor>());
this->setGridMapVisible(settings.value("grid_visible", this->isGridMapVisible()).toBool());
@@ -919,6 +1088,8 @@ void GraphViewer::loadSettings(QSettings & settings, const QString & group)
this->setGlobalPathVisible(settings.value("global_path_visible", this->isGlobalPathVisible()).toBool());
this->setLocalPathVisible(settings.value("local_path_visible", this->isLocalPathVisible()).toBool());
this->setGtGraphVisible(settings.value("gt_graph_visible", this->isGtGraphVisible()).toBool());
this->setGPSGraphVisible(settings.value("gps_graph_visible", this->isGPSGraphVisible()).toBool());
this->setOrientationENU(settings.value("orientation_ENU", this->isOrientationENU()).toBool());
if(!group.isEmpty())
{
settings.endGroup();
@@ -957,6 +1128,14 @@ bool GraphViewer::isGtGraphVisible() const
{
return _gtGraphRoot->isVisible();
}
bool GraphViewer::isGPSGraphVisible() const
{
return _gpsGraphRoot->isVisible();
}
bool GraphViewer::isOrientationENU() const
{
return _orientationENU;
}
void GraphViewer::setWorkingDirectory(const QString & path)
{
@@ -973,6 +1152,10 @@ void GraphViewer::setNodeRadius(float radius)
{
iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
}
for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
{
iter.value()->setRect(-_nodeRadius*100.0f, -_nodeRadius*100.0f, _nodeRadius*100.0f*2.0f, _nodeRadius*100.0f*2.0f);
}
}
void GraphViewer::setLinkWidth(float width)
{
@@ -1100,6 +1283,18 @@ void GraphViewer::setGTColor(const QColor & color)
iter.value()->setColor(_gtPathColor);
}
}
void GraphViewer::setGPSColor(const QColor & color)
{
_gpsPathColor = color;
for(QMap<int, NodeItem*>::iterator iter=_gpsNodeItems.begin(); iter!=_gpsNodeItems.end(); ++iter)
{
iter.value()->setColor(_gpsPathColor);
}
for(QMultiMap<int, LinkItem*>::iterator iter=_gpsLinkItems.begin(); iter!=_gpsLinkItems.end(); ++iter)
{
iter.value()->setColor(_gpsPathColor);
}
}
void GraphViewer::setIntraSessionLoopColor(const QColor & color)
{
_loopIntraSessionColor = color;
@@ -1192,6 +1387,18 @@ void GraphViewer::setGtGraphVisible(bool visible)
{
_gtGraphRoot->setVisible(visible);
}
void GraphViewer::setGPSGraphVisible(bool visible)
{
_gpsGraphRoot->setVisible(visible);
}
void GraphViewer::setOrientationENU(bool enabled)
{
if(_orientationENU!=enabled)
{
_orientationENU = enabled;
this->rotate(_orientationENU?90:270);
}
}
void GraphViewer::restoreDefaults()
{
@@ -1258,6 +1465,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aChangeLocalPathColor = menuLink->addAction(tr("Local path"));
QAction * aChangeGlobalPathColor = menuLink->addAction(tr("Global path"));
QAction * aChangeGTColor = menuLink->addAction(tr("Ground truth"));
QAction * aChangeGPSColor = menuLink->addAction(tr("GPS"));
menuLink->addSeparator();
QAction * aSetIntraInterSessionColors = menuLink->addAction(tr("Enable intra/inter-session colors"));
QAction * aChangeIntraSessionLoopColor = menuLink->addAction(tr("Intra-session loop closure"));
@@ -1272,6 +1480,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalPathColor->setIcon(createIcon(_localPathColor));
aChangeGlobalPathColor->setIcon(createIcon(_globalPathColor));
aChangeGTColor->setIcon(createIcon(_gtPathColor));
aChangeGPSColor->setIcon(createIcon(_gpsPathColor));
aChangeIntraSessionLoopColor->setIcon(createIcon(_loopIntraSessionColor));
aChangeInterSessionLoopColor->setIcon(createIcon(_loopInterSessionColor));
aChangeNeighborColor->setIconVisibleInMenu(true);
@@ -1284,6 +1493,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
aChangeLocalPathColor->setIconVisibleInMenu(true);
aChangeGlobalPathColor->setIconVisibleInMenu(true);
aChangeGTColor->setIconVisibleInMenu(true);
aChangeGPSColor->setIconVisibleInMenu(true);
aChangeIntraSessionLoopColor->setIconVisibleInMenu(true);
aChangeInterSessionLoopColor->setIconVisibleInMenu(true);
aSetIntraInterSessionColors->setCheckable(true);
@@ -1302,6 +1512,8 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
QAction * aShowHideGlobalPath;
QAction * aShowHideLocalPath;
QAction * aShowHideGtGraph;
QAction * aShowHideGPSGraph;
QAction * aOrientationENU;
if(_gridMap->isVisible())
{
aShowHideGridMap = menu.addAction(tr("Hide grid map"));
@@ -1366,10 +1578,22 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
aShowHideGtGraph = menu.addAction(tr("Show ground truth graph"));
}
if(_gpsGraphRoot->isVisible())
{
aShowHideGPSGraph = menu.addAction(tr("Hide GPS graph"));
}
else
{
aShowHideGPSGraph = menu.addAction(tr("Show GPS graph"));
}
aOrientationENU = menu.addAction(tr("ENU Orientation"));
aOrientationENU->setCheckable(true);
aOrientationENU->setChecked(_orientationENU);
aShowHideGraph->setEnabled(_nodeItems.size());
aShowHideGlobalPath->setEnabled(_globalPathLinkItems.size());
aShowHideLocalPath->setEnabled(_localPathLinkItems.size());
aShowHideGtGraph->setEnabled(_gtNodeItems.size());
aShowHideGPSGraph->setEnabled(_gpsNodeItems.size());
menu.addSeparator();
QAction * aRestoreDefaults = menu.addAction(tr("Restore defaults"));
@@ -1487,6 +1711,7 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
r == aChangeLocalPathColor ||
r == aChangeGlobalPathColor ||
r == aChangeGTColor ||
r == aChangeGPSColor ||
r == aChangeIntraSessionLoopColor ||
r == aChangeInterSessionLoopColor)
{
@@ -1535,6 +1760,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
color = _gtPathColor;
}
else if(r == aChangeGPSColor)
{
color = _gpsPathColor;
}
else if(r == aChangeIntraSessionLoopColor)
{
color = _loopIntraSessionColor;
@@ -1595,6 +1824,10 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setGTColor(color);
}
else if(r == aChangeGPSColor)
{
this->setGPSColor(color);
}
else if(r == aChangeIntraSessionLoopColor)
{
this->setIntraSessionLoopColor(color);
@@ -1671,6 +1904,15 @@ void GraphViewer::contextMenuEvent(QContextMenuEvent * event)
{
this->setGtGraphVisible(!this->isGtGraphVisible());
}
else if(r == aShowHideGPSGraph)
{
this->setGPSGraphVisible(!this->isGPSGraphVisible());
}
else if(r == aOrientationENU)
{
this->setOrientationENU(!this->isOrientationENU());
}
if(r)
{
emit configChanged();

1
guilib/src/PreferencesDialog.cpp
View File

@@ -792,6 +792,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
_ui->graphOptimization_maxError->setObjectName(Parameters::kRGBDOptimizeMaxError().c_str());
_ui->graphOptimization_stopEpsilon->setObjectName(Parameters::kOptimizerEpsilon().c_str());
_ui->graphOptimization_robust->setObjectName(Parameters::kOptimizerRobust().c_str());
_ui->graphOptimization_priorsIgnored->setObjectName(Parameters::kOptimizerPriorsIgnored().c_str());
_ui->comboBox_g2o_solver->setObjectName(Parameters::kg2oSolver().c_str());
_ui->comboBox_g2o_optimizer->setObjectName(Parameters::kg2oOptimizer().c_str());

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

@@ -51,8 +51,8 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-24</y>
<width>393</width>
<y>0</y>
<width>408</width>
<height>232</height>
</rect>
</property>
@@ -226,8 +226,8 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-24</y>
<width>393</width>
<y>0</y>
<width>408</width>
<height>232</height>
</rect>
</property>
@@ -534,6 +534,14 @@
<addaction name="actionKITTI_format_txt"/>
<addaction name="actionTORO_graph"/>
<addaction name="actionG2o_g2o"/>
<addaction name="actionPoses_KML"/>
</widget>
<widget class="QMenu" name="menuExport_GPS">
<property name="title">
<string>Export GPS...</string>
</property>
<addaction name="actionGPS_TXT"/>
<addaction name="actionGPS_KML"/>
</widget>
<addaction name="actionOpen_database"/>
<addaction name="separator"/>
@@ -543,6 +551,7 @@
<addaction name="actionExport"/>
<addaction name="actionExtract_images"/>
<addaction name="menuExport_poses"/>
<addaction name="menuExport_GPS"/>
<addaction name="separator"/>
<addaction name="actionQuit"/>
</widget>
@@ -898,7 +907,7 @@
<item row="6" column="1">
<widget class="QLabel" name="label_41">
<property name="text">
<string>Links (N, NM, G, LS, LT, U)</string>
<string>Links (N, NM, G, LS, LT, U, P)</string>
</property>
</widget>
</item>
@@ -1187,8 +1196,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>280</width>
<height>666</height>
<width>309</width>
<height>650</height>
</rect>
</property>
<attribute name="label">
@@ -1550,8 +1559,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>201</width>
<height>126</height>
<width>324</width>
<height>168</height>
</rect>
</property>
<attribute name="label">
@@ -1650,8 +1659,8 @@
<rect>
<x>0</x>
<y>0</y>
<width>186</width>
<height>496</height>
<width>309</width>
<height>256</height>
</rect>
</property>
<attribute name="label">
@@ -2263,6 +2272,21 @@
<string>g2o (*.g2o)</string>
</property>
</action>
<action name="actionGPS_KML">
<property name="text">
<string>Google Earth (*.kml)</string>
</property>
</action>
<action name="actionGPS_TXT">
<property name="text">
<string>Raw format (*.txt)</string>
</property>
</action>
<action name="actionPoses_KML">
<property name="text">
<string>Google Earth (*.kml)</string>
</property>
</action>
</widget>
<customwidgets>
<customwidget>

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

@@ -63,25 +63,16 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-629</y>
<width>678</width>
<height>2739</height>
<y>0</y>
<width>673</width>
<height>2749</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -95,7 +86,7 @@
<enum>QFrame::Raised</enum>
</property>
<property name="currentIndex">
<number>18</number>
<number>14</number>
</property>
<widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29" stretch="0,1">
@@ -4570,16 +4561,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>Directory of images (optional settings)</string>
</property>
<layout class="QVBoxLayout" name="verticalLayout_93">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -8489,21 +8471,21 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="0" rowspan="2">
<item row="4" column="0" rowspan="2">
<widget class="QCheckBox" name="graphOptimization_fromGraphEnd">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="3" column="1">
<item row="4" column="1">
<widget class="Line" name="line">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
</widget>
</item>
<item row="4" column="1">
<item row="5" column="1">
<widget class="QLabel" name="label_151">
<property name="text">
<string>Optimize graph from the newest node.</string>
@@ -8516,7 +8498,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="5" column="1">
<item row="6" column="1">
<widget class="QLabel" name="label_211">
<property name="text">
<string>-If true, there is no odometry correction computed. All previous poses in the map are corrected instead, not the last one (which corresponds to latest odometry value). So, the transform between frames /map to /odom will be always Identity even on loop closures.</string>
@@ -8529,7 +8511,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_183">
<property name="text">
<string>-If false, the graph is optimized from the oldest node of the current graph. It can be useful to preserve the map referential from the oldest node. An odometry correction between frames /map to /odom is computed. Warning: when some nodes are transferred, the first referential of the local map may change, resulting in momentary changes in robot/map position (which are annoying in teleoperation).</string>
@@ -8542,6 +8524,26 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
</widget>
</item>
<item row="3" column="1">
<widget class="QLabel" name="label_431">
<property name="text">
<string>Ignore pose priors.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="graphOptimization_priorsIgnored">
<property name="text">
<string/>
</property>
</widget>
</item>
</layout>
</widget>
</item>
@@ -12653,16 +12655,7 @@ If set to false, classic RTAB-Map loop closure detection is done using only imag
</property>
<widget class="QWidget" name="page_54">
<layout class="QVBoxLayout" name="verticalLayout_85">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -12802,16 +12795,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
</widget>
<widget class="QWidget" name="page_55">
<layout class="QVBoxLayout" name="verticalLayout_86">
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -12969,16 +12953,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -13058,16 +13033,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>
@@ -13179,16 +13145,7 @@ Lower the ratio -&gt; higher the precision. 0 means disabled, matching the neare
<property name="spacing">
<number>0</number>
</property>
<property name="leftMargin">
<number>0</number>
</property>
<property name="topMargin">
<number>0</number>
</property>
<property name="rightMargin">
<number>0</number>
</property>
<property name="bottomMargin">
<property name="margin">
<number>0</number>
</property>
<item>