| 223 | } |
| 224 | |
| 225 | void MLPnPsolver::SetRansacParameters(double probability, int minInliers, int maxIterations, int minSet, float epsilon, float th2){ |
| 226 | mRansacProb = probability; |
| 227 | mRansacMinInliers = minInliers; |
| 228 | mRansacMaxIts = maxIterations; |
| 229 | mRansacEpsilon = epsilon; |
| 230 | mRansacMinSet = minSet; |
| 231 | |
| 232 | N = mvP2D.size(); // number of correspondences |
| 233 | |
| 234 | mvbInliersi.resize(N); |
| 235 | |
| 236 | // Adjust Parameters according to number of correspondences |
| 237 | int nMinInliers = N*mRansacEpsilon; |
| 238 | if(nMinInliers<mRansacMinInliers) |
| 239 | nMinInliers=mRansacMinInliers; |
| 240 | if(nMinInliers<minSet) |
| 241 | nMinInliers=minSet; |
| 242 | mRansacMinInliers = nMinInliers; |
| 243 | |
| 244 | if(mRansacEpsilon<(float)mRansacMinInliers/N) |
| 245 | mRansacEpsilon=(float)mRansacMinInliers/N; |
| 246 | |
| 247 | // Set RANSAC iterations according to probability, epsilon, and max iterations |
| 248 | int nIterations; |
| 249 | |
| 250 | if(mRansacMinInliers==N) |
| 251 | nIterations=1; |
| 252 | else |
| 253 | nIterations = ceil(log(1-mRansacProb)/log(1-pow(mRansacEpsilon,3))); |
| 254 | |
| 255 | mRansacMaxIts = max(1,min(nIterations,mRansacMaxIts)); |
| 256 | |
| 257 | mvMaxError.resize(mvSigma2.size()); |
| 258 | for(size_t i=0; i<mvSigma2.size(); i++) |
| 259 | mvMaxError[i] = mvSigma2[i]*th2; |
| 260 | } |
| 261 | |
| 262 | void MLPnPsolver::CheckInliers(){ |
| 263 | mnInliersi=0; |