41 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_
42 #define PCL_SAMPLE_CONSENSUS_MODEL_H_
49 #include <pcl/console/print.h>
50 #include <pcl/point_cloud.h>
51 #include <pcl/sample_consensus/boost.h>
52 #include <pcl/sample_consensus/model_types.h>
54 #include <pcl/search/search.h>
58 template<
class T>
class ProgressiveSampleConsensus;
65 template <
typename Po
intT>
74 typedef boost::shared_ptr<SampleConsensusModel>
Ptr;
75 typedef boost::shared_ptr<const SampleConsensusModel>
ConstPtr;
84 ,
radius_min_ (-std::numeric_limits<double>::max ())
90 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
96 rng_alg_.seed (static_cast<unsigned> (std::time(0)));
111 ,
radius_min_ (-std::numeric_limits<double>::max ())
112 ,
radius_max_ (std::numeric_limits<double>::max ())
117 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
122 rng_alg_.seed (static_cast<unsigned> (std::time (0)));
139 const std::vector<int> &indices,
142 ,
indices_ (new std::vector<int> (indices))
143 ,
radius_min_ (-std::numeric_limits<double>::max ())
144 ,
radius_max_ (std::numeric_limits<double>::max ())
149 ,
rng_dist_ (new boost::uniform_int<> (0, std::numeric_limits<int>::max ()))
154 rng_alg_.seed (static_cast<unsigned> (std::time(0)));
160 PCL_ERROR (
"[pcl::SampleConsensusModel] Invalid index vector given with size %zu while the input PointCloud has size %zu!\n",
indices_->size (),
input_->points.size ());
183 PCL_ERROR (
"[pcl::SampleConsensusModel::getSamples] Can not select %zu unique points out of %zu!\n",
184 samples.size (),
indices_->size ());
187 iterations = INT_MAX - 1;
204 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] Selected %zu samples.\n", samples.size ());
208 PCL_DEBUG (
"[pcl::SampleConsensusModel::getSamples] WARNING: Could not select %d sample points in %d iterations!\n",
getSampleSize (), max_sample_checks_);
221 Eigen::VectorXf &model_coefficients) = 0;
235 const Eigen::VectorXf &model_coefficients,
236 Eigen::VectorXf &optimized_coefficients) = 0;
245 std::vector<double> &distances) = 0;
257 const double threshold,
258 std::vector<int> &inliers) = 0;
271 const double threshold) = 0;
283 const Eigen::VectorXf &model_coefficients,
285 bool copy_data_fields =
true) = 0;
297 const Eigen::VectorXf &model_coefficients,
298 const double threshold) = 0;
308 indices_.reset (
new std::vector<int> ());
312 indices_->resize (cloud->points.size ());
313 for (
size_t i = 0; i < cloud->points.size (); ++i)
314 (*
indices_)[i] =
static_cast<int> (i);
320 inline PointCloudConstPtr
327 setIndices (
const boost::shared_ptr <std::vector<int> > &indices)
339 indices_.reset (
new std::vector<int> (indices));
344 inline boost::shared_ptr <std::vector<int> >
415 std::vector<double> dists (error_sqr_dists);
416 std::sort (dists.begin (), dists.end ());
417 double median_error_sqr = dists[dists.size () >> 1];
418 return (2.1981 * median_error_sqr);
430 PCL_ERROR (
"[pcl::SampleConsensusModel::computeVariance] The variance of the Sample Consensus model distances cannot be estimated, as the model has not been computed yet. Please compute the model first or at least run selectWithinDistance before continuing. Returning NAN!\n");
431 return (std::numeric_limits<double>::quiet_NaN ());
443 size_t sample_size = sample.size ();
445 for (
unsigned int i = 0; i < sample_size; ++i)
460 size_t sample_size = sample.size ();
466 std::vector<int> indices;
467 std::vector<float> sqr_dists;
477 if (indices.size () < sample_size - 1)
480 for(
unsigned int i = 1; i < sample_size; ++i)
485 for (
unsigned int i = 0; i < sample_size-1; ++i)
486 std::swap (indices[i], indices[i + (
rnd () % (indices.size () - i))]);
487 for (
unsigned int i = 1; i < sample_size; ++i)
498 isModelValid (
const Eigen::VectorXf &model_coefficients) = 0;
505 isSampleGood (
const std::vector<int> &samples)
const = 0;
537 boost::shared_ptr<boost::variate_generator< boost::mt19937&, boost::uniform_int<> > >
rng_gen_;
549 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
555 template <
typename Po
intT,
typename Po
intNT>
562 typedef boost::shared_ptr<SampleConsensusModelFromNormals>
Ptr;
563 typedef boost::shared_ptr<const SampleConsensusModelFromNormals>
ConstPtr;
617 template<
typename _Scalar,
int NX=Eigen::Dynamic,
int NY=Eigen::Dynamic>
627 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1>
ValueType;
628 typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1>
InputType;
629 typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime>
JacobianType;
637 Functor (
int m_data_points) : m_data_points_ (m_data_points) {}
643 values ()
const {
return (m_data_points_); }
646 const int m_data_points_;
650 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_H_