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

Implementation of an OpenMP-parallelized version of Moving Least Squares #9

Merged
merged 1 commit into from
Mar 22, 2013
Merged

Implementation of an OpenMP-parallelized version of Moving Least Squares #9

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
110 changes: 98 additions & 12 deletions surface/include/pcl/surface/impl/mls.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,16 @@
#include <pcl/common/eigen.h>
#include <pcl/common/geometry.h>

#ifdef _OPENMP
#include <omp.h>
#endif

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
{
// Reset or initialize the collection of indices
corresponding_input_indices_.reset(new PointIndices);
corresponding_input_indices_.reset (new PointIndices);

// Check if normals have to be computed/saved
if (compute_normals_)
Expand Down Expand Up @@ -156,8 +160,13 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
const std::vector<int> &nn_indices,
std::vector<float> &nn_sqr_dists,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals)
NormalCloud &projected_points_normals,
PointIndices &corresponding_input_indices,
MLSResult &mls_result) const
{
// Note: this method is const because it needs to be thread-safe
// (MovingLeastSquaresOMP calls it from multiple threads)

// Compute the plane coefficients
EIGEN_ALIGN16 Eigen::Matrix3d covariance_matrix;
Eigen::Vector4d xyz_centroid;
Expand Down Expand Up @@ -285,7 +294,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
aux_normal.normal_z = static_cast<float> (normal[2]);
aux_normal.curvature = curvature;
projected_points_normals.push_back (aux_normal);
corresponding_input_indices_->indices.push_back (index);
corresponding_input_indices.indices.push_back (index);
}

break;
Expand All @@ -306,7 +315,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
projected_point, projected_normal);

projected_points.push_back (projected_point);
corresponding_input_indices_->indices.push_back (index);
corresponding_input_indices.indices.push_back (index);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);
}
Expand Down Expand Up @@ -336,7 +345,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
aux.y = static_cast<float> (point[1]);
aux.z = static_cast<float> (point[2]);
projected_points.push_back (aux);
corresponding_input_indices_->indices.push_back (index);
corresponding_input_indices.indices.push_back (index);

if (compute_normals_)
{
Expand Down Expand Up @@ -368,7 +377,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
projected_point, projected_normal);

projected_points.push_back (projected_point);
corresponding_input_indices_->indices.push_back (index);
corresponding_input_indices.indices.push_back (index);
if (compute_normals_)
projected_points_normals.push_back (projected_normal);

Expand All @@ -383,8 +392,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
{
// Take all point pairs and sample space between them in a grid-fashion
// \note consider only point pairs with increasing indices
MLSResult result (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
mls_results_[index] = result;
mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
break;
}
}
Expand All @@ -400,7 +408,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u
Eigen::VectorXd &c_vec,
int num_neighbors,
PointOutT &result_point,
pcl::Normal &result_normal)
pcl::Normal &result_normal) const
{
double n_disp = 0.0f;
double d_u = 0.0f, d_v = 0.0f;
Expand Down Expand Up @@ -447,7 +455,6 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::projectPointToMLSSurface (float &u
result_normal.curvature = curvature;
}


//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
Expand Down Expand Up @@ -477,7 +484,8 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &
PointCloudOut projected_points;
NormalCloud projected_points_normals;
// Get a plane approximating the local surface's tangent and project point onto it
computeMLSPointNormal ((*indices_)[cp], nn_indices, nn_sqr_dists, projected_points, projected_points_normals);
int index = (*indices_)[cp];
computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[index]);


// Copy all information from the input cloud to the output points (not doing any interpolation)
Expand All @@ -491,7 +499,81 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &
normals_->insert (normals_->end (), projected_points_normals.begin (), projected_points_normals.end ());
}

// Perform the distinct-cloud or voxel-grid upsampling
performUpsampling (output);
}

//////////////////////////////////////////////////////////////////////////////////////////////
#ifdef _OPENMP
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOut &output)
{
// Compute the number of coefficients
nr_coeff_ = (order_ + 1) * (order_ + 2) / 2;

// (Maximum) number of threads
unsigned int threads = threads_ == 0 ? 1 : threads_;

// Create temporaries for each thread in order to avoid synchronization
std::vector<PointCloudOut> projected_points (threads);
std::vector<NormalCloud> projected_points_normals (threads);
std::vector<PointIndices> corresponding_input_indices (threads);

// For all points
#pragma omp parallel for schedule (dynamic,1000) num_threads (threads)
for (size_t cp = 0; cp < indices_->size (); ++cp)
{
// Allocate enough space to hold the results of nearest neighbor searches
// \note resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices;
std::vector<float> nn_sqr_dists;

// Get the initial estimates of point positions and their neighborhoods
if (!searchForNeighbors ((*indices_)[cp], nn_indices, nn_sqr_dists))
continue;


// Check the number of nearest neighbors for normal estimation (and later
// for polynomial fit as well)
if (nn_indices.size () < 3)
continue;


// This thread's ID (range 0 to threads-1)
int tn = omp_get_thread_num ();

// Size of projected points before computeMLSPointNormal () adds points
size_t pp_size = projected_points[tn].size ();

// Get a plane approximating the local surface's tangent and project point onto it
int index = (*indices_)[cp];
computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[index]);

// Copy all information from the input cloud to the output points (not doing any interpolation)
for (size_t pp = pp_size; pp < projected_points[tn].size (); ++pp)
copyMissingFields (input_->points[(*indices_)[cp]], projected_points[tn][pp]);
}


