int guidance_callback(int data_type, int data_len, char *content)
{
g_lock.enter();
if(e_image == data_type && NULL!=content)
{
printf("Success..\n");
image_data image;
memcpy((char*)&image,content,sizeof(image));
printf("frame index:%d, time stamp:%d\n",image.frame_index,image.time_stamp);
for(int d=0;d<CAMERA_PAIR_NUM;d++)
{
if(image.m_greyscale_image_left[d])
{
memcpy(g_greyscale_image_left.data, image.m_greyscale_image_left[d],IMAGE_SIZE);
vector<Point2f> nextleftpts,kpts;
if(prevleftpts.size()<MIN_FEATURES)
{
//control input
}
else if(frame_num>0)
{
//vector<CvPoint2D32f> nextleftpts;
vector<uchar> status;
vector<float> error;
vector<Mat> nextleftpyr,prevleftpyr;
buildOpticalFlowPyramid(prevleftimg, prevleftpyr, Size(7,7), 4);
buildOpticalFlowPyramid(g_greyscale_image_left, nextleftpyr, Size(7,7), 4);
CvTermCriteria optical_flow_termination_criteria
= cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.1 );
calcOpticalFlowPyrLK(prevleftpyr, nextleftpyr, prevleftpts, nextleftpts, status, error,Size(7,7),4,optical_flow_termination_criteria,OPTFLOW_LK_GET_MIN_EIGENVALS);
//drawing featues
Mat left_mask(g_greyscale_image_left.size(),CV_8UC3);
cvtColor(g_greyscale_image_left, left_mask, CV_GRAY2BGR);
for(size_t i=0;i<nextleftpts.size();i++)
{
if(nextleftpts[i].x>=0 && nextleftpts[i].x<=WIDTH && nextleftpts[i].y>=0 && nextleftpts[i].y<=HEIGHT)
{
CvScalar line_color; line_color = CV_RGB(255,0,0);
//int line_thickness = 1;
CvPoint p,q;
p.x = (int) prevleftpts[i].x;
p.y = (int) prevleftpts[i].y;
q.x = (int) nextleftpts[i].x;
q.y = (int) nextleftpts[i].y;
// double angle; angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
// double hypotenuse; hypotenuse = sqrt( pow((p.y - q.y),2) + pow((p.x - q.x),2) );
//
// //scale the line by a factor of 3
// q.x = (int) (p.x - 2 * hypotenuse * cos(angle));
// q.y = (int) (p.y - 2 * hypotenuse * sin(angle));
arrowedLine(left_mask, p, q, line_color);
//cvLine( &left_mask, p, q, line_color, line_thickness, CV_AA, 0 );
// p.x = (int) (q.x + 9 * cos(angle + pi / 4));
// p.y = (int) (q.y + 9 * sin(angle + pi / 4));
// cvLine( &prevleftimg, p, q, line_color, line_thickness, CV_AA, 0 );
// p.x = (int) (q.x + 9 * cos(angle - pi / 4));
// p.y = (int) (q.y + 9 * sin(angle - pi / 4));
// cvLine( &prevleftimg, p, q, line_color, line_thickness, CV_AA, 0 );
circle(left_mask, nextleftpts[i], 2, Scalar(0,255,0),-1);
kpts.push_back(nextleftpts[i]);
}
}
imshow("left", left_mask);
moveWindow("left", 0, 0);
}
if(frame_num>0)
{
if(nextleftpts.size()<=MIN_FEATURES)
{
goodFeaturesToTrack(g_greyscale_image_left, prevleftpts, MAX_FEATURES, QUALITY_EIGVALUE, MIN_DISTANCE);
}
else
{
prevleftpts.assign(kpts.begin(), kpts.end());
}
}
g_greyscale_image_left.copyTo(prevleftimg);
}
if(image.m_greyscale_image_right[d])
{
memcpy(g_greyscale_image_right.data, image.m_greyscale_image_right[d],IMAGE_SIZE);
vector<Point2f> nextrightpts,kpts;
if(prevrightpts.size()<MIN_FEATURES)
{
//control input
}
else if(frame_num>0)
{
//vector<CvPoint2D32f> nextleftpts;
vector<uchar> status;
vector<float> error;
vector<Mat> nextrightpyr,prevrightpyr;
buildOpticalFlowPyramid(prevrightimg, prevrightpyr, Size(7,7), 4);
buildOpticalFlowPyramid(g_greyscale_image_right, nextrightpyr, Size(7,7), 4);
CvTermCriteria optical_flow_termination_criteria
= cvTermCriteria( CV_TERMCRIT_ITER | CV_TERMCRIT_EPS, 20, 0.1 );
calcOpticalFlowPyrLK(prevrightpyr, nextrightpyr, prevrightpts, nextrightpts, status, error,Size(7,7),4,optical_flow_termination_criteria,OPTFLOW_LK_GET_MIN_EIGENVALS);
//drawing featues
Mat right_mask(g_greyscale_image_right.size(),CV_8UC3);
cvtColor(g_greyscale_image_right, right_mask, CV_GRAY2BGR);
for(size_t i=0;i<nextrightpts.size();i++)
{
if(nextrightpts[i].x>=0 && nextrightpts[i].x<=WIDTH && nextrightpts[i].y>=0 && nextrightpts[i].y<=HEIGHT)
{
CvScalar line_color; line_color = CV_RGB(255,0,0);
//int line_thickness = 1;
CvPoint p,q;
p.x = (int) prevrightpts[i].x;
p.y = (int) prevrightpts[i].y;
q.x = (int) nextrightpts[i].x;
q.y = (int) nextrightpts[i].y;
// double angle; angle = atan2( (double) p.y - q.y, (double) p.x - q.x );
// double hypotenuse; hypotenuse = sqrt( pow((p.y - q.y),2) + pow((p.x - q.x),2) );
//
//// //scale the line by a factor of 3
// q.x = (int) (p.x - 2 * hypotenuse * cos(angle));
// q.y = (int) (p.y - 2 * hypotenuse * sin(angle));
//
// cvLine( &prevrightimg, p, q, line_color, line_thickness, CV_AA, 0 );
//
// p.x = (int) (q.x + 9 * cos(angle + pi / 4));
// p.y = (int) (q.y + 9 * sin(angle + pi / 4));
// cvLine( &prevrightimg, p, q, line_color, line_thickness, CV_AA, 0 );
// p.x = (int) (q.x + 9 * cos(angle - pi / 4));
// p.y = (int) (q.y + 9 * sin(angle - pi / 4));
// cvLine( &prevrightimg, p, q, line_color, line_thickness, CV_AA, 0 );
//
arrowedLine(right_mask, p, q, line_color);
circle(right_mask, nextrightpts[i], 2, Scalar(0,255,0),-1);
kpts.push_back(nextrightpts[i]);
}
}
imshow("right", right_mask);
moveWindow("right", 500, 0);
}
if(frame_num>0)
{
if(nextrightpts.size()<=MIN_FEATURES)
{
goodFeaturesToTrack(g_greyscale_image_right, prevrightpts, MAX_FEATURES, QUALITY_EIGVALUE, MIN_DISTANCE);
}
else
{
prevrightpts.assign(kpts.begin(), kpts.end());
}
}
g_greyscale_image_right.copyTo(prevrightimg);
frame_num++;
}
//Stero matching
// Mat g_l,g_r;
//
// cvtColor(g_greyscale_image_left, g_l, CV_BGR2GRAY);
// cvtColor(g_greyscale_image_right,g_r, CV_BGR2GRAY);
Mat imgDisparity16S = Mat( g_greyscale_image_left.rows, g_greyscale_image_left.cols, CV_16S );
Mat imgDisparity8U = Mat( g_greyscale_image_left.rows, g_greyscale_image_left.cols, CV_8UC1 );
int ndisparities = 16*4;
int SADWindowSize = 7;
Ptr<StereoBM> sbm = StereoBM::create( ndisparities, SADWindowSize );
sbm->compute( g_greyscale_image_left, g_greyscale_image_right, imgDisparity16S );
double minVal; double maxVal;
minMaxLoc( imgDisparity16S, &minVal, &maxVal );
printf("Min disp: %f Max value: %f \n", minVal, maxVal);
imgDisparity16S.convertTo( imgDisparity8U, CV_8UC1, 255/(maxVal - minVal));
namedWindow( "windowDisparity", WINDOW_AUTOSIZE );
imshow( "windowDisparity", imgDisparity8U );
moveWindow("windowDisparity", 500, 500);
}
waitKey(1);
}
g_lock.leave();
g_event.set_DJI_event();
return 0;
}
