sac_model_plane.h
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2009, Willow Garage, Inc.
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of Willow Garage, Inc. nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* $Id$
*
*/
#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
#define PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
#include <pcl/cuda/sample_consensus/sac_model.h>
#include <thrust/random.h>
namespace pcl
{
namespace cuda
{
/** \brief Check if a certain tuple is a point inlier. */
struct CountPlanarInlier
{
float4 coefficients;
float threshold;
CountPlanarInlier (float4 coeff, float thresh) :
coefficients(coeff), threshold(thresh)
{}
template <typename Tuple> __inline__ __host__ __device__ bool
operator () (const Tuple &t);
};
/** \brief Check if a certain tuple is a point inlier. */
struct CheckPlanarInlier
{
float4 coefficients;
float threshold;
CheckPlanarInlier (float4 coeff, float thresh) :
coefficients(coeff), threshold(thresh)
{}
template <typename Tuple> __inline__ __host__ __device__ int
operator () (const Tuple &t);
};
////////////////////////////////////////////////////////////////////////////////////////////
/** \brief @b SampleConsensusModelPlane defines a model for 3D plane segmentation.
*/
template <template <typename> class Storage>
class SampleConsensusModelPlane : public SampleConsensusModel<Storage>
{
public:
using SampleConsensusModel<Storage>::input_;
using SampleConsensusModel<Storage>::indices_;
using SampleConsensusModel<Storage>::rngl_;
typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
typedef typename PointCloud::Ptr PointCloudPtr;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;
typedef typename SampleConsensusModel<Storage>::Indices Indices;
typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
typedef typename SampleConsensusModel<Storage>::Coefficients Coefficients;
typedef typename SampleConsensusModel<Storage>::Hypotheses Hypotheses;
typedef typename SampleConsensusModel<Storage>::Samples Samples;
typedef boost::shared_ptr<SampleConsensusModelPlane> Ptr;
/** \brief Constructor for base SampleConsensusModelPlane.
* \param cloud the input point cloud dataset
*/
SampleConsensusModelPlane (const PointCloudConstPtr &cloud);
/* \brief Constructor for base SampleConsensusModelPlane.
* \param cloud the input point cloud dataset
* \param indices a vector of point indices to be used from \a cloud
*/
// SampleConsensusModelPlane (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<PointT> (cloud, indices) {};
/** \brief Get 3 random non-collinear points as data samples and return them as point indices.
* \param iterations the internal number of iterations used by SAC methods
* \param samples the resultant model samples
* \note assumes unique points!
*/
void
getSamples (int &iterations, Indices &samples);
/** \brief Check whether the given index samples can form a valid plane model, compute the model coefficients from
* these samples and store them in model_coefficients. The plane coefficients are:
* a, b, c, d (ax+by+cz+d=0)
* \param samples the point indices found as possible good candidates for creating a valid model
* \param model_coefficients the resultant model coefficients
*/
bool
computeModelCoefficients (const Indices &samples, Coefficients &model_coefficients);
bool
generateModelHypotheses (Hypotheses &h, int max_iterations);
virtual bool
generateModelHypotheses (Hypotheses &h, Samples &s, int max_iterations)
{
// TODO: hack.. Samples should be vector<int>, not int..
return generateModelHypotheses (h, max_iterations);
};
/* \brief Compute all distances from the cloud data to a given plane model.
* \param model_coefficients the coefficients of a plane model that we need to compute distances to
* \param distances the resultant estimated distances
*/
// void
// getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<float> &distances);
/** \brief Select all the points which respect the given model coefficients as inliers.
* \param model_coefficients the coefficients of a plane model that we need to
* compute distances to
* \param threshold a maximum admissible distance threshold for determining the
* inliers from the outliers
* \param inliers the resultant model inliers
* \param inliers_stencil
*/
int
selectWithinDistance (const Coefficients &model_coefficients,
float threshold, IndicesPtr &inliers, IndicesPtr &inliers_stencil);
int
selectWithinDistance (const Hypotheses &h, int idx,
float threshold,
IndicesPtr &inliers, IndicesPtr &inliers_stencil);
int
selectWithinDistance (Hypotheses &h, int idx,
float threshold,
IndicesPtr &inliers_stencil,
float3 &centroid);
int
countWithinDistance (const Coefficients &model_coefficients, float threshold);
int
countWithinDistance (const Hypotheses &h, int idx, float threshold);
/* \brief Recompute the plane coefficients using the given inlier set and return them to the user.
* @note: these are the coefficients of the plane model after refinement (eg. after SVD)
* \param inliers the data inliers found as supporting the model
* \param model_coefficients the initial guess for the model coefficients
* \param optimized_coefficients the resultant recomputed coefficients after non-linear optimization
*/
// void
// optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients);
/* \brief Create a new point cloud with inliers projected onto the plane model.
* \param inliers the data inliers that we want to project on the plane model
* \param model_coefficients the *normalized* coefficients of a plane model
* \param projected_points the resultant projected points
* \param copy_data_fields set to true if we need to copy the other data fields
*/
// void
// projectPoints (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true);
/* \brief Verify whether a subset of indices verifies the given plane model coefficients.
* \param indices the data indices that need to be tested against the plane model
* \param model_coefficients the plane model coefficients
* \param threshold a maximum admissible distance threshold for determining the inliers from the outliers
*/
// bool
// doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, float threshold);
/* \brief Return an unique id for this model (SACMODEL_PLANE). */
// inline pcl::SacModel getModelType () const { return (SACMODEL_PLANE); }
// protected:
/* \brief Check whether a model is valid given the user constraints.
* \param model_coefficients the set of model coefficients
*/
// inline bool
// isModelValid (const Eigen::VectorXf &model_coefficients)
// {
// // Needs a valid model coefficients
// if (model_coefficients.size () != 4)
// {
// ROS_ERROR ("[pcl::SampleConsensusModelPlane::isModelValid] Invalid number of model coefficients given (%lu)!", (unsigned long) model_coefficients.size ());
// return (false);
// }
// return (true);
// }
// private:
/* \brief Define the maximum number of iterations for collinearity checks */
const static int MAX_ITERATIONS_COLLINEAR = 1000;
};
/** \brief Check if a certain tuple is a point inlier. */
template <template <typename> class Storage>
struct CreatePlaneHypothesis
{
typedef typename SampleConsensusModel<Storage>::PointCloud PointCloud;
typedef typename PointCloud::ConstPtr PointCloudConstPtr;
typedef typename SampleConsensusModel<Storage>::Indices Indices;
typedef typename SampleConsensusModel<Storage>::IndicesPtr IndicesPtr;
typedef typename SampleConsensusModel<Storage>::IndicesConstPtr IndicesConstPtr;
const PointXYZRGB *input;
const int *indices;
int nr_indices;
float bad_value;
CreatePlaneHypothesis (const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad) :
input(_input), indices(_indices), nr_indices(_nr_indices), bad_value(bad)
{}
//template <typename Tuple>
__inline__ __host__ __device__ float4
//operator () (const Tuple &t);
operator () (int t);
};
struct parallel_random_generator
{
__inline__ __host__ __device__
parallel_random_generator(unsigned int seed)
{
m_seed = seed;
}
__inline__ __host__ __device__
unsigned int operator()(const unsigned int n) const
{
thrust::default_random_engine rng(m_seed);
// discard n numbers to avoid correlation
rng.discard(n);
// return a random number
return rng();
}
unsigned int m_seed;
};
} // namespace
} // namespace
#endif //#ifndef PCL_CUDA_SAMPLE_CONSENSUS_MODEL_PLANE_H_
pcl::cuda::CreatePlaneHypothesis::IndicesPtr
SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
Definition: sac_model_plane.h:232
pcl::cuda::CreatePlaneHypothesis::nr_indices
int nr_indices
Definition: sac_model_plane.h:237
pcl::cuda::SampleConsensusModelPlane
SampleConsensusModelPlane defines a model for 3D plane segmentation.
Definition: sac_model_plane.h:81
pcl::cuda::CheckPlanarInlier::CheckPlanarInlier
CheckPlanarInlier(float4 coeff, float thresh)
Definition: sac_model_plane.h:69
pcl::cuda::SampleConsensusModel
SampleConsensusModel represents the base model class.
Definition: sac_model.h:88
pcl::cuda::SampleConsensusModel::IndicesPtr
boost::shared_ptr< typename Storage< int >::type > IndicesPtr
Definition: sac_model.h:99
pcl
Definition: convolution.h:45
pcl::cuda::SampleConsensusModelPlane::Indices
SampleConsensusModel< Storage >::Indices Indices
Definition: sac_model_plane.h:92
pcl::cuda::PointCloudAOS
PointCloudAOS represents an AOS (Array of Structs) PointCloud implementation for CUDA processing...
Definition: point_cloud.h:133
pcl::cuda::CountPlanarInlier::threshold
float threshold
Definition: sac_model_1point_plane.h:54
pcl::cuda::SampleConsensusModel::Samples
Storage< int >::type Samples
Definition: sac_model.h:108
pcl::cuda::CreatePlaneHypothesis::CreatePlaneHypothesis
CreatePlaneHypothesis(const PointXYZRGB *_input, const int *_indices, int _nr_indices, float bad)
Definition: sac_model_plane.h:240
pcl::cuda::SampleConsensusModel::IndicesConstPtr
boost::shared_ptr< const typename Storage< int >::type > IndicesConstPtr
Definition: sac_model.h:100
pcl::cuda::SampleConsensusModel::Coefficients
Storage< float >::type Coefficients
Definition: sac_model.h:102
pcl::cuda::SampleConsensusModel::Indices
Storage< int >::type Indices
Definition: sac_model.h:98
pcl::cuda::SampleConsensusModelPlane::PointCloud
SampleConsensusModel< Storage >::PointCloud PointCloud
Definition: sac_model_plane.h:88
pcl::cuda::CreatePlaneHypothesis::bad_value
float bad_value
Definition: sac_model_plane.h:238
pcl::cuda::CreatePlaneHypothesis::Indices
SampleConsensusModel< Storage >::Indices Indices
Definition: sac_model_plane.h:231
pcl::cuda::SampleConsensusModelPlane::IndicesConstPtr
SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
Definition: sac_model_plane.h:94
pcl::cuda::SampleConsensusModelPlane::PointCloudPtr
PointCloud::Ptr PointCloudPtr
Definition: sac_model_plane.h:89
pcl::cuda::SampleConsensusModelPlane::Coefficients
SampleConsensusModel< Storage >::Coefficients Coefficients
Definition: sac_model_plane.h:96
pcl::cuda::SampleConsensusModelPlane::generateModelHypotheses
virtual bool generateModelHypotheses(Hypotheses &h, Samples &s, int max_iterations)
Definition: sac_model_plane.h:134
pcl::cuda::SampleConsensusModel::Hypotheses
Storage< float4 >::type Hypotheses
Definition: sac_model.h:106
pcl::cuda::CreatePlaneHypothesis::input
const PointXYZRGB * input
Definition: sac_model_plane.h:235
pcl::cuda::CreatePlaneHypothesis::PointCloudConstPtr
PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model_plane.h:229
pcl::cuda::PointCloudAOS::Ptr
boost::shared_ptr< PointCloudAOS< Storage > > Ptr
Definition: point_cloud.h:201
pcl::cuda::parallel_random_generator::operator()
__inline__ __host__ __device__ unsigned int operator()(const unsigned int n) const
Definition: sac_model_plane.h:261
pcl::cuda::CreatePlaneHypothesis::PointCloud
SampleConsensusModel< Storage >::PointCloud PointCloud
Definition: sac_model_plane.h:228
pcl::cuda::SampleConsensusModelPlane::PointCloudConstPtr
PointCloud::ConstPtr PointCloudConstPtr
Definition: sac_model_plane.h:90
pcl::cuda::parallel_random_generator::parallel_random_generator
__inline__ __host__ __device__ parallel_random_generator(unsigned int seed)
Definition: sac_model_plane.h:255
pcl::cuda::SampleConsensusModelPlane::Samples
SampleConsensusModel< Storage >::Samples Samples
Definition: sac_model_plane.h:98
pcl::cuda::SampleConsensusModelPlane::Hypotheses
SampleConsensusModel< Storage >::Hypotheses Hypotheses
Definition: sac_model_plane.h:97
pcl::cuda::parallel_random_generator
Definition: sac_model_1point_plane.h:294
pcl::cuda::CreatePlaneHypothesis
Check if a certain tuple is a point inlier.
Definition: sac_model_plane.h:226
pcl::cuda::SampleConsensusModelPlane::Ptr
boost::shared_ptr< SampleConsensusModelPlane > Ptr
Definition: sac_model_plane.h:100
pcl::cuda::CountPlanarInlier::operator()
__inline__ __host__ __device__ bool operator()(const Tuple &t)
pcl::cuda::PointCloudAOS::ConstPtr
boost::shared_ptr< const PointCloudAOS< Storage > > ConstPtr
Definition: point_cloud.h:202
pcl::cuda::CreatePlaneHypothesis::indices
const int * indices
Definition: sac_model_plane.h:236
pcl::cuda::PointXYZRGB
Default point xyz-rgb structure.
Definition: point_types.h:49
pcl::cuda::CountPlanarInlier::coefficients
float4 coefficients
Definition: sac_model_1point_plane.h:53
pcl::cuda::SampleConsensusModelPlane::IndicesPtr
SampleConsensusModel< Storage >::IndicesPtr IndicesPtr
Definition: sac_model_plane.h:93
pcl::cuda::CountPlanarInlier::CountPlanarInlier
CountPlanarInlier(float4 coeff, float thresh)
Definition: sac_model_plane.h:55
pcl::cuda::CheckPlanarInlier
Check if a certain tuple is a point inlier.
Definition: sac_model_1point_plane.h:81
pcl::cuda::CreatePlaneHypothesis::IndicesConstPtr
SampleConsensusModel< Storage >::IndicesConstPtr IndicesConstPtr
Definition: sac_model_plane.h:233