Merge pull request #43 from ethz-asl/feature/zero_copy

Zero copy for Eigen::Matrix3XT and Eigen::Map<const Eigen::Matrix3XT>
norlab-ulaval · Sep 10, 2015 · 8437511 · 8437511
2 parents 89e891f + 7d6b111
commit 8437511
Show file tree

Hide file tree

Showing 11 changed files with 486 additions and 366 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -71,32 +71,32 @@ find_path(EIGEN_INCLUDE_DIR Eigen/Core
 
 # optionally, opencl
 # OpenCL disabled as its code is not up-to-date with API
-# set(USE_OPEN_CL "false" CACHE BOOL "Set to ON to look for OpenCL")
-# if (USE_OPEN_CL)
-# 	find_path(OPENCL_INCLUDE_DIR CL/cl.h
-# 		/usr/local/include
-# 		/usr/include
-# 	)
-# 	if (WIN32)
-# 		find_library(OPENCL_LIBRARIES opencl64)
-# 		if (!OPENCL_LIBRARIES)
-# 			find_library(OPENCL_LIBRARIES opencl32)
-# 		endif (!OPENCL_LIBRARIES)
-# 	else (WIN32)
-# 		find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
-# 	endif (WIN32)
-# 	if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
-# 		add_definitions(-DHAVE_OPENCL)
-# 		set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
-# 		include_directories(${OPENCL_INCLUDE_DIR})
-# 		add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
-# 		message("OpenCL enabled and found, enabling CL support")
-# 	else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
-# 		message("OpenCL enabled but not found, disabling CL support")
-# 	endif (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
-# else(USE_OPEN_CL)
-# 	message("OpenCL disabled, not looking for it")
-# endif(USE_OPEN_CL)
+set(USE_OPEN_CL "FALSE" CACHE BOOL "Set to ON to look for OpenCL")
+ if (USE_OPEN_CL)
+ 	find_path(OPENCL_INCLUDE_DIR CL/cl.h
+ 		/usr/local/include
+ 		/usr/include
+ 	)
+ 	if (WIN32)
+ 		find_library(OPENCL_LIBRARIES opencl64)
+ 		if (!OPENCL_LIBRARIES)
+ 			find_library(OPENCL_LIBRARIES opencl32)
+ 		endif (!OPENCL_LIBRARIES)
+ 	else (WIN32)
+ 		find_library(OPENCL_LIBRARIES OpenCL ENV LD_LIBRARY_PATH)
+ 	endif (WIN32)
+ 	if (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
+ 		add_definitions(-DHAVE_OPENCL)
+ 		set(EXTRA_LIBS ${OPENCL_LIBRARIES} ${EXTRA_LIBS})
+ 		include_directories(${OPENCL_INCLUDE_DIR})
+ 		add_definitions(-DOPENCL_SOURCE_DIR=\"${CMAKE_SOURCE_DIR}/nabo/opencl/\")
+ 		message("OpenCL enabled and found, enabling CL support")
+ 	else (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
+ 		message("OpenCL enabled but not found, disabling CL support")
+ 	endif (OPENCL_INCLUDE_DIR AND OPENCL_LIBRARIES)
+ else(USE_OPEN_CL)
+ 	message("OpenCL disabled, not looking for it")
+ endif(USE_OPEN_CL)
 
 # include all libs so far
 include_directories(${CMAKE_SOURCE_DIR} ${EIGEN_INCLUDE_DIR} ${Boost_INCLUDE_DIRS})

diff --git a/experimental/kdtree_cpu.cpp b/experimental/kdtree_cpu.cpp
diff --git a/experimental/nabo_experimental.h b/experimental/nabo_experimental.h
@@ -38,13 +38,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 namespace Nabo
 {
 	// KDTree, balanced, points in nodes
-	template<typename T>
-	struct KDTreeBalancedPtInNodes:public NearestNeighbourSearch<T>
+	template<typename T, typename CloudType>
+	struct KDTreeBalancedPtInNodes:public NearestNeighbourSearch<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
 
 	protected:
 		struct BuildPoint
@@ -84,24 +84,24 @@ namespace Nabo
 		void dump(const Vector minValues, const Vector maxValues, const size_t pos) const;
 
 	protected:
-		KDTreeBalancedPtInNodes(const Matrix& cloud);
+		KDTreeBalancedPtInNodes(const CloudType& cloud);
 	};
 
 	// KDTree, balanced, points in nodes, priority queue
-	template<typename T>
-	struct KDTreeBalancedPtInNodesPQ: public KDTreeBalancedPtInNodes<T>
+	template<typename T, typename CloudType>
+	struct KDTreeBalancedPtInNodesPQ: public KDTreeBalancedPtInNodes<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
-		typedef typename KDTreeBalancedPtInNodes<T>::Node Node;
-		typedef typename KDTreeBalancedPtInNodes<T>::Nodes Nodes;
-
-		using NearestNeighbourSearch<T>::statistics;
-		using KDTreeBalancedPtInNodes<T>::nodes;
-		using KDTreeBalancedPtInNodes<T>::childLeft;
-		using KDTreeBalancedPtInNodes<T>::childRight;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
+		typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Node Node;
+		typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Nodes Nodes;
+
+		using NearestNeighbourSearch<T, CloudType>::statistics;
+		using KDTreeBalancedPtInNodes<T, CloudType>::nodes;
+		using KDTreeBalancedPtInNodes<T, CloudType>::childLeft;
+		using KDTreeBalancedPtInNodes<T, CloudType>::childRight;
 
 	protected:
 		struct SearchElement
@@ -115,50 +115,50 @@ namespace Nabo
 		};
 
 	public:
-		KDTreeBalancedPtInNodesPQ(const Matrix& cloud);
+		KDTreeBalancedPtInNodesPQ(const CloudType& cloud);
 		virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
 	};
 
 	// KDTree, balanced, points in nodes, stack
-	template<typename T>
-	struct KDTreeBalancedPtInNodesStack: public KDTreeBalancedPtInNodes<T>
+	template<typename T, typename CloudType>
+	struct KDTreeBalancedPtInNodesStack: public KDTreeBalancedPtInNodes<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
-		typedef typename KDTreeBalancedPtInNodes<T>::Node Node;
-		typedef typename KDTreeBalancedPtInNodes<T>::Nodes Nodes;
-
-		using NearestNeighbourSearch<T>::statistics;
-		using KDTreeBalancedPtInNodes<T>::nodes;
-		using KDTreeBalancedPtInNodes<T>::childLeft;
-		using KDTreeBalancedPtInNodes<T>::childRight;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
+		typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Node Node;
+		typedef typename KDTreeBalancedPtInNodes<T, CloudType>::Nodes Nodes;
+
+		using NearestNeighbourSearch<T, CloudType>::statistics;
+		using KDTreeBalancedPtInNodes<T, CloudType>::nodes;
+		using KDTreeBalancedPtInNodes<T, CloudType>::childLeft;
+		using KDTreeBalancedPtInNodes<T, CloudType>::childRight;
 
 		typedef IndexHeapSTL<Index, T> Heap;
 
 	protected:
 		void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
 
 	public:
-		KDTreeBalancedPtInNodesStack(const Matrix& cloud);
+		KDTreeBalancedPtInNodesStack(const CloudType& cloud);
 		virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
 	};
 
 
 	//  KDTree, balanced, points in leaves, stack
-	template<typename T>
-	struct KDTreeBalancedPtInLeavesStack: public NearestNeighbourSearch<T>
+	template<typename T, typename CloudType>
+	struct KDTreeBalancedPtInLeavesStack: public NearestNeighbourSearch<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
 
-		using NearestNeighbourSearch<T>::statistics;
-		using NearestNeighbourSearch<T>::cloud;
-		using NearestNeighbourSearch<T>::minBound;
-		using NearestNeighbourSearch<T>::maxBound;
+		using NearestNeighbourSearch<T, CloudType>::statistics;
+		using NearestNeighbourSearch<T, CloudType>::cloud;
+		using NearestNeighbourSearch<T, CloudType>::minBound;
+		using NearestNeighbourSearch<T, CloudType>::maxBound;
 
 	protected:
 		struct BuildPoint
@@ -199,24 +199,24 @@ namespace Nabo
 		void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
 
 	public:
-		KDTreeBalancedPtInLeavesStack(const Matrix& cloud, const bool balanceVariance);
+		KDTreeBalancedPtInLeavesStack(const CloudType& cloud, const bool balanceVariance);
 		virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
 	};
 
 	//  KDTree, unbalanced, points in leaves, stack, implicit bounds, ANN_KD_SL_MIDPT
