void SyntheticTrack::trackCorners(const cv::Mat& rendered_image, const cv::Mat& next_img, std::vector<cv::Point2f>& prev_pts, std::vector<cv::Point2f>& next_pts) { vector<KeyPoint> kps; FAST(rendered_image, kps, 7, true); if (kps.empty()) return; vector<Point2f> tmp_next_pts, tmp_prev_pts, backflow_pts; for (auto kp : kps) tmp_prev_pts.push_back(kp.pt); vector<uchar> next_status, prev_status; vector<float> next_errors, prev_errors; calcOpticalFlowPyrLK(rendered_image, next_img, tmp_prev_pts, tmp_next_pts, next_status, next_errors); calcOpticalFlowPyrLK(next_img, rendered_image, tmp_next_pts, backflow_pts, prev_status, prev_errors); for (int i = 0; i < tmp_prev_pts.size(); ++i) { float error = getDistance(backflow_pts[i], tmp_prev_pts[i]); float speed = getDistance(tmp_prev_pts[i], tmp_next_pts[i]); if (prev_status[i] == 1 && error < 5 && speed < 50) { // const int& id = ids[i]; prev_pts.push_back(tmp_prev_pts[i]); next_pts.push_back(tmp_next_pts[i]); } } }
void fft_float(float *orig, float *fftd, int npoint) { int i; for(i = 0; i< npoint; i++) fftd[i] = orig[i]; if(FAST(fftd, npoint) == 0 ){ fprintf(stderr, "Error calculating fft.\n"); exit(1); } }
int fft_pow(float *orig, float *power, long winlength, long log2length) { int i, j, k; static real *temp = NULL; static int npoints, npoints2; char *funcname; funcname = "fft_pow"; if(temp == (real*) NULL) { npoints = (int) (pow(2.0,(real) log2length) + 0.5); npoints2 = npoints / 2; temp = (real*) malloc(npoints * sizeof(real)); if(temp == (real*) NULL) { fprintf(stderr, "Error mallocing memory in fft_pow()\n"); exit(1); } } for(i=0;i<winlength;i++) temp[i] = (real) orig[i]; for(i = winlength; i < npoints; i++) temp[i] = 0.0; if(FAST(temp, npoints) == 0 ){ fprintf(stderr,"Error calculating fft.\n"); exit(1); } /* convert the complex data to power */ power[0] = temp[0]*temp[0]; power[npoints2] = temp[1]*temp[1]; /* Only the first half of the power[] array is filled with data. The second half would just be a mirror image of the first half. */ for(i=1;i<npoints2;i++){ j=2*i; k=2*i+1; power[i] = temp[j]*temp[j]+temp[k]*temp[k]; } return(0); }
int chooseFASTThreshold(const Mat &img, const int lowerBound, const int upperBound) { static vector<cv::KeyPoint> kpts; int left = 0; int right = 255; int currentThreshold = 128; int currentScore = 256; while (currentScore < lowerBound || currentScore > upperBound) { FAST(img, kpts, currentThreshold, true); currentScore = (int)kpts.size(); if (lowerBound > currentScore) { // we look for a lower threshold to increase the number of corners: right = currentThreshold; currentThreshold = (currentThreshold + left) >> 1; if (right == currentThreshold) break; } else {
void FastFeatureDetector::detectImpl(const Mat& image, const Mat& mask, vector<KeyPoint>& keypoints) const { FAST(image, keypoints, threshold, nonmaxSuppression); removeInvalidPoints(mask, keypoints); }
static void initMorePoints(const cv::Mat &img_l, const cv::Mat &img_r, std::vector<int> &updateVect, std::vector<cv::Point2f> &z_all_l, std::vector<cv::Point2f> &z_all_r, int stereo) { if (!img_l.data) throw "Left image is invalid"; if (stereo && !img_r.data) throw "Right image is invalid"; unsigned int targetNumPoints = 0; // count the features that need to be initialized for (int i = 0; i < updateVect.size(); i++) { if (updateVect[i] == 2) // 2 means new feature requested targetNumPoints++; } if (!targetNumPoints) return; std::vector<cv::KeyPoint> keypointsL, keypointsR, goodKeypointsL, unusedKeypoints; cv::Mat descriptorsL, descriptorsR; int numBinsX = 4; int numBinsY = 4; int binWidth = img_l.cols / numBinsX; int binHeight = img_l.rows / numBinsY; int targetFeaturesPerBin = (updateVect.size() - 1) / (numBinsX * numBinsY) + 1; // total number of features that should be in each bin std::vector<std::vector<int> > featuresPerBin(numBinsX, std::vector<int>(numBinsY, 0)); // count the number of active features in each bin for (int i = 0; i < prev_corners.size(); i++) { if (updateVect[i] == 1) { int binX = prev_corners[i].x / binWidth; int binY = prev_corners[i].y / binHeight; if (binX >= numBinsX) { printf("Warning: writing to binX out of bounds: %d >= %d\n", binX, numBinsX); continue; } if (binY >= numBinsY) { printf("Warning: writing to binY out of bounds: %d >= %d\n", binY, numBinsY); continue; } featuresPerBin[binX][binY]++; } } unsigned int dist = binWidth / targetFeaturesPerBin; // go through each cell and detect features for (int x = 0; x < numBinsX; x++) { for (int y = 0; y < numBinsY; y++) { int neededFeatures = std::max(0, targetFeaturesPerBin - featuresPerBin[x][y]); if (neededFeatures) { int col_from = x * binWidth; int col_to = std::min((x + 1) * binWidth, img_l.cols); int row_from = y * binHeight; int row_to = std::min((y + 1) * binHeight, img_l.rows); std::vector<cv::KeyPoint> keypoints, goodKeypointsBin; FAST(img_l.rowRange(row_from, row_to).colRange(col_from, col_to), keypoints, 10); sort(keypoints.begin(), keypoints.end(), compareKeypoints); // add bin offsets to the points for (int i = 0; i < keypoints.size(); i++) { keypoints[i].pt.x += col_from; keypoints[i].pt.y += row_from; } // check if the new features are far enough from existing points int newPtIdx = 0; for (; newPtIdx < keypoints.size(); newPtIdx++) { int new_pt_x = keypoints[newPtIdx].pt.x; int new_pt_y = keypoints[newPtIdx].pt.y; bool far_enough = true; for (int j = 0; j < prev_corners.size(); j++) { if (prev_status[j] == 0) continue; int existing_pt_x = prev_corners[j].x; int existing_pt_y = prev_corners[j].y; if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) { far_enough = false; unusedKeypoints.push_back(keypoints[newPtIdx]); break; } } if (far_enough) { // check if the new feature is too close to a new one for (int j = 0; j < goodKeypointsBin.size(); j++) { int existing_pt_x = goodKeypointsBin[j].pt.x; int existing_pt_y = goodKeypointsBin[j].pt.y; if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) { far_enough = false; unusedKeypoints.push_back(keypoints[newPtIdx]); break; } } if (far_enough) { goodKeypointsBin.push_back(keypoints[newPtIdx]); if (goodKeypointsBin.size() == neededFeatures) break; } } } // insert the good points into the vector containing the new points of the whole image goodKeypointsL.insert(goodKeypointsL.end(), goodKeypointsBin.begin(), goodKeypointsBin.end()); // save the unused keypoints for later if (newPtIdx < keypoints.size() - 1) { unusedKeypoints.insert(unusedKeypoints.end(), keypoints.begin() + newPtIdx, keypoints.end()); } } } } // if not many features were requested, we may have found too many features. delete from all bins for equal distancing if (goodKeypointsL.size() > targetNumPoints) { int numFeaturesToRemove = goodKeypointsL.size() - targetNumPoints; int stepSize = targetNumPoints / numFeaturesToRemove + 2; // make sure the step size is big enough so we dont remove too many features std::vector<cv::KeyPoint> goodKeypointsL_shortened; for (int i = 0; i < goodKeypointsL.size(); i++) { if (i % stepSize) { goodKeypointsL_shortened.push_back(goodKeypointsL[i]); } } goodKeypointsL = goodKeypointsL_shortened; } if (goodKeypointsL.size() < targetNumPoints) { // try to insert new points that were not used in the bins sort(unusedKeypoints.begin(), unusedKeypoints.end(), compareKeypoints); dist /= 2; // reduce the distance criterion for (int newPtIdx = 0; newPtIdx < unusedKeypoints.size(); newPtIdx++) { int new_pt_x = unusedKeypoints[newPtIdx].pt.x; int new_pt_y = unusedKeypoints[newPtIdx].pt.y; bool far_enough = true; for (int j = 0; j < prev_corners.size(); j++) { if (prev_status[j] == 0) continue; int existing_pt_x = prev_corners[j].x; int existing_pt_y = prev_corners[j].y; if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) { far_enough = false; break; } } if (far_enough) { // check if the new feature is too close to a new one for (int j = 0; j < goodKeypointsL.size(); j++) { int existing_pt_x = goodKeypointsL[j].pt.x; int existing_pt_y = goodKeypointsL[j].pt.y; if (abs(existing_pt_x - new_pt_x) < dist && abs(existing_pt_y - new_pt_y) < dist) { far_enough = false; break; } } if (far_enough) { goodKeypointsL.push_back(unusedKeypoints[newPtIdx]); if (goodKeypointsL.size() == targetNumPoints) break; } } } } if (goodKeypointsL.empty()) { for (int i = 0; i < updateVect.size(); i++) { if (updateVect[i] == 2) updateVect[i] = 0; } return; } std::vector<cv::Point2f> leftPoints, rightPoints; if (stereo) { if (!stereoMatchOpticalFlow(img_l, img_r, goodKeypointsL, leftPoints, rightPoints)) { for (int i = 0; i < updateVect.size(); i++) { if (updateVect[i] == 2) updateVect[i] = 0; } return; } if (leftPoints.size() != rightPoints.size()) // debug debug_msg_count ++; if (debug_msg_count % 50 == 0) { printf("Left and right points have different sizes: left %d, right %d\n", (int) leftPoints.size(), (int) rightPoints.size()); } } else { leftPoints.resize(goodKeypointsL.size()); for (int i = 0; i < goodKeypointsL.size(); i++) { leftPoints[i] = goodKeypointsL[i].pt; } } if (leftPoints.size() < targetNumPoints) debug_msg_count ++; if (debug_msg_count % 50 == 0) { printf("Number of good matches: %d, desired: %d\n", (int) leftPoints.size(), targetNumPoints); } if (prev_corners.size() < updateVect.size()) prev_corners.resize(updateVect.size()); int matches_idx = 0; for (int i = 0; i < updateVect.size(); i++) { if (updateVect[i] == 2) { if (matches_idx < leftPoints.size()) { prev_corners[i] = leftPoints[matches_idx]; prev_status[i] = 1; z_all_l[i] = leftPoints[matches_idx]; if (stereo) { z_all_r[i] = rightPoints[matches_idx]; } matches_idx++; } else { updateVect[i] = 0; } } } }