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
00033 #include <cfloat>
00034 #include <limits>
00035 #include <stereo_wall_detection/sample_consensus/msac.h>
00036
00037 namespace sample_consensus
00038 {
00040
00044 MSAC::MSAC (SACModel *model, double threshold) : SAC (model)
00045 {
00046 this->threshold_ = threshold;
00047
00048 this->probability_ = 0.99;
00049
00050 this->max_iterations_ = 10000;
00051
00052 this->iterations_ = 0;
00053 }
00054
00056
00059 MSAC::MSAC (SACModel* model) : SAC (model) { }
00060
00062
00065 bool
00066 MSAC::computeModel (int debug)
00067 {
00068 iterations_ = 0;
00069 double d_best_penalty = DBL_MAX;
00070
00071 double k = 1.0;
00072
00073 std::vector<int> best_model;
00074 std::vector<int> best_inliers, inliers;
00075 std::vector<int> selection;
00076 std::vector<double> distances;
00077
00078 int n_inliers_count = 0;
00079
00080
00081 while (iterations_ < k)
00082 {
00083
00084 sac_model_->getSamples (iterations_, selection);
00085
00086 if (selection.size () == 0) break;
00087
00088
00089 sac_model_->computeModelCoefficients (selection);
00090
00091 double d_cur_penalty = 0;
00092
00093
00094
00095 sac_model_->getDistancesToModel (sac_model_->getModelCoefficients (), distances);
00096 if (distances.size () == 0 && k != 1.0)
00097 {
00098 if (debug > 1)
00099 std::cerr << "[MSAC::computeModel] Distances to model has size 0 for k = " << k << std::endl;
00100 iterations_ += 1;
00101 continue;
00102 }
00103
00104 for (unsigned int i = 0; i < sac_model_->getIndices ()->size (); i++)
00105 d_cur_penalty += std::min ((double)distances[i], threshold_);
00106
00107
00108 if (d_cur_penalty < d_best_penalty)
00109 {
00110 d_best_penalty = d_cur_penalty;
00111 best_model = selection;
00112
00113
00114 best_inliers.resize (sac_model_->getIndices ()->size ());
00115 n_inliers_count = 0;
00116 for (unsigned int i = 0; i < sac_model_->getIndices ()->size (); i++)
00117 {
00118 if (distances[i] <= threshold_)
00119 {
00120 best_inliers[n_inliers_count] = sac_model_->getIndices ()->at (i);
00121 n_inliers_count++;
00122 }
00123 }
00124 best_inliers.resize (n_inliers_count);
00125
00126
00127 double w = (double)((double)n_inliers_count / (double)sac_model_->getIndices ()->size ());
00128 double p_no_outliers = 1 - pow (w, (double)selection.size ());
00129 p_no_outliers = std::max (std::numeric_limits<double>::epsilon (), p_no_outliers);
00130 p_no_outliers = std::min (1 - std::numeric_limits<double>::epsilon (), p_no_outliers);
00131 k = log (1 - probability_) / log (p_no_outliers);
00132 }
00133
00134 iterations_ += 1;
00135 if (debug > 1)
00136 std::cerr << "[MSAC::computeModel] Trial " << iterations_ << " out of " << ceil (k) << ": best number of inliers so far is " << best_inliers.size () << "." << std::endl;
00137 if (iterations_ > max_iterations_)
00138 {
00139 if (debug > 0)
00140 std::cerr << "[MSAC::computeModel] MSAC reached the maximum number of trials." << std::endl;
00141 break;
00142 }
00143 }
00144
00145 if (best_model.size () != 0)
00146 {
00147 if (debug > 0)
00148 std::cerr << "[MSAC::computeModel] Model found: " << best_inliers.size () << " inliers." << std::endl;
00149 sac_model_->setBestModel (best_model);
00150 sac_model_->setBestInliers (best_inliers);
00151 return (true);
00152 }
00153 else
00154 if (debug > 0)
00155 std::cerr << "[MSAC::computeModel] Unable to find a solution!" << std::endl;
00156 return (false);
00157 }
00158 }