sac_model.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008 Radu Bogdan Rusu <rusu -=- cs.tum.edu>
3  *
4  * All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions are met:
8  *
9  * * Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * * Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in the
13  * documentation and/or other materials provided with the distribution.
14  *
15  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25  * POSSIBILITY OF SUCH DAMAGE.
26  *
27  * $Id: sac_model.h 21050 2009-08-07 21:24:30Z jfaustwg $
28  *
29  */
30 
33 #ifndef _SAMPLE_CONSENSUS_SACMODEL_H_
34 #define _SAMPLE_CONSENSUS_SACMODEL_H_
35 
36 #include <pcl_ros/point_cloud.h>
37 #include <pcl/point_types.h>
38 
39 #include <set>
40 
41 namespace sample_consensus
42 {
43  typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
44 
45  class SACModel
46  {
47  public:
49 
50  SACModel () : cloud_(NULL) { }
51  SACModel (PointCloud cloud) : cloud_(&cloud) { }
52 
54 
55  virtual ~SACModel () { }
56 
58 
62  virtual void getSamples (int &iterations, std::vector<int> &samples) = 0;
63 
65 
68  virtual bool testModelCoefficients (const std::vector<double> &model_coefficients) = 0;
69 
71 
75  virtual bool computeModelCoefficients (const std::vector<int> &samples) = 0;
76 
78 
83  virtual void refitModel (const std::vector<int> &inliers, std::vector<double> &refit_coefficients) = 0;
84 
86 
90  virtual void getDistancesToModel (const std::vector<double> &model_coefficients, std::vector<double> &distances) = 0;
91 
93 
100  virtual void selectWithinDistance (const std::vector<double> &model_coefficients, double threshold, std::vector<int> &inliers) = 0;
101 
103 
108  virtual void projectPoints (const std::vector<int> &inliers, const std::vector<double> &model_coefficients, PointCloud &projected_points) = 0;
109 
111 
115  virtual void projectPointsInPlace (const std::vector<int> &inliers, const std::vector<double> &model_coefficients) = 0;
116 
118 
122  virtual bool doSamplesVerifyModel (const std::set<int> &indices, double threshold) = 0;
123 
124 
126 
128  inline void
129  setDataSet (PointCloud *cloud)
130  {
131  this->cloud_ = cloud;
132  indices_.clear ();
133  indices_.resize (cloud_->points.size ());
134  for (unsigned int i = 0; i < cloud_->points.size (); i++)
135  indices_[i] = i;
136  }
138 
141  inline void
142  setDataSet (PointCloud *cloud, std::vector<int> indices)
143  {
144  this->cloud_ = cloud;
145  this->indices_ = indices;
146  }
148 
150  void setDataIndices (std::vector<int> indices) { this->indices_ = indices; }
151 
153 
154  virtual int removeInliers ();
155 
157 
158  virtual int getModelType () = 0;
159 
161 
163  void setBestInliers (const std::vector<int> &best_inliers) { this->best_inliers_ = best_inliers; }
164 
166 
167  std::vector<int> getBestInliers () { return (this->best_inliers_); }
168 
170 
172  void setBestModel (std::vector<int> best_model) { this->best_model_ = best_model; }
173 
175 
176  std::vector<int> getBestModel () { return (this->best_model_); }
177 
179 
180  std::vector<double> getModelCoefficients () { return (this->model_coefficients_); }
181 
183  PointCloud* getCloud () { return (this->cloud_); }
184 
186  std::vector<int>* getIndices () { return (&this->indices_); }
187 
188  protected:
189 
191  PointCloud *cloud_;
192 
194  std::vector<int> indices_;
195 
197  std::vector<double> model_coefficients_;
198 
200  std::vector<int> best_model_;
202  std::vector<int> best_inliers_;
203  };
204 }
205 
206 #endif
virtual int removeInliers()
Remove the inliers found from the initial set of given point indices.
Definition: sac_model.cpp:41
std::vector< int > indices_
The list of internal point indices used.
Definition: sac_model.h:194
std::vector< double > model_coefficients_
The coefficients of our model computed directly from the best samples found.
Definition: sac_model.h:197
std::vector< int > getBestInliers()
Return the best set of inliers found so far for this model.
Definition: sac_model.h:167
pcl::PointCloud< pcl::PointXYZ > PointCloud
Definition: sac_model.h:43
virtual void projectPoints(const std::vector< int > &inliers, const std::vector< double > &model_coefficients, PointCloud &projected_points)=0
Create a new point cloud with inliers projected onto the model. Pure virtual.
std::vector< int > best_model_
The model found after the last computeModel () as pointcloud indices.
Definition: sac_model.h:200
virtual bool computeModelCoefficients(const std::vector< int > &samples)=0
Check whether the given index samples can form a valid model, compute the model coefficients from the...
std::vector< int > getBestModel()
Return the best model found so far.
Definition: sac_model.h:176
SACModel()
Constructor for base SACModel.
Definition: sac_model.h:50
virtual void selectWithinDistance(const std::vector< double > &model_coefficients, double threshold, std::vector< int > &inliers)=0
Select all the points which respect the given model coefficients as inliers. Pure virtual...
PointCloud * cloud_
Holds a pointer to the point cloud data array, since we don&#39;t want to copy the whole thing here...
Definition: sac_model.h:191
virtual int getModelType()=0
Return an unique id for each type of model employed.
std::vector< int > * getIndices()
Return a pointer to the point cloud data indices.
Definition: sac_model.h:186
std::vector< int > best_inliers_
The indices of the points that were chosen as inliers after the last computeModel () call...
Definition: sac_model.h:202
virtual bool testModelCoefficients(const std::vector< double > &model_coefficients)=0
Test whether the given model coefficients are valid given the input point cloud data. Pure virtual.
virtual void refitModel(const std::vector< int > &inliers, std::vector< double > &refit_coefficients)=0
Recompute the model coefficients using the given inlier set and return them to the user...
void setDataSet(PointCloud *cloud, std::vector< int > indices)
Set the dataset and indices.
Definition: sac_model.h:142
virtual bool doSamplesVerifyModel(const std::set< int > &indices, double threshold)=0
Verify whether a subset of indices verifies the internal model coefficients. Pure virtual...
virtual void getDistancesToModel(const std::vector< double > &model_coefficients, std::vector< double > &distances)=0
Compute all distances from the cloud data to a given model. Pure virtual.
virtual ~SACModel()
Destructor for base SACModel.
Definition: sac_model.h:55
SACModel(PointCloud cloud)
Definition: sac_model.h:51
virtual void getSamples(int &iterations, std::vector< int > &samples)=0
Get a set of random data samples and return them as point indices. Pure virtual.
std::vector< double > getModelCoefficients()
Return the model coefficients of the best model found so far.
Definition: sac_model.h:180
void setBestModel(std::vector< int > best_model)
Set the best model. Used by SAC methods. Do not call this except if you know what you&#39;re doing...
Definition: sac_model.h:172
PointCloud * getCloud()
Return a pointer to the point cloud data.
Definition: sac_model.h:183
void setBestInliers(const std::vector< int > &best_inliers)
Set the best set of inliers. Used by SAC methods. Do not call this except if you know what you&#39;re doi...
Definition: sac_model.h:163
virtual void projectPointsInPlace(const std::vector< int > &inliers, const std::vector< double > &model_coefficients)=0
Project inliers (in place) onto the given model. Pure virtual.
void setDataIndices(std::vector< int > indices)
Set the indices.
Definition: sac_model.h:150
void setDataSet(PointCloud *cloud)
Set the dataset.
Definition: sac_model.h:129


semantic_point_annotator
Author(s): Radu Bogdan Rusu
autogenerated on Mon Jun 10 2019 14:29:03