i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +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/core/LaserScan.h>
#include <rtabmap/utilite/ULogger.h> #include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UConversion.h>
namespace rtabmap { 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 else // verify that format corresponds to expected number of channels
{ {
UASSERT(data.channels() != 2 || (data.channels() == 2 && format == kXY)); UASSERT_MSG(data.channels() != 2 || (data.channels() == 2 && format == kXY), uFormat("format=%d", format).c_str());
UASSERT(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI))); UASSERT_MSG(data.channels() != 3 || (data.channels() == 3 && (format == kXYZ || format == kXYI)), uFormat("format=%d", format).c_str());
UASSERT(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB))); UASSERT_MSG(data.channels() != 4 || (data.channels() == 4 && (format == kXYZI || format == kXYZRGB)), uFormat("format=%d", format).c_str());
UASSERT(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal))); UASSERT_MSG(data.channels() != 5 || (data.channels() == 5 && (format == kXYNormal)), uFormat("format=%d", format).c_str());
UASSERT(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal))); UASSERT_MSG(data.channels() != 6 || (data.channels() == 6 && (format == kXYINormal || format == kXYZNormal)), uFormat("format=%d", format).c_str());
UASSERT(data.channels() != 7 || (data.channels() == 7 && (format == kXYZRGBNormal || format == kXYZINormal))); 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::PointNormal>::Ptr assembledToNormalClouds(new pcl::PointCloud<pcl::PointNormal>);
pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr assembledToIClouds(new pcl::PointCloud<pcl::PointXYZI>);
pcl::PointCloud<pcl::PointXYZINormal>::Ptr assembledToNormalIClouds(new pcl::PointCloud<pcl::PointXYZINormal>); 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) for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
{ {
if(iter->first != fromId) if(iter->first != fromId)
@@ -2533,21 +2532,19 @@ Transform Memory::computeIcpTransformMulti(
{ {
LaserScan scan; LaserScan scan;
s->sensorData().uncompressData(0, 0, &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.hasIntensity())
{ {
if(scan.hasNormals()) if(scan.hasNormals())
{ {
*assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan, *assembledToNormalIClouds += *util3d::laserScanToPointCloudINormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform()); toPoseInv * iter->second * scan.localTransform());
} }
else else
{ {
*assembledToIClouds += *util3d::laserScanToPointCloudI(scan, *assembledToIClouds += *util3d::laserScanToPointCloudI(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform()); toPoseInv * iter->second * scan.localTransform());
} }
} }
else else
@@ -2555,12 +2552,12 @@ Transform Memory::computeIcpTransformMulti(
if(scan.hasNormals()) if(scan.hasNormals())
{ {
*assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan, *assembledToNormalClouds += *util3d::laserScanToPointCloudNormal(scan,
toPoseInv * iter->second * s->sensorData().laserScanCompressed().localTransform()); toPoseInv * iter->second * scan.localTransform());
} }
else else
{ {
*assembledToClouds += *util3d::laserScanToPointCloud(scan, *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()) 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 else
@@ -2584,27 +2581,28 @@ Transform Memory::computeIcpTransformMulti(
cv::Mat assembledScan; cv::Mat assembledScan;
if(assembledToNormalClouds->size()) if(assembledToNormalClouds->size())
{ {
assembledScan = is2D?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds); assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToNormalClouds):util3d::laserScanFromPointCloud(*assembledToNormalClouds);
} }
else if(assembledToClouds->size()) 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()) 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()) 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 // scans are in base frame but for 2d scans, set the height so that correspondences matching works
assembledData.setLaserScanRaw( assembledData.setLaserScanRaw(
LaserScan(assembledScan, LaserScan(assembledScan,
fromS->sensorData().laserScanRaw().maxPoints()?fromS->sensorData().laserScanRaw().maxPoints():maxPoints, fromScan.maxPoints()?fromScan.maxPoints():maxPoints,
fromS->sensorData().laserScanRaw().maxRange(), fromScan.maxRange(),
fromS->sensorData().laserScanRaw().format(), fromScan.format(),
is2D?Transform(0,0,fromS->sensorData().laserScanRaw().localTransform().z(),0,0,0):Transform::getIdentity())); fromScan.is2d()?Transform(0,0,fromScan.localTransform().z(),0,0,0):Transform::getIdentity()));
Transform guess = poses.at(fromId).inverse() * poses.at(toId); Transform guess = poses.at(fromId).inverse() * poses.at(toId);
t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info); t = _registrationIcp->computeTransformation(fromS->sensorData(), assembledData, guess, info);