int TrackThread::process(Mat &frame,Mat &output)
{
int re=0;
cvtColor (frame,gray,CV_BGR2GRAY);
frame.copyTo (output);
//特征点太少了，重新检测特征点
//if(points[0].size ()<=10)
{
goodFeaturesToTrack (gray,//图片
features,//输出特征点
max_count,//特征点最大数目
qlevel,//质量指标
minDist);//最小容忍距离
//插入检测到的特征点
// points[0].insert (points[0].end (),features.begin (),features.end ());
// initial.insert (initial.end (),features.begin (),features.end ());
points[0]=features;
initial=features;
}
//第一帧
if(gray_prev.empty ()){
gray.copyTo (gray_prev);
}
//根据前后两帧灰度图估计前一帧特征点在当前帧的位置
//默认窗口是15*15
calcOpticalFlowPyrLK (
gray_prev,//前一帧灰度图
gray,//当前帧灰度图
points[0],//前一帧特征点位置
points[1],//当前帧特征点位置
status,//特征点被成功跟踪的标志
err);//前一帧特征点点小区域和当前特征点小区域间的差，根据差的大小可删除那些运动变化剧烈的点
int k = 0;
//去除那些未移动的特征点
for(int i=0;i<points[1].size();i++){
if(acceptTrackedPoint (i))
{
initial[k]=initial[i];
points[1][k++] = points[1][i];
}
}
points[1].resize (k);
initial.resize (k);
//标记被跟踪的特征点
for(int i=0;i<points[1].size ();i++)
{
//当前特征点到初始位置用直线表示
line(output,initial[i],points[1][i],Scalar(20,150,210));
//当前位置用圈标出
circle(output,points[1][i],3,Scalar(120,250,10));
}
if(points[1].size()!=0)
trend(output);
//向心速度
curx*=0.9;
cury*=0.9;
lens=min(output.rows,output.cols)/8;
edge=min(output.rows,output.cols)/5;
//控制域
rectangle(output,
Point(output.cols/2-lens,output.rows/2-lens),
Point(output.cols/2+lens,output.rows/2+lens),
Scalar(1,100,200),3);
//追踪域
rectangle(output,
Point(edge,edge),
Point(output.cols-edge,output.rows-edge),
Scalar(10,210,1),3);
if(first){
if(dirx==1){
re=4;
line(output,
Point(output.cols/2+lens,output.rows/2-lens),
Point(output.cols/2+lens,output.rows/2+lens),
Scalar(20,250,110),5);
}
else if(dirx==-1){
re=3;
line(output,
Point(output.cols/2-lens,output.rows/2-lens),
Point(output.cols/2-lens,output.rows/2+lens),
Scalar(20,250,110),5);
}
if(diry==1){
re=2;
line(output,
Point(output.cols/2-lens,output.rows/2+lens),
Point(output.cols/2+lens,output.rows/2+lens),
Scalar(20,250,110),5);
}
else if(diry==-1){
re=1;
line(output,
Point(output.cols/2-lens,output.rows/2-lens),
Point(output.cols/2+lens,output.rows/2-lens),
Scalar(20,250,110),5);
}
}
//为下一帧跟踪初始化特征点集和灰度图像
circle(output,Point(output.cols/2+curx,output.rows/2+cury),10,Scalar(2,1,250),3);
std::swap(points[1],points[0]);
cv::swap(gray_prev,gray);
return re;
}
void MotionTrackerLK::process(cv::Mat &input, cv::Mat &output, double minDisplace){
//Shift images to be processed
if ( frameCurrent.data ){ //have only one frame, need another frame to calculate the motion:
framePrev.release();
framePrev = frameCurrent;
} else {//init code:
// pointSize = input.cols/128;
pointSize = 3;
repulsiveRange = 3 * input.cols / 4;
leftRepulsivePoint.x = 0;
rightRepulsivePoint.x = input.cols;
leftRepulsivePoint.y = input.rows;
rightRepulsivePoint.y = input.rows;
}
frameCurrent = input.clone();
// output = input.clone();
if (pointsPrev.size() > 0){
// Find position of feature in new image
cv::calcOpticalFlowPyrLK(
framePrev, frameCurrent, // 2 consecutive images
pointsPrev, // input: interesting features points
pointsCurrent, // output: the respective positions (in second frame) of the input points
statusLk, // tracking success
err // tracking error
);
cv::calcOpticalFlowPyrLK(
frameCurrent, framePrev, // 2 consecutive images (first the current then the previous, to verify that the points we got in the previous OF were correct)
pointsCurrent, // input: interesting features points
pointsPrevReverse, // output: the respective positions (in second frame) of the input points
statusLkReverse, // tracking success
err // tracking error
);
//resize the
diffReverse.resize(pointsPrev.size());
displacement.resize(pointsPrev.size());
// magnitude.resize(pointsPrev.size());
//Use findHomography to find the most probably correct displacements (ransac), we don't really care for the homography:
cv::findHomography(pointsPrev, pointsCurrent, CV_RANSAC, 2, statusHomography);
displacementScaler = 0;
hulls.clear();
hull.clear();
// calculate the displacement. (TODO: do this more efficiently! it is a matrix operation)
for(int i= 0; i < pointsPrev.size(); i++ ) {
//Calculate the displacement between the original feature position and the estimated origial feature position by the second OF pass:
diffReverse.at(i) = cv::norm(pointsPrevReverse.at(i)-pointsPrev.at(i));
if (acceptTrackedPoint(i)){
//Calculate the displacement of the feature between frames
// displacement[i] = cv::norm(pointsCurrent[i]-pointsPrev[i]); // Total displacement
displacement[i] = pointsCurrent.at(i).y - pointsPrev.at(i).y; // Y-axis displacement
// std::cout << "norm: " << displacement[i] << ". x,y 2: " << pointsCurrent[i].x << ", " << pointsCurrent[i].y << ". x,y 1: " << pointsPrev[i].x << ", " << pointsPrev[i].y << std::endl;
if (displacement[i] < minDisplace)
displacement[i] = -1;
// if (displacement[i] > displacementScaler)
// displacementScaler = displacement[i];
}
}
cv::Point2f center(0.0f, 0.0f); // Implicitly the position of the obstacle center would stay to be 0, 0 if there are no fast moving features
objectAvgIntensity = 0; //
double avgDisplacement = 0;//if no obstacle present, will remain being zero.
for(int i= 0; i < pointsPrev.size(); i++ ) {
if (acceptTrackedPoint(i)){
if (displacement.at(i) > (1.3 * minDisplace)){
//Everything going three times as fast as the target is moving really fast:
colorIntensity = 255 * displacement.at(i) / (4 * minDisplace);
// if (colorIntensity>120){
hull.push_back(pointsCurrent.at(i));
objectAvgIntensity = objectAvgIntensity + ((colorIntensity - objectAvgIntensity)/hull.size());
center.x = center.x + ((pointsCurrent.at(i).x - center.x)/hull.size());
center.y = center.y + ((pointsCurrent.at(i).y - center.y)/hull.size());
avgDisplacement = avgDisplacement + (displacement.at(i) - avgDisplacement)/hull.size();
// }
std::cout << "colorIntensity: " << colorIntensity << std::endl;
color = cv::Scalar(colorIntensity, colorIntensity, colorIntensity, 255);
// if (colorIntensity > 255)
// color = cv::Scalar(0, 255, 0, 255);
} else {
color = cv::Scalar(0, 255, 255, 255);
// continue;//Don't draw features moving slower than 3 * minimum displacement
}
} else {
// continue;//Don't draw invalid features
color = cv::Scalar(0, 0, 255, 255);
}
cv::line(output,
pointsPrev.at(i),// initial position
pointsCurrent.at(i),// new position
color);
cv::circle(output, pointsCurrent.at(i), pointSize, color, -1 /*thickness, so that it is filled*/);
}
//Make sure that the intensity (how fast the object moves), is consistent:
objectAvgIntensity = std::min(objectAvgIntensity, 255.);
double effectProximity = objectAvgIntensity / 255.;//Avg color is proportional to the speed of the object
double direction = ((center.x < input.cols / 2) ? 1 : -1);//set direction (if obstacle to the left: rotate to the right)
double effectXPosition = direction * sin(CV_PI * center.x / input.cols);//set magnitude. Range: 0->1
//set the desired rotation depending on the visible obstacles:
if (hull.size() > 2 && avgDisplacement > 1.3 * minDisplace){
std::cout << "effectProximity: " << effectProximity << std::endl;
std::cout << "effectXPosition: " << effectXPosition << std::endl;
hulls.push_back(hull);
convexHull(cv::Mat(hulls[0]), hulls[0]);
rotation = effectProximity * effectXPosition;
std::cout << "rotation: " << rotation << std::endl;
cv::drawContours(output, hulls, 0, cv::Scalar(objectAvgIntensity, objectAvgIntensity, objectAvgIntensity, 255), 1);
//draw center of convex hull
cv::circle(output, center, 2*pointSize, cv::Scalar(255, 0, 0, 255), -1 /*thickness, so that it is filled*/);
mTimestampObstacleLastSeen = (double)cv::getTickCount();
} else {
double timeLapse = (double)cv::getTickCount() - mTimestampObstacleLastSeen;
if (timeLapse/((double)cvGetTickFrequency()*1000.) > 1000){//if an obstacle has not been seen in the last 1000ms
rotation = 0;
} //else (an obstacle was seen at least 1000ms ago) keep last rotation value
}
}
// Keep track of the good features in this frame (used as baseline in next iteration)
cv::goodFeaturesToTrack(frameCurrent, // the image
pointsPrev, // the output detected features
maxFeatures, // the maximum number of features
qLevel, // quality level
minDist // min distance between two features
);
}
