Skip to content
Snippets Groups Projects
Commit 2dd20091 authored by Arnau Ramisa's avatar Arnau Ramisa
Browse files

changes

parent c41450ec
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/finddd_descriptor.cpp
+ 92
− 75
View file @ 2dd20091
......@@ -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");
......
src/finddd_descriptor.hpp
+ 21
− 2
View file @ 2dd20091
......@@ -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:
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment