Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

inlined setInputFeatureCloud() and nearestNeighborSearch() #891

Merged
merged 1 commit into from
Sep 11, 2014
Merged

inlined setInputFeatureCloud() and nearestNeighborSearch() #891

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
57 changes: 0 additions & 57 deletions registration/include/pcl/registration/impl/ppf_registration.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,63 +48,6 @@
#include <pcl/common/transforms.h>

#include <pcl/features/pfh.h>
//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
{
// Discretize the feature cloud and insert it in the hash map
feature_hash_map_->clear ();
unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
int d1, d2, d3, d4;
max_dist_ = -1.0;
alpha_m_.resize (n);
for (size_t i = 0; i < n; ++i)
{
std::vector <float> alpha_m_row (n);
for (size_t j = 0; j < n; ++j)
{
d1 = static_cast<int> (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_));
d2 = static_cast<int> (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_));
d3 = static_cast<int> (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_));
d4 = static_cast<int> (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_));
feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<size_t, size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m;

if (max_dist_ < feature_cloud->points[i*n + j].f4)
max_dist_ = feature_cloud->points[i*n + j].f4;
}
alpha_m_[i] = alpha_m_row;
}

internals_initialized_ = true;
}


//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
std::vector<std::pair<size_t, size_t> > &indices)
{
if (!internals_initialized_)
{
PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
return;
}

int d1 = static_cast<int> (floor (f1 / angle_discretization_step_)),
d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
d4 = static_cast<int> (floor (f4 / distance_discretization_step_));

indices.clear ();
HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
map_iterator_pair.first->second.second));
}


//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointSource, typename PointTarget> void
pcl::PPFRegistration<PointSource, PointTarget>::setInputTarget (const PointCloudTargetConstPtr &cloud)
Expand Down
4 changes: 2 additions & 2 deletions registration/include/pcl/registration/ppf_registration.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -285,7 +285,7 @@ namespace pcl
Eigen::Affine3f &pose2);
};
}

#ifdef PCL_NO_PRECOMPILE
#include <pcl/registration/impl/ppf_registration.hpp>

#endif // PCL_NO_PRECOMPILE
#endif // PCL_PPF_REGISTRATION_H_
71 changes: 63 additions & 8 deletions registration/src/ppf_registration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -35,12 +35,67 @@
* $Id$
*
*/
#include <pcl/registration/ppf_registration.h>

#ifndef PCL_NO_PRECOMPILE
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
#include <pcl/registration/impl/ppf_registration.hpp>

/** Re-enable these once all of registration is separated into H/HPP correctly. */
//#include <pcl/point_types.h>
//#include <pcl/impl/instantiate.hpp>
//#include <pcl/registration/ppf_registration.h>
//#include <pcl/registration/impl/ppf_registration.hpp>
//
//PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES));
//
PCL_INSTANTIATE_PRODUCT(PPFRegistration, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES));
#endif // PCL_NO_PRECOMPILE

void
pcl::PPFHashMapSearch::setInputFeatureCloud (PointCloud<PPFSignature>::ConstPtr feature_cloud)
{
// Discretize the feature cloud and insert it in the hash map
feature_hash_map_->clear ();
unsigned int n = static_cast<unsigned int> (sqrt (static_cast<float> (feature_cloud->points.size ())));
int d1, d2, d3, d4;
max_dist_ = -1.0;
alpha_m_.resize (n);
for (size_t i = 0; i < n; ++i)
{
std::vector <float> alpha_m_row (n);
for (size_t j = 0; j < n; ++j)
{
d1 = static_cast<int> (floor (feature_cloud->points[i*n+j].f1 / angle_discretization_step_));
d2 = static_cast<int> (floor (feature_cloud->points[i*n+j].f2 / angle_discretization_step_));
d3 = static_cast<int> (floor (feature_cloud->points[i*n+j].f3 / angle_discretization_step_));
d4 = static_cast<int> (floor (feature_cloud->points[i*n+j].f4 / distance_discretization_step_));
feature_hash_map_->insert (std::pair<HashKeyStruct, std::pair<size_t, size_t> > (HashKeyStruct (d1, d2, d3, d4), std::pair<size_t, size_t> (i, j)));
alpha_m_row [j] = feature_cloud->points[i*n + j].alpha_m;

if (max_dist_ < feature_cloud->points[i*n + j].f4)
max_dist_ = feature_cloud->points[i*n + j].f4;
}
alpha_m_[i] = alpha_m_row;
}

internals_initialized_ = true;
}


//////////////////////////////////////////////////////////////////////////////////////////////
void
pcl::PPFHashMapSearch::nearestNeighborSearch (float &f1, float &f2, float &f3, float &f4,
std::vector<std::pair<size_t, size_t> > &indices)
{
if (!internals_initialized_)
{
PCL_ERROR("[pcl::PPFRegistration::nearestNeighborSearch]: input feature cloud has not been set - skipping search!\n");
return;
}

int d1 = static_cast<int> (floor (f1 / angle_discretization_step_)),
d2 = static_cast<int> (floor (f2 / angle_discretization_step_)),
d3 = static_cast<int> (floor (f3 / angle_discretization_step_)),
d4 = static_cast<int> (floor (f4 / distance_discretization_step_));

indices.clear ();
HashKeyStruct key = HashKeyStruct (d1, d2, d3, d4);
std::pair <FeatureHashMapType::iterator, FeatureHashMapType::iterator> map_iterator_pair = feature_hash_map_->equal_range (key);
for (; map_iterator_pair.first != map_iterator_pair.second; ++ map_iterator_pair.first)
indices.push_back (std::pair<size_t, size_t> (map_iterator_pair.first->second.first,
map_iterator_pair.first->second.second));
}