/// Triangulate a set of points between two view void triangulate2View_Vector(const Mat34 & P1, const Mat34 & P2, const std::vector<SIOPointFeature> & vec_feat1, const std::vector<SIOPointFeature> & vec_feat2, const std::vector<IndMatch> & vec_index, std::vector<Vec3> * pvec_3dPoint, std::vector<double> * pvec_residual) { assert(pvec_3dPoint); assert(pvec_residual); pvec_3dPoint->reserve(vec_index.size()); pvec_residual->reserve(vec_index.size()); for (size_t i=0; i < vec_index.size(); ++i) { //Get corresponding point and triangulate it const SIOPointFeature & imaA = vec_feat1[vec_index[i]._i]; const SIOPointFeature & imaB = vec_feat2[vec_index[i]._j]; const Vec2 x1 = imaA.coords().cast<double>(), x2 = imaB.coords().cast<double>(); Vec3 X_euclidean = Vec3::Zero(); TriangulateDLT(P1, x1, P2, x2, &X_euclidean); double dResidual2D = (PinholeCamera::Residual(P1, X_euclidean, x1) + PinholeCamera::Residual(P2, X_euclidean, x2)) /2.0; // store 3DPoint and associated residual pvec_3dPoint->push_back(X_euclidean); pvec_residual->push_back(dResidual2D); } if (!vec_index.empty()) { double dMin = *min_element(pvec_residual->begin(), pvec_residual->end()), dMax = *max_element(pvec_residual->begin(), pvec_residual->end()); std::cout << std::endl << "SfMRobust::triangulate2View_Vector" << std::endl << "\t-- Residual min max -- " << dMin <<"\t" << dMax << std::endl; } }
int TriangulatePoints(const std::vector<cv::Vec2d> &kp0, const std::vector<cv::Vec2d> &kp1, const cv::Mat &K, const cv::Mat &R0, const cv::Mat &t0, const cv::Mat &R1, const cv::Mat &t1, std::vector<cv::Point3f> &points3d, std::vector<bool> &points3d_mask) { bool generate_test = false; if (generate_test) { cv::FileStorage file("triangulation_test_data.txt", cv::FileStorage::WRITE); file << "NumPoints" << (int)kp0.size(); write(file, "kp0", kp0); write(file, "kp1", kp1); file << "K" << K << "R0" << R0 << "t0" << t0 << "R1" << R1 << "t1" << t1; /* cv::FileStorage file_read; file_read.open("triangulation_test_data.txt", cv::FileStorage::READ); int kp_num = (int)file_read["NumPoints"]; FileNode kp0node = file_read["kp0"]; std::vector<cv::Vec2d> kp0_new; std::vector<cv::Vec2d> kp1_new; cv::Mat K_new, R0_new, t0_new, R1_new, t1_new; read(file_read["kp0"], kp0_new); read(file_read["kp1"], kp1_new); file_read["K"] >> K_new; file_read["R0"] >> R0_new; file_read["t0"] >> t0_new; file_read["R1"] >> R1_new; file_read["t1"] >> t1_new; */ } double reprojection_error_thres = 5.0; cv::Mat P0, P1; RtToP(R0, t0, P0); RtToP(R1, t1, P1); P0 = K * P0; P1 = K * P1; // TODO: Why cv::Mat center0 = -R0.t() * t0; cv::Mat center1 = -R1.t() * t1; int nParallal = 0; int nInfinite = 0; int nLargeError = 0; int nNegativeDepth = 0; int nGood = 0; points3d.resize(kp0.size()); points3d_mask.resize(kp0.size(), true); for (int i = 0; i < kp0.size(); ++i) { TriangulateDLT(kp0[i], kp1[i], P0, P1, points3d[i]); cv::Mat p_global(3, 1, CV_64F); p_global.at<double>(0) = points3d[i].x; p_global.at<double>(1) = points3d[i].y; p_global.at<double>(2) = points3d[i].z; cv::Mat p3dC0 = R0 * p_global + t0; cv::Mat p3dC1 = R1 * p_global + t1; double depth0 = p3dC0.at<double>(2); double depth1 = p3dC1.at<double>(2); if (depth0 <= 0 || depth1 <= 0) { points3d_mask[i] = false; nNegativeDepth++; continue; } // TODO: Make sure isfinite is in std if (!std::isfinite(p3dC0.at<double>(0)) || !std::isfinite(p3dC0.at<double>(1)) || !std::isfinite(p3dC0.at<double>(2))) { nInfinite++; points3d_mask[i] = false; continue; } // TODO: Make sure isfinite is in std if (!std::isfinite(p3dC1.at<double>(0)) || !std::isfinite(p3dC1.at<double>(1)) || !std::isfinite(p3dC1.at<double>(2))) { nInfinite++; points3d_mask[i] = false; continue; } // Check parallax cv::Mat p0_vec = p3dC0 - center0; cv::Mat p1_vec = p3dC1 - center1; double dist0 = cv::norm(p0_vec); double dist1 = cv::norm(p1_vec); double cosParallax = p0_vec.dot(p1_vec) / (dist0 * dist1); if (cosParallax > 0.998) { nParallal++; points3d_mask[i] = false; continue; } double error0 = ComputeReprojectionError(points3d[i], kp0[i], P0); double error1 = ComputeReprojectionError(points3d[i], kp1[i], P1); if (error0 > reprojection_error_thres || error1 > reprojection_error_thres) { nLargeError++; points3d_mask[i] = false; continue; } nGood++; } std::cout << "Found " << nGood << " / " << kp0.size() << " good triangulated points.\n"; std::cout << "Found " << nInfinite << " / " << kp0.size() << " infinite points during triangulation.\n"; std::cout << "Found " << nParallal << " / " << kp0.size() << " parallal points during triangulation.\n"; std::cout << "Found " << nLargeError << " / " << kp0.size() << " large error points during triangulation.\n"; std::cout << "Found " << nNegativeDepth << " / " << kp0.size() << " negative depth points during triangulation.\n"; return nGood; }
/// From the essential matrix test the 4 possible solutions /// and return the best one. (point in front of the selected solution) bool estimate_Rt_fromE(const Mat3 & K1, const Mat3 & K2, const Mat & x1, const Mat & x2, const Mat3 & E, const std::vector<size_t> & vec_inliers, Mat3 * R, Vec3 * t) { bool bOk = false; // Accumulator to find the best solution std::vector<size_t> f(4, 0); std::vector<Mat3> Es; // Essential, std::vector<Mat3> Rs; // Rotation matrix. std::vector<Vec3> ts; // Translation matrix. Es.push_back(E); // Recover best rotation and translation from E. MotionFromEssential(E, &Rs, &ts); //-> Test the 4 solutions will all the point assert(Rs.size() == 4); assert(ts.size() == 4); Mat34 P1, P2; Mat3 R1 = Mat3::Identity(); Vec3 t1 = Vec3::Zero(); P_From_KRt(K1, R1, t1, &P1); for (int i = 0; i < 4; ++i) { const Mat3 &R2 = Rs[i]; const Vec3 &t2 = ts[i]; P_From_KRt(K2, R2, t2, &P2); Vec3 X; for (size_t k = 0; k < vec_inliers.size(); ++k) { const Vec2 & x1_ = x1.col(vec_inliers[k]); const Vec2 & x2_ = x2.col(vec_inliers[k]); TriangulateDLT(P1, x1_, P2, x2_, &X); // Test if point is front to the two cameras. if (Depth(R1, t1, X) > 0 && Depth(R2, t2, X) > 0) { ++f[i]; } } } // Check the solution : std::cout << "\t Number of points in front of both cameras:" << f[0] << " " << f[1] << " " << f[2] << " " << f[3] << std::endl; std::vector<size_t>::iterator iter = max_element(f.begin(), f.end()); if(*iter != 0) { size_t index = std::distance(f.begin(), iter); (*R) = Rs[index]; (*t) = ts[index]; bOk = true; } else { std::cerr << std::endl << "/!\\There is no right solution," <<" probably intermediate results are not correct or no points" <<" in front of both cameras" << std::endl; bOk = false; } return bOk; }