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

Added cloud smoothing (MLS) to save clouds

Browse Source 
Modified saving questions to yes for merging clouds before saving or meshing

git-svn-id: http://rtabmap.googlecode.com/svn/trunk/rtabmap@1335 f169173b-cf89-36c8-b27e-44dbe73f0c83
This commit is contained in:
matlabbe
2014-06-08 05:16:14 +00:00
parent df0f9d60d5
commit 4fa947b7a6
5 changed files with 106 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/src/Memory.cpp
+6 -4
View File
@@ -1777,10 +1777,10 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
UWARN("Clouds empty ?!?"); UWARN("Clouds empty ?!?");
} }
//pcl::io::savePCDFile("old.pcd", *oldCloud); //pcl::io::savePCDFile("old.pcd", *oldCloudXYZ);
//pcl::io::savePCDFile("newguess.pcd", *newCloud); //pcl::io::savePCDFile("newguess.pcd", *newCloudXYZ);
//newCloud = util3d::transformPointCloud(newCloud, icpT); //newCloudXYZ = util3d::transformPointCloud(newCloudXYZ, icpT);
//pcl::io::savePCDFile("newicp.pcd", *newCloud); //pcl::io::savePCDFile("newicp.pcd", *newCloudXYZ);
UDEBUG("fitness=%f", fitness); UDEBUG("fitness=%f", fitness);
@@ -1886,6 +1886,8 @@ Transform Memory::computeIcpTransform(const Signature & oldS, const Signature &
UERROR("Depths 2D empty?!?"); UERROR("Depths 2D empty?!?");
} }
} }
UDEBUG("New transform = %s", transform.prettyPrint().c_str());
return transform; return transform;
} }
corelib/src/Odometry.cpp
+4
View File
@@ -493,6 +493,10 @@ Transform OdometryBOW::computeTransform(Image & image, int * quality)
transform.setNull(); transform.setNull();
UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences); UWARN("Transform not valid (inliers = %d/%d)", inliers, correspondences);
} }
//else if(!transform.isNull() && true)
//{
// transform = _memory->computeIcpTransform(*newSignature, *previousSignature, transform, true);
//}
} }
else else
{ {
guilib/src/MainWindow.cpp
+90 -12
View File
@@ -1125,9 +1125,19 @@ void MainWindow::updateMapCloud(const std::map<int, Transform> & posesIn, const
} }
} }
else if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, iter->second)) else
{ {
UERROR("Adding cloud %d to viewer failed!", iter->first); if(_preferencesDialog->getMeshSmoothing(0))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(0));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
if(!_ui->widget_cloudViewer->addOrUpdateCloud(cloudName, cloud, iter->second))
{
UERROR("Adding cloud %d to viewer failed!", iter->first);
}
} }
_ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0)); _ui->widget_cloudViewer->setCloudOpacity(cloudName, _preferencesDialog->getCloudOpacity(0));
@@ -2910,7 +2920,7 @@ void MainWindow::savePointClouds()
{ {
int button = QMessageBox::question(this, int button = QMessageBox::question(this,
tr("One or multiple files?"), tr("One or multiple files?"),
tr("Save clouds separately?"), tr("Merge all clouds together?"),
QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
if(button == QMessageBox::Yes || button == QMessageBox::No) if(button == QMessageBox::Yes || button == QMessageBox::No)
@@ -2921,9 +2931,21 @@ void MainWindow::savePointClouds()
_initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1); _initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds; std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
if(button == QMessageBox::No) if(button == QMessageBox::Yes)
{ {
clouds.insert(std::make_pair(0, this->createAssembledCloud())); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
if(_preferencesDialog->getMeshSmoothing(1))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
clouds.insert(std::make_pair(0, cloud));
} }
else else
{ {
@@ -2938,7 +2960,9 @@ void MainWindow::saveMeshes()
{ {
int button = QMessageBox::question(this, int button = QMessageBox::question(this,
tr("One or multiple files?"), tr("One or multiple files?"),
tr("Save meshes separately?"), tr("Merge all clouds together before surface reconstruction?\n"
" Yes: Output a single mesh for the merged clouds.\n"
" No: Output a mesh for each cloud."),
QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
if(button == QMessageBox::Yes || button == QMessageBox::No) if(button == QMessageBox::Yes || button == QMessageBox::No)
@@ -2949,18 +2973,35 @@ void MainWindow::saveMeshes()
_initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1); _initProgressDialog->setMaximumSteps(_currentPosesMap.size()*2+1);
std::map<int, pcl::PolygonMesh::Ptr> meshes; std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(button == QMessageBox::No) if(button == QMessageBox::Yes)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
_initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size()));
_initProgressDialog->incrementStep();
QApplication::processEvents();
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing(1)) if(_preferencesDialog->getMeshSmoothing(1))
{ {
_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1));
} }
else else
{ {
_initProgressDialog->appendText(tr("Computing surface normals (without smoothing)..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1));
} }
_initProgressDialog->appendText(tr("Greedy projection triangulation..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1));
meshes.insert(std::make_pair(0, mesh)); meshes.insert(std::make_pair(0, mesh));
} }
@@ -2979,7 +3020,7 @@ void MainWindow::viewPointClouds()
{ {
int button = QMessageBox::question(this, int button = QMessageBox::question(this,
tr("One or multiple clouds?"), tr("One or multiple clouds?"),
tr("View clouds separately?"), tr("Merge all clouds together?"),
QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
if(button == QMessageBox::Yes || button == QMessageBox::No) if(button == QMessageBox::Yes || button == QMessageBox::No)
@@ -2990,9 +3031,23 @@ void MainWindow::viewPointClouds()
_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1); _initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds; std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr> clouds;
if(button == QMessageBox::No) if(button == QMessageBox::Yes)
{ {
clouds.insert(std::make_pair(0, this->createAssembledCloud())); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
if(_preferencesDialog->getMeshSmoothing(1))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
clouds.insert(std::make_pair(0, cloud));
} }
else else
{ {
@@ -3039,7 +3094,9 @@ void MainWindow::viewMeshes()
{ {
int button = QMessageBox::question(this, int button = QMessageBox::question(this,
tr("One or multiple meshes?"), tr("One or multiple meshes?"),
tr("View meshes separately?"), tr("Merge all clouds together before surface reconstruction?\n"
" Yes: Output a single mesh for the merged clouds.\n"
" No: Output a mesh for each cloud."),
QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel); QMessageBox::Yes | QMessageBox::No | QMessageBox::Cancel);
if(button == QMessageBox::Yes || button == QMessageBox::No) if(button == QMessageBox::Yes || button == QMessageBox::No)
@@ -3050,7 +3107,7 @@ void MainWindow::viewMeshes()
_initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1); _initProgressDialog->setMaximumSteps(_currentPosesMap.size()+1);
std::map<int, pcl::PolygonMesh::Ptr> meshes; std::map<int, pcl::PolygonMesh::Ptr> meshes;
if(button == QMessageBox::No) if(button == QMessageBox::Yes)
{ {
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud(); pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = this->createAssembledCloud();
_initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size())); _initProgressDialog->appendText(tr("Meshing the assembled cloud (%1 points)...").arg(cloud->size()));
@@ -3060,12 +3117,25 @@ void MainWindow::viewMeshes()
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals; pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
if(_preferencesDialog->getMeshSmoothing(1)) if(_preferencesDialog->getMeshSmoothing(1))
{ {
_initProgressDialog->appendText(tr("Smoothing the surface using Moving Least Squares algorithm..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1)); cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1));
} }
else else
{ {
_initProgressDialog->appendText(tr("Computing surface normals (without smoothing)..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1)); cloudWithNormals = util3d::computeNormals(cloud, _preferencesDialog->getMeshNormalKSearch(1));
} }
_initProgressDialog->appendText(tr("Greedy projection triangulation..."));
_initProgressDialog->incrementStep();
QApplication::processEvents();
pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1)); pcl::PolygonMesh::Ptr mesh = util3d::createMesh(cloudWithNormals, _preferencesDialog->getMeshGP3Radius(1));
meshes.insert(std::make_pair(0, mesh)); meshes.insert(std::make_pair(0, mesh));
} }
@@ -3507,6 +3577,14 @@ std::map<int, pcl::PointCloud<pcl::PointXYZRGB>::Ptr > MainWindow::createPointCl
if(cloud->size()) if(cloud->size())
{ {
if(_preferencesDialog->getMeshSmoothing(1))
{
pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudWithNormals;
cloudWithNormals = util3d::computeNormalsSmoothed(cloud, (float)_preferencesDialog->getMeshSmoothingRadius(1));
cloud->clear();
pcl::copyPointCloud(*cloudWithNormals, *cloud);
}
clouds.insert(std::make_pair(iter->first, cloud)); clouds.insert(std::make_pair(iter->first, cloud));
inserted = true; inserted = true;
} }
guilib/src/PreferencesDialog.cpp
+2 -2
View File
@@ -713,7 +713,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
for(int i=0; i<3; ++i) for(int i=0; i<3; ++i)
{ {
_3dRenderingShowClouds[i]->setChecked(true); _3dRenderingShowClouds[i]->setChecked(true);
_3dRenderingVoxelSize[i]->setValue(i==2?0.01:0.00); _3dRenderingVoxelSize[i]->setValue(i==2?0.005:0.00);
_3dRenderingDecimation[i]->setValue(i==0?4:i==1?2:1); _3dRenderingDecimation[i]->setValue(i==0?4:i==1?2:1);
_3dRenderingMaxDepth[i]->setValue(i==1?0.0:4.0); _3dRenderingMaxDepth[i]->setValue(i==1?0.0:4.0);
_3dRenderingShowScans[i]->setChecked(true); _3dRenderingShowScans[i]->setChecked(true);
@@ -727,7 +727,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_3dRenderingNormalKSearch[i]->setValue(20); _3dRenderingNormalKSearch[i]->setValue(20);
_3dRenderingGP3Radius[i]->setValue(0.04); _3dRenderingGP3Radius[i]->setValue(0.04);
_3dRenderingSmoothing[i]->setChecked(true); _3dRenderingSmoothing[i]->setChecked(i==0?false:true);
_3dRenderingSmoothingRadius[i]->setValue(0.04); _3dRenderingSmoothingRadius[i]->setValue(0.04);
} }
guilib/src/ui/preferencesDialog.ui
+4 -4
View File
@@ -65,7 +65,7 @@
<x>0</x> <x>0</x>
<y>0</y> <y>0</y>
<width>729</width> <width>729</width>
<height>823</height> <height>836</height>
</rect> </rect>
</property> </property>
<layout class="QVBoxLayout" name="verticalLayout_16"> <layout class="QVBoxLayout" name="verticalLayout_16">
@@ -86,7 +86,7 @@
<enum>QFrame::Raised</enum> <enum>QFrame::Raised</enum>
</property> </property>
<property name="currentIndex"> <property name="currentIndex">
<number>15</number> <number>1</number>
</property> </property>
<widget class="QWidget" name="page_22"> <widget class="QWidget" name="page_22">
<layout class="QVBoxLayout" name="verticalLayout_29"> <layout class="QVBoxLayout" name="verticalLayout_29">
@@ -490,7 +490,7 @@ High-res view</string>
<double>0.010000000000000</double> <double>0.010000000000000</double>
</property> </property>
<property name="value"> <property name="value">
<double>0.010000000000000</double> <double>0.005000000000000</double>
</property> </property>
</widget> </widget>
</item> </item>
@@ -916,7 +916,7 @@ High-res view</string>
<string/> <string/>
</property> </property>
<property name="checked"> <property name="checked">
<bool>true</bool> <bool>false</bool>
</property> </property>
</widget> </widget>
</item> </item>