| 306 | } |
| 307 | |
| 308 | void Keyframe::PnPRANSAC(const std::vector<cv::Point2f>& matched_2d_old, |
| 309 | const std::vector<cv::Point3f>& matched_3d, |
| 310 | std::vector<uchar>& status, |
| 311 | Eigen::Vector3d& PnP_T_old, |
| 312 | Eigen::Matrix3d& PnP_R_old) { |
| 313 | cv::Mat r, rvec, t, tmp_r; |
| 314 | cv::Mat K = (cv::Mat_<double>(3, 3) << params_.camera_calibration_.focal_length_.x(), |
| 315 | 0, |
| 316 | params_.camera_calibration_.principal_point_.x(), |
| 317 | 0, |
| 318 | params_.camera_calibration_.focal_length_.y(), |
| 319 | params_.camera_calibration_.principal_point_.y(), |
| 320 | 0, |
| 321 | 0, |
| 322 | 1.0); |
| 323 | |
| 324 | // std::cout << "Camera Matrix: " << K << std::endl; |
| 325 | // std::cout << "distortion coeffs: " << params_.camera_calibration_.distortion_coefficients_ << std::endl; |
| 326 | |
| 327 | Eigen::Matrix3d R_inital; |
| 328 | Eigen::Vector3d P_inital; |
| 329 | |
| 330 | Eigen::Matrix3d R_w_c = origin_svin_R; |
| 331 | Eigen::Vector3d T_w_c = origin_svin_T; |
| 332 | |
| 333 | R_inital = R_w_c.inverse(); |
| 334 | P_inital = -(R_inital * T_w_c); |
| 335 | |
| 336 | cv::eigen2cv(R_inital, tmp_r); |
| 337 | cv::Rodrigues(tmp_r, rvec); |
| 338 | cv::eigen2cv(P_inital, t); |
| 339 | |
| 340 | cv::Mat inliers; |
| 341 | |
| 342 | // bjoshi |
| 343 | // Temporary fix for https://github.com/opencv/opencv/issues/17799 |
| 344 | // This is a bug in opencv. The bug is fixed in opencv master branch. |
| 345 | |
| 346 | try { |
| 347 | solvePnPRansac(matched_3d, |
| 348 | matched_2d_old, |
| 349 | K, |
| 350 | params_.camera_calibration_.distortion_coefficients_, |
| 351 | rvec, |
| 352 | t, |
| 353 | false, |
| 354 | params_.loop_closure_params_.pnp_ransac_iterations, |
| 355 | params_.loop_closure_params_.pnp_reprojection_thresh, |
| 356 | 0.99, |
| 357 | inliers); |
| 358 | } catch (cv::Exception e) { |
| 359 | // std::cout << "Caught exception in PnPRANSAC:" << e.what() << std::endl; |
| 360 | inliers.setTo(cv::Scalar(0)); |
| 361 | } |
| 362 | |
| 363 | for (int i = 0; i < static_cast<int>(matched_2d_old.size()); i++) status.push_back(0); |
| 364 | |
| 365 | for (int i = 0; i < inliers.rows; i++) { |