diff --git a/CMakeLists.txt b/CMakeLists.txt
index dd63f3c448f9b6a57896ddcf56c24305ab6ca28b..688e3f2b82169b81c0dcdc7617cb48c5560fe25f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -2,11 +2,11 @@
 CMAKE_MINIMUM_REQUIRED(VERSION 2.4)
 
 if(COMMAND cmake_policy)
-  cmake_policy(SET CMP0007 NEW) 
-  cmake_policy(SET CMP0005 NEW) 
+  cmake_policy(SET CMP0005 NEW)
   cmake_policy(SET CMP0003 NEW)
+  cmake_policy(SET CMP0007 NEW)
   cmake_policy(SET CMP0002 NEW)
-   
+
 endif(COMMAND cmake_policy)
 
 # The project name and the type of project
@@ -17,7 +17,7 @@ SET(LIBRARY_OUTPUT_PATH ${CMAKE_CURRENT_SOURCE_DIR}/lib)
 SET(CMAKE_INSTALL_PREFIX /usr/local)
 
 IF (NOT CMAKE_BUILD_TYPE)
- SET(CMAKE_BUILD_TYPE "DEBUG") 
+ SET(CMAKE_BUILD_TYPE "DEBUG")
 ENDIF (NOT CMAKE_BUILD_TYPE)
 
 SET(CMAKE_CXX_FLAGS_DEBUG "-g -Wall -D_REENTRANT")
@@ -66,5 +66,3 @@ ELSE(UNIX)
     TARGET  uninstall
   )
 ENDIF(UNIX)
