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
00035 #ifndef _SAMPLE_CONSENSUS_SACMODELPARALLELLINES_H_
00036 #define _SAMPLE_CONSENSUS_SACMODELPARALLELLINES_H_
00037
00038 #include <door_handle_detector/sample_consensus/sac_model.h>
00039 #include <door_handle_detector/sample_consensus/model_types.h>
00040
00042 #define MAX_ITERATIONS_UNIQUE 10000
00043
00044 namespace sample_consensus
00045 {
00048 class SACModelParallelLines : public SACModel
00049 {
00050 public:
00052
00053 SACModelParallelLines (double min_line_sep_m, double max_line_sep_m)
00054 {
00055 min_line_sep_m_ = min_line_sep_m;
00056 max_line_sep_m_ = max_line_sep_m;
00057 }
00058
00060
00061 virtual ~SACModelParallelLines () { }
00062
00063 virtual void getSamples (int &iterations, std::vector<int> &samples);
00064
00066
00070 bool testModelCoefficients (const std::vector<double> &model_coefficients) { return true; }
00071
00072 virtual bool computeModelCoefficients (const std::vector<int> &samples);
00073
00074 virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients);
00075 virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances);
00076 virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers);
00077
00078 virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, sensor_msgs::PointCloud &projected_points);
00079
00080 virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients);
00081 virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold);
00082
00084
00085 virtual int getModelType () { return (SACMODEL_PARALLEL_LINES); }
00086
00087 void closestLine (const std::vector<int> &indices, const std::vector<double> &model_coefficients,
00088 std::vector<int> *closest_line, std::vector<double> *closest_dist);
00089 void closestLine (const std::set<int> &indices, const std::vector<double> &model_coefficients,
00090 std::vector<int> *closest_line, std::vector<double> *closest_dist);
00091 double pointToLineSquareDistance (const geometry_msgs::Point32 &line_point1, const geometry_msgs::Point32 &line_point2, const geometry_msgs::Point32 &point);
00092
00093 double min_line_sep_m_;
00094 double max_line_sep_m_;
00095 };
00096 }
00097
00098 #endif