Fixed normals update with corresponding cameras

corelib/include/rtabmap/core/util3d_surface.h

@@ -80,7 +80,9 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr RTABMAP_EXP mls(
 		int dilationIterations = 0);     // VOXEL_GRID_DILATION
 
 void RTABMAP_EXP adjustNormalsToViewPoints(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		int k = 0); // optional: recompute normal with k neighbors (min k=3)
 

corelib/src/util3d_surface.cpp

@@ -324,15 +324,16 @@ pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr mls(
 }
 
 void adjustNormalsToViewPoints(
-		const pcl::PointCloud<pcl::PointXYZ>::Ptr & viewpoints,
+		const std::map<int, Transform> & poses,
+		const pcl::PointCloud<pcl::PointXYZ>::Ptr & rawCloud,
+		const std::vector<int> & rawCameraIndices,
 		pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr & cloud,
 		int k)
 {
-	// FIXME: maybe better to project points in camera planes to know if they are visible from a specified viewpoint
+	if(poses.size() && rawCloud->size() && rawCloud->size() == rawCameraIndices.size() && cloud->size())
-	if(viewpoints->size() && cloud->size())
 	{
-		pcl::search::KdTree<pcl::PointXYZ>::Ptr viewpointsTree (new pcl::search::KdTree<pcl::PointXYZ>);
+		pcl::search::KdTree<pcl::PointXYZ>::Ptr rawTree (new pcl::search::KdTree<pcl::PointXYZ>);
-		viewpointsTree->setInputCloud (viewpoints);
+		rawTree->setInputCloud (rawCloud);
 
 		pcl::search::KdTree<pcl::PointXYZRGBNormal>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBNormal>);
 		tree->setInputCloud (cloud);
@@ -341,12 +342,13 @@ void adjustNormalsToViewPoints(
 		{
 			std::vector<int> indices;
 			std::vector<float> dist;
-			viewpointsTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
+			rawTree->nearestKSearch(pcl::PointXYZ(cloud->points[i].x, cloud->points[i].y, cloud->points[i].z), 1, indices, dist);
 			UASSERT(indices.size() == 1);
 			if(indices.size() && indices[0]>=0)
 			{
-
+				Transform p = poses.at(rawCameraIndices[indices[0]]);
-				Eigen::Vector3f v = viewpoints->at(indices[0]).getVector3fMap() - cloud->points[i].getVector3fMap();
+				pcl::PointXYZ viewpoint(p.x(), p.y(), p.z());
+				Eigen::Vector3f v = viewpoint.getVector3fMap() - cloud->points[i].getVector3fMap();
 
 				//compute point normal
 				if(k >= 3)

guilib/src/ExportCloudsDialog.cpp

@@ -214,7 +214,7 @@ void ExportCloudsDialog::restoreDefaults()
 	_ui->groupBox_gp3->setChecked(false);
 	_ui->doubleSpinBox_gp3Radius->setValue(0.04);
 	_ui->doubleSpinBox_gp3Mu->setValue(2.5);
-	_ui->doubleSpinBox_meshDecimationFactor->setValue(1.0);
+	_ui->doubleSpinBox_meshDecimationFactor->setValue(0.0);
 	_ui->checkBox_textureMapping->setChecked(false);
 
 	this->update();

guilib/src/MainWindow.cpp

@@ -4623,6 +4623,8 @@ bool MainWindow::getExportedClouds(
 				_exportDialog->getFiltering()?_exportDialog->getFilteringRadius():0.0f,
 				_exportDialog->getFiltering()?_exportDialog->getFilteringMinNeighbors():0.0f);
 
+		pcl::PointCloud<pcl::PointXYZ>::Ptr rawAssembledCloud(new pcl::PointCloud<pcl::PointXYZ>);
+		std::vector<int> rawCameraIndices;
 		if(_exportDialog->getAssemble())
 		{
 			_initProgressDialog->appendText(tr("Assembling %1 clouds...").arg(clouds.size()));
@@ -4636,12 +4638,15 @@ bool MainWindow::getExportedClouds(
 			{
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr transformed = util3d::transformPointCloud(iter->second, poses.at(iter->first));
 				*assembledCloud += *transformed;
+				rawCameraIndices.resize(assembledCloud->size(), iter->first);
 
 				_initProgressDialog->appendText(tr("Assembled cloud %1 (%2/%3).").arg(iter->first).arg(++i).arg(clouds.size()));
 				_initProgressDialog->incrementStep();
 				QApplication::processEvents();
 			}
 
+			pcl::copyPointCloud(*assembledCloud, *rawAssembledCloud);
+
 			_initProgressDialog->appendText(tr("Voxelize assembled cloud (%1 points, voxel size = %2 m)...")
 					.arg(assembledCloud->size())
 					.arg(_exportDialog->getAssembleVoxel()));
@@ -4653,14 +4658,15 @@ bool MainWindow::getExportedClouds(
 						_exportDialog->getAssembleVoxel());
 			}
 
-			_initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...")
-					.arg(assembledCloud->size())
-					.arg(_exportDialog->getFilteringRadius())
-					.arg(_exportDialog->getFilteringMinNeighbors()));
 			if(_exportDialog->getFiltering() &&
 				_exportDialog->getFilteringRadius() > 0.0 &&
 				_exportDialog->getFilteringMinNeighbors() > 0)
 			{
+				_initProgressDialog->appendText(tr("Noise filtering (%1 points, radius = %2 m, min neighbors = %3)...")
+							.arg(assembledCloud->size())
+							.arg(_exportDialog->getFilteringRadius())
+							.arg(_exportDialog->getFilteringMinNeighbors()));
+
 				pcl::IndicesPtr indices = util3d::radiusFiltering(assembledCloud, (float)_exportDialog->getFilteringRadius(), _exportDialog->getFilteringMinNeighbors());
 				pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZRGB>);
 				pcl::copyPointCloud(*assembledCloud, *indices, *cloudFiltered);
@@ -4723,7 +4729,12 @@ bool MainWindow::getExportedClouds(
 					(*viewpoints)[oi].y = iter->second.y();
 					(*viewpoints)[oi++].z = iter->second.z();
 				}
-				util3d::adjustNormalsToViewPoints(viewpoints, cloudWithNormals, _exportDialog->getNormalKSearch());
+				util3d::adjustNormalsToViewPoints(
+						poses,
+						rawAssembledCloud,
+						rawCameraIndices,
+						cloudWithNormals,
+						_exportDialog->getNormalKSearch());
 			}
 			cloudsWithNormals.insert(std::make_pair(iter->first, cloudWithNormals));
 
@@ -4745,7 +4756,7 @@ bool MainWindow::getExportedClouds(
 				pcl::PolygonMesh::Ptr mesh = util3d::createMesh(iter->second, _exportDialog->getMeshGp3Radius(), _exportDialog->getMeshGp3Mu());
 				_initProgressDialog->appendText(tr("Mesh %1 created with %2 polygons (%3/%4).").arg(iter->first).arg(mesh->polygons.size()).arg(++i).arg(clouds.size()));
 
-				if(_exportDialog->getMeshDecimationFactor() < 1.0)
+				if(_exportDialog->getMeshDecimationFactor() > 0.0)
 				{
 					mesh = util3d::meshDecimation(mesh, (float)_exportDialog->getMeshDecimationFactor());
 					_initProgressDialog->appendText(tr("Mesh %1 decimation (factor=%2) to %3 polygons").arg(iter->first).arg(_exportDialog->getMeshDecimationFactor()).arg(mesh->polygons.size()));

guilib/src/ui/exportCloudsDialog.ui

@@ -23,7 +23,7 @@
       <property name="geometry">
        <rect>
         <x>0</x>
-        <y>0</y>
+        <y>-242</y>
         <width>773</width>
         <height>1064</height>
        </rect>
@@ -750,7 +750,7 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
           <item row="3" column="1">
            <widget class="QLabel" name="label_170">
             <property name="text">
-             <string>Mesh quadric decimation factor (1=no decimation). Used to reduce the number of polygons.</string>
+             <string>Mesh quadric decimation factor (0=no decimation). Used to reduce the number of polygons.</string>
             </property>
             <property name="wordWrap">
              <bool>true</bool>
@@ -763,16 +763,16 @@ Guidelines: 4 times the voxel size, 0.025 for voxel=0.</string>
              <number>2</number>
             </property>
             <property name="minimum">
-             <double>0.010000000000000</double>
+             <double>0.000000000000000</double>
             </property>
             <property name="maximum">
-             <double>1.000000000000000</double>
+             <double>0.990000000000000</double>
             </property>
             <property name="singleStep">
              <double>0.100000000000000</double>
             </property>
             <property name="value">
-             <double>1.000000000000000</double>
+             <double>0.000000000000000</double>
             </property>
            </widget>
           </item>