// Reconstruction function for API void reconstruct(const InputArrayOfArrays points2d, OutputArrayOfArrays projection_matrices, OutputArray points3d, bool is_projective, bool has_outliers, bool is_sequence) { int nviews = points2d.total(); cv::Mat F; // OpenCV data types std::vector<cv::Mat> pts2d; points2d.getMatVector(pts2d); int depth = pts2d[0].depth(); // Projective reconstruction if (is_projective) { // Two view reconstruction if (nviews == 2) { // Get fundamental matrix fundamental8Point(pts2d[0], pts2d[1], F, has_outliers); // Get Projection matrices cv::Mat P, Pp; projectionsFromFundamental(F, P, Pp); projection_matrices.create(2, 1, depth); P.copyTo(projection_matrices.getMatRef(0)); Pp.copyTo(projection_matrices.getMatRef(1)); // Triangulate and find 3D points using inliers triangulatePoints(points2d, projection_matrices, points3d); } } // Affine reconstruction else { // Two view reconstruction if (nviews == 2) { } else { } } }
void AAM::triangulateMeanPoints() { cout<<"in triangulation mean function"<<endl; if (!this->meanShapePointsOrder.empty()) this->meanShapePointsOrder.clear(); for(int i=0; i<meanShape.cols/2; i++) { //pair<int, int> point=make_pair(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1)); Point point(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1)); this->meanShapePointsOrder[point]=i; this->meanPoints.push_back(Point(meanShape.at<int>(0, 2*i), meanShape.at<int>(0, 2*i+1))); } cout<<"mean points: "<<this->meanPoints<<endl; this->meanShapeTriangulation=triangulatePoints(this->meanPoints); this->displayTriangulation(this->meanShapeTriangulation); for (std::map<Point,unsigned int, comparePoint>::iterator it=this->meanShapePointsOrder.begin(); it!=this->meanShapePointsOrder.end(); ++it) std::cout << it->first << " => " << it->second << '\n'; }
void reconstruct(Mat& K, Mat& R, Mat& T, vector<Point2f>& p1, vector<Point2f>& p2, Mat& structure) { Mat proj1(3, 4, CV_32FC1); Mat proj2(3, 4, CV_32FC1); proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1); proj1.col(3) = Mat::zeros(3, 1, CV_32FC1); R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1); T.convertTo(proj2.col(3), CV_32FC1); Mat fK; K.convertTo(fK, CV_32FC1); proj1 = fK*proj1; proj2 = fK*proj2; triangulatePoints(proj1, proj2, p1, p2, structure); }
bool ReconstructionHandler::triangulatePointsBetweenViews( int workingView, int olderView, std::vector<struct CloudPoint>& newTriangulated, std::vector<int>& addToCloud, int numViews) { LOG (Debug, "Triangulating image ", workingView, " and ", olderView); cv::Matx34d P0 = sceneModel->poseMats[olderView]; cv::Matx34d P1 = sceneModel->poseMats[workingView]; // Get aligned matched 2D points from the two views std::vector<cv::KeyPoint> pt_set1,pt_set2; std::vector<cv::DMatch> matches = sceneModel->getMatches()[std::make_pair(olderView,workingView)]; for (unsigned int i=0; i<matches.size(); i++) { pt_set1.push_back((sceneModel->getKeypoints())[olderView][matches[i].queryIdx]); pt_set2.push_back((sceneModel->getKeypoints())[workingView][matches[i].trainIdx]); } // Adding more triangulated points to general cloud // Why not 4 triangulations as in stereo initialization? double reprojectionError = triangulatePoints(pt_set1, pt_set2, sceneModel->K, sceneModel->Kinv, sceneModel->distortionCoefficients, P0, P1, newTriangulated); LOG(Info, "Triangulation reprojection error ", reprojectionError); if (newTriangulated.size()==0){ LOG(Warn, "Zero points were triangulated"); return false; } std::vector<uchar> trig_status; if(!testTriangulation(newTriangulated, P0, trig_status) || !testTriangulation(newTriangulated, P1, trig_status)) { LOG(Warn, "Triangulation did not succeed"); return false; } if(reprojectionError > 50.0) { LOG(Warn, "Reprojection error too high - triangulation failed."); return false; } // Filter out outlier points with high reprojection std::vector<double> reprojectionErrors; for(int i=0;i<newTriangulated.size();i++) { reprojectionErrors.push_back(newTriangulated[i].reprojection_error); } std::sort(reprojectionErrors.begin(),reprojectionErrors.end()); // Get the 80% precentile double reprj_err_cutoff = reprojectionErrors[4 * reprojectionErrors.size() / 5] * 2.4; //threshold from Snavely07 4.2 std::vector<CloudPoint> newTriangulatedFiltered; std::vector<cv::DMatch> newMatches; for(int i=0;i<newTriangulated.size();i++) { if(trig_status[i] == 0) continue; //point was not in front of either camera if(newTriangulated[i].reprojection_error > 16.0) { continue; //reject point as it is inaccurate } if(newTriangulated[i].reprojection_error < 4.0 || newTriangulated[i].reprojection_error < reprj_err_cutoff) { newTriangulatedFiltered.push_back(newTriangulated[i]); newMatches.push_back(matches[i]); } else { continue; } } LOG(Info, " Filtered out ", (newTriangulated.size() - newTriangulatedFiltered.size()), " high-error points"); // Check if all points are filtered out if(newTriangulatedFiltered.size() <= 0){ LOG(Warn, "All points have been filtered out."); return false; } // Use filtered points now newTriangulated.clear(); newTriangulated.insert(newTriangulated.begin(), newTriangulatedFiltered.begin(), newTriangulatedFiltered.end()); // Use filtered matches matches = newMatches; // Update the matches storage sceneModel->getMatches()[std::make_pair(olderView,workingView)] = newMatches; //just to make sure, remove if unneccesary sceneModel->getMatches()[std::make_pair(workingView,olderView)] = featureHandler->flipMatches(newMatches); // Now determine which points should be added to the cloud based on which already exist addToCloud.clear(); addToCloud.resize(newTriangulated.size(),1); int foundOtherViewsCount = 0; // Scan new triangulated points, if they were already triangulated before - strengthen cloud // If not, mark them to be added for (int j = 0; j<newTriangulated.size(); j++) { newTriangulated[j].imgpt_for_img.resize(numViews,-1); //matches[j] corresponds to new_triangulated[j] //matches[j].queryIdx = point in <olderView> //matches[j].trainIdx = point in <workingView> newTriangulated[j].imgpt_for_img[olderView] = matches[j].queryIdx; //2D reference to <olderView> newTriangulated[j].imgpt_for_img[workingView] = matches[j].trainIdx; //2D reference to <workingView> bool foundInOtherView = false; for (unsigned int view = 0; view < numViews; view++) { if(view != olderView) { // Look for points in <view> that match to points in <workingView> std::vector<cv::DMatch> submatches = sceneModel->getMatches()[std::make_pair(view,workingView)]; for (unsigned int ii = 0; ii < submatches.size(); ii++) { if (submatches[ii].trainIdx == matches[j].trainIdx && !foundInOtherView) { // Point was already found in <view> - strengthen it in the known cloud, if it exists there for (unsigned int pt3d=0; pt3d<sceneModel->reconstructedPts.size(); pt3d++) { if (sceneModel->reconstructedPts[pt3d].imgpt_for_img[view] == submatches[ii].queryIdx) { //sceneModel->reconstructedPts[pt3d] - a point that has 2d reference in <view> { // As this point has already been reconstructed in <view>, update 2D match references sceneModel->reconstructedPts[pt3d].imgpt_for_img[workingView] = matches[j].trainIdx; sceneModel->reconstructedPts[pt3d].imgpt_for_img[olderView] = matches[j].queryIdx; foundInOtherView = true; addToCloud[j] = 0; } } } } } } } { if (foundInOtherView) { // If point was found in any of the previous views foundOtherViewsCount++; } else { addToCloud[j] = 1; } } } LOG(Debug, "Number of points found in other views: ", foundOtherViewsCount, "/", newTriangulated.size()); LOG(Debug, "Adding new points from triangulation: ", cv::countNonZero(addToCloud)); return true; }
void EstimateCameraPose::calcCameraPoseFromE() { cv::SVD svd(E, CV_SVD_MODIFY_A); cv::Mat svd_u = svd.u; cv::Mat svd_vt = svd.vt; cv::Mat svd_w = svd.w; cv::Matx33d W(0, -1, 0, 1, 0, 0, 0, 0, 1); cv::Matx33d W_t; cv::transpose(W, W_t); cv::Mat_<double> R = svd_u * cv::Mat(W) * svd_vt; //HZ 9.19 cv::Mat_<double> R2 = svd_u * cv::Mat(W_t) * svd_vt; cv::Mat_<double> t = svd_u.col(2); //u3 cv::Mat_<double> t2 = -t; if (!CheckCoherentRotation(R)||!CheckCoherentRotation(R2)) { std::cout << "resulting rotation is not coherent\n"; return; } cv::Matx34d origin = cv::Matx34d(1, 0, 0, 0 ,0, 1, 0, 0, 0, 0, 1, 0); M1 = intrinsic*origin; //four possible camera matrices M2.resize(4); M2[0] = cv::Matx34d(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), R(2, 0), R(2, 1), R(2, 2), t(2)); M2[1] = cv::Matx34d(R(0, 0), R(0, 1), R(0, 2), t2(0), R(1, 0), R(1, 1), R(1, 2), t2(1), R(2, 0), R(2, 1), R(2, 2), t2(2)); M2[2] = cv::Matx34d(R2(0, 0), R2(0, 1), R2(0, 2), t2(0), R2(1, 0), R2(1, 1), R2(1, 2), t2(1), R2(2, 0), R2(2, 1), R2(2, 2), t2(2)); M2[3] = cv::Matx34d(R2(0, 0), R2(0, 1), R2(0, 2), t(0), R2(1, 0), R2(1, 1), R2(1, 2), t(1), R2(2, 0), R2(2, 1), R2(2, 2), t(2)); for (int i = 0; i < 4; i++) { M2[i] = intrinsic*M2[i]; } //homogeneous 3D points cv::Mat points4d; //record the number of zeros that are negative std::vector<int> falsezero={0,0,0,0}; points3d.resize(4); for (int i = 0; i < M2.size(); i++) { triangulatePoints(M1, M2[i], matchedpoints1, matchedpoints2, points4d); for (int j = 0; j < points4d.cols; j++) { //convert homogeneous 3D points to 3D points float w = points4d.at<float>(3, j); float x = points4d.at<float>(0, j) / w; float y = points4d.at<float>(1, j) / w; float z = points4d.at<float>(2, j) / w; cv::Point3d p(x, y, z); if (z < 0) { falsezero[i]++; } points3d[i].push_back(p); } } int i = 0; int small = falsezero[0]; for (int j = 1; j < 4; j++) { if (falsezero[j] < small) { small = falsezero[j]; i = j; } } truepoints3d = points3d[i]; switch (i) { case 0: { rotation = R; translation = t; break; } case 1: { rotation = R; translation = t2; break; } case 2: { rotation = R2; translation = t2; break; } case 3: { rotation = R2; translation = t; break; } default: break; } }
// Reconstruction function for API void reconstruct(InputArrayOfArrays points2d, OutputArray Ps, OutputArray points3d, InputOutputArray K, bool is_projective) { const int nviews = points2d.total(); CV_Assert( nviews >= 2 ); // OpenCV data types std::vector<Mat> pts2d; points2d.getMatVector(pts2d); const int depth = pts2d[0].depth(); Matx33d Ka = K.getMat(); // Projective reconstruction if (is_projective) { if ( nviews == 2 ) { // Get Projection matrices Matx33d F; Matx34d P, Pp; normalizedEightPointSolver(pts2d[0], pts2d[1], F); projectionsFromFundamental(F, P, Pp); Ps.create(2, 1, depth); Mat(P).copyTo(Ps.getMatRef(0)); Mat(Pp).copyTo(Ps.getMatRef(1)); // Triangulate and find 3D points using inliers triangulatePoints(points2d, Ps, points3d); } else { std::vector<Mat> Rs, Ts; reconstruct(points2d, Rs, Ts, Ka, points3d, is_projective); // From Rs and Ts, extract Ps const int nviews = Rs.size(); Ps.create(nviews, 1, depth); Matx34d P; for (size_t i = 0; i < nviews; ++i) { projectionFromKRt(Ka, Rs[i], Vec3d(Ts[i]), P); Mat(P).copyTo(Ps.getMatRef(i)); } Mat(Ka).copyTo(K.getMat()); } } // Affine reconstruction else { // TODO: implement me } }
int recoverPose( InputArray E, InputArray _points1, InputArray _points2, InputArray _cameraMatrix, OutputArray _R, OutputArray _t, InputOutputArray _mask) { Mat points1, points2, cameraMatrix; _points1.getMat().convertTo(points1, CV_64F); _points2.getMat().convertTo(points2, CV_64F); _cameraMatrix.getMat().convertTo(cameraMatrix, CV_64F); int npoints = points1.checkVector(2); CV_Assert( npoints >= 0 && points2.checkVector(2) == npoints && points1.type() == points2.type()); CV_Assert(cameraMatrix.rows == 3 && cameraMatrix.cols == 3 && cameraMatrix.channels() == 1); if (points1.channels() > 1) { points1 = points1.reshape(1, npoints); points2 = points2.reshape(1, npoints); } double fx = cameraMatrix.at<double>(0,0); double fy = cameraMatrix.at<double>(1,1); double cx = cameraMatrix.at<double>(0,2); double cy = cameraMatrix.at<double>(1,2); points1.col(0) = (points1.col(0) - cx) / fx; points2.col(0) = (points2.col(0) - cx) / fx; points1.col(1) = (points1.col(1) - cy) / fy; points2.col(1) = (points2.col(1) - cy) / fy; points1 = points1.t(); points2 = points2.t(); Mat R1, R2, t; decomposeEssentialMat(E, R1, R2, t); Mat P0 = Mat::eye(3, 4, R1.type()); Mat P1(3, 4, R1.type()), P2(3, 4, R1.type()), P3(3, 4, R1.type()), P4(3, 4, R1.type()); P1(Range::all(), Range(0, 3)) = R1 * 1.0; P1.col(3) = t * 1.0; P2(Range::all(), Range(0, 3)) = R2 * 1.0; P2.col(3) = t * 1.0; P3(Range::all(), Range(0, 3)) = R1 * 1.0; P3.col(3) = -t * 1.0; P4(Range::all(), Range(0, 3)) = R2 * 1.0; P4.col(3) = -t * 1.0; // Do the cheirality check. // Notice here a threshold dist is used to filter // out far away points (i.e. infinite points) since // there depth may vary between postive and negtive. double dist = 50.0; Mat Q; triangulatePoints(P0, P1, points1, points2, Q); Mat mask1 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask1 = (Q.row(2) < dist) & mask1; Q = P1 * Q; mask1 = (Q.row(2) > 0) & mask1; mask1 = (Q.row(2) < dist) & mask1; triangulatePoints(P0, P2, points1, points2, Q); Mat mask2 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask2 = (Q.row(2) < dist) & mask2; Q = P2 * Q; mask2 = (Q.row(2) > 0) & mask2; mask2 = (Q.row(2) < dist) & mask2; triangulatePoints(P0, P3, points1, points2, Q); Mat mask3 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask3 = (Q.row(2) < dist) & mask3; Q = P3 * Q; mask3 = (Q.row(2) > 0) & mask3; mask3 = (Q.row(2) < dist) & mask3; triangulatePoints(P0, P4, points1, points2, Q); Mat mask4 = Q.row(2).mul(Q.row(3)) > 0; Q.row(0) /= Q.row(3); Q.row(1) /= Q.row(3); Q.row(2) /= Q.row(3); Q.row(3) /= Q.row(3); mask4 = (Q.row(2) < dist) & mask4; Q = P4 * Q; mask4 = (Q.row(2) > 0) & mask4; mask4 = (Q.row(2) < dist) & mask4; mask1 = mask1.t(); mask2 = mask2.t(); mask3 = mask3.t(); mask4 = mask4.t(); // If _mask is given, then use it to filter outliers. if (!_mask.empty()) { Mat mask = _mask.getMat(); CV_Assert(mask.size() == mask1.size()); bitwise_and(mask, mask1, mask1); bitwise_and(mask, mask2, mask2); bitwise_and(mask, mask3, mask3); bitwise_and(mask, mask4, mask4); } if (_mask.empty() && _mask.needed()) { _mask.create(mask1.size(), CV_8U); } CV_Assert(_R.needed() && _t.needed()); _R.create(3, 3, R1.type()); _t.create(3, 1, t.type()); int good1 = countNonZero(mask1); int good2 = countNonZero(mask2); int good3 = countNonZero(mask3); int good4 = countNonZero(mask4); if (good1 >= good2 && good1 >= good3 && good1 >= good4) { R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask1.copyTo(_mask); return good1; } else if (good2 >= good1 && good2 >= good3 && good2 >= good4) { R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask2.copyTo(_mask); return good2; } else if (good3 >= good1 && good3 >= good2 && good3 >= good4) { t = -t; R1.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask3.copyTo(_mask); return good3; } else { t = -t; R2.copyTo(_R); t.copyTo(_t); if (_mask.needed()) mask4.copyTo(_mask); return good4; } }