i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-10-06 01:57:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Memory: compute scan max range if not set on signature creation.

Browse Source
This commit is contained in:
matlabbe
2018-02-23 15:52:59 -05:00
parent 215eff3212
commit 39b363d0b5
3 changed files with 70 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+42 -2
View File
@@ -2485,9 +2485,23 @@ Transform Memory::computeIcpTransformMulti(
LaserScan fromScan;
fromS->sensorData().uncompressData(0, 0, &fromScan);
Signature * toS = _getSignature(toId);
LaserScan toScan;
toS->sensorData().uncompressData(0, 0, &toScan);
Transform t;
if(!fromScan.isEmpty())
if(!fromScan.isEmpty() && !toScan.isEmpty())
{
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
float guessNorm = guess.getNorm();
if(fromScan.maxRange() > 0.0f && toScan.maxRange() > 0.0f &&
guessNorm > fromScan.maxRange() + toScan.maxRange())
{
// stop right known,it is impossible that scans overlay.
UINFO("Too far scans between %d and %d to compute transformation: guessNorm=%f, scan range from=%f to=%f", fromId, toId, guessNorm, fromScan.maxRange(), toScan.maxRange());
return t;
}
// Create a fake signature with all scans merged in oldId referential
SensorData assembledData;
Transform toPoseInv = poses.at(toId).inverse();
@@ -2578,7 +2592,6 @@ Transform Memory::computeIcpTransformMulti(
fromScan.format(),
fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId);
t = _registrationIcpMulti->computeTransformation(fromS->sensorData(), assembledData, guess, info);
}
@@ -3825,6 +3838,33 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
LaserScan laserScan = data.laserScanRaw();
if(!isIntermediateNode && laserScan.size())
{
if(laserScan.maxRange() == 0.0f)
{
bool id2d = laserScan.is2d();
float maxRange = 0.0f;
for(int i=0; i<laserScan.size(); ++i)
{
const float * ptr = laserScan.data().ptr<float>(0, i);
float r;
if(id2d)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
}
if(r>maxRange)
{
maxRange = r;
}
}
if(maxRange > 0.0f)
{
laserScan=LaserScan(laserScan.data(), laserScan.maxPoints(), sqrt(maxRange), laserScan.format(), laserScan.localTransform());
}
}
laserScan = util3d::commonFiltering(laserScan,
_laserScanDownsampleStepSize,
0,