// Combine all threads' results into the output vectors
for (unsigned int tn = 0; tn < threads; ++tn)
{
output.insert (output.end (), projected_points[tn].begin (), projected_points[tn].end ());
corresponding_input_indices_->indices.insert (corresponding_input_indices_->indices.end (),
corresponding_input_indices[tn].indices.begin (), corresponding_input_indices[tn].indices.end ());
if (compute_normals_)
normals_->insert (normals_->end (), projected_points_normals[tn].begin (), projected_points_normals[tn].end ());
}

// Perform the distinct-cloud or voxel-grid upsampling
performUpsampling (output);
}
#endif

//////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> void
pcl::MovingLeastSquares<PointInT, PointOutT>::performUpsampling (PointCloudOut &output)
{
if (upsample_method_ == DISTINCT_CLOUD)
{
for (size_t dp_i = 0; dp_i < distinct_cloud_->size (); ++dp_i) // dp_i = distinct_point_i
Expand Down Expand Up @@ -588,7 +670,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &
copyMissingFields (input_->points[input_index], result_point);

// Store the id of the original point
corresponding_input_indices_->indices.push_back(input_index);
corresponding_input_indices_->indices.push_back (input_index);

output.push_back (result_point);

Expand Down Expand Up @@ -687,4 +769,8 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::copyMissingFields (const PointInT

#define PCL_INSTANTIATE_MovingLeastSquares(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquares<T,OutT>;

#ifdef _OPENMP
#define PCL_INSTANTIATE_MovingLeastSquaresOMP(T,OutT) template class PCL_EXPORTS pcl::MovingLeastSquaresOMP<T,OutT>;
#endif

#endif // PCL_SURFACE_IMPL_MLS_H_
83 changes: 75 additions & 8 deletions surface/include/pcl/surface/mls.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -342,7 +342,7 @@ namespace pcl


/** \brief Data structure used to store the results of the MLS fitting
* \note Used only in the case of VOXEL_GRID_DILATION upsampling
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
*/
struct MLSResult
{
Expand All @@ -364,7 +364,7 @@ namespace pcl
};

/** \brief Stores the MLS result for each point in the input cloud
* \note Used only in the case of VOXEL_GRID_DILATION upsampling
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
*/
std::vector<MLSResult> mls_results_;

Expand Down Expand Up @@ -434,13 +434,16 @@ namespace pcl
/** \brief Number of coefficients, to be computed from the requested order.*/
int nr_coeff_;

/** \brief Collects for each point in output the corrseponding point in the input. */
PointIndicesPtr corresponding_input_indices_;

/** \brief Search for the closest nearest neighbors of a given point using a radius search
* \param[in] index the index of the query point
* \param[out] indices the resultant vector of indices representing the k-nearest neighbors
* \param[out] sqr_distances the resultant squared distances from the query point to the k-nearest neighbors
*/
inline int
searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances)
searchForNeighbors (int index, std::vector<int> &indices, std::vector<float> &sqr_distances) const
{
return (search_method_ (index, search_radius_, indices, sqr_distances));
}
Expand All @@ -453,13 +456,18 @@ namespace pcl
* (in the case of upsampling method NONE, only the query point projected to its own fitted surface will be returned,
* in the case of the other upsampling methods, multiple points will be returned)
* \param[out] projected_points_normals the normals corresponding to the projected points
* \param[out] corresponding_input_indices the set of indices with each point in output having the corresponding point in input
* \param[out] mls_result stores the MLS result for each point in the input cloud
* (used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling)
*/
void
computeMLSPointNormal (int index,
const std::vector<int> &nn_indices,
std::vector<float> &nn_sqr_dists,
PointCloudOut &projected_points,
NormalCloud &projected_points_normals);
NormalCloud &projected_points_normals,
PointIndices &corresponding_input_indices,
MLSResult &mls_result) const;

/** \brief Fits a point (sample point) given in the local plane coordinates of an input point (query point) to
* the MLS surface of the input point
Expand All @@ -484,21 +492,23 @@ namespace pcl
Eigen::VectorXd &c_vec,
int num_neighbors,
PointOutT &result_point,
pcl::Normal &result_normal);
pcl::Normal &result_normal) const;

void
copyMissingFields (const PointInT &point_in,
PointOutT &point_out) const;

private:
/** \brief Abstract surface reconstruction method.
* \param[out] output the result of the reconstruction
*/
virtual void performProcessing (PointCloudOut &output);

/** \brief Collects for each point in output the corrseponding point in the input. */
PointIndicesPtr corresponding_input_indices_;
/** \brief Perform upsampling for the distinct-cloud and voxel-grid methods
* \param[out] output the result of the reconstruction
*/
void performUpsampling (PointCloudOut &output);

private:
/** \brief Boost-based random number generator algorithm. */
boost::mt19937 rng_alg_;

Expand All @@ -515,6 +525,63 @@ namespace pcl
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

#ifdef _OPENMP
/** \brief MovingLeastSquaresOMP is a parallelized version of MovingLeastSquares, using the OpenMP standard.
* \note Compared to MovingLeastSquares, an overhead is incurred in terms of runtime and memory usage.
* \note The upsampling methods DISTINCT_CLOUD and VOXEL_GRID_DILATION are not parallelized completely, i.e. parts of the algorithm run on a single thread only.
* \author Robert Huitl
* \ingroup surface
*/
template <typename PointInT, typename PointOutT>
class MovingLeastSquaresOMP: public MovingLeastSquares<PointInT, PointOutT>
{
public:
typedef boost::shared_ptr<MovingLeastSquares<PointInT, PointOutT> > Ptr;
typedef boost::shared_ptr<const MovingLeastSquares<PointInT, PointOutT> > ConstPtr;

using PCLBase<PointInT>::input_;
using PCLBase<PointInT>::indices_;
using MovingLeastSquares<PointInT, PointOutT>::normals_;
using MovingLeastSquares<PointInT, PointOutT>::corresponding_input_indices_;
using MovingLeastSquares<PointInT, PointOutT>::nr_coeff_;
using MovingLeastSquares<PointInT, PointOutT>::order_;
using MovingLeastSquares<PointInT, PointOutT>::compute_normals_;

typedef pcl::PointCloud<pcl::Normal> NormalCloud;
typedef pcl::PointCloud<pcl::Normal>::Ptr NormalCloudPtr;

typedef pcl::PointCloud<PointOutT> PointCloudOut;
typedef typename PointCloudOut::Ptr PointCloudOutPtr;
typedef typename PointCloudOut::ConstPtr PointCloudOutConstPtr;

/** \brief Constructor for parallelized Moving Least Squares
* \param threads the maximum number of hardware threads to use (0 sets the value to 1)
*/
MovingLeastSquaresOMP (unsigned int threads = 0) : threads_ (threads)
{

}

/** \brief Set the maximum number of threads to use
* \param threads the maximum number of hardware threads to use (0 sets the value to 1)
*/
inline void
setNumberOfThreads (unsigned int threads = 0)
{
threads_ = threads;
}

protected:
/** \brief Abstract surface reconstruction method.
* \param[out] output the result of the reconstruction
*/
virtual void performProcessing (PointCloudOut &output);

/** \brief The maximum number of threads the scheduler should use. */
unsigned int threads_;
};
#endif
}

