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

Export: normal estimation can be disabled, clouds can be exported with normals

Browse Source
This commit is contained in:
matlabbe
2019-04-17 13:45:42 -04:00
parent 7acaac7a6d
commit a668ec35ab
2 changed files with 137 additions and 50 deletions
Download Patch File Download Diff File Expand all files Collapse all files

183
guilib/src/ExportCloudsDialog.cpp
View File

@@ -1617,7 +1617,10 @@ bool ExportCloudsDialog::getExportedClouds(
} }
assembledCloud->is_dense = true; assembledCloud->is_dense = true;
pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud); if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
{
pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud);
}
if(_ui->doubleSpinBox_voxelSize_assembled->value()) if(_ui->doubleSpinBox_voxelSize_assembled->value())
{ {
@@ -1643,7 +1646,8 @@ bool ExportCloudsDialog::getExportedClouds(
indices->at(i) = i; indices->at(i) = i;
} }
if(!_ui->checkBox_fromDepth->isChecked() && !has2dScans) if(!_ui->checkBox_fromDepth->isChecked() && !has2dScans &&
(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0))
{ {
// recompute normals // recompute normals
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr cloudWithoutNormals(new pcl::PointCloud<pcl::PointXYZ>);
@@ -3056,8 +3060,15 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
viewPoint[2] = data.stereoCameraModel().localTransform().z(); viewPoint[2] = data.stereoCameraModel().localTransform().z();
} }
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); {
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
}
else
{
pcl::copyPointCloud(*cloudWithoutNormals, *cloud);
}
if(_ui->checkBox_subtraction->isChecked() && if(_ui->checkBox_subtraction->isChecked() &&
_ui->doubleSpinBox_subtractPointFilteringRadius->value() > 0.0) _ui->doubleSpinBox_subtractPointFilteringRadius->value() > 0.0)
@@ -3180,8 +3191,15 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
_progressDialog->appendText(tr("Cached cloud %1 is not found in cached data, the view point for normal computation will not be set (%2/%3).").arg(iter->first).arg(index).arg(poses.size()), Qt::darkYellow); _progressDialog->appendText(tr("Cached cloud %1 is not found in cached data, the view point for normal computation will not be set (%2/%3).").arg(iter->first).arg(index).arg(poses.size()), Qt::darkYellow);
} }
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint); if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud); {
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloudWithoutNormals, indices, _ui->spinBox_normalKSearch->value(), _ui->doubleSpinBox_normalRadiusSearch->value(), viewPoint);
pcl::concatenateFields(*cloudWithoutNormals, *normals, *cloud);
}
else
{
pcl::copyPointCloud(*cloudWithoutNormals, *cloud);
}
} }
else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first)) else if(!_ui->checkBox_fromDepth->isChecked() && uContains(cachedScans, iter->first))
{ {
@@ -3285,23 +3303,45 @@ void ExportCloudsDialog::saveClouds(
{ {
if(clouds.begin()->second->size()) if(clouds.begin()->second->size())
{ {
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudI; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
if(!_ui->checkBox_fromDepth->isChecked()) if(!_ui->checkBox_fromDepth->isChecked())
{ {
// When laser scans are exported, convert RGB to Intensity // When laser scans are exported, convert RGB to Intensity
cloudI.reset(new pcl::PointCloud<pcl::PointXYZINormal>); if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
cloudI->resize(clouds.begin()->second->size());
for(unsigned int i=0; i<cloudI->size(); ++i)
{ {
cloudI->points[i].x = clouds.begin()->second->points[i].x; cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
cloudI->points[i].y = clouds.begin()->second->points[i].y; cloudIWithNormals->resize(clouds.begin()->second->size());
cloudI->points[i].z = clouds.begin()->second->points[i].z; for(unsigned int i=0; i<cloudIWithNormals->size(); ++i)
cloudI->points[i].normal_x = clouds.begin()->second->points[i].normal_x; {
cloudI->points[i].normal_y = clouds.begin()->second->points[i].normal_y; cloudIWithNormals->points[i].x = clouds.begin()->second->points[i].x;
cloudI->points[i].normal_z = clouds.begin()->second->points[i].normal_z; cloudIWithNormals->points[i].y = clouds.begin()->second->points[i].y;
cloudI->points[i].curvature = clouds.begin()->second->points[i].curvature; cloudIWithNormals->points[i].z = clouds.begin()->second->points[i].z;
cloudI->points[i].intensity = (float)clouds.begin()->second->points[i].r; cloudIWithNormals->points[i].normal_x = clouds.begin()->second->points[i].normal_x;
cloudIWithNormals->points[i].normal_y = clouds.begin()->second->points[i].normal_y;
cloudIWithNormals->points[i].normal_z = clouds.begin()->second->points[i].normal_z;
cloudIWithNormals->points[i].curvature = clouds.begin()->second->points[i].curvature;
cloudIWithNormals->points[i].intensity = (float)clouds.begin()->second->points[i].r;
}
} }
else
{
cloudIWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZI>);
cloudIWithoutNormals->resize(clouds.begin()->second->size());
for(unsigned int i=0; i<cloudIWithoutNormals->size(); ++i)
{
cloudIWithoutNormals->points[i].x = clouds.begin()->second->points[i].x;
cloudIWithoutNormals->points[i].y = clouds.begin()->second->points[i].y;
cloudIWithoutNormals->points[i].z = clouds.begin()->second->points[i].z;
cloudIWithoutNormals->points[i].intensity = (float)clouds.begin()->second->points[i].r;
}
}
}
else if(_ui->spinBox_normalKSearch->value()<=0 && _ui->doubleSpinBox_normalRadiusSearch->value()<=0.0)
{
cloudRGBWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*clouds.begin()->second, *cloudRGBWithoutNormals);
} }
_progressDialog->appendText(tr("Saving the cloud (%1 points)...").arg(clouds.begin()->second->size())); _progressDialog->appendText(tr("Saving the cloud (%1 points)...").arg(clouds.begin()->second->size()));
@@ -3309,33 +3349,42 @@ void ExportCloudsDialog::saveClouds(
bool success =false; bool success =false;
if(QFileInfo(path).suffix() == "pcd") if(QFileInfo(path).suffix() == "pcd")
{ {
if(cloudI.get()) if(cloudIWithNormals.get())
{ {
success = pcl::io::savePCDFile(path.toStdString(), *cloudI, binaryMode) == 0; success = pcl::io::savePCDFile(path.toStdString(), *cloudIWithNormals, binaryMode) == 0;
}
else if(cloudIWithoutNormals.get())
{
success = pcl::io::savePCDFile(path.toStdString(), *cloudIWithoutNormals, binaryMode) == 0;
}
else if(cloudRGBWithoutNormals.get())
{
success = pcl::io::savePCDFile(path.toStdString(), *cloudRGBWithoutNormals, binaryMode) == 0;
} }
else else
{ {
success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; success = pcl::io::savePCDFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0;
} }
} }
else if(QFileInfo(path).suffix() == "ply") else if(QFileInfo(path).suffix() == "ply" || QFileInfo(path).suffix() == "")
{ {
if(cloudI.get()) if(QFileInfo(path).suffix() == "")
{ {
success = pcl::io::savePLYFile(path.toStdString(), *cloudI, binaryMode) == 0; //use ply by default
path += ".ply";
} }
else
if(cloudIWithNormals.get())
{ {
success = pcl::io::savePLYFile(path.toStdString(), *clouds.begin()->second, binaryMode) == 0; success = pcl::io::savePLYFile(path.toStdString(), *cloudIWithNormals, binaryMode) == 0;
} }
} else if(cloudIWithoutNormals.get())
else if(QFileInfo(path).suffix() == "")
{
//use ply by default
path += ".ply";
if(cloudI.get())
{ {
success = pcl::io::savePLYFile(path.toStdString(), *cloudI, binaryMode) == 0; success = pcl::io::savePLYFile(path.toStdString(), *cloudIWithoutNormals, binaryMode) == 0;
}
else if(cloudRGBWithoutNormals.get())
{
success = pcl::io::savePLYFile(path.toStdString(), *cloudRGBWithoutNormals, binaryMode) == 0;
} }
else else
{ {
@@ -3388,32 +3437,62 @@ void ExportCloudsDialog::saveClouds(
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr transformedCloud;
transformedCloud = util3d::transformPointCloud(iter->second, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity()); transformedCloud = util3d::transformPointCloud(iter->second, !_ui->comboBox_frame->isEnabled()||_ui->comboBox_frame->currentIndex()==0?poses.at(iter->first):Transform::getIdentity());
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudI; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudRGBWithoutNormals;
pcl::PointCloud<pcl::PointXYZI>::Ptr cloudIWithoutNormals;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloudIWithNormals;
if(!_ui->checkBox_fromDepth->isChecked()) if(!_ui->checkBox_fromDepth->isChecked())
{ {
// When laser scans are exported, convert RGB to Intensity // When laser scans are exported, convert RGB to Intensity
cloudI.reset(new pcl::PointCloud<pcl::PointXYZINormal>); if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
cloudI->resize(transformedCloud->size());
for(unsigned int i=0; i<cloudI->size(); ++i)
{ {
cloudI->points[i].x = transformedCloud->points[i].x; cloudIWithNormals.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
cloudI->points[i].y = transformedCloud->points[i].y; cloudIWithNormals->resize(transformedCloud->size());
cloudI->points[i].z = transformedCloud->points[i].z; for(unsigned int i=0; i<cloudIWithNormals->size(); ++i)
cloudI->points[i].normal_x = transformedCloud->points[i].normal_x; {
cloudI->points[i].normal_y = transformedCloud->points[i].normal_y; cloudIWithNormals->points[i].x = transformedCloud->points[i].x;
cloudI->points[i].normal_z = transformedCloud->points[i].normal_z; cloudIWithNormals->points[i].y = transformedCloud->points[i].y;
cloudI->points[i].curvature = transformedCloud->points[i].curvature; cloudIWithNormals->points[i].z = transformedCloud->points[i].z;
cloudI->points[i].intensity = (float)transformedCloud->points[i].r; cloudIWithNormals->points[i].normal_x = transformedCloud->points[i].normal_x;
cloudIWithNormals->points[i].normal_y = transformedCloud->points[i].normal_y;
cloudIWithNormals->points[i].normal_z = transformedCloud->points[i].normal_z;
cloudIWithNormals->points[i].curvature = transformedCloud->points[i].curvature;
cloudIWithNormals->points[i].intensity = (float)transformedCloud->points[i].r;
}
} }
else
{
cloudIWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZI>);
cloudIWithoutNormals->resize(transformedCloud->size());
for(unsigned int i=0; i<cloudIWithoutNormals->size(); ++i)
{
cloudIWithoutNormals->points[i].x = transformedCloud->points[i].x;
cloudIWithoutNormals->points[i].y = transformedCloud->points[i].y;
cloudIWithoutNormals->points[i].z = transformedCloud->points[i].z;
cloudIWithoutNormals->points[i].intensity = (float)transformedCloud->points[i].r;
}
}
}
else if(_ui->spinBox_normalKSearch->value()<=0 && _ui->doubleSpinBox_normalRadiusSearch->value()<=0.0)
{
cloudRGBWithoutNormals.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*transformedCloud, *cloudRGBWithoutNormals);
} }
QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix); QString pathFile = path+QDir::separator()+QString("%1%2.%3").arg(prefix).arg(iter->first).arg(suffix);
bool success =false; bool success =false;
if(suffix == "pcd") if(suffix == "pcd")
{ {
if(cloudI.get()) if(cloudIWithNormals.get())
{ {
success = pcl::io::savePCDFile(pathFile.toStdString(), *cloudI, binaryMode) == 0; success = pcl::io::savePCDFile(pathFile.toStdString(), *cloudIWithNormals, binaryMode) == 0;
}
else if(cloudIWithoutNormals.get())
{
success = pcl::io::savePCDFile(pathFile.toStdString(), *cloudIWithoutNormals, binaryMode) == 0;
}
else if(cloudRGBWithoutNormals.get())
{
success = pcl::io::savePCDFile(pathFile.toStdString(), *cloudRGBWithoutNormals, binaryMode) == 0;
} }
else else
{ {
@@ -3422,9 +3501,17 @@ void ExportCloudsDialog::saveClouds(
} }
else if(suffix == "ply") else if(suffix == "ply")
{ {
if(cloudI.get()) if(cloudIWithNormals.get())
{ {
success = pcl::io::savePLYFile(pathFile.toStdString(), *cloudI, binaryMode) == 0; success = pcl::io::savePLYFile(pathFile.toStdString(), *cloudIWithNormals, binaryMode) == 0;
}
else if(cloudIWithoutNormals.get())
{
success = pcl::io::savePLYFile(pathFile.toStdString(), *cloudIWithoutNormals, binaryMode) == 0;
}
else if(cloudRGBWithoutNormals.get())
{
success = pcl::io::savePLYFile(pathFile.toStdString(), *cloudRGBWithoutNormals, binaryMode) == 0;
} }
else else
{ {

4
guilib/src/ui/exportCloudsDialog.ui
View File

@@ -126,7 +126,7 @@
<item row="6" column="0"> <item row="6" column="0">
<widget class="QSpinBox" name="spinBox_normalKSearch"> <widget class="QSpinBox" name="spinBox_normalKSearch">
<property name="minimum"> <property name="minimum">
<number>3</number> <number>0</number>
</property> </property>
<property name="value"> <property name="value">
<number>20</number> <number>20</number>
@@ -216,7 +216,7 @@
<item row="6" column="1"> <item row="6" column="1">
<widget class="QLabel" name="label_normal"> <widget class="QLabel" name="label_normal">
<property name="text"> <property name="text">
<string>Set the number of k nearest neighbors to use for the normal estimation.</string> <string>Set the number of k nearest neighbors to use for the normal estimation. Set 0 to disable.</string>
</property> </property>
<property name="wordWrap"> <property name="wordWrap">
<bool>true</bool> <bool>true</bool>