From 7949ba74b2000b5c93aa7502e96db8f0be9c07a1 Mon Sep 17 00:00:00 2001
From: Mathieu Labbe <matlabbe@gmail.com>
Date: Tue, 31 Mar 2015 10:48:43 -0400
Subject: [PATCH] Added some asserts on pose radius filtering

---
 corelib/src/Graph.cpp | 20 +++++++++++++-------
 1 file changed, 13 insertions(+), 7 deletions(-)

diff --git a/corelib/src/Graph.cpp b/corelib/src/Graph.cpp
index 24f714dd..15250b5b 100644
--- a/corelib/src/Graph.cpp
+++ b/corelib/src/Graph.cpp
@@ -899,9 +899,10 @@ std::map<int, Transform> radiusPosesFiltering(
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(poses.size());
 		int i=0;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
 		{
-			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			(*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
 		}
 
 		// radius filtering
@@ -1003,9 +1004,10 @@ std::multimap<int, int> radiusPosesClustering(const std::map<int, Transform> & p
 		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 		cloud->resize(poses.size());
 		int i=0;
-		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter)
+		for(std::map<int, Transform>::const_iterator iter = poses.begin(); iter!=poses.end(); ++iter, ++i)
 		{
-			(*cloud)[i++] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			(*cloud)[i] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
+			UASSERT_MSG(pcl::isFinite((*cloud)[i]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
 		}
 
 		// radius clustering (nearest neighbors)
@@ -1249,17 +1251,21 @@ std::map<int, float> getNodesInRadius(
 	}
 
 	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
-	cloud->resize(nodes.size()-1);
-	std::vector<int> ids(nodes.size()-1);
+	cloud->resize(nodes.size());
+	std::vector<int> ids(nodes.size());
 	int oi = 0;
 	for(std::map<int, Transform>::const_iterator iter = nodes.begin(); iter!=nodes.end(); ++iter)
 	{
 		if(iter->first != nodeId)
 		{
 			(*cloud)[oi] = pcl::PointXYZ(iter->second.x(), iter->second.y(), iter->second.z());
-			ids[oi++] = iter->first;
+			UASSERT_MSG(pcl::isFinite((*cloud)[oi]), uFormat("Invalid pose (%d) %s", iter->first, iter->second.prettyPrint().c_str()).c_str());
+			ids[oi] = iter->first;
+			++oi;
 		}
 	}
+	cloud->resize(oi);
+	ids.resize(oi);
 
 	Transform fromT = nodes.at(nodeId);