| 198 | } |
| 199 | |
| 200 | void KeyFrame::PnPRANSAC(const vector<cv::Point2f> &matched_2d_old_norm, |
| 201 | const std::vector<cv::Point3f> &matched_3d, |
| 202 | std::vector<uchar> &status, |
| 203 | Eigen::Vector3d &PnP_T_old, Eigen::Matrix3d &PnP_R_old) |
| 204 | { |
| 205 | //for (int i = 0; i < matched_3d.size(); i++) |
| 206 | // printf("3d x: %f, y: %f, z: %f\n",matched_3d[i].x, matched_3d[i].y, matched_3d[i].z ); |
| 207 | //printf("match size %d \n", matched_3d.size()); |
| 208 | cv::Mat r, rvec, t, D, tmp_r; |
| 209 | cv::Mat K = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 0, 1.0, 0, 0, 0, 1.0); |
| 210 | Matrix3d R_inital; |
| 211 | Vector3d P_inital; |
| 212 | Matrix3d R_w_c = origin_vio_R * qic; |
| 213 | Vector3d T_w_c = origin_vio_T + origin_vio_R * tic; |
| 214 | |
| 215 | R_inital = R_w_c.inverse(); |
| 216 | P_inital = -(R_inital * T_w_c); |
| 217 | |
| 218 | cv::eigen2cv(R_inital, tmp_r); |
| 219 | cv::Rodrigues(tmp_r, rvec); |
| 220 | cv::eigen2cv(P_inital, t); |
| 221 | |
| 222 | cv::Mat inliers; |
| 223 | TicToc t_pnp_ransac; |
| 224 | |
| 225 | if (CV_MAJOR_VERSION < 3) |
| 226 | solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 100, inliers); |
| 227 | else |
| 228 | { |
| 229 | if (CV_MINOR_VERSION < 2) |
| 230 | solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, sqrt(10.0 / 460.0), 0.99, inliers); |
| 231 | else |
| 232 | solvePnPRansac(matched_3d, matched_2d_old_norm, K, D, rvec, t, true, 100, 10.0 / 460.0, 0.99, inliers); |
| 233 | |
| 234 | } |
| 235 | |
| 236 | for (int i = 0; i < (int)matched_2d_old_norm.size(); i++) |
| 237 | status.push_back(0); |
| 238 | |
| 239 | for( int i = 0; i < inliers.rows; i++) |
| 240 | { |
| 241 | int n = inliers.at<int>(i); |
| 242 | status[n] = 1; |
| 243 | } |
| 244 | |
| 245 | cv::Rodrigues(rvec, r); |
| 246 | Matrix3d R_pnp, R_w_c_old; |
| 247 | cv::cv2eigen(r, R_pnp); |
| 248 | R_w_c_old = R_pnp.transpose(); |
| 249 | Vector3d T_pnp, T_w_c_old; |
| 250 | cv::cv2eigen(t, T_pnp); |
| 251 | T_w_c_old = R_w_c_old * (-T_pnp); |
| 252 | |
| 253 | PnP_R_old = R_w_c_old * qic.transpose(); |
| 254 | PnP_T_old = T_w_c_old - PnP_R_old * tic; |
| 255 | |
| 256 | } |
| 257 | |