float IC_Angle(const Mat& image, const int half_k, Point2f pt,
const vector<int> & u_max)
{
int m_01 = 0, m_10 = 0;
const uchar* center = &image.at<uchar> (cvRound(pt.y), cvRound(pt.x));
// Treat the center line differently, v=0
for (int u = -half_k; u <= half_k; ++u)
m_10 += u * center[u];
// Go line by line in the circular patch
int step = (int)image.step1();
for (int v = 1; v <= half_k; ++v)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v];
for (int u = -d; u <= d; ++u)
{
int val_plus = center[u + v*step], val_minus = center[u - v*step];
v_sum += (val_plus - val_minus);
m_10 += u * (val_plus + val_minus);
}
m_01 += v * v_sum;
}
return fastAtan2((float)m_01, (float)m_10);
}
float Index::getKeyPointOrientation(const Mat& image, const KeyPoint &pnt)
{
int half_k = ROTATION_PATCH_SIZE/2;
Point2f pt = pnt.pt;
int m_01 = 0, m_10 = 0;
const uchar* center = image.ptr<uchar> (cvRound(pt.y), cvRound(pt.x));
// Treat the center line differently, v=0
for (int u = -half_k; u <= half_k; ++u)
m_10 += u * center[u];
// Go line by line in the circular patch
int step = (int)image.step1();
for (int v = 1; v <= half_k; ++v)
{
// Proceed over the two lines
int v_sum = 0;
int d = u_max[v];
for (int u = -d; u <= d; ++u)
{
int val_plus = center[u + v*step], val_minus = center[u - v*step];
v_sum += (val_plus - val_minus);
m_10 += u * (val_plus + val_minus);
}
m_01 += v * v_sum;
}
return fastAtan2((float)m_01, (float)m_10);
}
void CControl::FilterLineAngle(vector<Point2f> n_inlinevct,
vector<Point2f>& n_outlinevct,float n_angle,
float n_threshold)
{
for(int i=0;i<n_inlinevct.size();i++)
{
//get points
Point2f n_pt1=n_inlinevct.at(i++);
Point2f n_pt2=n_inlinevct.at(i);
//cal angle
float n_dy=n_pt1.y-n_pt2.y;
float n_dx=n_pt1.x-n_pt2.x;
float n_angleline=fastAtan2(n_dy,n_dx);
if(IsAligned(n_angle,n_angleline,n_threshold))
{
//add to out linevct
n_outlinevct.push_back(n_pt1);
n_outlinevct.push_back(n_pt2);
}
}
}
void cv::calcMotionGradient( InputArray _mhi, OutputArray _mask,
OutputArray _orientation,
double delta1, double delta2,
int aperture_size )
{
static int runcase = 0; runcase++;
Mat mhi = _mhi.getMat();
Size size = mhi.size();
_mask.create(size, CV_8U);
_orientation.create(size, CV_32F);
Mat mask = _mask.getMat();
Mat orient = _orientation.getMat();
if( aperture_size < 3 || aperture_size > 7 || (aperture_size & 1) == 0 )
CV_Error( Error::StsOutOfRange, "aperture_size must be 3, 5 or 7" );
if( delta1 <= 0 || delta2 <= 0 )
CV_Error( Error::StsOutOfRange, "both delta's must be positive" );
if( mhi.type() != CV_32FC1 )
CV_Error( Error::StsUnsupportedFormat,
"MHI must be single-channel floating-point images" );
if( orient.data == mhi.data )
{
_orientation.release();
_orientation.create(size, CV_32F);
orient = _orientation.getMat();
}
if( delta1 > delta2 )
std::swap(delta1, delta2);
float gradient_epsilon = 1e-4f * aperture_size * aperture_size;
float min_delta = (float)delta1;
float max_delta = (float)delta2;
Mat dX_min, dY_max;
// calc Dx and Dy
Sobel( mhi, dX_min, CV_32F, 1, 0, aperture_size, 1, 0, BORDER_REPLICATE );
Sobel( mhi, dY_max, CV_32F, 0, 1, aperture_size, 1, 0, BORDER_REPLICATE );
int x, y;
if( mhi.isContinuous() && orient.isContinuous() && mask.isContinuous() )
{
size.width *= size.height;
size.height = 1;
}
// calc gradient
for( y = 0; y < size.height; y++ )
{
const float* dX_min_row = dX_min.ptr<float>(y);
const float* dY_max_row = dY_max.ptr<float>(y);
float* orient_row = orient.ptr<float>(y);
uchar* mask_row = mask.ptr<uchar>(y);
fastAtan2(dY_max_row, dX_min_row, orient_row, size.width, true);
// make orientation zero where the gradient is very small
for( x = 0; x < size.width; x++ )
{
float dY = dY_max_row[x];
float dX = dX_min_row[x];
if( std::abs(dX) < gradient_epsilon && std::abs(dY) < gradient_epsilon )
{
mask_row[x] = (uchar)0;
orient_row[x] = 0.f;
}
else
mask_row[x] = (uchar)1;
}
}
erode( mhi, dX_min, noArray(), Point(-1,-1), (aperture_size-1)/2, BORDER_REPLICATE );
dilate( mhi, dY_max, noArray(), Point(-1,-1), (aperture_size-1)/2, BORDER_REPLICATE );
// mask off pixels which have little motion difference in their neighborhood
for( y = 0; y < size.height; y++ )
{
const float* dX_min_row = dX_min.ptr<float>(y);
const float* dY_max_row = dY_max.ptr<float>(y);
float* orient_row = orient.ptr<float>(y);
uchar* mask_row = mask.ptr<uchar>(y);
for( x = 0; x < size.width; x++ )
{
float d0 = dY_max_row[x] - dX_min_row[x];
if( mask_row[x] == 0 || d0 < min_delta || max_delta < d0 )
{
mask_row[x] = (uchar)0;
orient_row[x] = 0.f;
}
}
}
}
// Compute single tracking score
TrackingScore* TrackingEvaluator::computeScore(const TrackedObject& tracked_object, const Mat& frame, int frame_number, TrackingScoreHistory *history)
{
// Create score
TrackingScore* score = new TrackingScore(&nbc);
// Add score to history
history->addScore(score, frame_number);
// Get blob
const cvb::CvBlob& orig_blob = tracked_object.currentRegion();
// Random move blobs - to test certainties
cvb::CvBlob blob;
cvCloneBlob(orig_blob, blob);
// Save shape ratio
score->setShapeRatio(((float)blob.width())/((float)blob.height()));
// Save area
score->setArea(blob.width()*blob.height());
// Compute object's binary mask, for the histogram
Mat mask = drawBlob(blob, true, frame.cols, frame.rows);
// Get grayscale frame
Mat frame_gs;
cvtColor(frame, frame_gs, CV_BGR2GRAY);
// Split the color channels
Mat* channels = new Mat[3];
split(frame, channels);
Mat frame_b = channels[0];
Mat frame_g = channels[1];
Mat frame_r = channels[2];
delete [] channels;
// Compute histograms
Histogram hist_gs(frame_gs, mask);
Histogram hist_r(frame_r, mask);
Histogram hist_g(frame_g, mask);
Histogram hist_b(frame_b, mask);
// Save histogram
score->setHistograms(hist_gs, hist_r, hist_g, hist_b);
// Select part of frame on which to compute the texture features
Mat copy_gs = frame_gs.clone();
// Blacken non-mask pixels
for(unsigned int x=blob.x; x<=blob.maxx; x++)
{
for(unsigned int y=blob.y; y<=blob.maxy; y++)
{
if(mask.at<uchar>(y,x) == 0)
{
copy_gs.at<uchar>(y,x) = 0;
}
}
}
// Crop image
Rect region(blob.x, blob.y, blob.width(), blob.height());
Mat texture_input = copy_gs(region);
// Compute texture features for this object
vector<float> texture_features = GaborFilter::applyFilterSet(texture_input, gabor_scales, parameters.get<int>("num_orientations"), false, 1, 0.5, 101);
// Save texture features
score->setTextureFeatures(texture_features);
// Set temporal score to the number of appearances
score->temporal_score = history->numScores();
// Get the frame number of the previous detection of this object, if available
int prev_frame = -1;
TrackingScore* prev_score = NULL;
cvb::CvBlob prev_blob;
if(history->numScores() > 1)
{
// Read frame number
prev_frame = *(tracked_object.frameList().end() - 2);
// Get previous score
prev_score = (TrackingScore*) history->getScore(prev_frame);
// Get previous blob
prev_blob = *(tracked_object.regionList().end() - 2);
// Compute shape ratio score
score->shape_ratio_score = computeShapeRatioScore(prev_score->getShapeRatio(), score->getShapeRatio());
// Compute area score
score->area_score = computeAreaScore(prev_score->getArea(), score->getArea());
// Compute histogram difference score
score->histogram_diff_score = computeHistogramDiffScore(score, prev_score);
// Compute texture difference score
score->texture_diff_score = computeTextureDiffScore(score->getTextureFeatures(), prev_score->getTextureFeatures());
// Compute velocity for this score
Point2f prev_position((prev_blob.maxx+prev_blob.x)/2, (prev_blob.maxy+prev_blob.y)/2);
Point2f curr_position((blob.maxx+blob.x)/2, (blob.maxy+blob.y)/2);
float velocity = sqrt((prev_position.x-curr_position.x)*(prev_position.x-curr_position.x) + (prev_position.y-curr_position.y)*(prev_position.y-curr_position.y));
score->setVelocity(velocity);
// Add velocity to the accumulated velocity for this object (will be used to compute an average according to the number of detections)
history->accumulateVelocity(velocity);
// Check if the previous score has its velocity set
if(prev_score->getVelocity() > 0)
{
//cout << "computing ms: " << score->getVelocity() << ", " << history->getAverageVelocity() << endl;
// Compute motion smoothness score, based on current velocity and average velocity
score->motion_smoothness = computeMotionSmoothnessScore(score->getVelocity(), history->getAverageVelocity());
}
else
{
// Set score to 0
score->motion_smoothness = 0.0f;
// Mark score as not full
score->full = false;
}
// Compute direction for this score
float direction = fastAtan2(-curr_position.y + prev_position.y, curr_position.x - prev_position.x);
score->setDirection(direction);
// Check if the previous score has its direction set
//cout << "direction: " << direction << ", prev_score direction: " << prev_score->getDirection() << endl;
if(prev_score->getDirection() > 0)
{
//cout << "computing d : " << prev_score->getDirection() << ", " << score->getDirection() << endl;
// Compute direction score, based on current direction and previous direction
score->direction_score = computeDirectionScore(score->getDirection(), prev_score->getDirection());
}
else
{
// Set score to 0
score->direction_score = 0.0f;
// Mark score as not full
score->full = false;
}
//if(score->full)// && (frame_number % 10) == 0)
//{
// out_file << score->shape_ratio_score << " " << score->area_score << " " << score->histogram_diff_score << " " << score->motion_smoothness << " " << score->direction_score << " " << score->texture_diff_score << " 1" << endl;
// cout << score->shape_ratio_score << " " << score->area_score << " " << score->histogram_diff_score << " " << score->motion_smoothness << " " << score->direction_score << " " << score->texture_diff_score << " 1" << endl;
//}
//out_file << score->shape_ratio_score << " " << score->histogram_diff_score << " 1";
//if(pd)
// out_file << score->value() << endl;
}
else
{
// Set score as certain
score->certain = true;
}
Log::debug() << "tracking score: " << score->value() << "(" << score->shape_ratio_score << ", " << score->area_score << ", " << score->histogram_diff_score << ", " << (score->full ? score->motion_smoothness : -1) << ", " << (score->full ? score->direction_score : -1 ) << ", " << score->texture_diff_score << ")" << endl;
// Return score
return score;
}
static void calcSIFTDescriptor( const Mat& img, Point2f ptf, float ori, float scl,
int d, int n, float* dst )
{
Point pt(cvRound(ptf.x), cvRound(ptf.y));
float cos_t = cosf(ori*(float)(CV_PI/180));
float sin_t = sinf(ori*(float)(CV_PI/180));
float bins_per_rad = n / 360.f;
float exp_scale = -1.f/(d * d * 0.5f);
float hist_width = SIFT_DESCR_SCL_FCTR * scl;
int radius = cvRound(hist_width * 1.4142135623730951f * (d + 1) * 0.5f);
// Clip the radius to the diagonal of the image to avoid autobuffer too large exception
radius = std::min(radius, (int) sqrt((double) img.cols*img.cols + img.rows*img.rows));
cos_t /= hist_width;
sin_t /= hist_width;
int i, j, k, len = (radius*2+1)*(radius*2+1), histlen = (d+2)*(d+2)*(n+2);
int rows = img.rows, cols = img.cols;
AutoBuffer<float> buf(len*6 + histlen);
float *X = buf, *Y = X + len, *Mag = Y, *Ori = Mag + len, *W = Ori + len;
float *RBin = W + len, *CBin = RBin + len, *hist = CBin + len;
for( i = 0; i < d+2; i++ )
{
for( j = 0; j < d+2; j++ )
for( k = 0; k < n+2; k++ )
hist[(i*(d+2) + j)*(n+2) + k] = 0.;
}
for( i = -radius, k = 0; i <= radius; i++ )
for( j = -radius; j <= radius; j++ )
{
// Calculate sample's histogram array coords rotated relative to ori.
// Subtract 0.5 so samples that fall e.g. in the center of row 1 (i.e.
// r_rot = 1.5) have full weight placed in row 1 after interpolation.
float c_rot = j * cos_t - i * sin_t;
float r_rot = j * sin_t + i * cos_t;
float rbin = r_rot + d/2 - 0.5f;
float cbin = c_rot + d/2 - 0.5f;
int r = pt.y + i, c = pt.x + j;
if( rbin > -1 && rbin < d && cbin > -1 && cbin < d &&
r > 0 && r < rows - 1 && c > 0 && c < cols - 1 )
{
float dx = (float)(img.at<sift_wt>(r, c+1) - img.at<sift_wt>(r, c-1));
float dy = (float)(img.at<sift_wt>(r-1, c) - img.at<sift_wt>(r+1, c));
X[k] = dx; Y[k] = dy; RBin[k] = rbin; CBin[k] = cbin;
W[k] = (c_rot * c_rot + r_rot * r_rot)*exp_scale;
k++;
}
}
len = k;
fastAtan2(Y, X, Ori, len, true);
magnitude(X, Y, Mag, len);
exp(W, W, len);
for( k = 0; k < len; k++ )
{
float rbin = RBin[k], cbin = CBin[k];
float obin = (Ori[k] - ori)*bins_per_rad;
float mag = Mag[k]*W[k];
int r0 = cvFloor( rbin );
int c0 = cvFloor( cbin );
int o0 = cvFloor( obin );
rbin -= r0;
cbin -= c0;
obin -= o0;
if( o0 < 0 )
o0 += n;
if( o0 >= n )
o0 -= n;
// histogram update using tri-linear interpolation
float v_r1 = mag*rbin, v_r0 = mag - v_r1;
float v_rc11 = v_r1*cbin, v_rc10 = v_r1 - v_rc11;
float v_rc01 = v_r0*cbin, v_rc00 = v_r0 - v_rc01;
float v_rco111 = v_rc11*obin, v_rco110 = v_rc11 - v_rco111;
float v_rco101 = v_rc10*obin, v_rco100 = v_rc10 - v_rco101;
float v_rco011 = v_rc01*obin, v_rco010 = v_rc01 - v_rco011;
float v_rco001 = v_rc00*obin, v_rco000 = v_rc00 - v_rco001;
int idx = ((r0+1)*(d+2) + c0+1)*(n+2) + o0;
hist[idx] += v_rco000;
hist[idx+1] += v_rco001;
hist[idx+(n+2)] += v_rco010;
hist[idx+(n+3)] += v_rco011;
hist[idx+(d+2)*(n+2)] += v_rco100;
hist[idx+(d+2)*(n+2)+1] += v_rco101;
hist[idx+(d+3)*(n+2)] += v_rco110;
hist[idx+(d+3)*(n+2)+1] += v_rco111;
}
// finalize histogram, since the orientation histograms are circular
for( i = 0; i < d; i++ )
for( j = 0; j < d; j++ )
{
int idx = ((i+1)*(d+2) + (j+1))*(n+2);
hist[idx] += hist[idx+n];
hist[idx+1] += hist[idx+n+1];
for( k = 0; k < n; k++ )
dst[(i*d + j)*n + k] = hist[idx+k];
}
// copy histogram to the descriptor,
// apply hysteresis thresholding
// and scale the result, so that it can be easily converted
// to byte array
float nrm2 = 0;
len = d*d*n;
for( k = 0; k < len; k++ )
nrm2 += dst[k]*dst[k];
float thr = std::sqrt(nrm2)*SIFT_DESCR_MAG_THR;
for( i = 0, nrm2 = 0; i < k; i++ )
{
float val = std::min(dst[i], thr);
dst[i] = val;
nrm2 += val*val;
}
nrm2 = SIFT_INT_DESCR_FCTR/std::max(std::sqrt(nrm2), FLT_EPSILON);
#if 1
for( k = 0; k < len; k++ )
{
dst[k] = saturate_cast<uchar>(dst[k]*nrm2);
}
#else
float nrm1 = 0;
for( k = 0; k < len; k++ )
{
dst[k] *= nrm2;
nrm1 += dst[k];
}
nrm1 = 1.f/std::max(nrm1, FLT_EPSILON);
for( k = 0; k < len; k++ )
{
dst[k] = std::sqrt(dst[k] * nrm1);//saturate_cast<uchar>(std::sqrt(dst[k] * nrm1)*SIFT_INT_DESCR_FCTR);
}
#endif
}
// Computes a gradient orientation histogram at a specified pixel
static float calcOrientationHist( const Mat& img, Point pt, int radius,
float sigma, float* hist, int n )
{
int i, j, k, len = (radius*2+1)*(radius*2+1);
float expf_scale = -1.f/(2.f * sigma * sigma);
AutoBuffer<float> buf(len*4 + n+4);
float *X = buf, *Y = X + len, *Mag = X, *Ori = Y + len, *W = Ori + len;
float* temphist = W + len + 2;
for( i = 0; i < n; i++ )
temphist[i] = 0.f;
for( i = -radius, k = 0; i <= radius; i++ )
{
int y = pt.y + i;
if( y <= 0 || y >= img.rows - 1 )
continue;
for( j = -radius; j <= radius; j++ )
{
int x = pt.x + j;
if( x <= 0 || x >= img.cols - 1 )
continue;
float dx = (float)(img.at<sift_wt>(y, x+1) - img.at<sift_wt>(y, x-1));
float dy = (float)(img.at<sift_wt>(y-1, x) - img.at<sift_wt>(y+1, x));
X[k] = dx; Y[k] = dy; W[k] = (i*i + j*j)*expf_scale;
k++;
}
}
len = k;
// compute gradient values, orientations and the weights over the pixel neighborhood
exp(W, W, len);
fastAtan2(Y, X, Ori, len, true);
magnitude(X, Y, Mag, len);
for( k = 0; k < len; k++ )
{
int bin = cvRound((n/360.f)*Ori[k]);
if( bin >= n )
bin -= n;
if( bin < 0 )
bin += n;
temphist[bin] += W[k]*Mag[k];
}
// smooth the histogram
temphist[-1] = temphist[n-1];
temphist[-2] = temphist[n-2];
temphist[n] = temphist[0];
temphist[n+1] = temphist[1];
for( i = 0; i < n; i++ )
{
hist[i] = (temphist[i-2] + temphist[i+2])*(1.f/16.f) +
(temphist[i-1] + temphist[i+1])*(4.f/16.f) +
temphist[i]*(6.f/16.f);
}
float maxval = hist[0];
for( i = 1; i < n; i++ )
maxval = std::max(maxval, hist[i]);
return maxval;
}
