| 293 | } |
| 294 | |
| 295 | bool MLPnPsolver::Refine(){ |
| 296 | vector<int> vIndices; |
| 297 | vIndices.reserve(mvbBestInliers.size()); |
| 298 | |
| 299 | for(size_t i=0; i<mvbBestInliers.size(); i++) |
| 300 | { |
| 301 | if(mvbBestInliers[i]) |
| 302 | { |
| 303 | vIndices.push_back(i); |
| 304 | } |
| 305 | } |
| 306 | |
| 307 | //Bearing vectors and 3D points used for this ransac iteration |
| 308 | bearingVectors_t bearingVecs; |
| 309 | points_t p3DS; |
| 310 | vector<int> indexes; |
| 311 | |
| 312 | for(size_t i=0; i<vIndices.size(); i++) |
| 313 | { |
| 314 | int idx = vIndices[i]; |
| 315 | |
| 316 | bearingVecs.push_back(mvBearingVecs[idx]); |
| 317 | p3DS.push_back(mvP3Dw[idx]); |
| 318 | indexes.push_back(i); |
| 319 | } |
| 320 | |
| 321 | //By the moment, we are using MLPnP without covariance info |
| 322 | cov3_mats_t covs(1); |
| 323 | |
| 324 | //Result |
| 325 | transformation_t result; |
| 326 | |
| 327 | // Compute camera pose |
| 328 | computePose(bearingVecs,p3DS,covs,indexes,result); |
| 329 | |
| 330 | // Check inliers |
| 331 | CheckInliers(); |
| 332 | |
| 333 | mnRefinedInliers =mnInliersi; |
| 334 | mvbRefinedInliers = mvbInliersi; |
| 335 | |
| 336 | if(mnInliersi>mRansacMinInliers) |
| 337 | { |
| 338 | cv::Mat Rcw(3,3,CV_64F,mRi); |
| 339 | cv::Mat tcw(3,1,CV_64F,mti); |
| 340 | Rcw.convertTo(Rcw,CV_32F); |
| 341 | tcw.convertTo(tcw,CV_32F); |
| 342 | mRefinedTcw.setIdentity(); |
| 343 | |
| 344 | mRefinedTcw.block<3,3>(0,0) = Converter::toMatrix3f(Rcw); |
| 345 | mRefinedTcw.block<3,1>(0,3) = Converter::toVector3f(tcw); |
| 346 | |
| 347 | Eigen::Matrix<double, 3, 3, Eigen::RowMajor> eigRcw(mRi[0]); |
| 348 | Eigen::Vector3d eigtcw(mti); |
| 349 | |
| 350 | return true; |
| 351 | } |
| 352 | return false; |