From 0491125e9274422fa11d077780a6387c17b4ba83 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Wed, 26 Jul 2017 16:00:02 -0400
Subject: [PATCH] fixed voxelize assertion with c++ rgbd mapping example

---
 corelib/src/DBDriverSqlite3.cpp |  2 +-
 examples/RGBDMapping/main.cpp   | 11 +++++++++--
 2 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/corelib/src/DBDriverSqlite3.cpp b/corelib/src/DBDriverSqlite3.cpp
index 6de4a343..07bea762 100644
--- a/corelib/src/DBDriverSqlite3.cpp
+++ b/corelib/src/DBDriverSqlite3.cpp
@@ -428,7 +428,7 @@ void DBDriverSqlite3::disconnectDatabaseQuery(bool save, const std::string & out
 			}
 			if(outputFile.empty())
 			{
-				UERROR("Database was initialized with an empty url (in memory). To save it "
+				UERROR("Database was initialized with an empty url (in memory). To save it, "
 						"the output url should not be empty. The database is thus closed without being saved!");
 			}
 			else
diff --git a/examples/RGBDMapping/main.cpp b/examples/RGBDMapping/main.cpp
index 2bd99b7d..78d01777 100644
--- a/examples/RGBDMapping/main.cpp
+++ b/examples/RGBDMapping/main.cpp
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <stdio.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/io/ply_io.h>
+#include <pcl/filters/filter.h>
 
 #include "MapBuilder.h"
 
@@ -216,8 +217,14 @@ int main(int argc, char * argv[])
 				node.sensorData(),
 				4,           // image decimation before creating the clouds
 				4.0f,        // maximum depth of the cloud
-				0.01f);  // Voxel grid filtering
-		*cloud += *util3d::transformPointCloud(tmp, iter->second); // transform the point cloud to its pose
+				0.0f);
+		pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmpNoNaN(new pcl::PointCloud<pcl::PointXYZRGB>);
+		std::vector<int> index;
+		pcl::removeNaNFromPointCloud(*tmp, *tmpNoNaN, index);
+		if(!tmpNoNaN->empty())
+		{
+			*cloud += *util3d::transformPointCloud(tmpNoNaN, iter->second); // transform the point cloud to its pose
+		}
 	}
 	if(cloud->size())
 	{