-	template<typename T, typename Heap>
-	struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack: public NearestNeighbourSearch<T>
+	template<typename T, typename Heap, typename CloudType>
+	struct KDTreeUnbalancedPtInLeavesImplicitBoundsStack: public NearestNeighbourSearch<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
-		typedef typename NearestNeighbourSearch<T>::IndexMatrix IndexMatrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexMatrix IndexMatrix;
 
-		using NearestNeighbourSearch<T>::statistics;
-		using NearestNeighbourSearch<T>::cloud;
-		using NearestNeighbourSearch<T>::minBound;
-		using NearestNeighbourSearch<T>::maxBound;
+		using NearestNeighbourSearch<T, CloudType>::statistics;
+		using NearestNeighbourSearch<T, CloudType>::cloud;
+		using NearestNeighbourSearch<T, CloudType>::minBound;
+		using NearestNeighbourSearch<T, CloudType>::maxBound;
 
 	protected:
 		struct BuildPoint
@@ -264,25 +264,25 @@ namespace Nabo
 		void recurseKnn(const Vector& query, const unsigned n, T rd, Heap& heap, Vector& off, const T maxError, const bool allowSelfMatch);
 
 	public:
-		KDTreeUnbalancedPtInLeavesImplicitBoundsStack(const Matrix& cloud);
+		KDTreeUnbalancedPtInLeavesImplicitBoundsStack(const CloudType& cloud);
 		virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
 		virtual IndexMatrix knnM(const Matrix& query, const Index k, const T epsilon, const unsigned optionFlags);
 	};
 
 	//  KDTree, unbalanced, points in leaves, stack, explicit bounds, ANN_KD_SL_MIDPT
