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

Memory: readd GPS data as priors

Browse Source
This commit is contained in:
TSC21
2018-12-26 15:37:22 +00:00
parent 6bd9dd55b7
commit 97f956f138
1 changed files with 5 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
corelib/src/Memory.cpp
View File

@@ -4976,15 +4976,14 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{ {
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv())); s->addLink(Link(s->id(), s->id(), Link::kPosePrior, data.globalPose(), data.globalPoseCovariance().inv()));
/*if(data.gps().stamp() > 0.0) if(data.gps().stamp() > 0.0)
{ {
UWARN("GPS constraint ignored as global pose is also set."); UWARN("GPS constraint ignored as global pose is also set.");
}*/ }
} }
else if(data.gps().stamp() > 0.0) else if(data.gps().stamp() > 0.0)
{ {
// TODO: What kind of covariance should we set to have decent gtsam and g2o results!? if(_gpsOrigin.stamp() <= 0.0)
/*if(_gpsOrigin.stamp() <= 0.0)
{ {
_gpsOrigin = data.gps(); _gpsOrigin = data.gps();
} }
@@ -4995,13 +4994,13 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{ {
// only set x, y as we don't know variance for other degrees of freedom. // only set x, y as we don't know variance for other degrees of freedom.
gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1; gpsInfMatrix.at<double>(0,0) = gpsInfMatrix.at<double>(1,1) = 0.1;
gpsInfMatrix.at<double>(2,2) = 100000; gpsInfMatrix.at<double>(2,2) = 10000;
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix)); s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
} }
else else
{ {
UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error()); UERROR("Invalid GPS error value (%f m), must be > 0 m.", data.gps().error());
}*/ }
} }
} }