39 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
40 #define PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_
42 #include <pcl/sample_consensus/sac_model.h>
43 #include <pcl/sample_consensus/model_types.h>
44 #include <pcl/common/common.h>
45 #include <pcl/common/distances.h>
64 template <
typename Po
intT,
typename Po
intNT>
80 typedef boost::shared_ptr<SampleConsensusModelCone>
Ptr;
89 , axis_ (Eigen::Vector3f::Zero ())
91 , min_angle_ (-std::numeric_limits<double>::max ())
92 , max_angle_ (std::numeric_limits<double>::max ())
103 const std::vector<int> &indices,
107 , axis_ (Eigen::Vector3f::Zero ())
109 , min_angle_ (-std::numeric_limits<double>::max ())
110 , max_angle_ (std::numeric_limits<double>::max ())
121 axis_ (), eps_angle_ (), min_angle_ (), max_angle_ (), tmp_inliers_ ()
136 axis_ = source.axis_;
137 eps_angle_ = source.eps_angle_;
138 min_angle_ = source.min_angle_;
139 max_angle_ = source.max_angle_;
140 tmp_inliers_ = source.tmp_inliers_;
158 setAxis (
const Eigen::Vector3f &ax) { axis_ = ax; }
161 inline Eigen::Vector3f
172 min_angle_ = min_angle;
173 max_angle_ = max_angle;
183 min_angle = min_angle_;
184 max_angle = max_angle_;
195 Eigen::VectorXf &model_coefficients);
203 std::vector<double> &distances);
212 const double threshold,
213 std::vector<int> &inliers);
223 const double threshold);
234 const Eigen::VectorXf &model_coefficients,
235 Eigen::VectorXf &optimized_coefficients);
246 const Eigen::VectorXf &model_coefficients,
248 bool copy_data_fields =
true);
257 const Eigen::VectorXf &model_coefficients,
258 const double threshold);
270 pointToAxisDistance (
const Eigen::Vector4f &pt,
const Eigen::VectorXf &model_coefficients);
274 getName ()
const {
return (
"SampleConsensusModelCone"); }
281 isModelValid (
const Eigen::VectorXf &model_coefficients);
292 Eigen::Vector3f axis_;
302 const std::vector<int> *tmp_inliers_;
304 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
305 #pragma GCC diagnostic ignored "-Weffc++"
316 pcl::
Functor<float> (m_data_points), model_ (model) {}
324 operator() (
const Eigen::VectorXf &x, Eigen::VectorXf &fvec)
const
326 Eigen::Vector4f apex (x[0], x[1], x[2], 0);
327 Eigen::Vector4f axis_dir (x[3], x[4], x[5], 0);
328 float opening_angle = x[6];
330 float apexdotdir = apex.dot (axis_dir);
331 float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
333 for (
int i = 0; i < values (); ++i)
336 Eigen::Vector4f pt (model_->input_->points[(*model_->tmp_inliers_)[i]].x,
337 model_->input_->points[(*model_->tmp_inliers_)[i]].y,
338 model_->input_->points[(*model_->tmp_inliers_)[i]].z, 0);
341 float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
342 Eigen::Vector4f pt_proj = apex + k * axis_dir;
345 Eigen::Vector4f height = apex-pt_proj;
346 float actual_cone_radius = tanf (opening_angle) * height.norm ();
355 #if defined BUILD_Maintainer && defined __GNUC__ && __GNUC__ == 4 && __GNUC_MINOR__ > 3
356 #pragma GCC diagnostic warning "-Weffc++"
361 #ifdef PCL_NO_PRECOMPILE
362 #include <pcl/sample_consensus/impl/sac_model_cone.hpp>
365 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CONE_H_