-
-
diff --git a/ReadMe.md b/ReadMe.md
new file mode 100644
index 0000000000000000000000000000000000000000..e84433da33b779c1bed26421f2a56a6b7da85c4a
--- /dev/null
+++ b/ReadMe.md
@@ -0,0 +1,141 @@
+finddd_descriptor                        {#mainpage}
+============
+
+## Description
+
+FINDDD descriptor library
+
+## Dependencies
+
+This package requires of the following system libraries and packages
+
+* [cmake](https://www.cmake.org "CMake's Homepage"), a cross-platform build system.
+* [doxygen](http://www.doxygen.org "Doxygen's Homepage") and [graphviz](http://www.graphviz.org "Graphviz's Homepage") to generate the documentation.
+* stdc++ and pthread libraries.
+
+Under linux all of these utilities are available in ready-to-use packages.
+
+Under MacOS most of the packages are available via [fink](http://www.finkproject.org/ "Fink's Homepage")
+
+This package also requires of the following libraries:
+
+* FINDDD requires the PCL library (www.pointclouds.org) and the Boost 
+libraries (www.boost.org) for the "make_shared" function.
+
+## Compilation and installation
+
+Download this repository and create a build folder inside:
+
+``` mkdir build ```
+
+Inside the build folder execute the following commands:
+
+``` cmake .. ```
+
+The default build mode is DEBUG. That is, objects and executables include debug information.
+
+The RELEASE build mode optimizes for speed. To build in this mode execute instead
+``` cmake .. -DCMAKE_BUILD_TYPE=RELEASE ```
+
+The release mode will be kept until next time cmake is executed.
+
+``` make ``` 
+
+In case no errors are reported, the generated libraries (if any) will be located at the
+_lib_ folder and the executables (if any) will be located at the _bin_ folder.
+
+In order to be able to use the library, it it necessary to copy it into the system.
+To do that, execute
+
+``` make install ```
+
+as root and the shared libraries will be copied to */usr/local/lib/iridrivers* directory
+and the header files will be copied to */usr/local/include/iridrivers* dierctory. At
+this point, the library may be used by any user.
+
+To remove the library from the system, exceute
+
+``` make uninstall ```
+
+as root, and all the associated files will be removed from the system.
+
+To generate the documentation execute the following command:
+
+``` make doc ```
+
+## How to use it
+To compute FINDDD descriptors for a structured point cloud, compile
+and run the "bin/finddd_descriptor_test" command with appropriate
+parameters:
+
+finddd_descriptor -pcd <file_path> -centroids_file <centroids_path> 
+                  -descfile <output_descfile_path> [-use_soft_voting <bool>] 
+                  [-sample_each <step>] [-normalize_desc] [-desc_patch_radius 
+                  <radius>] [-num_spatial_bins <num_bins_per_side>] [-positions_file 
+                  <positions_file_path>] [-keep_nan] [-pca_file <file_path>] 
+                  [-pca_nkeep <num ev to keep>] [-ascii]
+
+Parameters:
+  -pcd <filename>              Input pcd file path. Mandatory.
+  -centroids_file <filename>   Input centroid file path. Mandatory.
+  -descfile <filename>         Output descriptor file path. Mandatory.
+  -use_soft_voting <bool>      Disable soft-voting. 
+  -sample_each <num_pix>       Density of descriptor sampling if positions_file is 
+                               not specified. Defaults to 6 pixels. 
+  -normalize_desc <norm>       0=none 1=L1 2=L2 3=sqrt+L2.
+  -desc_patch_radius <num_pix> Radius of the local patch. Defaults to 21 pixels.
+  -num_spatial_bins <num_bins> Number of spatial bins per side (sqrt of 
+                               total spatial bins). Defaults to 4.
+  -positions_file <filename>   Input locations file path.
+  -pca_file <filename>         PCA matrix filename. Should include mean vector .
+                               followed by the PCA matrix in space-separated ascii.
+                               See example file in "examples/pca_matrix_o13_d43.txt".
+  -pca_nkeep <num_ev_to_keep>  Number of eigenvectors to keep (output dimension).
+  -ascii                       Save descriptors in ASCII format.
+
+
+EXAMPLES
+--------
+In the "examples" directory you will find the following files:
+$> run_example.sh           Set of example executions to demonstrate functionality
+                           of the program.
+$> display_finddd.py        Python tool to visualize a set of FINDDD descriptors.
+$> example.png              Example RGB-D image, image part.
+$> example.pcd              Example RGB-D image, depth part.
+$> xyz_centers13.txt        Example file with 13 angular bin centroids.
+$> 50positions.txt          Fifty random image locations to compute descriptors in.
+$> pca_matrix_o13_d43.txt   Example file with PCA matrix to project the descriptors.
+
+example usage of dispaly_finddd.py (after running run_example.sh):
+$> python display_finddd.py -f test/features_step6.fvecs -pcd example.pcd -c xyz_centers13.txt
+
+CITATION
+--------
+If you use this code in your work, please cite:
+A. Ramisa, G. Alenya, F. Moreno-Noguer and C. Torras; "FINDDD: A fast 3D descriptor 
+to characterize textiles for robot manipulation", IROS 2013.
+
+To use this library in an other library or application, in the CMakeLists.txt file, first it is necessary to locate if the library has been installed or not using the following command
+
+``` FIND_PACKAGE(finddd_descriptor) ```
+
+In the case that the package is present, it is necessary to add the header files directory to the include directory path by using
+
+``` INCLUDE_DIRECTORIES(${finddd_descriptor_INCLUDE_DIR}) ```
+
+and it is also necessary to link with the desired libraries by using the following command
+
+``` TARGET_LINK_LIBRARIES(<executable name> ${finddd_descriptor_LIBRARY}) ```
+
+## Disclaimer  
+
+Copyright (C) 2009-2018 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
+Mantainer IRI labrobotics (labrobotica@iri.upc.edu)
+
+This package is distributed in the hope that it will be useful, but without any warranty. It is provided "as is" without warranty of any kind, either expressed or implied, including, but not limited to, the implied warranties of merchantability and fitness for a particular purpose. The entire risk as to the quality and performance of the program is with you. should the program prove defective, the GMR group does not assume the cost of any necessary servicing, repair  or correction.
+
+In no event unless required by applicable law the author will be liable to you for damages, including any general, special, incidental or consequential damages arising out of the use or inability to use the program (including but not limited to loss of data or data being rendered inaccurate or losses sustained by you or third parties or a failure of the program to operate with any other programs), even if the author has been advised of the possibility of such damages.
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>
+
diff --git a/ReadMe.txt b/ReadMe.txt
deleted file mode 100644
index b6f56b572b635d3eaef9528a11eb1467465603f9..0000000000000000000000000000000000000000
--- a/ReadMe.txt
+++ /dev/null
@@ -1,87 +0,0 @@
-Copyright (C) 2014 Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
-Author aramisa (aramisa@iri.upc.edu)
-All rights reserved.
-
-This file is part of FINDDD descriptor library. 
-FINDDD descriptor library is free software: you can redistribute it
-and/or modify it under the terms of the GNU Lesser General Public
-License as published by the Free Software Foundation, either version 3
-of the License, or at your option) any later version.
-
-This program is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU Lesser General Public License for more details.
-
-You should have received a copy of the GNU Lesser General Public License
-along with this program.  If not, see <http://www.gnu.org/licenses/>
-
-
-INSTALLATION
-------------
-This library is built using CMake. To compile it, create a sub-folder
-called build in the project root directory, and run "cmake .." followed 
-by "make" within it.
-
-FINDDD requires the PCL library (www.pointclouds.org) and the Boost 
-libraries (www.boost.org) for the "make_shared" function.
-
-
-USAGE
------
-To compute FINDDD descriptors for a structured point cloud, compile
-and run the "bin/finddd_descriptor" command with appropriate
-parameters:
-
-finddd_descriptor -pcd <file_path> -centroids_file <centroids_path> 
-                  -descfile <output_descfile_path> [-use_soft_voting <bool>] 
-                  [-sample_each <step>] [-normalize_desc] [-desc_patch_radius 
-                  <radius>] [-num_spatial_bins <num_bins_per_side>] [-positions_file 
-                  <positions_file_path>] [-keep_nan] [-pca_file <file_path>] 
-                  [-pca_nkeep <num ev to keep>] [-ascii]
-
-Parameters:
-  -pcd <filename>              Input pcd file path. Mandatory.
-  -centroids_file <filename>   Input centroid file path. Mandatory.
-  -descfile <filename>         Output descriptor file path. Mandatory.
-  -use_soft_voting <bool>      Disable soft-voting. 
-  -sample_each <num_pix>       Density of descriptor sampling if positions_file is 
-                               not specified. Defaults to 6 pixels. 
-  -normalize_desc <norm>       0=none 1=L1 2=L2 3=sqrt+L2.
-  -desc_patch_radius <num_pix> Radius of the local patch. Defaults to 21 pixels.
-  -num_spatial_bins <num_bins> Number of spatial bins per side (sqrt of 
-                               total spatial bins). Defaults to 4.
-  -positions_file <filename>   Input locations file path.
-  -pca_file <filename>         PCA matrix filename. Should include mean vector .
-                               followed by the PCA matrix in space-separated ascii.
-                               See example file in "examples/pca_matrix_o13_d43.txt".
-  -pca_nkeep <num_ev_to_keep>  Number of eigenvectors to keep (output dimension).
-  -ascii                       Save descriptors in ASCII format.
-
-
-EXAMPLES
---------
-In the "examples" directory you will find the following files:
-$> run_example.sh           Set of example executions to demonstrate functionality
-                           of the program.
-$> display_finddd.py        Python tool to visualize a set of FINDDD descriptors.
-$> example.png              Example RGB-D image, image part.
-$> example.pcd              Example RGB-D image, depth part.
-$> xyz_centers13.txt        Example file with 13 angular bin centroids.
-$> 50positions.txt          Fifty random image locations to compute descriptors in.
-$> pca_matrix_o13_d43.txt   Example file with PCA matrix to project the descriptors.
-
-example usage of dispaly_finddd.py (after running run_example.sh):
-$> python display_finddd.py -f test/features_step6.fvecs -pcd example.pcd -c xyz_centers13.txt
-
-CITATION
---------
-If you use this code in your work, please cite:
-A. Ramisa, G. Alenya, F. Moreno-Noguer and C. Torras; "FINDDD: A fast 3D descriptor 
-to characterize textiles for robot manipulation", IROS 2013.
-
-
-CHANGELOG
----------
-Version 1.0: Initial release.
-
diff --git a/doc/doxygen.conf b/doc/doxygen.conf
index a04f4ab79c848153fb2257baa5d872c25ec7cb11..f979a270136ac9b90ce49095d890a08f86b4b03c 100755
--- a/doc/doxygen.conf
+++ b/doc/doxygen.conf
@@ -83,12 +83,12 @@ WARN_LOGFILE           =
 # configuration options related to the input files
 #---------------------------------------------------------------------------
 INPUT                  = ../src \
-                         ../doc/main.dox
+                         ../include \
+                         ../ReadMe.md
 INPUT_ENCODING         = UTF-8
 FILE_PATTERNS          = *.c \
                          *.h \
-                         *.cpp \
-			 *.hpp
+                         *.cpp
 RECURSIVE              = YES
 EXCLUDE                = 
 EXCLUDE_SYMLINKS       = NO
diff --git a/doc/doxygen_project_name.conf b/doc/doxygen_project_name.conf
index dc1e89bf870e9077b929f9d4acd5348e4554141a..72eccff3a6e638a56a31d7cbde29ff0dc3fbd90c 100644
--- a/doc/doxygen_project_name.conf
+++ b/doc/doxygen_project_name.conf
@@ -1 +1 @@
-PROJECT_NAME = "FINDDD descriptor"
+PROJECT_NAME = "finddd_descriptor"
diff --git a/doc/main.dox b/doc/main.dox
index 18f8a59310da0f356544fb22ddd750088d028054..51d277ddffb99b1a91be90a07b8db56ee36a8f49 100644
--- a/doc/main.dox
+++ b/doc/main.dox
@@ -149,3 +149,4 @@ FINDDD depends on <a href="http://www.pointclouds.org">PCL</a> and <a href="http
    along with this program.  If not, see <http://www.gnu.org/licenses/>. 
 
  */
+
diff --git a/examples/run_example.sh b/examples/run_example.sh
deleted file mode 100644
index 1b03903580f4ef9c255d0804f98905a40ff0101b..0000000000000000000000000000000000000000
--- a/examples/run_example.sh
+++ /dev/null
@@ -1,36 +0,0 @@
-#!/bin/bash
-
-mkdir test
-
-# Step size
-echo "STEP SIZE: 6, 3, 1"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -descfile test/features_step6.fvecs
-../bin/finddd_descriptor -pcd example.pcd -sample_each 3 -centroids_file xyz_centers13.txt -descfile test/features_step3.fvecs
-../bin/finddd_descriptor -pcd example.pcd -sample_each 1 -centroids_file xyz_centers13.txt -descfile test/features_step1.fvecs
-
-# Descriptor area
-echo "SUPPORT AREA: 61x61, 81x81, 121x121"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 61 -descfile test/features_rad61.fvecs
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 81 -descfile test/features_rad81.fvecs
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 121 -descfile test/features_rad121.fvecs
-
-# Num spatial bins
-echo "NUMBER OF SPATIAL BINS: 2x2, 5x5"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -num_spatial_bins 2 -descfile test/features_spa2.fvecs
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -num_spatial_bins 5 -descfile test/features_spa5.fvecs
-
-# Disable soft voting
-echo "SOFT VOTING DISABLED"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -use_soft_voting 0 -descfile test/features_nosv.fvecs
-
-# Custom descriptor positions
-echo "CUSTOM DESCRIPTOR POSITIONS"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -positions_file 50positions.txt -descfile test/features_locations.fvecs
-
-#PCA compression
-echo "PCA COMPRESSION: 33"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -pca_file pca_matrix_o13_d43.txt -pca_nkeep 33 -descfile test/features_pca33.fvecs
-
-#ASCII OUTPUT
-echo "ASCII OUTPUT"
-../bin/finddd_descriptor -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -ascii -descfile test/features_ascii.txt
diff --git a/include/finddd_descriptor.h b/include/finddd_descriptor.h
new file mode 100644
index 0000000000000000000000000000000000000000..e67cbf1266d0c03f2505aae7d87f870447980fb3
--- /dev/null
+++ b/include/finddd_descriptor.h
@@ -0,0 +1,435 @@
+#ifndef _FINDDD_DESCRIPTOR_H
+#define _FINDDD_DESCRIPTOR_H
+
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/features/normal_3d.h>
+#include <pcl/features/integral_image_normal.h>
+#include <pcl/io/pcd_io.h>
+#include <boost/make_shared.hpp>
+#include <cmath>
+#include <vector>
+#include <sstream>
+#include <time.h>
+#include <iostream>
+#include <fstream>
+
+// util
+template <class T>
+inline std::string to_string (const T& t)
+{
+  std::stringstream ss;
+  ss << t;
+  return ss.str();
+}
+
+/** \brief Class for 3D point.
+ *  Very simple class to hold 3D point position information.
+ */
+class P3D
+{
+public:
+  float x;
+  float y;
+  float z;
+  int asfaf;
+
+  P3D():x(0),y(0),z(0) {}
+  P3D(float xx, float yy, float zz):x(xx),y(yy),z(zz) {}
+
+};
+
+/** \brief Class to hold one FINDDD descriptor.
+*  This class holds each one of the FINDDD descriptors in a set.
+*  %Descriptor data is stored in a vector<float>.
+*/
+class Descriptor
+{
+public:
+  /** \short X-coordinate of the descriptor in image. */
+  int u;
+  /** \short Y-coordinate of the descriptor in image. */
+  int v;
+  /** \short Orientation of the descriptor in image (not used yet). */
+  float orientation;
+  /** \short Corresponding 3D coordinates of the descriptor. */
+  P3D point3d;
+  /** \short %Descriptor vector. */
+  std::vector<float> descriptor;
+};
+
+/** \brief Class to hold FINDDD computation parameters.
+ *  This class holds a set (vector) of FINDDD descriptors as well as
+ *  parameters of the descriptors in the set and sizes of the original
+ *  range image.
+ */
+class DescriptorSet
+{
+public:
+  /** \short Number of orientation bins of each sub-descriptor. */
+  int num_orient_bins;
+  /** \short Number of spatial bins per side of the full descriptor region (sqrt of total). */
+  int num_spa_bins;
+  /** \short Number of descriptors. */
+  int num;
+  /** \short Number of dimensions of the descriptor. */
+  int len;
+  /** \short Width of the original range image. */
+  int width;
+  /** \short Height of the original range image. */
+  int height;
+  /** \short Vector with computed descriptor. */
+  std::vector<Descriptor> descriptors;
+
+  DescriptorSet();
+
+};
+
+/** \brief Class to hold FINDDD computation parameters.
+ *  This class holds all parameters related to FINDDD computation, and is
+ *  used to update the FindddAlgorithm class with user-defined parameters.
+ */
+class FindddConfig
+{
+public:
+  /** \short Num of spatial subdivisions. */
+  int num_spatial_bins;
+  /** \short Side of the patch in pixels. */
+  int desc_patch_radius;
+  /** \short Step in pixels between descriptors. */
+  int sample_each;
+  /** \short File with the normal centroids. */
+  std::string centroids_file;
+  /** \short File with the (u,v) positions where descriptors have to be computed (overrides "sample_each"). */
+  std::string positions_file;
+  /** \short Normalization of new descriptors.
+
+      Normalization of new descriptors:
+      - 0 - No norm
+      - 1 - L1
+      - 2 - L2
+      - 3 - sqrt + L2
+  */
+  int normalize_desc;
+  /** \short Whether to use soft-voting or direct assignment. */
+  int use_soft_voting;
+  /** \short Input pcd file where descriptors have to be computed. */
+  std::string pcd_file;
+  /** \short Output file in "fvec" format containing the computed descriptors. */
+  std::string geofvecs_file;
+  /** \short Whether to keep NaN points or discard them. */
+  int keep_nan_points;
+  /** \short File with PCA matrix.
+
+      Name of a file containing the mean and the PCA matrix to automatically compress the computed descriptors. If not specified no compression is done. */
+  std::string pca_file;
+  /** \short Number of eigenvectors to keep in the PCA matrix (=0 to keep all of them). */
+  int pca_nkeep;
+  /** \short Verbosity level. */
+  int verbose_level;
+  /** \short save descriptors in ascii format */
+  int save_in_ascii;
+  FindddConfig();
+};
+
+
+/**
+ * \brief Simple Integral Images class.
+ * Class to compute simple parallel integral images with floating point accumulators.
+ * Includes function to compute sum of sub-rectangle.
+ */
+class IIf
+{
+public:
+  /** \short Vector of float* to store the parallel integral images in linear form.*/
+  std::vector<float*> ii_;
+  /** \short Number of bins.*/
+  unsigned int bins_;
+  /** \short Width of the integral image.*/
+  unsigned int width_;
+  /** \short Height of the integral image.*/
+  unsigned int height_;
+
+  /**
+   * \param bins Number of bins/parallel integral images.
+   * \param width Width of the integral image.
+   * \param height Height of the integral image.
+   *
+   * Constructor for empty set of parallel integral image.
+   */
+  IIf(unsigned int bins, unsigned int width, unsigned int height);
+
+  /** Destructor */
+  ~IIf();
+
+  /**
+   * \param im Input vote matrix
+   *
+   * Function to construct the set of integral images given an input vote matrix.
+   */
+  void construct(std::vector<std::vector<float> > &im);
+
+  /**
+   *
+   * \param tu Top-left u coordinate.
+   * \param tv Top-left v coordinate.
+   * \param bu Bottom-right u coordinate.
+   * \param bv Bottom-right v coordinate.
+   * \return Vector with sum of the sub-region.
+   *
+   * Get sum for each parallel integral image for a rectangular sub-region.
+   */
+  std::vector<float> get_sub_rect(unsigned int tu, unsigned int tv, unsigned int bu, unsigned int bv);
+};
+
+class CFinddd_Descriptor
+{
+private:
+
+ /**
+ * /param pcl_normals Output cloud with 3D point normals computed using PCL library.
+ * /param cloud Input cloud with 3D points we want to compute normals for.
+ *
+ * Function to compute normal vectors fast taking advantage of the fact that the input point cloud is structured.
+ * Compute pointcloud normals fast using integral images.
+ */
+  void compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud);
+
+
+ /**
+ * \param nb_bins Number of bins in a sub-descriptor.
+ * \return Empty voting vector.
+ *
+ * Get empty normal voting vector.
+ */
+ inline std::vector<float> get_bin_float_empty(int nb_bins);
+
+
+ /**
+  * \param x X coordinate of normal
+  * \param y Y coordinate of normal
+  * \param z Z coordinate of normal
+  * \param bin_centers_x Vector with X coordinate of normal centers
+  * \param bin_centers_y Vector with Y coordinate of normal centers
+  * \param bin_centers_z Vector with Z coordinate of normal centers
+  * \param radius_inf Inter-bin distance among the normal centers.
+  * \return Voting vector corresponding to input normal.
+  *
+  * Get normal voting histogram *with* interpolation.
+  */
+ inline std::vector<float> get_bin_float_interp_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z, float radius_inf);
+
+
+ /**
+ * \param x X coordinate of normal
+ * \param y Y coordinate of normal
+ * \param z Z coordinate of normal
+ * \param bin_centers_x Vector with X coordinate of normal centers
+ * \param bin_centers_y Vector with Y coordinate of normal centers
+ * \param bin_centers_z Vector with Z coordinate of normal centers
+ * \return Voting vector corresponding to input normal.
+ *
+ * Get normal voting histogram *without* interpolation.
+ */
+ inline std::vector<float> get_bin_float_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z);
+
+protected:
+  // private attributes and methods
+  uint nt_;
+
+  /** \short Vector containing the X coordinate of normal centroids. */
+  std::vector<float> orient_bin_centers_x_;
+  /** \short Vector containing the Y coordinate of normal centroids. */
+  std::vector<float> orient_bin_centers_y_;
+  /** \short Vector containing the Z coordinate of normal centroids. */
+  std::vector<float> orient_bin_centers_z_;
+
+  /** \short Vector containing the U coordinate of points where descriptors have to be computed. */
+  std::vector<int> desc_compute_positionsX_;
+  /** \short Vector containing the V coordinate of points where descriptors have to be computed. */
+  std::vector<int> desc_compute_positionsY_;
+  /** \short Inter-bin distance, to compute soft-votes*/
+  float radius_inf_;
+  /** \short Number of orientation bins in each sub-descriptor. */
+  uint desc_num_orient_bins_;
+  /** \short Number of spatial bins per side of the full descriptor region (sqrt of total). */
+  uint desc_num_side_spatial_bins_;
+  /** \short Total size of the descriptors. */
+  uint desc_num_total_bins_;
+  /** \short Radius of area used to compute descriptor. */
+  uint desc_patch_radius_;
+  /** \short Filename of the normal centroids. */
+  std::string centroids_file_;
+  /** \short File with positions to compute descriptors.
+
+    Filename of the file with positions where descriptors have to be computed (if specidied overrides "sample_each"). */
+  std::string positions_file_;
+  /** \short File with PCA matrix
+
+    Name of file with mean and PCA matrix to compress descriptors on the fly. If not specified no compression is done. */
+  std::string pca_file_;
+  /** \short Number of eigenvectors to keep in the PCA matrix (=0 to keep all of them)*/
+  int pca_nkeep_;
+  /** \short Normalization of new descriptors.
+
+    Normalization of new descriptors:
+    - 0 - No norm
+    - 1 - L1
+    - 2 - L2
+    - 3 - sqrt + L2
+  */
+  uint normalize_desc_;
+  /** \short Whether to use soft-voting or direct assignment. */
+  uint use_soft_voting_;
+  /** \short Level of verbosity. */
+  int verbose_level_;
+  /** \short Step in pixels between descriptors. */
+  uint sample_each_;
+  /** \short Margin to skip around the image. Currently this variable is unused. */
+  uint margin_;
+
+  // [PCA stuff]
+  /** \short Variable holding the PCA matrix for on the fly compression. */
+  std::vector< std::vector<float> > pca_matrix_;
+  /** \short Mean of descriptors for PCA compression. */
+  std::vector<float> pca_mean_;
+public:
+ CFinddd_Descriptor();
+
+
+ /**
+ * \brief constructor.
+ *
+ * Constructor that takes a FindddConfig object as input parameter.
+ *
+ */
+ CFinddd_Descriptor(FindddConfig conf);
+
+
+ /**
+ * \brief Set total descriptor bins.
+ *
+ * Given the current parameters, this function computes the total
+ * number of bins of the descriptor.
+ *
+ */
+inline int compute_total_bins();
+
+
+/**
+ * \brief Set number of spatial bins (side).
+ * \param nb Number of spatial bins per side of the descriptor support area.
+ * \return New number of spatial bins per size (same as input).
+ * This function allows to change the number of spatial bins of
+ * the descriptor, and triggers all actions dependant on them.
+ *
+ */
+inline int set_num_side_spatial_bins(int nb);
+
+
+/**
+ * \brief Set patch radius.
+ * \param r Radius of the descriptor support region.
+ * \return New radius of descriptor support region (same as input).
+ * This function allows to change the patch radius of the
+ * descriptor, and triggers all actions dependant on them.
+ *
+ */
+inline int set_desc_patch_radius(int r);
+
+
+/**
+ * \brief Set sample each
+ * \param s Step at which descriptors have to be computed.
+ * \return New step at which descriptors have to be computed (same as input).
+ * Function used to change the spacing between each sampled
+ * position of the grid for descriptor computation.
+ *
+ */
+inline int set_sample_each(int s);
+
+
+/**
+ * \brief Update PCA matrix.
+ * \param new_pca_matrix_file File with the PCA matrix and mean vector.
+ * \param pca_nkeep Number of ev that must be kept.
+ * Update the PCA matrix used to project the computed
+ * descriptors. The function expects a string pointing
+ * to a plain text file with the mean vector followed by
+ * the PCA matrix.
+ *
+ */
+inline void update_pca_matrix(std::string new_pca_matrix_file, int pca_nkeep);
+
+
+/**
+ * \brief Project descriptor using PCA.
+ * \param v Descriptor to compress.
+ * \param PCA PCA matrix.
+ * \param mean Mean vector to center descriptor vector.
+ * \return Compressed descriptor vector.
+ * Project a newly computed descriptor using the PCA
+ * matrix passed as parameter. This function is automatically
+ * executed if "pca_file_" is not NULL.
+ *
+ */
+std:: vector<float> project_vector_pca(std::vector<float> &v, std::vector< std::vector<float> > &PCA, std::vector<float> &mean);
+
+
+/**
+ * \brief Set centroids file.
+ * \param new_centroids_file File containing the normal centroids.
+ * Function used to load the bin centers from a file
+ *
+ */
+inline void set_centroids_file(std::string new_centroids_file);
+
+
+/**
+ * \brief Load set of normal centers.
+ *
+ * Update centroids with contents of the file pointed by the centroids_file_ variable.
+ * TODO Not much checking is done to guarantee that the file is consistent!
+ *
+ */
+inline void update_centroids();
+
+
+/**
+ * \brief Set positions file.
+ * \param new_positions_file
+ * Function used to load the positions of the image where descriptors
+ * will be computed from a list in a file.
+ * TODO Not much checking is done to guarantee that the file is consistent!
+ *
+ */
+inline void set_positions_file(std::string new_positions_file);
+
+
+/**
+ * \brief Determine vote influence cutoff.
+ *
+ * Function to compute minimum distance between bin 0 and the rest of bins,
+ * for vote interpolation.
+ */
+inline void compute_radius_inf();
+
+
+/**
+ * \brief compute all descriptors with spatial bin support of image using an integral image.
+ * \param cloud input structured point cloud loaded from pcd file.
+ * \param descriptor_set Output computed descriptors.
+ * Main interface to compute normal descriptors with spatial support from a point cloud
+ */
+void compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, DescriptorSet &descriptor_set);
+
+ /**
+ * \brief Destructor.
+ *
+ * This destructor is called when the object is about to be destroyed.
+ *
+ */
+ ~CFinddd_Descriptor();
+};
+
+#endif
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 6062c4eb2b74661fd1cff362b409226c21542172..07430f0f7b1cab21da232d5c15e5ad3031ec6f0a 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,11 @@
+# driver source files
+SET(sources finddd_descriptor.cpp)
+# application header files
+SET(headers ../include/finddd_descriptor.h)
 # locate the necessary dependencies
