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::computeIcpTransformMulti() added support for 3d scans with RGB channel (L515)

Browse Source
This commit is contained in:
matlabbe
2020-07-08 13:43:28 -04:00
parent 4a09c4bdcf
commit 169b79215d
1 changed files with 38 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

39
corelib/src/Memory.cpp
View File

@@ -3086,6 +3086,8 @@ 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>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr assembledToRGBClouds(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr assembledToNormalRGBClouds(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints); UDEBUG("maxPoints from(%d) = %d", fromId, maxPoints);
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)
{ {
@@ -3111,6 +3113,19 @@ Transform Memory::computeIcpTransformMulti(
toPoseInv * iter->second * scan.localTransform()); toPoseInv * iter->second * scan.localTransform());
} }
} }
else if(scan.hasRGB())
{
if(scan.hasNormals())
{
*assembledToNormalRGBClouds += *util3d::laserScanToPointCloudRGBNormal(scan,
toPoseInv * iter->second * scan.localTransform());
}
else
{
*assembledToRGBClouds += *util3d::laserScanToPointCloudRGB(scan,
toPoseInv * iter->second * scan.localTransform());
}
}
else else
{ {
if(scan.hasNormals()) if(scan.hasNormals())
@@ -3160,6 +3175,28 @@ Transform Memory::computeIcpTransformMulti(
{ {
assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds); assembledScan = fromScan.is2d()?util3d::laserScan2dFromPointCloud(*assembledToIClouds):util3d::laserScanFromPointCloud(*assembledToIClouds);
} }
else if(assembledToNormalRGBClouds->size())
{
if(fromScan.is2d())
{
UERROR("Cannot handle 2d scan with RGB format.");
}
else
{
assembledScan = util3d::laserScanFromPointCloud(*assembledToNormalRGBClouds);
}
}
else if(assembledToRGBClouds->size())
{
if(fromScan.is2d())
{
UERROR("Cannot handle 2d scan with RGB format.");
}
else
{
assembledScan = util3d::laserScanFromPointCloud(*assembledToRGBClouds);
}
}
UDEBUG("assembledScan=%d points", assembledScan.cols); UDEBUG("assembledScan=%d points", assembledScan.cols);
// 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
@@ -4439,7 +4476,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
keypoints3D.resize(keypoints.size()); keypoints3D.resize(keypoints.size());
for(size_t i=0; i<keypoints.size(); ++i) for(size_t i=0; i<keypoints.size(); ++i)
{ {
UASSERT(keypoints[i].class_id < data.keypoints3D().size()); UASSERT(keypoints[i].class_id < (int)data.keypoints3D().size());
keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id]; keypoints3D[i] = data.keypoints3D()[keypoints[i].class_id];
} }
} }