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;
}
}
}
}
