Go to the documentation of this file.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 #ifndef MATH_UTIL_RANSAC_H_
00031 #define MATH_UTIL_RANSAC_H_
00032
00033 #include <cmath>
00034 #include <limits>
00035 #include <vector>
00036
00037 #include <boost/make_shared.hpp>
00038 #include <swri_math_util/random.h>
00039
00040 namespace swri_math_util
00041 {
00042 template <class Model>
00043 class Ransac
00044 {
00045 public:
00046 typedef typename Model::M ModelType;
00047 typedef typename Model::T DataType;
00048
00049 explicit Ransac(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}
00050
00051 ModelType FitModel(
00052 Model& model,
00053 double max_error,
00054 double confidence,
00055 int32_t min_iterations,
00056 int32_t max_iterations,
00057 std::vector<uint32_t>& inliers,
00058 int32_t& iterations)
00059 {
00060 int32_t breakout = std::numeric_limits<int32_t>::max();
00061 ModelType best_fit;
00062 inliers.clear();
00063 int32_t max_inliers = 0;
00064
00065 if (!model.ValidData())
00066 {
00067 return best_fit;
00068 }
00069
00070 if (!rng_)
00071 {
00072 rng_ = boost::make_shared<RandomGenerator>();
00073 }
00074
00075 std::vector<int32_t> indices;
00076
00077 ModelType hypothesis;
00078 for (iterations = 0; (iterations < max_iterations && iterations < breakout) || iterations < min_iterations; iterations++)
00079 {
00080 indices.clear();
00081 rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
00082
00083
00084
00085 if (model.GetModel(indices, hypothesis, max_error))
00086 {
00087 int32_t inlier_count = model.GetInlierCount(hypothesis, max_error);
00088
00089
00090
00091 if (inlier_count > max_inliers)
00092 {
00093 max_inliers = inlier_count;
00094 Model::CopyTo(hypothesis, best_fit);
00095
00096
00097 double ratio = inlier_count / static_cast<double>(model.Size());
00098 double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
00099 if (p_no_outliers == 0)
00100 {
00101 breakout = 0;
00102 }
00103 else if (p_no_outliers < .9999)
00104 {
00105 breakout = std::log(1 - confidence) / std::log(p_no_outliers);
00106 }
00107 }
00108 }
00109 }
00110
00111 if (max_inliers > 0)
00112 {
00113 model.GetInliers(best_fit, max_error, inliers);
00114 }
00115 return best_fit;
00116 }
00117
00118 private:
00119 RandomGeneratorPtr rng_;
00120 };
00121
00122 template <class Model>
00123 class RansacBatch
00124 {
00125 public:
00126 typedef typename Model::M ModelType;
00127 typedef typename Model::T DataType;
00128
00129 explicit RansacBatch(RandomGeneratorPtr rng = RandomGeneratorPtr()) : rng_(rng) {}
00130
00131 ModelType FitModel(
00132 const DataType& data,
00133 double max_error,
00134 double confidence,
00135 int32_t max_iterations,
00136 int32_t batch_size,
00137 std::vector<uint32_t>& inliers,
00138 int32_t& iterations)
00139 {
00140 Model model(data, batch_size);
00141 iterations = 0;
00142 int32_t breakout = std::numeric_limits<int32_t>::max();
00143 ModelType best_fit;
00144 inliers.clear();
00145 int32_t max_inliers = 0;
00146
00147 if (!model.ValidData())
00148 {
00149 return best_fit;
00150 }
00151
00152 if (!rng_)
00153 {
00154 rng_ = boost::make_shared<RandomGenerator>();
00155 }
00156
00157 std::vector<int32_t> indices;
00158
00159 ModelType hypothesis;
00160 while (iterations < max_iterations && iterations < breakout)
00161 {
00162 int32_t valid = 0;
00163 model.ClearSamples();
00164 while (iterations < max_iterations && iterations < breakout && model.Samples() < batch_size)
00165 {
00166 iterations++;
00167 indices.clear();
00168 rng_->GetUniformRandomSample(0, model.Size() - 1, Model::MIN_SIZE, indices);
00169 model.AddSample(indices, max_error);
00170 }
00171
00172 if (model.Samples() > 0)
00173 {
00174 int32_t inlier_count = model.ProcessSamples(hypothesis, max_error);
00175 if (inlier_count > 0 && inlier_count > max_inliers)
00176 {
00177 max_inliers = inlier_count;
00178 Model::CopyTo(hypothesis, best_fit);
00179
00180
00181 double ratio = inlier_count / static_cast<double>(model.Size());
00182 double p_no_outliers = 1.0 - std::pow(ratio, Model::MIN_SIZE);
00183 if (p_no_outliers == 0)
00184 {
00185 breakout = 0;
00186 }
00187 else if (p_no_outliers < .9999)
00188 {
00189 breakout = std::log(1 - confidence) / std::log(p_no_outliers);
00190 }
00191 }
00192 }
00193 }
00194
00195 if (max_inliers > 0)
00196 {
00197 model.GetInliers(best_fit, max_error, inliers);
00198 }
00199 return best_fit;
00200 }
00201
00202 private:
00203 RandomGeneratorPtr rng_;
00204 };
00205 }
00206
00207 #endif // MATH_UTIL_RANSAC_H_