-FIND_PACKAGE( Boost 1.46 COMPONENTS program_options REQUIRED )
-INCLUDE_DIRECTORIES( ${Boost_INCLUDE_DIR} )
 
-#IF PCL in non-standard directory, uncomment this: 
-#set(PCL_DIR "<path-to-pcl>/PCLConfig.cmake") 
+#IF PCL in non-standard directory, uncomment this:
+#set(PCL_DIR "<path-to-pcl>/PCLConfig.cmake")
 find_package(PCL  1.3  REQUIRED  COMPONENTS  common io features )
 include_directories(${PCL_INCLUDE_DIRS})
 link_directories(${PCL_LIBRARY_DIRS})
@@ -12,13 +14,15 @@ add_definitions(${PCL_DEFINITIONS})
 SET(CMAKE_BUILD_TYPE "RELEASE")
 
 # add the necessary include directories
-INCLUDE_DIRECTORIES(.)
-# application source files
-SET(sources finddd_descriptor.cpp)
-# application header files
-SET(headers finddd_descriptor.hpp)
-# create the executable file
-ADD_EXECUTABLE(finddd_descriptor ${sources})
+INCLUDE_DIRECTORIES(../include)
+# create the shared library
+ADD_LIBRARY(finddd_descriptor SHARED ${sources})
 # link necessary libraries
