Skip to content

Commit

Permalink
Add ability to cache mls results
Browse files Browse the repository at this point in the history
  • Loading branch information
@Levi-Armstrong
Levi-Armstrong committed Jul 30, 2017
1 parent ad85f52 commit e8c036e
Show file tree
Hide file tree
Showing 2 changed files with 59 additions and 31 deletions.
26 changes: 17 additions & 9 deletions surface/include/pcl/surface/impl/mls.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -116,24 +116,30 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::process (PointCloudOut &output)
float tmp = static_cast<float> (search_radius_ / 2.0f);
boost::uniform_real<float> uniform_distrib (-tmp, tmp);
rng_uniform_distribution_.reset (new boost::variate_generator<boost::mt19937&, boost::uniform_real<float> > (rng_alg_, uniform_distrib));

mls_results_.resize (1); // Need to have a reference to a single dummy result.

break;
}
case (VOXEL_GRID_DILATION):
case (DISTINCT_CLOUD):
{
mls_results_.resize (input_->size ());
cache_mls_results_ = true;
break;
}
default:
{
mls_results_.resize (1); // Need to have a reference to a single dummy result.
break;
}
}

if (cache_mls_results_)
{
mls_results_.resize (input_->size ());
}
else
{
mls_results_.resize (1); // Need to have a reference to a single dummy result.
}

// Perform the actual surface reconstruction
performProcessing (output);

Expand Down Expand Up @@ -271,6 +277,11 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
P_weight_Pt.llt ().solveInPlace (c_vec);
}

if (cache_mls_results_)
{
mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
}

switch (upsample_method_)
{
case (NONE):
Expand Down Expand Up @@ -396,9 +407,6 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::computeMLSPointNormal (int index,
case (VOXEL_GRID_DILATION):
case (DISTINCT_CLOUD):
{
// Take all point pairs and sample space between them in a grid-fashion
// \note consider only point pairs with increasing indices
mls_result = MLSResult (point, plane_normal, u_axis, v_axis, c_vec, static_cast<int> (nn_indices.size ()), curvature);
break;
}
}
Expand Down Expand Up @@ -494,7 +502,7 @@ pcl::MovingLeastSquares<PointInT, PointOutT>::performProcessing (PointCloudOut &
// Get a plane approximating the local surface's tangent and project point onto it
int index = (*indices_)[cp];

if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
if (cache_mls_results_)
mls_result_index = index; // otherwise we give it a dummy location.

computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points, projected_points_normals, *corresponding_input_indices_, mls_results_[mls_result_index]);
Expand Down Expand Up @@ -557,7 +565,7 @@ pcl::MovingLeastSquaresOMP<PointInT, PointOutT>::performProcessing (PointCloudOu
int index = (*indices_)[cp];
size_t mls_result_index = 0;

if (upsample_method_ == VOXEL_GRID_DILATION || upsample_method_ == DISTINCT_CLOUD)
if (this->cache_mls_results_)
mls_result_index = index; // otherwise we give it a dummy location.

this->computeMLSPointNormal (index, nn_indices, nn_sqr_dists, projected_points[tn], projected_points_normals[tn], corresponding_input_indices[tn], this->mls_results_[mls_result_index]);
Expand Down
64 changes: 42 additions & 22 deletions surface/include/pcl/surface/mls.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,6 +91,28 @@ namespace pcl

enum UpsamplingMethod {NONE, DISTINCT_CLOUD, SAMPLE_LOCAL_PLANE, RANDOM_UNIFORM_DENSITY, VOXEL_GRID_DILATION};

/** \brief Data structure used to store the results of the MLS fitting
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
*/
struct MLSResult
{
MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {}

MLSResult (const Eigen::Vector3d &a_mean,
const Eigen::Vector3d &a_plane_normal,
const Eigen::Vector3d &a_u,
const Eigen::Vector3d &a_v,
const Eigen::VectorXd a_c_vec,
const int a_num_neighbors,
const float &a_curvature);

Eigen::Vector3d mean, plane_normal, u_axis, v_axis;
Eigen::VectorXd c_vec;
int num_neighbors;
float curvature;
bool valid;
};

/** \brief Empty constructor. */
MovingLeastSquares () : CloudSurfaceProcessing<PointInT, PointOutT> (),
normals_ (),
Expand All @@ -106,6 +128,7 @@ namespace pcl
upsampling_radius_ (0.0),
upsampling_step_ (0.0),
desired_num_points_in_radius_ (0),
cache_mls_results_ (false),
mls_results_ (),
voxel_size_ (1.0),
dilation_iteration_num_ (0),
Expand Down Expand Up @@ -283,6 +306,22 @@ namespace pcl
inline int
getDilationIterations () { return dilation_iteration_num_; }

/** \brief Set wether the mls results should be stored for each point in the input cloud
* \param[in] True if the mls results should be stored, otherwise false.
*/
inline void
setCacheMLSResults (bool cache_mls_results) { cache_mls_results_ = cache_mls_results; }

/** \brief Get the cache_mls_results_ value (True if the mls results should be stored, otherwise false). */
inline bool
getCacheMLSResults () const { return cache_mls_results_; }

/** \brief Get the MLSResults for input cloud
* \note The results are only stored if setCacheMLSResults(true) was called
* or when using the upsampling method DISTINCT_CLOUD or VOXEL_GRID_DILATION.
*/
inline const std::vector<MLSResult>& getMLSResults() const { return mls_results_; }

/** \brief Base method for surface reconstruction for all points given in <setInputCloud (), setIndices ()>
* \param[out] output the resultant reconstructed surface model
*/
Expand Down Expand Up @@ -341,28 +380,8 @@ namespace pcl
*/
int desired_num_points_in_radius_;


/** \brief Data structure used to store the results of the MLS fitting
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
*/
struct MLSResult
{
MLSResult () : mean (), plane_normal (), u_axis (), v_axis (), c_vec (), num_neighbors (), curvature (), valid (false) {}

MLSResult (const Eigen::Vector3d &a_mean,
const Eigen::Vector3d &a_plane_normal,
const Eigen::Vector3d &a_u,
const Eigen::Vector3d &a_v,
const Eigen::VectorXd a_c_vec,
const int a_num_neighbors,
const float &a_curvature);

Eigen::Vector3d mean, plane_normal, u_axis, v_axis;
Eigen::VectorXd c_vec;
int num_neighbors;
float curvature;
bool valid;
};
/** \brief True if the mls results for the input cloud should be stored */
bool cache_mls_results_;

/** \brief Stores the MLS result for each point in the input cloud
* \note Used only in the case of VOXEL_GRID_DILATION or DISTINCT_CLOUD upsampling
Expand Down Expand Up @@ -549,6 +568,7 @@ namespace pcl
using MovingLeastSquares<PointInT, PointOutT>::order_;
using MovingLeastSquares<PointInT, PointOutT>::compute_normals_;
using MovingLeastSquares<PointInT, PointOutT>::upsample_method_;
using MovingLeastSquares<PointInT, PointOutT>::cache_mls_results_;
using MovingLeastSquares<PointInT, PointOutT>::VOXEL_GRID_DILATION;
using MovingLeastSquares<PointInT, PointOutT>::DISTINCT_CLOUD;

Expand Down

0 comments on commit e8c036e

Please sign in to comment.