i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

util3d: Fixed color not copied on laser scan conversion

Browse Source
This commit is contained in:
matlabbe
2017-09-18 17:00:15 -04:00
parent 2aa56c8d49
commit a70996f079
3 changed files with 27 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/util3d_transforms.cpp
+5 -1
View File
@@ -193,7 +193,9 @@ pcl::PointXYZRGB transformPoint(
const pcl::PointXYZRGB & pt, const pcl::PointXYZRGB & pt,
const Transform & transform) const Transform & transform)
{ {
return pcl::transformPoint(pt, transform.toEigen3f()); pcl::PointXYZRGB ptRGB = pcl::transformPoint(pt, transform.toEigen3f());
ptRGB.rgb = pt.rgb;
return ptRGB;
} }
pcl::PointNormal transformPoint( pcl::PointNormal transformPoint(
const pcl::PointNormal & point, const pcl::PointNormal & point,
@@ -227,6 +229,8 @@ pcl::PointXYZRGBNormal transformPoint(
ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2)); ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2)); ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2)); ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
ret.rgb = point.rgb;
return ret; return ret;
} }
guilib/src/DatabaseViewer.cpp
+19 -1
View File
@@ -2698,7 +2698,16 @@ void DatabaseViewer::update(int value,
//add scan //add scan
if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols) if(ui_->checkBox_showScan->isChecked() && data.laserScanRaw().cols)
{ {
if(data.laserScanRaw().channels() == 6) if(data.laserScanRaw().channels() == 7)
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr scan = util3d::laserScanToPointCloudRGBNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
}
else if(data.laserScanRaw().channels() == 6)
{ {
pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform()); pcl::PointCloud<pcl::PointNormal>::Ptr scan = util3d::laserScanToPointCloudNormal(data.laserScanRaw(), data.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0) if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
@@ -2707,6 +2716,15 @@ void DatabaseViewer::update(int value,
} }
cloudViewer_->addCloud("scan", scan, pose, Qt::yellow); cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
} }
else if(data.laserScanRaw().channels() == 4)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr scan = util3d::laserScanToPointCloudRGB(data.laserScanRaw(), data.laserScanInfo().localTransform());
if(ui_->doubleSpinBox_voxelSize->value() > 0.0)
{
scan = util3d::voxelize(scan, ui_->doubleSpinBox_voxelSize->value());
}
cloudViewer_->addCloud("scan", scan, pose, Qt::yellow);
}
else else
{ {
pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform()); pcl::PointCloud<pcl::PointXYZ>::Ptr scan = util3d::laserScanToPointCloud(data.laserScanRaw(), data.laserScanInfo().localTransform());
guilib/src/MainWindow.cpp
+3 -3
View File
@@ -2920,7 +2920,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
} }
// Do ceiling/floor filtering // Do ceiling/floor filtering
if(scan.channels() > 2 && // don't filter 2D scans if((scan.channels() > 2 && scan.channels() != 5) && // don't filter 2D scans
(_preferencesDialog->getScanFloorFilteringHeight() != 0.0 || (_preferencesDialog->getScanFloorFilteringHeight() != 0.0 ||
_preferencesDialog->getScanCeilingFilteringHeight() != 0.0)) _preferencesDialog->getScanCeilingFilteringHeight() != 0.0))
{ {
@@ -2987,7 +2987,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
iter->sensorData().laserScanInfo().localTransform().z()); iter->sensorData().laserScanInfo().localTransform().z());
pcl::PointCloud<pcl::Normal>::Ptr normals; pcl::PointCloud<pcl::Normal>::Ptr normals;
if(cloud->size()) if(cloud.get() && cloud->size())
{ {
if(scan.channels() == 2 || scan.channels() == 5) if(scan.channels() == 2 || scan.channels() == 5)
{ {
@@ -3003,7 +3003,7 @@ void MainWindow::createAndAddScanToMap(int nodeId, const Transform & pose, int m
} }
else else
{ {
UASSERT(cloudRGB->size()); // Assuming 4 channels cannot be 2D UASSERT(cloudRGB.get() && cloudRGB->size()); // Assuming 4 channels cannot be 2D
normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint); normals = util3d::computeNormals(cloudRGB, _preferencesDialog->getScanNormalKSearch(), _preferencesDialog->getScanNormalRadiusSearch(), scanViewpoint);
cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>); cloudRGBWithNormals.reset(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals); pcl::concatenateFields(*cloudRGB, *normals, *cloudRGBWithNormals);