+INSTALL(TARGETS finddd_descriptor
+        RUNTIME DESTINATION bin
+        LIBRARY DESTINATION lib/iridrivers
+        ARCHIVE DESTINATION lib/iridrivers)
+INSTALL(FILES ${headers} DESTINATION include/iridrivers)
+INSTALL(FILES ../Findfinddd_descriptor.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+ADD_SUBDIRECTORY(examples)
 target_link_libraries(finddd_descriptor  ${Boost_LIBRARIES} ${PCL_COMMON_LIBRARIES}  ${PCL_IO_LIBRARIES} ${PCL_FEATURES_LIBRARIES})
-#target_link_libraries(finddd_descriptor  ${Boost_LIBRARIES} ${PCL_LIBRARY_DIRS})
diff --git a/examples/50positions.txt b/src/examples/50positions.txt
similarity index 100%
rename from examples/50positions.txt
rename to src/examples/50positions.txt
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..083637655e688b16a6ef5434dc1f453ba698f804
--- /dev/null
+++ b/src/examples/CMakeLists.txt
@@ -0,0 +1,4 @@
+# create an example application
+ADD_EXECUTABLE(finddd_descriptor_test finddd_descriptor_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(finddd_descriptor_test finddd_descriptor)
diff --git a/examples/display_finddd.py b/src/examples/display_finddd.py
similarity index 100%
rename from examples/display_finddd.py
rename to src/examples/display_finddd.py
diff --git a/examples/example.pcd b/src/examples/example.pcd
similarity index 100%
rename from examples/example.pcd
rename to src/examples/example.pcd
diff --git a/examples/example.png b/src/examples/example.png
similarity index 100%
rename from examples/example.png
rename to src/examples/example.png
diff --git a/src/examples/finddd_descriptor_test.cpp b/src/examples/finddd_descriptor_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..38d01eff6f7170b0ee9271c42d91ca3a11c6e24d
--- /dev/null
+++ b/src/examples/finddd_descriptor_test.cpp
@@ -0,0 +1,195 @@
+#include "finddd_descriptor.h"
+
+/** Method parameters help.
+ */
+void about()
+{
+  std::cout<<"Usage:\n  finddd_descriptor -pcd <file_path> -centroids_file <centroids_path> -descfile <output_descfile_path> [-use_soft_voting <bool>] [-sample_each <step>] [-normalize_desc] [-desc_patch_radius <radius>] [-num_spatial_bins <num_bins_per_side>] [-positions_file <positions_file_path>] [-keep_nan] [-pca_file <file_path>] [-pca_nkeep <num ev to keep>] [-ascii]"<<std::endl;
+  std::cout<<"Parameters:\n  -pcd <filename>              Input pcd file path.\n  -use_soft_voting <bool>      Disable soft-voting.\n  -sample_each <num_pix>       Density of descriptor sampling.\n  -normalize_desc <norm>       0=none 1=L1 2=L2 3=sqrt+L2.\n  -desc_patch_radius <num_pix> Radius of the local patch.\n  -num_spatial_bins <num_bins> Number of local bins per side (sqrt of total bins).\n  -centroids_file <filename>   Input centroid file path.\n  -positions_file <filename>   Input locations file path.\n  -descfile <filename>         Output descriptor file path.\n  -pca_file <filename>         PCA matrix filename\n  -pca_nkeep <num_ev_to_keep>  Number of eigenvectors to keep (output dimension).\n  -ascii                       Save descriptors in ASCII format.\n";
+}
+
+/**
+ * \param argc Standard argc, number of arguments.
+ * \param argv Standard argv, arguments in vector of char pointers.
+ * \param new_conf Output configuration object with parameters according to arguments.
+ * \return Boolean, true if all parameters valid.
+ *
+ * Function to parse command-line arguments.
+ */
+bool parse_args(int argc, char* argv[], FindddConfig& new_conf)
+{
+  bool valid_args=true;
+  for(int i=1; i<argc; i++)
+  {
+    std::string arg(argv[i]);
+    if(arg == "-pcd") {
+      new_conf.pcd_file = std::string(argv[++i]);
+    } else if(arg == "-use_soft_voting") {
+      new_conf.use_soft_voting = atoi(argv[++i]);
+    } else if(arg == "-sample_each") {
+      new_conf.sample_each = atoi(argv[++i]);
+    } else if(arg == "-normalize_desc") {
+      new_conf.normalize_desc = atoi(argv[++i]);
+    } else if(arg == "-desc_patch_radius") {
+      new_conf.desc_patch_radius = atoi(argv[++i]);
+    } else if(arg == "-num_spatial_bins") {
+      new_conf.num_spatial_bins = atoi(argv[++i]);
+    } else if(arg == "-centroids_file") {
+      new_conf.centroids_file = std::string(argv[++i]);
+    } else if(arg == "-pca_file") {
+      new_conf.pca_file = std::string(argv[++i]);
+    } else if(arg == "-pca_nkeep") {
+      new_conf.pca_nkeep = atoi(argv[++i]);
+    } else if(arg == "-positions_file") {
+      new_conf.positions_file = std::string(argv[++i]);
+    } else if(arg == "-descfile") {
+      new_conf.geofvecs_file = std::string(argv[++i]);
+    } else if(arg == "-keep_nan") {
+      new_conf.keep_nan_points = 1;
+    } else if(arg == "-ascii") {
+      new_conf.save_in_ascii = 1;
+    } else {
+      std::cerr<<"Unknown argument: "<<arg<<"."<<std::endl;
+      return false;
+    }
+  }
+  if(new_conf.pcd_file=="")
+  {
+    std::cerr<<"Missing input pcd file."<<std::endl;
+    valid_args=false;
+  }
+  if (new_conf.centroids_file=="")
+  {
+    std::cerr<<"Missing centroids file."<<std::endl;
+    valid_args=false;
+  }
+  if(new_conf.geofvecs_file=="")
+  {
+    std::cerr<<"Missing output descriptorsfile."<<std::endl;
+    valid_args=false;
+  }
+  if(valid_args==true)
+    return true;
+  else
+    return false;
+}
+
+/**
+ * \param pf File pointer where to save the data.
+ * \param point Descriptor to be saved.
+ *
+ * Function to write a descriptor to a file pointer in fvec format (see the yael library for more details: https://gforge.inria.fr/projects/yael/)
+ * Save descs in fvec format.
+ */
+void write_point_ascii(FILE *pf, const Descriptor &point)
+{
+  int dim=point.descriptor.size();
+  if (fprintf(pf, "%d %d %d ", point.u, point.v, dim)<0)
+  {
+    fprintf(stderr, "Error writting to file\n"); exit(-1);
+  }
+  for(unsigned int j=0;j<dim-2;j++)
+    if (fprintf(pf, "%f ", point.descriptor[j])<0)
+    {
+      fprintf(stderr, "Error writting to file\n"); exit(-1);
+    }
+  fprintf(pf, "\n");
+}
+
+/**
+ * \param pf File pointer where to save the data.
+ * \param point Descriptor to be saved.
+ *
+ * Function to write a descriptor to a file pointer in fvec format (see the yael library for more details: https://gforge.inria.fr/projects/yael/)
+ * Save descs in fvec format.
+ */
+void write_point_fvec(FILE *pf, const Descriptor &point)
+{
+  int dim=point.descriptor.size();
+  float f;
+  dim+=2; //adding x,y coord
+  if (fwrite (&(dim), sizeof (int), 1, pf) != 1)
+  {
+    fprintf(stderr, "Error writting to file\n"); exit(-1);
+  }
+  f=point.u;
+  if (fwrite (&f, sizeof (float), 1, pf) != 1)
+  {
+    fprintf(stderr, "Error writting to file\n"); exit(-1);
+  }
+  f=point.v;
+  if (fwrite (&f, sizeof (float), 1, pf) != 1)
+  {
+    fprintf(stderr, "Error writting to file\n"); exit(-1);
+  }
+  for(unsigned int j=0;j<dim-2;j++)
+    if (fwrite (&(point.descriptor[j]), sizeof (float), 1, pf) != 1)
+    {
+      fprintf(stderr, "Error writting to file\n"); exit(-1);
+    }
+}
+
+/**
+ * \param output_file Filename where descriptors will be saved.
+ * \param descriptor_set Descriptor set to be saved.
+ * \param conf Configureation parameters of the descriptors.
+ * Function to save to disc a set of descriptors computed in a range image. The conf parameter is used to check if descriptors corresponding to NaN 3D points have to be saved or discarded.
+ * Save valid descriptors to disk.
+ */
+void write_feats(std::string &output_file, DescriptorSet &descriptor_set, FindddConfig &conf)
+{
+  FILE* pf=0;
+  if(conf.save_in_ascii)
+    pf=fopen(output_file.c_str(),"w");
+  else
+    pf=fopen(output_file.c_str(),"wb");
+  int saved_points = 0;
+
+  for(int ii=0;ii<descriptor_set.num;ii++)
+  {
+    if( (!isnan(descriptor_set.descriptors[ii].point3d.x) &&
+	 !isnan(descriptor_set.descriptors[ii].point3d.y) &&
+	 !isnan(descriptor_set.descriptors[ii].point3d.z)) ||
+	conf.keep_nan_points)
+    {
+      if(conf.save_in_ascii) write_point_ascii(pf, descriptor_set.descriptors[ii]);
+      else write_point_fvec(pf, descriptor_set.descriptors[ii]);
+      saved_points++;
+    }
+  }
+  fclose(pf);
+}
+
+int main(int argc, char *argv[])
+{
+  // parse arguments
+  FindddConfig new_conf;
+  bool args_ok = false;
+  args_ok = parse_args(argc, argv, new_conf);
+  // For debug:
+  // std::cout<<"pcdfile="<<new_conf.pcd_file<<" centroids="<<new_conf.centroids_file<<" positions="<<new_conf.positions_file<<" geofvecsfile="<<new_conf.geofvecs_file<<" num_spatial_bins="<<new_conf.num_spatial_bins<<" desc_patch_radius="<<new_conf.desc_patch_radius<<" sample_each="<<new_conf.sample_each<<" normalize_desc="<<new_conf.normalize_desc<<" use_soft_voting="<<new_conf.use_soft_voting<<" keep_nan_points="<<new_conf.keep_nan_points<<std::endl;
+
+  if(args_ok)
+  {
+    // create finddd object
+    CFinddd_Descriptor comp(new_conf);
+    // load pointcloud
+    pcl::PointCloud<pcl::PointXYZ> cloud;
+    if(pcl::io::loadPCDFile<pcl::PointXYZ>(new_conf.pcd_file.c_str(), cloud)==-1)
+    {
+      std::cerr<<"Error in pcd file: "<<new_conf.pcd_file<<std::endl;
+      exit(-1);
+    }
+    DescriptorSet descriptor_set;
+    // compute features
+    comp.compute_ndescs_integral_spatial_interpolation(cloud, descriptor_set);
+    // save features to disk
+    write_feats(new_conf.geofvecs_file, descriptor_set, new_conf);
+  }
+  else
+  {
+    about();
+  }
+
+  return 0;
+}
diff --git a/examples/pca_matrix_o13_d43.txt b/src/examples/pca_matrix_o13_d43.txt
similarity index 100%
rename from examples/pca_matrix_o13_d43.txt
rename to src/examples/pca_matrix_o13_d43.txt
diff --git a/src/examples/run_example.sh b/src/examples/run_example.sh
new file mode 100644
index 0000000000000000000000000000000000000000..37266535f68c81093e00154eb651656acc995a14
--- /dev/null
+++ b/src/examples/run_example.sh
@@ -0,0 +1,36 @@
+#!/bin/bash
+
+mkdir test
+
+# Step size
+echo "STEP SIZE: 6, 3, 1"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -descfile test/features_step6.fvecs
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 3 -centroids_file xyz_centers13.txt -descfile test/features_step3.fvecs
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 1 -centroids_file xyz_centers13.txt -descfile test/features_step1.fvecs
+
+# Descriptor area
+echo "SUPPORT AREA: 61x61, 81x81, 121x121"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 61 -descfile test/features_rad61.fvecs
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 81 -descfile test/features_rad81.fvecs
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -desc_patch_radius 121 -descfile test/features_rad121.fvecs
+
+# Num spatial bins
+echo "NUMBER OF SPATIAL BINS: 2x2, 5x5"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -num_spatial_bins 2 -descfile test/features_spa2.fvecs
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -num_spatial_bins 5 -descfile test/features_spa5.fvecs
+
+# Disable soft voting
+echo "SOFT VOTING DISABLED"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -use_soft_voting 0 -descfile test/features_nosv.fvecs
+
+# Custom descriptor positions
+echo "CUSTOM DESCRIPTOR POSITIONS"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -positions_file 50positions.txt -descfile test/features_locations.fvecs
+
+#PCA compression
+echo "PCA COMPRESSION: 33"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -pca_file pca_matrix_o13_d43.txt -pca_nkeep 33 -descfile test/features_pca33.fvecs
+
+#ASCII OUTPUT
+echo "ASCII OUTPUT"
+../../bin/finddd_descriptor_test -pcd example.pcd -sample_each 6 -centroids_file xyz_centers13.txt -ascii -descfile test/features_ascii.txt
diff --git a/examples/xyz_centers13.txt b/src/examples/xyz_centers13.txt
similarity index 100%
rename from examples/xyz_centers13.txt
rename to src/examples/xyz_centers13.txt
diff --git a/src/finddd_descriptor.cpp b/src/finddd_descriptor.cpp
old mode 100755
new mode 100644
index 2408b2c3a5bac821db443c1ee6da0ac11d8f6be4..55b26c082336b31e76d8c97ded126892b3fb4e58
--- a/src/finddd_descriptor.cpp
+++ b/src/finddd_descriptor.cpp
@@ -1,248 +1,113 @@
-#include "finddd_descriptor.hpp"
+#include "finddd_descriptor.h"
 
 
-#define PI 3.1415926535
-
-/** 
- * \param nb_bins Number of bins in a sub-descriptor.
- * \return Empty voting vector.
- *
- * Get empty normal voting vector.
- */
-inline std::vector<float> get_bin_float_empty(int nb_bins)
-{
-  std::vector<float> val_bins;
-  val_bins.resize(nb_bins,0);
-  return val_bins;
-}
-
-/** 
- * \param x X coordinate of normal 
- * \param y Y coordinate of normal 
- * \param z Z coordinate of normal 
- * \param bin_centers_x Vector with X coordinate of normal centers
- * \param bin_centers_y Vector with Y coordinate of normal centers
- * \param bin_centers_z Vector with Z coordinate of normal centers
- * \param radius_inf Inter-bin distance among the normal centers.
- * \return Voting vector corresponding to input normal.
- *
- * Get normal voting histogram *with* interpolation.
- */
-inline std::vector<float> get_bin_float_interp_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z, float radius_inf)
+DescriptorSet::DescriptorSet()
 {
-  int nb_bins=bin_centers_x.size();
-  std::vector<float> val_bins;
-  val_bins.resize(nb_bins,0);
-
-  for(int ith=0; ith<nb_bins; ith++)
-  {
-    float dx=(x-bin_centers_x[ith]);
-    float dy=(y-bin_centers_y[ith]);
-    float dz=(z-bin_centers_z[ith]);
-    float dist = sqrt(dx*dx+dy*dy+dz*dz);
-    //weight based on one unit of histogram distance
-    float vote = std::max(1-dist/radius_inf, (float)0.0);
-    val_bins[ith] = vote;
-  }
-  return val_bins;
+  num_orient_bins=0;
+  num_spa_bins=0;
+  num=0;
+  len=0;
+  width=0;
+  height=0;
 }
 
-/** 
- * \param x X coordinate of normal 
- * \param y Y coordinate of normal 
- * \param z Z coordinate of normal 
- * \param bin_centers_x Vector with X coordinate of normal centers
- * \param bin_centers_y Vector with Y coordinate of normal centers
- * \param bin_centers_z Vector with Z coordinate of normal centers
- * \return Voting vector corresponding to input normal.
- *
- * Get normal voting histogram *without* interpolation.
- */
-inline std::vector<float> get_bin_float_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z)
+FindddConfig::FindddConfig()
 {
-  int nb_bins=bin_centers_x.size();
-  std::vector<float> val_bins;
-  val_bins.resize(nb_bins,0);
-
-  int best_match=0;
-  float dist_best_match=999999999999;
-
-  for(int ith=0; ith<nb_bins; ith++)
-  {
-    float dx=(x-bin_centers_x[ith]);
-    float dy=(y-bin_centers_y[ith]);
-    float dz=(z-bin_centers_z[ith]);
-    float dist = sqrt(dx*dx+dy*dy+dz*dz);
-    if(dist < dist_best_match)
-    {
-      best_match = ith;
-      dist_best_match = dist;
-    }
-  }
-  //vote weight = 1
-  val_bins[best_match] = 1;
-
-  return val_bins;
+  verbose_level=1;
+  num_spatial_bins=4;
+  desc_patch_radius=21;
+  sample_each=6;
+  centroids_file="";
+  positions_file="";
+  normalize_desc=0;
+  use_soft_voting=1;
+  pcd_file="";
+  geofvecs_file="";
+  keep_nan_points=0;
+  pca_file="";
+  pca_nkeep=0;
+  save_in_ascii=0;
 }
 
-/**
- * /param pcl_normals Output cloud with 3D point normals computed using PCL library.
- * /param cloud Input cloud with 3D points we want to compute normals for.
- *
- * Function to compute normal vectors fast taking advantage of the fact that the input point cloud is structured. 
- * Compute pointcloud normals fast using integral images.
- */
-void compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
+IIf::IIf(unsigned int bins, unsigned int width, unsigned int height):bins_(bins),width_(width),height_(height)
 {
-  //compute normals
-  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
-  normal_estimator.setNormalEstimationMethod (normal_estimator.AVERAGE_3D_GRADIENT);
-  normal_estimator.setMaxDepthChangeFactor(0.02f);
-  normal_estimator.setNormalSmoothingSize(10.0f);
-  normal_estimator.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
-  normal_estimator.compute (pcl_normals);
-  //For debug: pcl::io::savePCDFileASCII("/tmp/cloud_pointnormals.pcd", pcl_normals);
+  ii_.resize(bins);
+  for(unsigned int i=0;i<bins;i++)
+    ii_[i]=new float[width*height];
 }
 
