00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038 #ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00039 #define PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_
00040
00041 #include <pcl/sample_consensus/sac_model.h>
00042 #include <pcl/sample_consensus/model_types.h>
00043 #include <cminpack.h>
00044 #include <boost/thread/mutex.hpp>
00045
00046 namespace pcl
00047 {
00049
00052 template <typename PointT>
00053 class SampleConsensusModelCircle2D : public SampleConsensusModel<PointT>
00054 {
00055 using SampleConsensusModel<PointT>::input_;
00056 using SampleConsensusModel<PointT>::indices_;
00057 using SampleConsensusModel<PointT>::radius_min_;
00058 using SampleConsensusModel<PointT>::radius_max_;
00059
00060 public:
00061 typedef typename SampleConsensusModel<PointT>::PointCloud PointCloud;
00062 typedef typename SampleConsensusModel<PointT>::PointCloudPtr PointCloudPtr;
00063 typedef typename SampleConsensusModel<PointT>::PointCloudConstPtr PointCloudConstPtr;
00064
00065 typedef boost::shared_ptr<SampleConsensusModelCircle2D> Ptr;
00066
00070 SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud) : SampleConsensusModel<PointT> (cloud) {};
00071
00076 SampleConsensusModelCircle2D (const PointCloudConstPtr &cloud, const std::vector<int> &indices) : SampleConsensusModel<PointT> (cloud, indices) {};
00077
00083 void getSamples (int &iterations, std::vector<int> &samples);
00084
00090 bool computeModelCoefficients (const std::vector<int> &samples, Eigen::VectorXf &model_coefficients);
00091
00096 void getDistancesToModel (const Eigen::VectorXf &model_coefficients, std::vector<double> &distances);
00097
00103 void selectWithinDistance (const Eigen::VectorXf &model_coefficients, double threshold, std::vector<int> &inliers);
00104
00111 void optimizeModelCoefficients (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients);
00112
00120 void projectPoints (const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields = true);
00121
00127 bool doSamplesVerifyModel (const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, double threshold);
00128
00130 inline pcl::SacModel getModelType () const { return (SACMODEL_CIRCLE2D); }
00131
00132 protected:
00136 bool
00137 isModelValid (const Eigen::VectorXf &model_coefficients);
00138
00139 private:
00141 boost::mutex tmp_mutex_;
00142
00144 const std::vector<int> *tmp_inliers_;
00145
00147 const static int MAX_ITERATIONS_COLLINEAR = 1000;
00148
00157 static int functionToOptimize (void *p, int m, int n, const double *x, double *fvec, int iflag);
00158
00159 };
00160 }
00161
00162 #endif //#ifndef PCL_SAMPLE_CONSENSUS_MODEL_CIRCLE2D_H_