#ifdef PCL_NO_PRECOMPILE
Expand Down
3 changes: 3 additions & 0 deletions surface/src/mls.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,5 +46,8 @@
PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal)))

PCL_INSTANTIATE_PRODUCT(MovingLeastSquaresOMP, ((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
((pcl::PointXYZ)(pcl::PointXYZRGB)(pcl::PointXYZRGBA)(pcl::PointXYZRGBNormal)(pcl::PointNormal)))

/// Ideally, we should instantiate like below, but it takes large amounts of main memory for compilation
//PCL_INSTANTIATE_PRODUCT(MovingLeastSquares, (PCL_XYZ_POINT_TYPES)(PCL_XYZ_POINT_TYPES))
4 changes: 2 additions & 2 deletions test/surface/test_moving_least_squares.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -89,13 +89,13 @@ TEST (PCL, MovingLeastSquares)


// Testing OpenMP version
MovingLeastSquares<PointXYZ, PointNormal> mls_omp;
MovingLeastSquaresOMP<PointXYZ, PointNormal> mls_omp;
mls_omp.setInputCloud (cloud);
mls_omp.setComputeNormals (true);
mls_omp.setPolynomialFit (true);
mls_omp.setSearchMethod (tree);
mls_omp.setSearchRadius (0.03);
//mls_omp.setNumberOfThreads (4);
mls_omp.setNumberOfThreads (4);

// Reconstruct
mls_normals->clear ();
Expand Down