-/**
- * /param pcl_normals Output cloud with 3D point normals computed using PCL library.
- * /param cloud Input cloud with 3D points we want to compute normals for.
- *
- * This is a much slower function to compute normal vectors, apt for non-structured point clouds.
- * Compute pointcloud normals *without* using integral images.
- */
-void compute_normals(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
-{
-  //compute normal
-  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
-  normal_estimator.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
-  normal_estimator.setKSearch (400);
-  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
-  normal_estimator.setSearchMethod (tree);
-  normal_estimator.compute (pcl_normals);
+IIf::~IIf(){
+  for(unsigned int i=0;i<this->bins_;i++)
+    delete [] ii_[i];
 }
 
-/***************************************************************************************/
-/** 
- * \brief Simple Integral Images class.
- * Class to compute simple parallel integral images with floating point accumulators.
- * Includes function to compute sum of sub-rectangle.
- */
-class IIf
-{
-public:
-  /** \short Vector of float* to store the parallel integral images in linear form.*/
-  std::vector<float*> ii_;
-  /** \short Number of bins.*/
-  unsigned int bins_;
-  /** \short Width of the integral image.*/
-  unsigned int width_;
-  /** \short Height of the integral image.*/
-  unsigned int height_;
-
-  /**
-   * \param bins Number of bins/parallel integral images.
-   * \param width Width of the integral image.
-   * \param height Height of the integral image.
-   *
-   * Constructor for empty set of parallel integral image.
-   */
-  IIf(unsigned int bins, unsigned int width, unsigned int height):bins_(bins),width_(width),height_(height){
-    ii_.resize(bins);
-    for(unsigned int i=0;i<bins;i++)
-      ii_[i]=new float[width*height];
-  }
-
-  /** Destructor */
-  ~IIf(){
-    for(unsigned int i=0;i<this->bins_;i++)
-      delete [] ii_[i];
+void IIf::construct(std::vector<std::vector<float> > &im){
+  //0,0 case
+  unsigned int u=0;
+  unsigned int v=0;
+  for(unsigned int b=0;b<this->bins_;b++)
+  {
+    ii_[b][this->width_*v+u] = im[this->width_*v+u][b];
   }
-
-  /** 
-   * \param im Input vote matrix
-   *
-   * Function to construct the set of integral images given an input vote matrix.
-   */
-  void construct(std::vector<std::vector<float> > &im){
-    //0,0 case
-    unsigned int u=0;
-    unsigned int v=0;
+  //n,0 case
+  v=0;
+  for(u=1;u<this->width_;u++)
+  {
     for(unsigned int b=0;b<this->bins_;b++)
     {
-      ii_[b][this->width_*v+u] = im[this->width_*v+u][b];
+      ii_[b][this->width_*v+u] =		     \
+      ii_[b][this->width_*v+(u-1)] +	     \
+      im[this->width_*v+u][b];
     }
-    //n,0 case
-    v=0;
-    for(u=1;u<this->width_;u++)
+  }
+  //0,n case
+  u=0;
+  for(v=1;v<this->height_;v++)
+  {
+    for(unsigned int b=0;b<this->bins_;b++)
     {
-      for(unsigned int b=0;b<this->bins_;b++)
-      {
-	ii_[b][this->width_*v+u] =		     \
-	  ii_[b][this->width_*v+(u-1)] +	     \
-	  im[this->width_*v+u][b];
-      }
+      ii_[b][this->width_*v+u] =		     \
+      ii_[b][this->width_*(v-1)+u] +	     \
+      im[this->width_*v+u][b];
     }
-    //0,n case
-    u=0;
-    for(v=1;v<this->height_;v++)
+  }
+  //n,n case
+  for(v=1;v<this->height_;v++)
+  {
+    for(u=1;u<this->width_;u++)
     {
       for(unsigned int b=0;b<this->bins_;b++)
       {
-	ii_[b][this->width_*v+u] =		     \
-	  ii_[b][this->width_*(v-1)+u] +	     \
-	  im[this->width_*v+u][b];
-      }
-    }
-    //n,n case
-    for(v=1;v<this->height_;v++)
-    {
-      for(u=1;u<this->width_;u++)
-      {
-	for(unsigned int b=0;b<this->bins_;b++)
-	{
-	  ii_[b][this->width_*v+u] =		     \
-	    ii_[b][this->width_*(v-1)+u] +	     \
-	    ii_[b][this->width_*v+(u-1)] -	     \
-	    ii_[b][this->width_*(v-1)+(u-1)] +	     \
-	    im[this->width_*v+u][b];
-	}
+        ii_[b][this->width_*v+u] =		     \
+          ii_[b][this->width_*(v-1)+u] +	     \
+          ii_[b][this->width_*v+(u-1)] -	     \
+          ii_[b][this->width_*(v-1)+(u-1)] +	     \
+          im[this->width_*v+u][b];
       }
     }
   }
+}
 
-  /**
-   * 
-   * \param tu Top-left u coordinate.
-   * \param tv Top-left v coordinate.
-   * \param bu Bottom-right u coordinate.
-   * \param bv Bottom-right v coordinate.
-   * \return Vector with sum of the sub-region.
-   *
-   * Get sum for each parallel integral image for a rectangular sub-region.
-   */
-  std::vector<float> get_sub_rect(unsigned int tu, unsigned int tv, unsigned int bu, unsigned int bv)
+std::vector<float> IIf::get_sub_rect(unsigned int tu, unsigned int tv, unsigned int bu, unsigned int bv)
+{
+  std::vector<float> ret(this->bins_);
+  for(unsigned int b=0;b<this->bins_;b++)
   {
-    std::vector<float> ret(this->bins_);
-    for(unsigned int b=0;b<this->bins_;b++)
-    {
-      ret[b] = ii_[b][this->width_*tv+tu] + ii_[b][this->width_*bv+bu] - ii_[b][this->width_*tv+bu] - ii_[b][this->width_*bv+tu];
-      if(ret[b]<0){
-	ret[b]=0;
-      }
+    ret[b] = ii_[b][this->width_*tv+tu] + ii_[b][this->width_*bv+bu] - ii_[b][this->width_*tv+bu] - ii_[b][this->width_*bv+tu];
+    if(ret[b]<0){
+      ret[b]=0;
     }
-    return ret;
   }
-};
-/***************************************************************************************/
+  return ret;
+}
 
 // Constructor
-FindddAlgorithm::FindddAlgorithm(void)
+CFinddd_Descriptor::CFinddd_Descriptor()
 {
   //default values
   this->desc_num_side_spatial_bins_ = 1;
-  this->desc_patch_radius_=15;  
-  
+  this->desc_patch_radius_=15;
+
   //patch radius
   this->set_desc_patch_radius(15);
 
@@ -257,12 +122,8 @@ FindddAlgorithm::FindddAlgorithm(void)
   this->update_pca_matrix("",0);
 }
 
-FindddAlgorithm::~FindddAlgorithm(void)
-{
-}
-
 // Construct with config
-FindddAlgorithm::FindddAlgorithm(FindddConfig new_cfg)
+CFinddd_Descriptor::CFinddd_Descriptor(FindddConfig new_cfg)
 {
   this->set_num_side_spatial_bins(new_cfg.num_spatial_bins);
   this->set_desc_patch_radius(new_cfg.desc_patch_radius);
@@ -270,14 +131,234 @@ FindddAlgorithm::FindddAlgorithm(FindddConfig new_cfg)
   this->set_centroids_file(new_cfg.centroids_file);
   this->update_centroids();
   this->set_positions_file(new_cfg.positions_file);
-  this->normalize_desc_ = new_cfg.normalize_desc;  // * 0 - No norm // * 1 - L1 // * 2 - L2 // * 3 - sqrt + L2 
+  this->normalize_desc_ = new_cfg.normalize_desc;  // * 0 - No norm // * 1 - L1 // * 2 - L2 // * 3 - sqrt + L2
   this->use_soft_voting_ = new_cfg.use_soft_voting;
   this->verbose_level_ = new_cfg.verbose_level;
   this->update_pca_matrix(new_cfg.pca_file, new_cfg.pca_nkeep);
 }
 
+inline int CFinddd_Descriptor::compute_total_bins()
+{
+  this->desc_num_total_bins_ = this->desc_num_side_spatial_bins_ * this->desc_num_side_spatial_bins_ * this->orient_bin_centers_x_.size();
+  return this->desc_num_total_bins_;
+}
+
+inline int CFinddd_Descriptor::set_num_side_spatial_bins(int nb)
+{
+  this->desc_num_side_spatial_bins_ = nb;
+  this->compute_total_bins();
+  return this->desc_num_side_spatial_bins_;
+}
+
+inline int CFinddd_Descriptor::set_desc_patch_radius(int r)
+{
+  this->desc_patch_radius_ = r;
+  this->margin_ = this->desc_patch_radius_;
+  return this->desc_patch_radius_;
+}
+
+inline int CFinddd_Descriptor::set_sample_each(int s)
+{
+  this->sample_each_ = s;
+  return this->sample_each_;
+}
+
+inline void CFinddd_Descriptor::update_pca_matrix(std::string new_pca_matrix_file, int pca_nkeep)
+{
+  this->pca_file_ = new_pca_matrix_file;
+  this->pca_nkeep_=pca_nkeep;
+  if(new_pca_matrix_file=="")
+    return;
+  std::ifstream ifs(this->pca_file_.c_str(), std::ifstream::in);
+  if(!ifs.is_open())
+  {
+    if(this->verbose_level_>=1)
+      std::cout<<"FINDDD: could not open PCA file. "<<this->pca_file_<<std::endl;
+    exit(-1);
+  }
+  this->pca_matrix_.clear();
+  this->pca_mean_.clear();
+
+  std::string line;
+  if (ifs.good()) // First the mean!
+  {
+    float num;
+    getline(ifs, line);
+    std::istringstream iss(line);
+    int i=0;
+    do
+    {
+      iss >> num;
+      this->pca_mean_.push_back(num);
+    }
+    while(!iss.eof());
+  }
+  int dout=0;
+  while (ifs.good())
+  {
+    float num;
+    getline(ifs, line);
+    //line.erase(line.find_last_not_of(" \n\r\t")+1);
+    if(line.size()>0)
+    {
+      this->pca_matrix_.push_back(std::vector<float>());
+      std::istringstream iss(line);
+      do{
+        iss >> num;
+        this->pca_matrix_.back().push_back(num);
+      }
+      while(!iss.eof());
+      if(dout==0)
+        dout=this->pca_matrix_.back().size();
+      else if(dout!=this->pca_matrix_.back().size()){
+        if(this->verbose_level_>=1)
+          std::cout<<"Not consistent PCA data. Previous PCA matrix row width: "<<dout<<". Current PCA matrix row width: "<<this->pca_matrix_.back().size()<<std::endl;
+        exit(-1);
+        }
+      }
+    }
+  if(this->pca_mean_.size() != this->pca_matrix_.size())
+  {
+    if (this->verbose_level_>=1)
+      std::cout<<"Not consistent PCA data: mean vector size: "<<this->pca_mean_.size()<<" PCA matrix size: "<<this->pca_matrix_.size()<<"x"<<pca_matrix_[0].size()<<std::endl;
+    exit(-1);
+  }
+  //matrix correctly loaded, keeping only relevant eigenvectors and transposing for ease of use later
+  int nkeep=0;
+  if(pca_nkeep>0)
+    nkeep=pca_nkeep>this->pca_matrix_.size()?this->pca_matrix_.size():pca_nkeep; //min
+  else
+    nkeep=this->pca_matrix_.size();
+  std::vector< std::vector<float> > PCA_t;
+  PCA_t.resize(nkeep, std::vector<float>(this->pca_matrix_.size()));
+  for(int i=0;i<nkeep;i++)
+    for(int j=0;j<this->pca_matrix_.size();j++)
+    {
+      PCA_t[i][j]=this->pca_matrix_[j][i];
+    }
+  this->pca_matrix_=PCA_t;
+}
+
+
+std:: vector<float> CFinddd_Descriptor::project_vector_pca(std::vector<float> &v, std::vector< std::vector<float> > &PCA, std::vector<float> &mean)
+{
+  std::vector<float> o;
+  o.resize(PCA.size(),0);
+  //PCA matrix is transposed, so can be used directly with an iterator
+  std::vector<float>::iterator o_it = o.begin();
+  for(std::vector< std::vector<float> >::iterator PCA_it=PCA.begin(); PCA_it!=PCA.end(); PCA_it++)
+  {
+    for(std::vector<float>::iterator v_it=v.begin(), pcav_it=PCA_it->begin(), m_it=mean.begin(); v_it!=v.end(); v_it++, pcav_it++, m_it++)
+    {
+      *o_it += (*v_it-*m_it) * *pcav_it;
+    }
+    o_it++;
+  }
+  return o;
+}
+
+inline void CFinddd_Descriptor::set_centroids_file(std::string new_centroids_file)
+{
+  this->centroids_file_ = new_centroids_file;
+  this->update_centroids();
+}
+
+inline void CFinddd_Descriptor::update_centroids()
+{
+  std::string path = this->centroids_file_.c_str();
+  std::ifstream ifs(path.c_str(), std::ifstream::in);
+  if(!ifs.is_open())
+  {
+    if(this->verbose_level_>=1)
+      std::cout<<"FINDDD: Incorrect number of centers loaded from file "<<path<<std::endl;
+    exit(-1);
+  }
+
+  this->orient_bin_centers_x_.clear();
+  this->orient_bin_centers_y_.clear();
+  this->orient_bin_centers_z_.clear();
+
+  while (ifs.good()) //trusting the file...
+  {
+    float x,y,z;
+    ifs >> x;
+    ifs >> y;
+    ifs >> z;
+    if(ifs.good())
+    {
+      this->orient_bin_centers_x_.push_back(x);
+      this->orient_bin_centers_y_.push_back(y);
+      this->orient_bin_centers_z_.push_back(z);
+    }
+  }
+  ifs.close();
+  this->desc_num_orient_bins_ = this->orient_bin_centers_x_.size();
+
+  if(this->verbose_level_>=2)
+    std::cout<<"Loaded "<<this->desc_num_orient_bins_<<" centers"<<std::endl;
+  this->compute_total_bins();
+  this->compute_radius_inf();
+}
+
+inline void CFinddd_Descriptor::set_positions_file(std::string new_positions_file)
+{
+  this->positions_file_ = new_positions_file;
+
+  this->desc_compute_positionsX_.clear();
+  this->desc_compute_positionsY_.clear();
+
+  if(this->positions_file_=="") // No positions file specified.
+  {
+    if(this->verbose_level_>=2)
+      std::cout<<"Generating positions with 'sample_each' param."<<std::endl;
+    return;
+  }
+
+  std::string path = this->positions_file_.c_str();
+  std::ifstream ifs(path.c_str(), std::ifstream::in);
+  if(!ifs.is_open())
+  {
+    if(this->verbose_level_>=2)
+      std::cout<<"FINDDD: Incorrect descriptor positions file "<<path;
+    exit(-1);
+  }
+
+  while (ifs.good()) //trusting the file...
+  {
+    int x,y;
+    ifs >> x;
+    ifs >> y;
+    this->desc_compute_positionsX_.push_back(x);
+    this->desc_compute_positionsY_.push_back(y);
+  }
+  ifs.close();
+  if(this->verbose_level_>=2)
+    std::cout<<"Loaded "<<this->desc_compute_positionsX_.size()<<" positions"<<std::endl;
+}
+
+inline void CFinddd_Descriptor::compute_radius_inf()
+{
+  //finding minimum distance between point zero and the other points
+  float x = this->orient_bin_centers_x_[0];
+  float y = this->orient_bin_centers_y_[0];
+  float z = this->orient_bin_centers_z_[0];
+  int npoints = this->orient_bin_centers_x_.size();
+  this->radius_inf_ = 9999999999;
+  for(int i=1; i<npoints; i++)
+  {
+    // Maybe radius_inf could be squared to avoid the sqrt
+    float dist = sqrt(pow(x-this->orient_bin_centers_x_[i],2) +
+		pow(y-this->orient_bin_centers_y_[i],2) +
+		pow(z-this->orient_bin_centers_z_[i],2));
+    if(dist<this->radius_inf_)
+      this->radius_inf_ = dist;
+  }
+  if(this->verbose_level_>=2)
+    std::cout<<"Using ratio_inf_ = "<<this->radius_inf_<<std::endl;
+}
+
 // Main function to compute FINDDD descriptors
-void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, DescriptorSet &descriptor_set)
+void CFinddd_Descriptor::compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, DescriptorSet &descriptor_set)
 {
   clock_t start=clock();
   if(this->verbose_level_>=2)
@@ -286,7 +367,7 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     std::cout<<"PointCloud received"<<std::endl;
   }
   //compute normals
-  pcl::PointCloud<pcl::Normal> pcl_normals; 
+  pcl::PointCloud<pcl::Normal> pcl_normals;
   compute_normals_integral(pcl_normals, cloud);
 
   //set up object to hold descriptors
@@ -306,19 +387,24 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
   if(this->use_soft_voting_) {
     for(uint v=0;v<cloud.height;v++) {
       for(uint u=0;u<cloud.width;u++)  {
-	if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
-	  binned[u+cloud.width*v]=get_bin_float_empty(this->orient_bin_centers_x_.size()); // empty vector
-	else {
-	  binned[u+cloud.width*v] = get_bin_float_interp_xyz(pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_, this->radius_inf_);
-	} } } } 
+	       if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
+	         binned[u+cloud.width*v]=get_bin_float_empty(this->orient_bin_centers_x_.size()); // empty vector
+	       else {
+	          binned[u+cloud.width*v] = get_bin_float_interp_xyz(pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_, this->radius_inf_);
+	       }
+      }
+    }
+  }
   else {
     for(uint v=0;v<cloud.height;v++) {
       for(uint u=0;u<cloud.width;u++) {
-	if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
-	  binned[u+cloud.width*v] = get_bin_float_empty(this->orient_bin_centers_x_.size()); // empty vector
-	else {
-	  binned[u+cloud.width*v] = get_bin_float_xyz(pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_);
-	} } }
+	       if(isnan(pcl_normals(u,v).normal_x) || isnan(pcl_normals(u,v).normal_y) || isnan(pcl_normals(u,v).normal_z))
+	         binned[u+cloud.width*v] = get_bin_float_empty(this->orient_bin_centers_x_.size()); // empty vector
+	       else {
+	         binned[u+cloud.width*v] = get_bin_float_xyz(pcl_normals(u,v).normal_x, pcl_normals(u,v).normal_y, pcl_normals(u,v).normal_z, this->orient_bin_centers_x_, this->orient_bin_centers_y_, this->orient_bin_centers_z_);
+	       }
+      }
+    }
   }
 
   //Build II
