diff --git a/src/finddd_descriptor.cpp b/src/finddd_descriptor.cpp
index 6a9187d65c9833ea00dc2e2ed35379e047322471..dbed46ac86049c0df3e0f15fa418d43a7799fd08 100755
--- a/src/finddd_descriptor.cpp
+++ b/src/finddd_descriptor.cpp
@@ -3,17 +3,16 @@
#define PI 3.1415926535
-//helper fun
-inline
-std::vector<float> get_bin_float_empty(int nb_bins)
+// Get empty normal voting histogram
+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;
}
-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)
+// 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)
{
int nb_bins=bin_centers_x.size();
std::vector<float> val_bins;
@@ -25,14 +24,15 @@ std::vector<float> get_bin_float_interp_xyz(float x, float y, float z, std::vect
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;
}
-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)
+// 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)
{
int nb_bins=bin_centers_x.size();
std::vector<float> val_bins;
@@ -53,14 +53,16 @@ std::vector<float> get_bin_float_xyz(float x, float y, float z, std::vector<floa
dist_best_match = dist;
}
}
+ //vote weight = 1
val_bins[best_match] = 1;
return val_bins;
}
+// Compute pointcloud normals fast using integral images
void compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
- //compute normal
+ //compute normals
pcl::IntegralImageNormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;
normal_estimator.setNormalEstimationMethod (normal_estimator.AVERAGE_3D_GRADIENT);
normal_estimator.setMaxDepthChangeFactor(0.02f);
@@ -70,6 +72,7 @@ void compute_normals_integral(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::Po
//For debug: pcl::io::savePCDFileASCII("/tmp/cloud_pointnormals.pcd", pcl_normals);
}
+// Compute pointcloud normals without using integral images
void compute_normals(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<pcl::PointXYZ>& cloud)
{
//compute normal
@@ -81,6 +84,7 @@ void compute_normals(pcl::PointCloud<pcl::Normal> &pcl_normals, pcl::PointCloud<
normal_estimator.compute (pcl_normals);
}
+/***************************************************************************************/
// Simple Integral Images class
class IIf
{
@@ -147,6 +151,7 @@ public:
}
}
+ //get sum of rectangular sub-region
std::vector<float> get_sub_rect(unsigned int tu, unsigned int tv, unsigned int bu, unsigned int bv)
{
std::vector<float> ret(this->bins_);
@@ -157,14 +162,12 @@ public:
ret[b]=0;
}
}
-
return ret;
}
};
+/***************************************************************************************/
-///////////////////////////////////////////////////////////////////////////////////////////
-
-
+// Constructor
FindddAlgorithm::FindddAlgorithm(void)
{
//default values
@@ -189,6 +192,7 @@ FindddAlgorithm::~FindddAlgorithm(void)
{
}
+// Construct with config
FindddAlgorithm::FindddAlgorithm(FindddConfig new_cfg)
{
this->set_num_side_spatial_bins(new_cfg.num_spatial_bins);
@@ -203,8 +207,7 @@ FindddAlgorithm::FindddAlgorithm(FindddConfig new_cfg)
this->update_pca_matrix(new_cfg.pca_file);
}
-// FindddAlgorithm Public API
-
+// Main function to compute FINDDD descriptors
void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCloud<pcl::PointXYZ>& cloud, DescriptorSet &descriptor_set)
{
@@ -214,11 +217,11 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
std::cout<<"II with interpolation method."<<std::endl;
std::cout<<"PointCloud received"<<std::endl;
}
- //compute normal
+ //compute normals
pcl::PointCloud<pcl::Normal> pcl_normals;
compute_normals_integral(pcl_normals, cloud);
- //compute descriptors
+ //set up object to hold descriptors
descriptor_set.descriptors.clear();
descriptor_set.len = this->desc_num_total_bins_;
descriptor_set.width = pcl_normals.width;
@@ -226,45 +229,28 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
descriptor_set.num_orient_bins = this->orient_bin_centers_x_.size();
descriptor_set.num_spa_bins = this->desc_num_side_spatial_bins_;
- //Compile all normal orientations and add them to the II
+ //Init II
std::vector<std::vector<float> > binned(cloud.width*cloud.height);
- if(this->use_soft_voting_)
- {
- for(uint v=0;v<cloud.height;v++)
- {
- for(uint u=0;u<cloud.width;u++)
- {
+ //Compile all normal orientations and add them to the II
+ //soft-voting vs not-soft-voting checked here for efficiency to avoid if in a loop.
+ //(sorry for the ugly code).
+ 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_);
- }
- }
- }
- }
- else
- {
- for(uint v=0;v<cloud.height;v++)
- {
- for(uint u=0;u<cloud.width;u++)
- {
+ 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_);
- }
- }
- }
+ 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
@@ -276,14 +262,14 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
unsigned int spatial_bin_size = (2*this->desc_patch_radius_+1)/this->desc_num_side_spatial_bins_;
unsigned int spatial_bin_orients = this->desc_num_orient_bins_;
- //If no positions given, generate standard positions
+ //If no positions file given, generate standard positions
if(this->desc_compute_positionsX_.size()==0)
{
if(this->verbose_level_>=2)
std::cout<<"Generating default positions"<<std::endl;
- for(unsigned int v=this->desc_patch_radius_;v<cloud.height-this->desc_patch_radius_-1;v+=this->sample_each_)
+ 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_)
+ 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);
@@ -297,6 +283,7 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
unsigned int u=this->desc_compute_positionsX_[i_pos];
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 ||
@@ -314,7 +301,7 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
desc.v=v;
desc.descriptor.resize(this->desc_num_total_bins_,0);
- float total_votes=0; // for normalization
+ //float total_votes=0; // for normalization
unsigned int absolute_top = v-this->desc_patch_radius_;
//int absolute_bottom = v+this->desc_patch_radius_;
@@ -336,27 +323,33 @@ void FindddAlgorithm::compute_ndescs_integral_spatial_interpolation(pcl::PointCl
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];
+ //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 necessary
+ //project w\ PCA if required
if(this->pca_file_!="")
{
desc.descriptor = this->project_vector_pca(desc.descriptor, this->pca_matrix_, this->pca_mean_);
@@ -370,14 +363,16 @@ 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()
{
- std::cout<<"Usage:\n finddd -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>]"<<std::endl;
+ 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>]"<<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";
}
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]);
@@ -408,33 +403,55 @@ bool parse_args(int argc, char* argv[], FindddConfig& new_conf)
return false;
}
}
- if (new_conf.centroids_file=="" || new_conf.geofvecs_file=="" || new_conf.pcd_file=="")
+ if(new_conf.pcd_file=="")
{
- std::cerr<<"Incorrect parameters."<<std::endl;
- about();
- return false;
+ 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;
}
- return true;
+ if(valid_args==true)
+ return true;
+ else
+ return false;
}
+//save descs in fvec format (see the yael library for more details. https://gforge.inria.fr/projects/yael/)
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);}
+ 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);}
+ 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);}
+ 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);}
+ if (fwrite (&(point.descriptor[j]), sizeof (float), 1, pf) != 1)
+ {
+ fprintf(stderr, "Error writting to file\n"); exit(-1);
+ }
}
+//save valid descriptors to disk
void write_feats(std::string &output_file, DescriptorSet &descriptor_set, FindddConfig &conf)
{
FILE* pf=fopen(output_file.c_str(),"wb");
diff --git a/src/finddd_descriptor.hpp b/src/finddd_descriptor.hpp
index 47aa5bfb07636c94a947fad8897164b8a0e76881..372b88994426635be3273c31d0c280bac17aa546 100644
--- a/src/finddd_descriptor.hpp
+++ b/src/finddd_descriptor.hpp
@@ -16,7 +16,7 @@
#include <iostream>
#include <fstream>
-
+// util
template <class T>
inline std::string to_string (const T& t)
{
@@ -25,6 +25,9 @@ inline std::string to_string (const T& t)
return ss.str();
}
+/** \brief Class for 3D point
+ * Very simple class to hold 3D point position information.
+ */
class P3D
{
public:
@@ -37,6 +40,10 @@ public:
};
+/** \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:
@@ -47,7 +54,11 @@ public:
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 and the set and sizes of the original
+ * range image.
+ */
class DescriptorSet
{
public:
@@ -71,6 +82,10 @@ public:
};
+/** \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:
@@ -104,6 +119,10 @@ public:
}
};
+/** \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: