1 /******************************************************************************
2 * Authors: Johannes Mikulasch *
3 * License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
4 * *
5 * Redistribution and use in source and binary forms, with or without *
6 * modification, are permitted provided that the following conditions *
7 * are met: *
8 * * Redistributions of source code must retain the above copyright *
9 * notice, this list of conditions and the following disclaimer. *
10 * * Redistributions in binary form must reproduce the above copyright *
11 * notice, this list of conditions and the following disclaimer in the *
12 * documentation and/or other materials provided with the distribution. *
13 * * Neither the name of ANU nor the names of its contributors may be *
14 * used to endorse or promote products derived from this software without *
15 * specific prior written permission. *
16 * *
17 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
18 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
19 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
20 * ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
21 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
22 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
23 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
24 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
25 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
26 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
27 * SUCH DAMAGE. *
28 ******************************************************************************/
29
30 //Note: has been derived from PCL and from Ransac.hpp which has been derived from ROS
31
32 template<typename P>
Lmeds(int maxIterations,double threshold,double probability)33 opengv::sac::Lmeds<P>::Lmeds(
34 int maxIterations, double threshold, double probability) :
35 SampleConsensus<P>(maxIterations, threshold, probability)
36 {}
37
38 template<typename P>
~Lmeds()39 opengv::sac::Lmeds<P>::~Lmeds(){}
40
41 template<typename PROBLEM_T>
42 bool
computeModel(int debug_verbosity_level)43 opengv::sac::Lmeds<PROBLEM_T>::computeModel(int debug_verbosity_level)
44 {
45 typedef PROBLEM_T problem_t;
46 typedef typename problem_t::model_t model_t;
47
48 // Warn and exit if no threshold was set
49 if (threshold_ == std::numeric_limits<double>::max())
50 {
51 fprintf(stderr,"[sm::LeastMedianSquares::computeModel] No threshold set!\n");
52 return (false);
53 }
54
55 iterations_ = 0;
56 double d_best_penalty = std::numeric_limits<double>::max();
57
58 std::vector<int> best_model;
59 std::vector<int> selection;
60 model_t model_coefficients;
61 std::vector<double> distances;
62
63 int n_inliers_count = 0;
64
65 unsigned skipped_count = 0;
66 // suppress infinite loops by just allowing 10 x maximum allowed iterations for
67 // invalid model parameters!
68 const unsigned max_skip = max_iterations_ * 10;
69
70 if(debug_verbosity_level > 1)
71 fprintf(stdout,
72 "[sm::LeastMedianSquares::computeModel] Starting Least Median of Squares\n"
73 "max_iterations: %d\n"
74 "max_skip: %d\n",
75 max_iterations_, max_skip);
76
77 // Iterate
78 while(iterations_ < max_iterations_ && skipped_count < max_skip)
79 {
80 // Get X samples which satisfy the model criteria
81 sac_model_->getSamples(iterations_, selection);
82
83 if(selection.empty())
84 {
85 fprintf(stderr, "[sm::LeastMedianSquares::computeModel] No samples could be selected!\n");
86 break;
87 }
88
89 if(!sac_model_->computeModelCoefficients(selection, model_coefficients))
90 {
91 ++ skipped_count;
92 continue;
93 }
94
95 double d_cur_penalty = 0;
96
97 // Iterate through the 3d points and calculate the distances from them to the model
98 distances.clear();
99 sac_model_->getDistancesToModel(model_coefficients, distances);
100
101 // No distances? The model must not respect the user given constraints
102 if (distances.empty ())
103 {
104 ++skipped_count;
105 continue;
106 }
107
108 // Clip distances smaller than 0. Square the distances
109 for (std::size_t i = 0; i < distances.size(); ++i) {
110 if (distances[i] < 0) {
111 distances[i] = 0;
112 }
113 distances[i] = distances[i] * distances[i];
114 }
115
116 std::sort (distances.begin(), distances.end());
117
118 size_t mid = sac_model_->getIndices()->size() / 2;
119 if (mid >= distances.size())
120 {
121 ++skipped_count;
122 continue;
123 }
124
125 // Do we have a "middle" point or should we "estimate" one ?
126 if (sac_model_->getIndices()->size() % 2 == 0) {
127 d_cur_penalty = (distances[mid-1] + distances[mid]) / 2;
128 } else {
129 d_cur_penalty = distances[mid];
130 }
131
132 // Better match ?
133 if(d_cur_penalty < d_best_penalty)
134 {
135 d_best_penalty = d_cur_penalty;
136
137 // Save the current model/inlier/coefficients selection as being the best so far
138 model_ = selection;
139 model_coefficients_ = model_coefficients;
140 }
141
142 ++iterations_;
143
144 if(debug_verbosity_level > 1)
145 fprintf(stdout,
146 "[sm::LeastMedianSquares::computeModel] Trial %d out of %d. Best penalty is: %f so far. Current penalty is: %f\n",
147 iterations_, max_iterations_, d_best_penalty, d_cur_penalty);
148 }
149
150 if(model_.empty())
151 {
152 if (debug_verbosity_level > 0)
153 fprintf(stdout,"[sm::LeastMedianSquares::computeModel] Unable to find a solution!\n");
154 return (false);
155 }
156
157 // Classify the data points into inliers and outliers
158 // Sigma = 1.4826 * (1 + 5 / (n-d)) * sqrt (M)
159 // @note: See "Robust Regression Methods for Computer Vision: A Review"
160 //double sigma = 1.4826 * (1 + 5 / (sac_model_->getIndices ()->size () - best_model.size ())) * sqrt (d_best_penalty);
161 //double threshold = 2.5 * sigma;
162
163 // Iterate through the 3d points and calculate the distances from them to the model again
164 distances.clear();
165 sac_model_->getDistancesToModel(model_coefficients_, distances);
166 // No distances? The model must not respect the user given constraints
167 if (distances.empty())
168 {
169 fprintf(stderr,"[sm::LeastMedianSquares::computeModel] The model found failed to verify against the given constraints!\n");
170 return (false);
171 }
172
173 std::vector<int> &indices = *sac_model_->getIndices();
174
175 if (distances.size () != indices.size ())
176 {
177 fprintf(stderr,"[sm::LeastMedianSquares::computeModel] Estimated distances (%zu) differs than the normal of indices (%zu).\n", distances.size (), indices.size ());
178 return false;
179 }
180
181 inliers_.resize(distances.size());
182 // Get the inliers for the best model found
183 n_inliers_count = 0;
184 for (size_t i = 0; i < distances.size(); ++i)
185 if (distances[i] <= threshold_)
186 inliers_[n_inliers_count++] = indices[i];
187
188 // Resize the inliers vector
189 inliers_.resize(n_inliers_count);
190
191 if (debug_verbosity_level > 0)
192 fprintf(stdout,"[sm::LeastMedianSquares::computeModel] Model: %zu size, %d inliers.\n", model_.size (), n_inliers_count);
193
194 return (true);
195 }
196