38 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_
39 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_
41 #include <pcl/sample_consensus/sac_model.h>
42 #include <pcl/sample_consensus/model_types.h>
60 template <
typename Po
intT>
73 typedef boost::shared_ptr<SampleConsensusModelCircle3D<PointT> >
Ptr;
74 typedef boost::shared_ptr<const SampleConsensusModelCircle3D<PointT> >
ConstPtr;
90 const std::vector<int> &indices,
113 tmp_inliers_ = source.tmp_inliers_;
124 Eigen::VectorXf &model_coefficients);
132 std::vector<double> &distances);
141 const double threshold,
142 std::vector<int> &inliers);
152 const double threshold);
162 const Eigen::VectorXf &model_coefficients,
163 Eigen::VectorXf &optimized_coefficients);
173 const Eigen::VectorXf &model_coefficients,
175 bool copy_data_fields =
true);
184 const Eigen::VectorXf &model_coefficients,
185 const double threshold);
196 isModelValid (
const Eigen::VectorXf &model_coefficients);
206 const std::vector<int> *tmp_inliers_;
217 pcl::
Functor<double> (m_data_points), model_ (model) {}
224 int operator() (
const Eigen::VectorXd &x, Eigen::VectorXd &fvec)
const
226 for (
int i = 0; i < values (); ++i)
230 Eigen::Vector3d P (model_->input_->points[(*model_->tmp_inliers_)[i]].x, model_->input_->points[(*model_->tmp_inliers_)[i]].y, model_->input_->points[(*model_->tmp_inliers_)[i]].z);
232 Eigen::Vector3d C (x[0], x[1], x[2]);
234 Eigen::Vector3d N (x[4], x[5], x[6]);
238 Eigen::Vector3d helperVectorPC = P - C;
241 double lambda = (-(helperVectorPC.dot (N))) / N.dot (N);
243 Eigen::Vector3d P_proj = P + lambda * N;
244 Eigen::Vector3d helperVectorP_projC = P_proj - C;
247 Eigen::Vector3d
K = C + r * helperVectorP_projC.normalized ();
248 Eigen::Vector3d distanceVector = P -
K;
250 fvec[i] = distanceVector.norm ();
260 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE3D_H_