@@ -338,13 +424,13 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     for(unsigned int v = this->desc_patch_radius_;v<cloud.height-this->desc_patch_radius_-1;v+=this->sample_each_)
     {
       for(unsigned int u = this->desc_patch_radius_;u<cloud.width-this->desc_patch_radius_-1;u+=this->sample_each_)
-      {      
-	this->desc_compute_positionsX_.push_back(u);
-	this->desc_compute_positionsY_.push_back(v);
+      {
+      	this->desc_compute_positionsX_.push_back(u);
+      	this->desc_compute_positionsY_.push_back(v);
       }
     }
   }
-  
+
   //Compute descriptors
   for(unsigned int i_pos=0; i_pos<this->desc_compute_positionsX_.size();i_pos++)
   {
@@ -352,10 +438,7 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     unsigned int v=this->desc_compute_positionsY_[i_pos];
 
     //skip invalid margin positions
-    if( v<this->desc_patch_radius_ || 
-	u<this->desc_patch_radius_ || 
-        u>=cloud.width-this->desc_patch_radius_-1 || 
-        v>=cloud.height-this->desc_patch_radius_-1)
+    if( v<this->desc_patch_radius_ || u<this->desc_patch_radius_ || u>=cloud.width-this->desc_patch_radius_-1 || v>=cloud.height-this->desc_patch_radius_-1)
     {
       continue;
     }
@@ -363,58 +446,58 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     desc.point3d.x=cloud(u,v).x;
     desc.point3d.y=cloud(u,v).y;
     desc.point3d.z=cloud(u,v).z;
-    
+
     //insert point2D
     desc.u=u;
     desc.v=v;
-    
+
     desc.descriptor.resize(this->desc_num_total_bins_,0);
     //float total_votes=0; // for normalization
-    
+
     unsigned int absolute_top  = v-this->desc_patch_radius_;
     //int absolute_bottom = v+this->desc_patch_radius_;
     unsigned int absolute_left = u-this->desc_patch_radius_;
     //int absolute_right = u+this->desc_patch_radius_;
-    
+
     for(unsigned int k=0;k<this->desc_num_side_spatial_bins_; k++)
     {
       unsigned int bound_left  = absolute_left + k*spatial_bin_size;
       unsigned int bound_right = absolute_left + (k+1)*spatial_bin_size;
       for(unsigned int l=0; l<this->desc_num_side_spatial_bins_; l++)
       {
-	unsigned int bound_top    = absolute_top + l*spatial_bin_size;
-	unsigned int bound_bottom = absolute_top + (l+1)*spatial_bin_size;
-
-	std::vector<float> tmp = intim.get_sub_rect(bound_left, bound_top, bound_right, bound_bottom);
-	for(unsigned int i=0;i<spatial_bin_orients;i++)
-	{
-	  unsigned int offset_full_rows = l*this->desc_num_side_spatial_bins_*spatial_bin_orients;
-	  unsigned int offset_this_row  = k*spatial_bin_orients;
-	  desc.descriptor[offset_full_rows + offset_this_row + i]=tmp[i];
-	  //total_votes+=tmp[i];
-	}
+      	 unsigned int bound_top    = absolute_top + l*spatial_bin_size;
+      	 unsigned int bound_bottom = absolute_top + (l+1)*spatial_bin_size;
+
+	       std::vector<float> tmp = intim.get_sub_rect(bound_left, bound_top, bound_right, bound_bottom);
+	       for(unsigned int i=0;i<spatial_bin_orients;i++)
+	       {
+        	  unsigned int offset_full_rows = l*this->desc_num_side_spatial_bins_*spatial_bin_orients;
+        	  unsigned int offset_this_row  = k*spatial_bin_orients;
+        	  desc.descriptor[offset_full_rows + offset_this_row + i]=tmp[i];
+        	  //total_votes+=tmp[i];
+	      }
       }
     }
- 
+
     //normalize desc if required
     if(this->normalize_desc_ != 0)
     {
       float sum=0;
       for(int iii=0; iii<desc.descriptor.size(); iii++)
       {
-	if((this->normalize_desc_==1) || (this->normalize_desc_==3)) 
-	  sum += desc.descriptor[iii];
-	else if(this->normalize_desc_==2) 
-	  sum += desc.descriptor[iii]*desc.descriptor[iii];
+      	if((this->normalize_desc_==1) || (this->normalize_desc_==3))
+      	  sum += desc.descriptor[iii];
+      	else if(this->normalize_desc_==2)
+      	  sum += desc.descriptor[iii]*desc.descriptor[iii];
       }
-      if(this->normalize_desc_==2) 
-	sum=sqrt(sum);
+      if(this->normalize_desc_==2)
+	       sum=sqrt(sum);
       for(int iii=0; iii<desc.descriptor.size(); iii++)
       {
-	if((this->normalize_desc_==1) || (this->normalize_desc_==2)) 
-	  desc.descriptor[iii] /= sum;
-	else if(this->normalize_desc_==3) 
-	  desc.descriptor[iii] = sqrt(desc.descriptor[iii])/sqrt(sum);
+      	if((this->normalize_desc_==1) || (this->normalize_desc_==2))
+      	  desc.descriptor[iii] /= sum;
+      	else if(this->normalize_desc_==3)
+      	  desc.descriptor[iii] = sqrt(desc.descriptor[iii])/sqrt(sum);
       }
     }
     //project w\ PCA if required
@@ -422,7 +505,7 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     {
       desc.descriptor = this->project_vector_pca(desc.descriptor, this->pca_matrix_, this->pca_mean_);
       if(this->verbose_level_>=2)
-	std::cout<<"Dimension of vector "<<desc.descriptor.size()<<" after projecting with PCA"<<std::endl;
+	       std::cout<<"Dimension of vector "<<desc.descriptor.size()<<" after projecting with PCA"<<std::endl;
     }
     descriptor_set.descriptors.push_back(desc);
     descriptor_set.num++;
@@ -431,201 +514,72 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
     std::cout<<"Computed "<<descriptor_set.num<<" descriptors, in "<<(clock()-start)/(float)CLOCKS_PER_SEC<< "s"<<std::endl;
 }
 
-/** Method parameters help.
- */
-void about()
+void CFinddd_Descriptor::compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
 {
-  std::cout<<"Usage:\n  finddd_descriptor -pcd <file_path> -centroids_file <centroids_path> -descfile <output_descfile_path> [-use_soft_voting <bool>] [-sample_each <step>] [-normalize_desc] [-desc_patch_radius <radius>] [-num_spatial_bins <num_bins_per_side>] [-positions_file <positions_file_path>] [-keep_nan] [-pca_file <file_path>] [-pca_nkeep <num ev to keep>] [-ascii]"<<std::endl;
-  std::cout<<"Parameters:\n  -pcd <filename>              Input pcd file path.\n  -use_soft_voting <bool>      Disable soft-voting.\n  -sample_each <num_pix>       Density of descriptor sampling.\n  -normalize_desc <norm>       0=none 1=L1 2=L2 3=sqrt+L2.\n  -desc_patch_radius <num_pix> Radius of the local patch.\n  -num_spatial_bins <num_bins> Number of local bins per side (sqrt of total bins).\n  -centroids_file <filename>   Input centroid file path.\n  -positions_file <filename>   Input locations file path.\n  -descfile <filename>         Output descriptor file path.\n  -pca_file <filename>         PCA matrix filename\n  -pca_nkeep <num_ev_to_keep>  Number of eigenvectors to keep (output dimension).\n  -ascii                       Save descriptors in ASCII format.\n";
+  //compute normals
+  pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
+  normal_estimator.setNormalEstimationMethod (normal_estimator.AVERAGE_3D_GRADIENT);
+  normal_estimator.setMaxDepthChangeFactor(0.02f);
+  normal_estimator.setNormalSmoothingSize(10.0f);
+  normal_estimator.setInputCloud(boost::make_shared<pcl::PointCloud<pcl::PointXYZ> > (cloud));
+  normal_estimator.compute (pcl_normals);
+  //For debug: pcl::io::savePCDFileASCII("/tmp/cloud_pointnormals.pcd", pcl_normals);
 }
 
-/** 
- * \param argc Standard argc, number of arguments.
- * \param argv Standard argv, arguments in vector of char pointers.
- * \param new_conf Output configuration object with parameters according to arguments.
- * \return Boolean, true if all parameters valid.
- *
- * Function to parse command-line arguments.
- */
-bool parse_args(int argc, char* argv[], FindddConfig& new_conf)
+inline std::vector<float> CFinddd_Descriptor::get_bin_float_empty(int nb_bins)
 {
-  bool valid_args=true;
-  for(int i=1; i<argc; i++)
-  {
-    std::string arg(argv[i]);
-    if(arg == "-pcd") {
-      new_conf.pcd_file = std::string(argv[++i]);
-    } else if(arg == "-use_soft_voting") {
-      new_conf.use_soft_voting = atoi(argv[++i]);
-    } else if(arg == "-sample_each") {
-      new_conf.sample_each = atoi(argv[++i]);
-    } else if(arg == "-normalize_desc") {
-      new_conf.normalize_desc = atoi(argv[++i]);
-    } else if(arg == "-desc_patch_radius") {
-      new_conf.desc_patch_radius = atoi(argv[++i]);
-    } else if(arg == "-num_spatial_bins") {
-      new_conf.num_spatial_bins = atoi(argv[++i]);
-    } else if(arg == "-centroids_file") {
-      new_conf.centroids_file = std::string(argv[++i]);
-    } else if(arg == "-pca_file") {
-      new_conf.pca_file = std::string(argv[++i]);
-    } else if(arg == "-pca_nkeep") {
-      new_conf.pca_nkeep = atoi(argv[++i]);
-    } else if(arg == "-positions_file") {
-      new_conf.positions_file = std::string(argv[++i]);
-    } else if(arg == "-descfile") {
-      new_conf.geofvecs_file = std::string(argv[++i]);
-    } else if(arg == "-keep_nan") {
-      new_conf.keep_nan_points = 1;
-    } else if(arg == "-ascii") {
-      new_conf.save_in_ascii = 1;
-    } else {
-      std::cerr<<"Unknown argument: "<<arg<<"."<<std::endl;
-      return false;
-    }
-  }
-  if(new_conf.pcd_file=="")
-  {
-    std::cerr<<"Missing input pcd file."<<std::endl;
-    valid_args=false;
-  }
-  if (new_conf.centroids_file=="")
-  {
-    std::cerr<<"Missing centroids file."<<std::endl;
-    valid_args=false;
-  }
-  if(new_conf.geofvecs_file=="")
-  {
-    std::cerr<<"Missing output descriptorsfile."<<std::endl;
-    valid_args=false;    
-  }
-  if(valid_args==true)
-    return true;
-  else
-    return false;
+  std::vector<float> val_bins;
+  val_bins.resize(nb_bins,0);
+  return val_bins;
 }
 
