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

Memory: pass globalPoseCovariance without inverting it

Browse Source
This commit is contained in:
TSC21
2018-12-27 15:06:29 +00:00
parent cebecc3fc3
commit 8beea1984e
1 changed files with 2 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/Memory.cpp
View File

@@ -4974,7 +4974,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{ {
if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1) if(!data.globalPose().isNull() && data.globalPoseCovariance().cols==6 && data.globalPoseCovariance().rows==6 && data.globalPoseCovariance().cols==CV_64FC1)
{ {
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()));
if(data.gps().stamp() > 0.0) if(data.gps().stamp() > 0.0)
{ {
@@ -4994,7 +4994,7 @@ 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) = 10000; gpsInfMatrix.at<double>(2,2) = 100000;
s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix)); s->addLink(Link(s->id(), s->id(), Link::kPosePrior, gpsPose, gpsInfMatrix));
} }
else else