-	template<typename T>
-	struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack: public NearestNeighbourSearch<T>
+	template<typename T, typename CloudType>
+	struct KDTreeUnbalancedPtInLeavesExplicitBoundsStack: public NearestNeighbourSearch<T, CloudType>
 	{
-		typedef typename NearestNeighbourSearch<T>::Vector Vector;
-		typedef typename NearestNeighbourSearch<T>::Matrix Matrix;
-		typedef typename NearestNeighbourSearch<T>::Index Index;
-		typedef typename NearestNeighbourSearch<T>::IndexVector IndexVector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Vector Vector;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Matrix Matrix;
+		typedef typename NearestNeighbourSearch<T, CloudType>::Index Index;
+		typedef typename NearestNeighbourSearch<T, CloudType>::IndexVector IndexVector;
 
 
-		using NearestNeighbourSearch<T>::statistics;
-		using NearestNeighbourSearch<T>::cloud;
-		using NearestNeighbourSearch<T>::minBound;
-		using NearestNeighbourSearch<T>::maxBound;
+		using NearestNeighbourSearch<T, CloudType>::statistics;
+		using NearestNeighbourSearch<T, CloudType>::cloud;
+		using NearestNeighbourSearch<T, CloudType>::minBound;
+		using NearestNeighbourSearch<T, CloudType>::maxBound;
 
 	protected:
 		struct BuildPoint
@@ -322,7 +322,7 @@ namespace Nabo
 		void recurseKnn(const Vector& query, const size_t n, T rd, Heap& heap, const T maxError, const bool allowSelfMatch);
 
 	public:
-		KDTreeUnbalancedPtInLeavesExplicitBoundsStack(const Matrix& cloud);
+		KDTreeUnbalancedPtInLeavesExplicitBoundsStack(const CloudType& cloud);
 		virtual IndexVector knn(const Vector& query, const Index k, const T epsilon, const unsigned optionFlags);
 	};
 }

diff --git a/nabo/brute_force_cpu.cpp b/nabo/brute_force_cpu.cpp
@@ -41,9 +41,9 @@ namespace Nabo
 {
 	using namespace std;
 
-	template<typename T>
-	BruteForceSearch<T>::BruteForceSearch(const Matrix& cloud, const Index dim, const unsigned creationOptionFlags):
-		NearestNeighbourSearch<T>::NearestNeighbourSearch(cloud, dim, creationOptionFlags)
+	template<typename T, typename CloudType>
+	BruteForceSearch<T, CloudType>::BruteForceSearch(const CloudType& cloud, const Index dim, const unsigned creationOptionFlags):
+		NearestNeighbourSearch<T, CloudType>::NearestNeighbourSearch(cloud, dim, creationOptionFlags)
 	{
 #ifdef EIGEN3_API
 		const_cast<Vector&>(this->minBound) = cloud.topRows(this->dim).rowwise().minCoeff();
@@ -60,15 +60,15 @@ namespace Nabo
 	}
 
 
-	template<typename T>
-	unsigned long BruteForceSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
+	template<typename T, typename CloudType>
+	unsigned long BruteForceSearch<T, CloudType>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Index k, const T epsilon, const unsigned optionFlags, const T maxRadius) const
 	{
 		const Vector maxRadii(Vector::Constant(query.cols(), maxRadius));
 		return knn(query, indices, dists2, maxRadii, k, epsilon, optionFlags);
 	}
 
-	template<typename T>
-	unsigned long BruteForceSearch<T>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T /*epsilon*/, const unsigned optionFlags) const
+	template<typename T, typename CloudType>
+	unsigned long BruteForceSearch<T, CloudType>::knn(const Matrix& query, IndexMatrix& indices, Matrix& dists2, const Vector& maxRadii, const Index k, const T /*epsilon*/, const unsigned optionFlags) const
 	{
 		checkSizesKnn(query, indices, dists2, k, optionFlags, &maxRadii);
 
@@ -104,4 +104,8 @@ namespace Nabo
 
 	template struct BruteForceSearch<float>;
 	template struct BruteForceSearch<double>;
+	template struct BruteForceSearch<float, Eigen::Matrix3Xf>;
+	template struct BruteForceSearch<double, Eigen::Matrix3Xd>;
+	template struct BruteForceSearch<float, Eigen::Map<const Eigen::Matrix3Xf, Eigen::Aligned> >;
+	template struct BruteForceSearch<double, Eigen::Map<const Eigen::Matrix3Xd, Eigen::Aligned> >;
 }