-/** 
- * \param pf File pointer where to save the data.
- * \param point Descriptor to be saved.
- *
- * Function to write a descriptor to a file pointer in fvec format (see the yael library for more details: https://gforge.inria.fr/projects/yael/)
- * Save descs in fvec format.
- */
-void write_point_ascii(FILE *pf, const Descriptor &point)
+inline std::vector<float> CFinddd_Descriptor::get_bin_float_interp_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z, float radius_inf)
 {
-  int dim=point.descriptor.size();
-  if (fprintf(pf, "%d %d %d ", point.u, point.v, dim)<0)
-  {
-    fprintf(stderr, "Error writting to file\n"); exit(-1);
-  }
-  for(unsigned int j=0;j<dim-2;j++)
-    if (fprintf(pf, "%f ", point.descriptor[j])<0)
-    {
-      fprintf(stderr, "Error writting to file\n"); exit(-1);
-    }
-  fprintf(pf, "\n");
-}
+  int nb_bins=bin_centers_x.size();
+  std::vector<float> val_bins;
+  val_bins.resize(nb_bins,0);
 
-/** 
- * \param pf File pointer where to save the data.
- * \param point Descriptor to be saved.
- *
- * Function to write a descriptor to a file pointer in fvec format (see the yael library for more details: https://gforge.inria.fr/projects/yael/)
- * Save descs in fvec format.
- */
-void write_point_fvec(FILE *pf, const Descriptor &point)
-{
-  int dim=point.descriptor.size();
-  float f;
-  dim+=2; //adding x,y coord
-  if (fwrite (&(dim), sizeof (int), 1, pf) != 1)
-  {
-    fprintf(stderr, "Error writting to file\n"); exit(-1);
-  }
-  f=point.u;
-  if (fwrite (&f, sizeof (float), 1, pf) != 1)
-  {
-    fprintf(stderr, "Error writting to file\n"); exit(-1);
-  }
-  f=point.v;
-  if (fwrite (&f, sizeof (float), 1, pf) != 1)
+  for(int ith=0; ith<nb_bins; ith++)
   {
-    fprintf(stderr, "Error writting to file\n"); exit(-1);
+    float dx=(x-bin_centers_x[ith]);
+    float dy=(y-bin_centers_y[ith]);
+    float dz=(z-bin_centers_z[ith]);
+    float dist = sqrt(dx*dx+dy*dy+dz*dz);
+    //weight based on one unit of histogram distance
+    float vote = std::max(1-dist/radius_inf, (float)0.0);
+    val_bins[ith] = vote;
   }
-  for(unsigned int j=0;j<dim-2;j++)
-    if (fwrite (&(point.descriptor[j]), sizeof (float), 1, pf) != 1)
-    {
-      fprintf(stderr, "Error writting to file\n"); exit(-1);
-    }
+  return val_bins;
 }
 
-/** 
- * \param output_file Filename where descriptors will be saved.
- * \param descriptor_set Descriptor set to be saved.
- * \param conf Configureation parameters of the descriptors.
- * Function to save to disc a set of descriptors computed in a range image. The conf parameter is used to check if descriptors corresponding to NaN 3D points have to be saved or discarded.
- * Save valid descriptors to disk.
- */
-void write_feats(std::string &output_file, DescriptorSet &descriptor_set, FindddConfig &conf)
+inline std::vector<float> CFinddd_Descriptor::get_bin_float_xyz(float x, float y, float z, std::vector<float> &bin_centers_x, std::vector<float> &bin_centers_y, std::vector<float> &bin_centers_z)
 {
-  FILE* pf=0;
-  if(conf.save_in_ascii)
-    pf=fopen(output_file.c_str(),"w");
-  else
-    pf=fopen(output_file.c_str(),"wb");
-  int saved_points = 0;
+  int nb_bins=bin_centers_x.size();
+  std::vector<float> val_bins;
+  val_bins.resize(nb_bins,0);
 
-  for(int ii=0;ii<descriptor_set.num;ii++)
-  {
-    if( (!isnan(descriptor_set.descriptors[ii].point3d.x) &&
-	 !isnan(descriptor_set.descriptors[ii].point3d.y) &&
-	 !isnan(descriptor_set.descriptors[ii].point3d.z)) ||
-	conf.keep_nan_points)
-    {
-      if(conf.save_in_ascii) write_point_ascii(pf, descriptor_set.descriptors[ii]);
-      else write_point_fvec(pf, descriptor_set.descriptors[ii]);
-      saved_points++;
-    }
-  }
-  fclose(pf);
-}
+  int best_match=0;
+  float dist_best_match=999999999999;
 
-int main(int argc, char* argv[])
-{
-  // parse arguments
-  FindddConfig new_conf;
-  bool args_ok = false;
-  args_ok = parse_args(argc, argv, new_conf);
-  // For debug:
-  // std::cout<<"pcdfile="<<new_conf.pcd_file<<" centroids="<<new_conf.centroids_file<<" positions="<<new_conf.positions_file<<" geofvecsfile="<<new_conf.geofvecs_file<<" num_spatial_bins="<<new_conf.num_spatial_bins<<" desc_patch_radius="<<new_conf.desc_patch_radius<<" sample_each="<<new_conf.sample_each<<" normalize_desc="<<new_conf.normalize_desc<<" use_soft_voting="<<new_conf.use_soft_voting<<" keep_nan_points="<<new_conf.keep_nan_points<<std::endl;
-
-  if(args_ok)
+  for(int ith=0; ith<nb_bins; ith++)
   {
-    // create finddd object
-    FindddAlgorithm comp(new_conf);
-    // load pointcloud
-    pcl::PointCloud<pcl::PointXYZ> cloud;
-    if(pcl::io::loadPCDFile<pcl::PointXYZ>(new_conf.pcd_file.c_str(), cloud)==-1)
+    float dx=(x-bin_centers_x[ith]);
+    float dy=(y-bin_centers_y[ith]);
+    float dz=(z-bin_centers_z[ith]);
+    float dist = sqrt(dx*dx+dy*dy+dz*dz);
+    if(dist < dist_best_match)
     {
-      std::cerr<<"Error in pcd file: "<<new_conf.pcd_file<<std::endl;
-      exit(-1);
+      best_match = ith;
+      dist_best_match = dist;
     }
-    DescriptorSet descriptor_set;
-    // compute features
-    comp.compute_ndescs_integral_spatial_interpolation(cloud, descriptor_set);
-    // save features to disk
-    write_feats(new_conf.geofvecs_file, descriptor_set, new_conf);
   }
-  else
-  {
-    about();
-  }
-  
-  return 0;
-}
-
-
+  //vote weight = 1
+  val_bins[best_match] = 1;
 
