i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-10 13:30:20 +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
corelib/include/rtabmap/core/GeodeticCoords.h
+4
View File
@@ -70,6 +70,10 @@ public:
void fromENU_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
static cv::Point3d ENU_WGS84ToGeocentric_WGS84(const cv::Point3d & enu, const GeodeticCoords & origin);
static cv::Point3d Geocentric_WGS84ToENU_WGS84(
const cv::Point3d & geocentric_WGS84,
const cv::Point3d & origin_geocentric_WGS84,
const GeodeticCoords & origin);
private:
double latitude_; // deg
corelib/include/rtabmap/core/Memory.h
+2
View File
@@ -161,6 +161,7 @@ public:
int getSignatureIdByLabel(const std::string & label, bool lookInDatabase = true) const;
bool labelSignature(int id, const std::string & label);
std::map<int, std::string> getAllLabels() const;
/**
* Set user data. Detect automatically if raw or compressed. If raw, the data is
* compressed too. A matrix of type CV_8UC1 with 1 row is considered as compressed.
@@ -175,6 +176,7 @@ public:
double getDbSavingTime() const;
Transform getOdomPose(int signatureId, bool lookInDatabase = false) const;
Transform getGroundTruthPose(int signatureId, bool lookInDatabase = false) const;
void getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth = 0) const;
bool getNodeInfo(int signatureId,
Transform & odomPose,
int & mapId,
corelib/include/rtabmap/core/Rtabmap.h
+2
View File
@@ -292,6 +292,8 @@ private:
Transform _mapCorrectionBackup; // used in localization mode when odom is lost
Transform _lastLocalizationPose; // Corrected odometry pose. In mapping mode, this corresponds to last pose return by getLocalOptimizedPoses().
int _lastLocalizationNodeId; // for localization mode
std::map<int, std::pair<cv::Point3d, Transform> > _gpsGeocentricCache;
bool _currentSessionHasGPS;
// Planning stuff
int _pathStatus;
corelib/src/GeodeticCoords.cpp
+18 -9
View File
@@ -106,6 +106,20 @@ cv::Point3d GeodeticCoords::toGeocentric_WGS84() const
geodeticToENU_WGS84
---------------------------------------------------------------*/
cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
{
// Generate 3D point:
cv::Point3d P_geocentric = this->toGeocentric_WGS84();
// Generate reference 3D point:
cv::Point3d P_geocentric_ref = origin.toGeocentric_WGS84();
return Geocentric_WGS84ToENU_WGS84(P_geocentric, P_geocentric_ref, origin);
}
cv::Point3d GeodeticCoords::Geocentric_WGS84ToENU_WGS84(
const cv::Point3d & geocentric_WGS84,
const cv::Point3d & origin_geocentric_WGS84,
const GeodeticCoords & origin)
{
// --------------------------------------------------------------------
// Explanation: We compute the earth-centric coordinates first,
@@ -116,25 +130,20 @@ cv::Point3d GeodeticCoords::toENU_WGS84(const GeodeticCoords &origin) const
// (JLBC 21/DEC/2006) (Fixed: JLBC 9/JUL/2008)
// - Oct/2013, Emilio Sanjurjo: Fixed UP vector pointing exactly normal to ellipsoid surface.
// --------------------------------------------------------------------
// Generate 3D point:
cv::Point3d P_geocentric = this->toGeocentric_WGS84();
// Generate reference 3D point:
cv::Point3d P_geocentric_ref = origin.toGeocentric_WGS84();
const double clat = cos(DEG2RAD(origin.latitude())), slat = sin(DEG2RAD(origin.latitude()));
const double clon = cos(DEG2RAD(origin.longitude())), slon = sin(DEG2RAD(origin.longitude()));
// Compute the resulting relative coordinates:
// For using smaller numbers:
P_geocentric -= P_geocentric_ref;
cv::Point3d geocentric_WGS84_rel = geocentric_WGS84-origin_geocentric_WGS84;
// Optimized calculation: Local transformed coordinates of P_geo(x,y,z)
// after rotation given by the transposed rotation matrix from ENU -> ECEF.
cv::Point3d out;
out.x = -slon*P_geocentric.x + clon*P_geocentric.y;
out.y = -clon*slat*P_geocentric.x -slon*slat*P_geocentric.y + clat*P_geocentric.z;
out.z = clon*clat*P_geocentric.x + slon*clat*P_geocentric.y +slat*P_geocentric.z;
out.x = -slon*geocentric_WGS84_rel.x + clon*geocentric_WGS84_rel.y;
out.y = -clon*slat*geocentric_WGS84_rel.x -slon*slat*geocentric_WGS84_rel.y + clat*geocentric_WGS84_rel.z;
out.z = clon*clat*geocentric_WGS84_rel.x + slon*clat*geocentric_WGS84_rel.y +slat*geocentric_WGS84_rel.z;
return out;
}
corelib/src/Memory.cpp
+47
View File
@@ -3446,6 +3446,53 @@ Transform Memory::getGroundTruthPose(int signatureId, bool lookInDatabase) const
return groundTruth;
}
void Memory::getGPS(int id, GPS & gps, Transform & offsetENU, bool lookInDatabase, int maxGraphDepth) const
{
gps = GPS();
offsetENU=Transform::getIdentity();
Transform odomPose, groundTruth;
int mapId, weight;
std::string label;
double stamp;
std::vector<float> velocity;
getNodeInfo(id, odomPose, mapId, weight, label, stamp, groundTruth, velocity, gps, lookInDatabase);
if(gps.stamp() == 0.0)
{
// Search for the nearest node with GPS, and compute its relative transform in ENU coordinates
std::map<int, int> nearestIds;
nearestIds = getNeighborsId(id, maxGraphDepth, lookInDatabase?-1:0, true, false, true);
std::multimap<int, int> nearestIdsSorted;
for(std::map<int, int>::iterator iter=nearestIds.begin(); iter!=nearestIds.end(); ++iter)
{
nearestIdsSorted.insert(std::make_pair(iter->second, iter->first));
}
for(std::map<int, int>::iterator iter=nearestIdsSorted.begin(); iter!=nearestIdsSorted.end(); ++iter)
{
const Signature * s = getSignature(iter->second);
UASSERT(s!=0);
if(s->sensorData().gps().stamp() > 0.0)
{
std::list<std::pair<int, Transform> > path = graph::computePath(s->id(), id, this, lookInDatabase);
if(path.size() >= 2)
{
gps = s->sensorData().gps();
Transform localToENU(0,0,(float)((-(gps.bearing()-90))*M_PI/180.0) - s->getPose().theta());
offsetENU = localToENU*(s->getPose().rotation()*path.rbegin()->second);
break;
}
else
{
UWARN("Failed to find path %d -> %d", s->id(), id);
}
}
}
}
}
bool Memory::getNodeInfo(int signatureId,
Transform & odomPose,
int & mapId,
corelib/src/Rtabmap.cpp
+95 -1
View File
@@ -138,6 +138,7 @@ Rtabmap::Rtabmap() :
_wDir(""),
_mapCorrection(Transform::getIdentity()),
_lastLocalizationNodeId(0),
_currentSessionHasGPS(false),
_pathStatus(0),
_pathCurrentIndex(0),
_pathGoalIndex(0),
@@ -360,6 +361,8 @@ void Rtabmap::close(bool databaseSaved, const std::string & ouputDatabasePath)
_lastLocalizationNodeId = 0;
_distanceTravelled = 0.0f;
this->clearPath(0);
_gpsGeocentricCache.clear();
_currentSessionHasGPS = false;
flushStatisticLogs();
if(_foutFloat)
@@ -1068,6 +1071,7 @@ bool Rtabmap::process(
}
signature = _memory->getLastWorkingSignature();
_currentSessionHasGPS = _currentSessionHasGPS || signature->sensorData().gps().stamp() > 0.0;
if(!signature)
{
UFATAL("Not supposed to be here...last signature is null?!?");
@@ -1395,6 +1399,39 @@ bool Rtabmap::process(
ULOGGER_INFO("computing likelihood...");
std::list<int> signaturesToCompare;
GPS originGPS = signature->sensorData().gps();
Transform originOffsetENU = Transform::getIdentity();
if(originGPS.stamp() == 0.0 && _currentSessionHasGPS)
{
UTimer tmpT;
if(_optimizedPoses.size() && _memory->isIncremental())
{
//Search for latest node having GPS linked to current signature not too far.
std::map<int, float> nearestIds = graph::getNodesInRadius(signature->id(), _optimizedPoses, _localRadius);
for(std::map<int, float>::reverse_iterator iter=nearestIds.rbegin(); iter!=nearestIds.rend(); ++iter)
{
const Signature * s = _memory->getSignature(iter->first);
UASSERT(s!=0);
if(s->sensorData().gps().stamp() > 0.0)
{
originGPS = s->sensorData().gps();
const Transform & sPose = _optimizedPoses.at(s->id());
Transform localToENU(0,0,(float)((-(originGPS.bearing()-90))*M_PI/180.0) - sPose.theta());
originOffsetENU = localToENU * (sPose.rotation()*(sPose.inverse()*_optimizedPoses.at(signature->id())));
break;
}
}
}
//else if(!_memory->isIncremental()) // TODO, how can we estimate current GPS position in localization?
//{
//}
}
if(originGPS.stamp() > 0.0)
{
// no need to save it if it is in localization mode
_gpsGeocentricCache.insert(std::make_pair(signature->id(), std::make_pair(originGPS.toGeodeticCoords().toGeocentric_WGS84(), originOffsetENU)));
}
for(std::map<int, double>::const_iterator iter=_memory->getWorkingMem().begin();
iter!=_memory->getWorkingMem().end();
++iter)
@@ -1405,7 +1442,58 @@ bool Rtabmap::process(
UASSERT(s!=0);
if(s->getWeight() != -1) // ignore intermediate nodes
{
signaturesToCompare.push_back(iter->first);
bool accept = true;
if(originGPS.stamp()>0.0)
{
std::map<int, std::pair<cv::Point3d, Transform> >::iterator cacheIter = _gpsGeocentricCache.find(s->id());
if(cacheIter == _gpsGeocentricCache.end())
{
GPS gps = s->sensorData().gps();
Transform offsetENU = Transform::getIdentity();
if(gps.stamp()==0.0)
{
_memory->getGPS(s->id(), gps, offsetENU, false);
}
if(gps.stamp() > 0.0)
{
cacheIter = _gpsGeocentricCache.insert(
std::make_pair(s->id(),
std::make_pair(gps.toGeodeticCoords().toGeocentric_WGS84(), offsetENU))).first;
}
}
if(cacheIter != _gpsGeocentricCache.end())
{
std::map<int, std::pair<cv::Point3d, Transform> >::iterator originIter = _gpsGeocentricCache.find(signature->id());
UASSERT(originIter != _gpsGeocentricCache.end());
cv::Point3d relativePose = GeodeticCoords::Geocentric_WGS84ToENU_WGS84(cacheIter->second.first, originIter->second.first, originGPS.toGeodeticCoords());
const double & error = originGPS.error();
const Transform & offsetENU = cacheIter->second.second;
relativePose.x += offsetENU.x() - originOffsetENU.x();
relativePose.y += offsetENU.y() - originOffsetENU.y();
relativePose.z += offsetENU.z() - originOffsetENU.z();
// ignore altitude if difference is under GPS error
if(relativePose.z>error)
{
relativePose.z -= error;
}
else if(relativePose.z < -error)
{
relativePose.z += error;
}
else
{
relativePose.z = 0;
}
accept = uNormSquared(relativePose.x, relativePose.y, relativePose.z) < _localRadius*_localRadius;
}
}
if(accept)
{
signaturesToCompare.push_back(iter->first);
}
}
}
else
@@ -2684,6 +2772,12 @@ bool Rtabmap::process(
}
_lastProcessTime = totalTime;
// cleanup cached gps values
for(std::list<int>::iterator iter=signaturesRemoved.begin(); iter!=signaturesRemoved.end() && _gpsGeocentricCache.size(); ++iter)
{
_gpsGeocentricCache.erase(*iter);
}
//Remove optimized poses from signatures transferred
if(signaturesRemoved.size() && (_optimizedPoses.size() || _constraints.size()))
{
guilib/include/rtabmap/gui/GraphViewer.h
+5
View File
@@ -56,6 +56,9 @@ public:
GraphViewer(QWidget * parent = 0);
virtual ~GraphViewer();
void setWorldMapRotation(const float & theta);
float getWorldMapRotation() const {return _worldMapRotation;}
void updateGraph(const std::map<int, Transform> & poses,
const std::multimap<int, Link> & constraints,
const std::map<int, int> & mapIds);
@@ -174,6 +177,8 @@ private:
QColor _loopIntraSessionColor;
QColor _loopInterSessionColor;
bool _intraInterSessionColors;
float _worldMapRotation;
QGraphicsItem * _world;
QGraphicsItem * _root;
QGraphicsItem * _graphRoot;
QGraphicsItem * _globalPathRoot;
guilib/src/DatabaseViewer.cpp
+1 -1
View File
@@ -1614,7 +1614,7 @@ void DatabaseViewer::updateIds()
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));
Transform pose(p.x, p.y, p.z, 0.0f, 0.0f, (float)((-(gps.bearing()-90))*M_PI/180.0));
gpsPoses_.insert(std::make_pair(ids_[i], pose));
}
}
guilib/src/GraphViewer.cpp
+33 -2
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()
guilib/src/MainWindow.cpp
+8
View File
@@ -1883,6 +1883,14 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
labels.insert(std::make_pair(iter->first, iter->second.getLabel()));
}
if(_ui->graphicsView_graphView->getWorldMapRotation()==0.0f &&
iter->second.sensorData().gps().stamp()!=0.0 &&
stat.poses().find(iter->first)!=stat.poses().end())
{
float bearing = (float)((-(iter->second.sensorData().gps().bearing()-90))*M_PI/180.0);
float gpsRotationOffset = stat.poses().at(iter->first).theta()-bearing;
_ui->graphicsView_graphView->setWorldMapRotation(gpsRotationOffset);
}
}
std::map<int, Transform> poses = stat.poses();