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: 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

44
corelib/src/Memory.cpp
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,

13
corelib/src/RegistrationIcp.cpp
View File

@@ -604,7 +604,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
catch(const std::exception & e)
{
UWARN("libpointmatcher has failed: %s", e.what());
msg = uFormat("libpointmatcher has failed: %s", e.what());
}
}
else
@@ -827,7 +827,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
catch(const std::exception & e)
{
UWARN("libpointmatcher has failed: %s", e.what());
msg = uFormat("libpointmatcher has failed: %s", e.what());
}
}
else
@@ -965,7 +965,7 @@ Transform RegistrationIcp::computeTransformationImpl(
}
catch(const std::exception & e)
{
UWARN("libpointmatcher has failed: %s", e.what());
msg = uFormat("libpointmatcher has failed: %s", e.what());
}
}
else
@@ -1112,8 +1112,11 @@ Transform RegistrationIcp::computeTransformationImpl(
}
else
{
msg = uFormat("Cannot compute transform (converged=%s var=%f)",
hasConverged?"true":"false", variance);
if(msg.empty())
{
msg = uFormat("Cannot compute transform (converged=%s var=%f)",
hasConverged?"true":"false", variance);
}
UINFO(msg.c_str());
}
}

36
corelib/src/util3d_filtering.cpp
View File

@@ -99,23 +99,27 @@ LaserScan commonFiltering(
for(int i=0; i<scan.size()-downsamplingStep+1; i+=downsamplingStep)
{
const float * ptr = scan.data().ptr<float>(0, i);
float r;
if(is2d)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
}
if(rangeMin > 0.0f && r < rangeMinSqrd)
if(rangeMin>0.0f || rangeMax>0.0f)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
float r;
if(is2d)
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1];
}
else
{
r = ptr[0]*ptr[0] + ptr[1]*ptr[1] + ptr[2]*ptr[2];
}
if(rangeMin > 0.0f && r < rangeMinSqrd)
{
continue;
}
if(rangeMax > 0.0f && r > rangeMaxSqrd)
{
continue;
}
}
cv::Mat(scan.data(), cv::Range::all(), cv::Range(i,i+1)).copyTo(cv::Mat(tmp, cv::Range::all(), cv::Range(oi,oi+1)));
@@ -123,7 +127,7 @@ LaserScan commonFiltering(
}
int previousSize = scan.size();
int scanMaxPtsTmp = scan.maxPoints();
scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, scan.maxRange(), scan.format(), scan.localTransform());
scan = LaserScan(cv::Mat(tmp, cv::Range::all(), cv::Range(0, oi)), scanMaxPtsTmp/downsamplingStep, rangeMax>0.0f&&rangeMax<scan.maxRange()?rangeMax:scan.maxRange(), scan.format(), scan.localTransform());
UDEBUG("Downsampling scan (step=%d): %d -> %d (scanMaxPts=%d->%d)", downsamplingStep, previousSize, scan.size(), scanMaxPtsTmp, scan.maxPoints());
}