+  return val_bins;
+}
 
 
+CFinddd_Descriptor::~CFinddd_Descriptor()
+{
+}
diff --git a/src/finddd_descriptor.hpp b/src/finddd_descriptor.hpp
deleted file mode 100644
index f2ee768acf7a16f9c3557936a318444570004cd4..0000000000000000000000000000000000000000
--- a/src/finddd_descriptor.hpp
+++ /dev/null
@@ -1,553 +0,0 @@
-
-#ifndef _finddd_alg_h_
-#define _finddd_alg_h_
-
-#include <pcl/point_cloud.h>
-#include <pcl/point_types.h>
-//#include <pcl/ros/conversions.h>
-#include <pcl/features/normal_3d.h>
-#include <pcl/features/integral_image_normal.h>
-#include <pcl/io/pcd_io.h>
-#include <boost/make_shared.hpp>
-#include <cmath>
-#include <vector>
-#include <sstream>
-#include <time.h>
-#include <iostream>
-#include <fstream>
-
-// util
-template <class T>
-inline std::string to_string (const T& t)
-{
-  std::stringstream ss;
-  ss << t;
-  return ss.str();
-}
-
-/** \brief Class for 3D point.
- *  Very simple class to hold 3D point position information.
- */
-class P3D
-{
-public:
-  float x;
-  float y;
-  float z;
-  
-  P3D():x(0),y(0),z(0) {}
-  P3D(float xx, float yy, float zz):x(xx),y(yy),z(zz) {}
-  
-};
-
-/** \brief Class to hold one FINDDD descriptor.
- *  This class holds each one of the FINDDD descriptors in a set. 
- *  %Descriptor data is stored in a vector<float>.
- */
-class Descriptor
-{
-public:
-  /** \short X-coordinate of the descriptor in image. */
-  int u;
-  /** \short Y-coordinate of the descriptor in image. */  
-  int v;
-  /** \short Orientation of the descriptor in image (not used yet). */
-  float orientation;  
-  /** \short Corresponding 3D coordinates of the descriptor. */
-  P3D point3d; 
-  /** \short %Descriptor vector. */
-  std::vector<float> descriptor;
-};
-
-/** \brief Class to hold FINDDD computation parameters.
- *  This class holds a set (vector) of FINDDD descriptors as well as 
- *  parameters of the descriptors in the set and sizes of the original
- *  range image.
- */
-class DescriptorSet
-{
-public:
-  /** \short Number of orientation bins of each sub-descriptor. */
-  int num_orient_bins;
-  /** \short Number of spatial bins per side of the full descriptor region (sqrt of total). */
-  int num_spa_bins;
-  /** \short Number of descriptors. */
-  int num;
-  /** \short Number of dimensions of the descriptor. */
-  int len;
-  /** \short Width of the original range image. */
-  int width;
-  /** \short Height of the original range image. */
-  int height;
-  /** \short Vector with computed descriptor. */
-  std::vector<Descriptor> descriptors;
-  
-  DescriptorSet()
-  {
-    num_orient_bins=0;
-    num_spa_bins=0;
-    num=0;
-    len=0;
-    width=0;
-    height=0;
-  }
-};
-
-
-/** \brief Class to hold FINDDD computation parameters.
- *  This class holds all parameters related to FINDDD computation, and is 
- *  used to update the FindddAlgorithm class with user-defined parameters.
- */
-class FindddConfig
-{
-public:
-  /** \short Num of spatial subdivisions. */
-  int num_spatial_bins;
-  /** \short Side of the patch in pixels. */
-  int desc_patch_radius;
-  /** \short Step in pixels between descriptors. */
-  int sample_each;
-  /** \short File with the normal centroids. */
-  std::string centroids_file;
-  /** \short File with the (u,v) positions where descriptors have to be computed (overrides "sample_each"). */
-  std::string positions_file;
-  /** \short Normalization of new descriptors.
-
-      Normalization of new descriptors:
-      - 0 - No norm
-      - 1 - L1
-      - 2 - L2
-      - 3 - sqrt + L2 
-  */
-  int normalize_desc;
-  /** \short Whether to use soft-voting or direct assignment. */
-  int use_soft_voting;
-  /** \short Input pcd file where descriptors have to be computed. */
-  std::string pcd_file;
-  /** \short Output file in "fvec" format containing the computed descriptors. */
-  std::string geofvecs_file;
-  /** \short Whether to keep NaN points or discard them. */
-  int keep_nan_points;
-  /** \short File with PCA matrix.
-
-      Name of a file containing the mean and the PCA matrix to automatically compress the computed descriptors. If not specified no compression is done. */
-  std::string pca_file;
-  /** \short Number of eigenvectors to keep in the PCA matrix (=0 to keep all of them). */
-  int pca_nkeep;
-  /** \short Verbosity level. */
-  int verbose_level;
-  /** \short save descriptors in ascii format */
-  int save_in_ascii;
-  FindddConfig()
-  {
-    verbose_level=1;
-    num_spatial_bins=4;
-    desc_patch_radius=21;
-    sample_each=6;
-    centroids_file="";
-    positions_file="";
-    normalize_desc=0;
-    use_soft_voting=1;
-    pcd_file="";
-    geofvecs_file="";
-    keep_nan_points=0;
-    pca_file="";
-    pca_nkeep=0;
-    save_in_ascii=0;
-  }
-};
-
-/** \brief Main class to compute FINDDD descriptors.
- * This class contains all the methods necessary to compute FINDDD
- * descriptors for a range image.
- */
-class FindddAlgorithm
-{
-protected:
-
-  // private attributes and methods
-  uint nt_;
-  
-  /** \short Vector containing the X coordinate of normal centroids. */
-  std::vector<float> orient_bin_centers_x_;
-  /** \short Vector containing the Y coordinate of normal centroids. */
-  std::vector<float> orient_bin_centers_y_;
-  /** \short Vector containing the Z coordinate of normal centroids. */
-  std::vector<float> orient_bin_centers_z_;
-  
-  /** \short Vector containing the U coordinate of points where descriptors have to be computed. */
-  std::vector<int> desc_compute_positionsX_;
-  /** \short Vector containing the V coordinate of points where descriptors have to be computed. */
-  std::vector<int> desc_compute_positionsY_;
-  /** \short Inter-bin distance, to compute soft-votes*/
-  float radius_inf_;
-  /** \short Number of orientation bins in each sub-descriptor. */
-  uint desc_num_orient_bins_;
-  /** \short Number of spatial bins per side of the full descriptor region (sqrt of total). */
-  uint desc_num_side_spatial_bins_;
-  /** \short Total size of the descriptors. */
-  uint desc_num_total_bins_;
-  /** \short Radius of area used to compute descriptor. */
-  uint desc_patch_radius_;
-  /** \short Filename of the normal centroids. */
-  std::string centroids_file_;
-  /** \short File with positions to compute descriptors.
-
-      Filename of the file with positions where descriptors have to be computed (if specidied overrides "sample_each"). */
-  std::string positions_file_;
-  /** \short File with PCA matrix
-
-      Name of file with mean and PCA matrix to compress descriptors on the fly. If not specified no compression is done. */
-  std::string pca_file_;
-  /** \short Number of eigenvectors to keep in the PCA matrix (=0 to keep all of them)*/
-  int pca_nkeep_;
-  /** \short Normalization of new descriptors.
-
-      Normalization of new descriptors:
-      - 0 - No norm
-      - 1 - L1
-      - 2 - L2
-      - 3 - sqrt + L2 
-  */
-  uint normalize_desc_;
-  /** \short Whether to use soft-voting or direct assignment. */
-  uint use_soft_voting_;
-  /** \short Level of verbosity. */
-  int verbose_level_;
-  /** \short Step in pixels between descriptors. */
-  uint sample_each_;
-  /** \short Margin to skip around the image. Currently this variable is unused. */
-  uint margin_;
-
-  // [PCA stuff]
-  /** \short Variable holding the PCA matrix for on the fly compression. */
-  std::vector< std::vector<float> > pca_matrix_;
-  /** \short Mean of descriptors for PCA compression. */
-  std::vector<float> pca_mean_;
-
-  public:
-  FindddAlgorithm();
-  /**
-   * \brief constructor.
-   *
-   * Constructor that takes a FindddConfig object as input parameter.
-   *
-   */
-  FindddAlgorithm(FindddConfig conf);
-
-  /**
-   * \brief Set total descriptor bins.
-   * 
-   * Given the current parameters, this function computes the total
-   * number of bins of the descriptor.
-   *
-   */
-  inline int compute_total_bins()
-  {
-    this->desc_num_total_bins_ = this->desc_num_side_spatial_bins_ * this->desc_num_side_spatial_bins_ * this->orient_bin_centers_x_.size();
-    return this->desc_num_total_bins_;
-  }
-  
-  /**
-   * \brief Set number of spatial bins (side).
-   * \param nb Number of spatial bins per side of the descriptor support area.
-   * \return New number of spatial bins per size (same as input). 
-   * This function allows to change the number of spatial bins of
-   * the descriptor, and triggers all actions dependant on them.
-   *
-   */
-  inline int set_num_side_spatial_bins(int nb)
-  {
-    this->desc_num_side_spatial_bins_ = nb;
-    this->compute_total_bins();
-    return this->desc_num_side_spatial_bins_;
-  }
-  
-
-  /**
-   * \brief Set patch radius.
-   * \param r Radius of the descriptor support region.
-   * \return New radius of descriptor support region (same as input).
-   * This function allows to change the patch radius of the
-   * descriptor, and triggers all actions dependant on them.
-   *
-   */
-  inline int set_desc_patch_radius(int r)
-  {
-    this->desc_patch_radius_ = r;
-    this->margin_ = this->desc_patch_radius_;
-    return this->desc_patch_radius_;
-  }
-
-  /**
-   * \brief Set sample each
-   * \param s Step at which descriptors have to be computed.
-   * \return New step at which descriptors have to be computed (same as input).
-   * Function used to change the spacing between each sampled
-   * position of the grid for descriptor computation.
-   *
-   */
-  inline int set_sample_each(int s)
-  {
-    this->sample_each_ = s;
-    return this->sample_each_;
-  }
-  
-  /**
-   * \brief Update PCA matrix.
-   * \param new_pca_matrix_file File with the PCA matrix and mean vector.
-   * \param pca_nkeep Number of ev that must be kept.
-   * Update the PCA matrix used to project the computed 
-   * descriptors. The function expects a string pointing
-   * to a plain text file with the mean vector followed by
-   * the PCA matrix.
-   *
-   */
-  inline void update_pca_matrix(std::string new_pca_matrix_file, int pca_nkeep)
-  {
-    this->pca_file_ = new_pca_matrix_file;
-    this->pca_nkeep_=pca_nkeep;
-    if(new_pca_matrix_file=="")
-      return;
-    std::ifstream ifs(this->pca_file_.c_str(), std::ifstream::in);
-    if(!ifs.is_open())
-    {
-      if(this->verbose_level_>=1) 
-	std::cout<<"FINDDD: could not open PCA file. "<<this->pca_file_<<std::endl;
-      exit(-1);
-    }
-    this->pca_matrix_.clear();
-    this->pca_mean_.clear();
-    
-    std::string line;
-    if (ifs.good()) // First the mean!
-    {
-      float num;
-      getline(ifs, line);
-      std::istringstream iss(line);
-      int i=0;
-      do 
-      {
-        iss >> num;
-        this->pca_mean_.push_back(num);
-      }while(!iss.eof());
-    }
-    int dout=0;
-    while (ifs.good())
-    {
-      float num;
-      getline(ifs, line);
-      //line.erase(line.find_last_not_of(" \n\r\t")+1);
-      if(line.size()>0)
-      {
-	this->pca_matrix_.push_back(std::vector<float>());
-	std::istringstream iss(line);
-	do 
-	{
-	  iss >> num;
-	  this->pca_matrix_.back().push_back(num);
-	}while(!iss.eof());
-	if(dout==0) dout=this->pca_matrix_.back().size();
-	else if(dout!=this->pca_matrix_.back().size())
-	{
-	  if(this->verbose_level_>=1)
-	    std::cout<<"Not consistent PCA data. Previous PCA matrix row width: "<<dout<<". Current PCA matrix row width: "<<this->pca_matrix_.back().size()<<std::endl;
-	  exit(-1);
-	}
-      }
-    }
-    if(this->pca_mean_.size() != this->pca_matrix_.size())
-    {
-      if (this->verbose_level_>=1)
-	std::cout<<"Not consistent PCA data: mean vector size: "<<this->pca_mean_.size()<<" PCA matrix size: "<<this->pca_matrix_.size()<<"x"<<pca_matrix_[0].size()<<std::endl;
-      exit(-1);
-    }
-    //matrix correctly loaded, keeping only relevant eigenvectors and transposing for ease of use later
-    int nkeep=0;
-    if(pca_nkeep>0)
-      nkeep=pca_nkeep>this->pca_matrix_.size()?this->pca_matrix_.size():pca_nkeep; //min
-    else
-      nkeep=this->pca_matrix_.size();
-    std::vector< std::vector<float> > PCA_t;
-    PCA_t.resize(nkeep, std::vector<float>(this->pca_matrix_.size()));
-    for(int i=0;i<nkeep;i++)
-      for(int j=0;j<this->pca_matrix_.size();j++)
-      {
-	PCA_t[i][j]=this->pca_matrix_[j][i];
-      }
-    this->pca_matrix_=PCA_t;
-  }
-
-  /**
-   * \brief Project descriptor using PCA.
-   * \param v Descriptor to compress. 
-   * \param PCA PCA matrix.
-   * \param mean Mean vector to center descriptor vector.
-   * \return Compressed descriptor vector.
-   * Project a newly computed descriptor using the PCA
-   * matrix passed as parameter. This function is automatically
-   * executed if "pca_file_" is not NULL.
-   *
-   */
-  std:: vector<float> project_vector_pca(std::vector<float> &v, std::vector< std::vector<float> > &PCA, std::vector<float> &mean)
-  {
-    std::vector<float> o;
-    o.resize(PCA.size(),0);
-    //PCA matrix is transposed, so can be used directly with an iterator
-    std::vector<float>::iterator o_it = o.begin();
-    for(std::vector< std::vector<float> >::iterator PCA_it=PCA.begin(); PCA_it!=PCA.end(); PCA_it++)
-    {
-      for(std::vector<float>::iterator v_it=v.begin(), pcav_it=PCA_it->begin(), m_it=mean.begin(); v_it!=v.end(); v_it++, pcav_it++, m_it++)
-      {
-	*o_it += (*v_it-*m_it) * *pcav_it;
-      }
-      o_it++;
-    }
-    return o;
-  }
-
-  /**
-   * \brief Set centroids file.
-   * \param new_centroids_file File containing the normal centroids.
-   * Function used to load the bin centers from a file
-   *
-   */
-  inline void set_centroids_file(std::string new_centroids_file)
-  {
-    this->centroids_file_ = new_centroids_file;
-    this->update_centroids();
-  }
-
-  /**
-   * \brief Load set of normal centers.
-   * 
-   * Update centroids with contents of the file pointed by the centroids_file_ variable.
-   * TODO Not much checking is done to guarantee that the file is consistent!
-   *
-   */
-  inline void update_centroids()
-  {
-    std::string path = this->centroids_file_.c_str();
-    std::ifstream ifs(path.c_str(), std::ifstream::in);
-    if(!ifs.is_open())
-    {
-      if(this->verbose_level_>=1) 
-	std::cout<<"FINDDD: Incorrect number of centers loaded from file "<<path<<std::endl;
-      exit(-1);
-    }
-
-    this->orient_bin_centers_x_.clear();
-    this->orient_bin_centers_y_.clear();
-    this->orient_bin_centers_z_.clear();
-    
-    while (ifs.good()) //trusting the file...
-    {
-      float x,y,z;
-      ifs >> x;
-      ifs >> y;
-      ifs >> z;
-      if(ifs.good())
-      {
-        this->orient_bin_centers_x_.push_back(x);
-        this->orient_bin_centers_y_.push_back(y);
-        this->orient_bin_centers_z_.push_back(z);
-      }
-    }
-    ifs.close();
-    this->desc_num_orient_bins_ = this->orient_bin_centers_x_.size();
-    
-    if(this->verbose_level_>=2)
-      std::cout<<"Loaded "<<this->desc_num_orient_bins_<<" centers"<<std::endl;
-    this->compute_total_bins();
-    this->compute_radius_inf();
-  }
-  
-  /**
-   * \brief Set positions file.
-   * \param new_positions_file
-   * Function used to load the positions of the image where descriptors
-   * will be computed from a list in a file.
-   * TODO Not much checking is done to guarantee that the file is consistent!
-   *
-   */
-  inline void set_positions_file(std::string new_positions_file)
-  {
-    this->positions_file_ = new_positions_file;
-    
-    this->desc_compute_positionsX_.clear();
-    this->desc_compute_positionsY_.clear();
-    
-    if(this->positions_file_=="") // No positions file specified.
-    {
-      if(this->verbose_level_>=2)
-	std::cout<<"Generating positions with 'sample_each' param."<<std::endl;
-      return;
-    }
-    
-    std::string path = this->positions_file_.c_str();
-    std::ifstream ifs(path.c_str(), std::ifstream::in); 
-    if(!ifs.is_open())
-    {
-      if(this->verbose_level_>=2)
-	std::cout<<"FINDDD: Incorrect descriptor positions file "<<path;
-      exit(-1);
-    }
-    
-    while (ifs.good()) //trusting the file...
-    {
-      int x,y;
-      ifs >> x;
-      ifs >> y;
-      this->desc_compute_positionsX_.push_back(x);
-      this->desc_compute_positionsY_.push_back(y);
-    }
-    ifs.close();
-    if(this->verbose_level_>=2)
-      std::cout<<"Loaded "<<this->desc_compute_positionsX_.size()<<" positions"<<std::endl;
-  }
-  
-  /**
-   * \brief Determine vote influence cutoff.
-   *
-   * Function to compute minimum distance between bin 0 and the rest of bins, 
-   * for vote interpolation. 
-   */    
-  inline void compute_radius_inf()
-  {
-    //finding minimum distance between point zero and the other points
-    float x = this->orient_bin_centers_x_[0];
-    float y = this->orient_bin_centers_y_[0];
-    float z = this->orient_bin_centers_z_[0];
-    int npoints = this->orient_bin_centers_x_.size();
-    this->radius_inf_ = 9999999999;
-    for(int i=1; i<npoints; i++)
-    {
-// Maybe radius_inf could be squared to avoid the sqrt
-      float dist = sqrt(pow(x-this->orient_bin_centers_x_[i],2) +
-			pow(y-this->orient_bin_centers_y_[i],2) +
-			pow(z-this->orient_bin_centers_z_[i],2));
-      if(dist<this->radius_inf_)
-	this->radius_inf_ = dist;
-    }
-    if(this->verbose_level_>=2) 
-      std::cout<<"Using ratio_inf_ = "<<this->radius_inf_<<std::endl;
-  }
-  
-  /**
-   * \brief compute all descriptors with spatial bin support of image using an integral image.
-   * \param cloud input structured point cloud loaded from pcd file.
-   * \param descriptor_set Output computed descriptors.
-   * Main interface to compute normal descriptors with spatial support from a point cloud
-   */
-  void compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, DescriptorSet &descriptor_set);
-  
-  /**
-   * \brief Destructor.
-   *
-   * This destructor is called when the object is about to be destroyed.
-   *
-   */
-  ~FindddAlgorithm(void);
-};
-
-#endif