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

Integrated GPS into likelihood computation

Browse Source
This commit is contained in:
matlabbe
2018-10-22 12:31:07 -04:00
parent 93a3a667c8
commit 3bc8fc4c11
10 changed files with 215 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
guilib/src/GraphViewer.cpp
View File

@@ -242,6 +242,8 @@ GraphViewer::GraphViewer(QWidget * parent) :
_loopIntraSessionColor(Qt::red),
_loopInterSessionColor(Qt::green),
_intraInterSessionColors(false),
_worldMapRotation(0.0f),
_world(0),
_root(0),
_graphRoot(0),
_globalPathRoot(0),
@@ -262,7 +264,9 @@ GraphViewer::GraphViewer(QWidget * parent) :
_workingDirectory = QDir::homePath();
this->scene()->clear();
_world = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
_root = (QGraphicsItem *)this->scene()->addEllipse(QRectF(-0.0001,-0.0001,0.0001,0.0001));
_root->setParentItem(_world);
// add referential
_originReferential = new QGraphicsItemGroup();
@@ -275,6 +279,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
item->setZValue(100);
item->setParentItem(_root);
_originReferential->addToGroup(item);
_originReferential->setParentItem(_root);
// current pose
_referential = new QGraphicsItemGroup();
@@ -287,6 +292,7 @@ GraphViewer::GraphViewer(QWidget * parent) :
item->setZValue(100);
item->setParentItem(_root);
_referential->addToGroup(item);
_referential->setParentItem(_root);
_localRadius = this->scene()->addEllipse(-0.0001,-0.0001,0.0001,0.0001);
_localRadius->setZValue(1);
@@ -327,6 +333,12 @@ GraphViewer::~GraphViewer()
{
}
void GraphViewer::setWorldMapRotation(const float & theta)
{
_worldMapRotation = theta;
setOrientationENU(isOrientationENU());
}
void GraphViewer::updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds)
@@ -808,7 +820,10 @@ void GraphViewer::updateGPSGraph(
void GraphViewer::updateReferentialPosition(const Transform & t)
{
QTransform qt(t.r11(), t.r12(), t.r21(), t.r22(), -t.o24()*100.0f, -t.o14()*100.0f);
QTransform qt;
qt.translate(-t.o24()*100.0f, -t.o14()*100.0f);
qt.rotateRadians(-t.theta());
_referential->setTransform(qt);
_localRadius->setTransform(qt);
@@ -922,7 +937,7 @@ void GraphViewer::setCurrentGoalID(int id, const Transform & pose)
void GraphViewer::setLocalRadius(float radius)
{
_localRadius->setRect(-radius, -radius, radius*2, radius*2);
_localRadius->setRect(-radius*100, -radius*100, radius*200, radius*200);
}
void GraphViewer::updateLocalPath(const std::vector<int> & localPath)
@@ -1011,6 +1026,8 @@ void GraphViewer::clearGraph()
qDeleteAll(_gpsLinkItems);
_gpsLinkItems.clear();
_root->resetTransform();
_worldMapRotation = 0.0f;
_referential->resetTransform();
_localRadius->resetTransform();
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
@@ -1439,6 +1456,20 @@ void GraphViewer::setOrientationENU(bool enabled)
_orientationENU = enabled;
this->rotate(_orientationENU?90:270);
}
if(_orientationENU)
{
QTransform t;
t.rotateRadians(_worldMapRotation);
_root->setTransform(t);
}
else
{
_root->resetTransform();
}
if(_nodeItems.size() || _linkItems.size())
{
this->scene()->setSceneRect(this->scene()->itemsBoundingRect()); // Re-shrink the scene to it's bounding contents
}
}
void GraphViewer::restoreDefaults()