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

Fixed 2d multi-scan matching fatal error

Browse Source
This commit is contained in:
matlabbe
2018-02-16 20:23:42 -05:00
parent 6e131dcd7e
commit a947f8c783
2 changed files with 22 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
corelib/src/LaserScan.cpp
View File

@@ -27,6 +27,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/core/LaserScan.h>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap {
@@ -139,12 +140,12 @@ LaserScan::LaserScan(const cv::Mat & data, int maxPoints, float maxRange, Format
}
else // verify that format corresponds to expected number of channels
{
UASSERT(data.channels() != 2 || (data.channels() == 2 && format == kXY));
UASSERT(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)));
UASSERT(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)));
UASSERT(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)));
UASSERT(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)));
UASSERT(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)));
UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
UASSERT_MSG(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal)), uFormat("format=%d", format).c_str());
}
}
}

32
corelib/src/Memory.cpp
View File

@@ -2523,7 +2523,6 @@ Transform Memory::computeIcpTransformMulti(
pcl::PointCloud<pcl::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>);
bool is2D = true;
for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{
if(iter->first != fromId)
@@ -2533,21 +2532,19 @@ Transform Memory::computeIcpTransformMulti(
{
LaserScan scan;
s->sensorData().uncompressData(0, 0, &scan);
if(!scan.isEmpty() && scan.format() == fromS->sensorData().laserScanRaw().format())
if(!scan.isEmpty() && scan.format() == fromScan.format())
{
is2D = !scan.is2d();
if(scan.hasIntensity())
{
if(scan.hasNormals())
{
*assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
toPoseInv * iter->second * scan.localTransform());
}
}
else
@@ -2555,12 +2552,12 @@ Transform Memory::computeIcpTransformMulti(
if(scan.hasNormals())
{
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToClouds += *util3d::laserScanToPointCloud(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform());
toPoseInv * iter->second * scan.localTransform());
}
}
@@ -2571,7 +2568,7 @@ Transform Memory::computeIcpTransformMulti(
}
else if(!scan.isEmpty())
{
UWARN("Incompatible scan format %d vs %d", (int)fromS->sensorData().laserScanRaw().format(), (int)scan.format());
UWARN("Incompatible scan format %d vs %d", (int)fromScan.format(), (int)scan.format());
}
}
else
@@ -2584,27 +2581,28 @@ Transform Memory::computeIcpTransformMulti(
cv::Mat assembledScan;
if(assembledToNormalClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
}
else if(assembledToClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToClouds):util3d::laserScanFromPointCloud(*assembledToClouds);
}
else if(assembledToNormalIClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalIClouds):util3d::laserScanFromPointCloud(*assembledToNormalIClouds);
}
else if(assembledToIClouds->size())
{
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
}
// scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw(
LaserScan(assembledScan,
fromS->sensorData().laserScanRaw().maxPoints()?fromS->sensorData().laserScanRaw().maxPoints():maxPoints,
fromS->sensorData().laserScanRaw().maxRange(),
fromS->sensorData().laserScanRaw().format(),
is2D?Transform(0,0,fromS->sensorData().laserScanRaw().localTransform().z(),0,0,0):Transform::getIdentity()));
fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
fromScan.maxRange(),
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 = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);