void pkmDetector::update()
{
if (bSetImageSearch && bSetImageTemplate)
{
if (!bInitialized)
{
// do matching between the template and image search
// without tracking previous features since none initialized
filteredMatches.clear();
switch( matcherFilterType )
{
case CROSS_CHECK_FILTER :
crossCheckMatching( descriptorMatcher, img_template_descriptors, img_search_descriptors, filteredMatches, 1 );
break;
default :
simpleMatching( descriptorMatcher, img_template_descriptors, img_search_descriptors, filteredMatches );
}
// reindex based on found matches
vector<int> queryIdxs( filteredMatches.size() ), trainIdxs( filteredMatches.size() );
for( size_t i = 0; i < filteredMatches.size(); i++ )
{
queryIdxs[i] = filteredMatches[i].queryIdx;
trainIdxs[i] = filteredMatches[i].trainIdx;
}
// build homograhpy w/ ransac
vector<cv::Point2f> points1; cv::KeyPoint::convert(img_template_keypoints, points1, queryIdxs);
vector<cv::Point2f> points2; cv::KeyPoint::convert(img_search_keypoints, points2, trainIdxs);
H12 = findHomography( cv::Mat(points1), cv::Mat(points2), CV_RANSAC, ransacReprojThreshold );
// create a mask of the current inliers based on transform distance
vector<char> matchesMask( filteredMatches.size(), 0 );
printf("Matched %d features.\n", filteredMatches.size());
// convert previous image points to current image points via homography
// although this is a transformation from image to world coordinates
// it should estimate the current image points
cv::Mat points1t; perspectiveTransform(cv::Mat(points1), points1t, H12);
for( size_t i1 = 0; i1 < points1.size(); i1++ )
{
if( norm(points2[i1] - points1t.at<cv::Point2f>((int)i1,0)) < 4 ) // inlier
{
matchesMask[i1] = 1;
img_search_points_inliers[1].push_back(points2[i1]);
}
else {
img_search_points_outliers[1].push_back(points2[i1]);
}
}
// update bounding box
cv::Mat bb;
perspectiveTransform(cv::Mat(img_template_boundingbox), bb, H12);
for( int i = 0; i < 4; i++ )
{
dst_corners[i] = bb.at<cv::Point2f>(i,0);
//img_template_boundingbox[i] = bb.at<cv::Point2f>(i,0);
}
/*
// draw inliers
drawMatches( img_search, img_template_keypoints,
img_template, img_search_keypoints,
filteredMatches, drawImg,
CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask
#if DRAW_RICH_KEYPOINTS_MODE
, DrawMatchesFlags::DRAW_RICH_KEYPOINTS
#endif
);
#if DRAW_OUTLIERS_MODE
// draw outliers
for( size_t i1 = 0; i1 < matchesMask.size(); i1++ )
matchesMask[i1] = !matchesMask[i1];
drawMatches( img_template, img_template_keypoints,
img_search, img_search_keypoints, filteredMatches, drawImg, CV_RGB(0, 0, 255), CV_RGB(255, 0, 0), matchesMask,
DrawMatchesFlags::DRAW_OVER_OUTIMG | DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
#endif
imshow( winName, drawImg );
*/
}
else {
// track features from previous frame into the current frame and see which
// features are inliers, and which are outliers. among the features that
// are outliers, see if any were marked as inliers in the previous frame and
// remark then as outliers
// mark decsriptors on new features marked as outliers once the number of
// inliers drops to a certain threshold and perform matching on the template.
// patch based seems powerful as well. creating a manifold or detecting planes
// in the set of inliers and tracking them as a whole may be more powerful.
//<#statements#>
}
//std::swap(current_template_points, previous_template_points);
std::swap(img_search_points_inliers[1], img_search_points_inliers[0]);
std::swap(img_search_points_outliers[1], img_search_points_outliers[0]);
swap(prev_img_search, img_search);
} // end bSetImageSearch && bSetImageTemplate
}
void callback(const sensor_msgs::ImageConstPtr& msg)
{
if (image_0_ == NULL)
{
// Take first image:
try
{
image_0_ = cv_bridge::toCvCopy(msg,
sensor_msgs::image_encodings::isColor(msg->encoding) ?
sensor_msgs::image_encodings::BGR8 :
sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_STREAM("Failed to take first image: " << e.what());
return;
}
ROS_INFO("First image taken");
// Detect keypoints:
detector_->detect(image_0_->image, keypoints_0_);
ROS_INFO_STREAM(keypoints_0_.size() << " points found.");
// Extract keypoints' descriptors:
extractor_->compute(image_0_->image, keypoints_0_, descriptors_0_);
}
else
{
// Take second image:
try
{
image_1_ = cv_bridge::toCvShare(msg,
sensor_msgs::image_encodings::isColor(msg->encoding) ?
sensor_msgs::image_encodings::BGR8 :
sensor_msgs::image_encodings::MONO8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR_STREAM("Failed to take image: " << e.what());
return;
}
// Detect keypoints:
std::vector<cv::KeyPoint> keypoints_1;
detector_->detect(image_1_->image, keypoints_1);
ROS_INFO_STREAM(keypoints_1.size() << " points found on the new image.");
// Extract keypoints' descriptors:
cv::Mat descriptors_1;
extractor_->compute(image_1_->image, keypoints_1, descriptors_1);
// Compute matches:
std::vector<cv::DMatch> matches;
match(descriptors_0_, descriptors_1, matches);
// Compute homography:
cv::Mat H;
homography(keypoints_0_, keypoints_1, matches, H);
// Draw matches:
const int s = std::max(image_0_->image.rows, image_0_->image.cols);
cv::Size size(s, s);
cv::Mat draw_image;
warped_image_ = boost::make_shared<cv_bridge::CvImage>(
image_0_->header, image_0_->encoding,
cv::Mat(size, image_0_->image.type()));
if (!H.empty()) // filter outliers
{
std::vector<char> matchesMask(matches.size(), 0);
const size_t N = matches.size();
std::vector<int> queryIdxs(N), trainIdxs(N);
for (size_t i = 0; i < N; ++i)
{
queryIdxs[i] = matches[i].queryIdx;
trainIdxs[i] = matches[i].trainIdx;
}
std::vector<cv::Point2f> points1, points2;
cv::KeyPoint::convert(keypoints_0_, points1, queryIdxs);
cv::KeyPoint::convert(keypoints_1, points2, trainIdxs);
cv::Mat points1t;
cv::perspectiveTransform(cv::Mat(points1), points1t, H);
double maxInlierDist = threshold_ < 0 ? 3 : threshold_;
for (size_t i1 = 0; i1 < points1.size(); ++i1)
{
if (cv::norm(points2[i1] - points1t.at<cv::Point2f>((int)i1,0)) <= maxInlierDist ) // inlier
matchesMask[i1] = 1;
}
// draw inliers
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image, cv::Scalar(0, 255, 0), cv::Scalar(0, 0, 255),
matchesMask,
cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
// draw outliers
for (size_t i1 = 0; i1 < matchesMask.size(); ++i1)
matchesMask[i1] = !matchesMask[i1];
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image, cv::Scalar(0, 0, 255), cv::Scalar(255, 0, 0),
matchesMask,
cv::DrawMatchesFlags::DRAW_OVER_OUTIMG | cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
ROS_INFO_STREAM("Number of inliers: " << cv::countNonZero(matchesMask));
// Wrap the new image using the homography,
// so we should have something similar to the original image:
warpPerspective(
image_1_->image, warped_image_->image, H, size,
cv::INTER_LINEAR | cv::WARP_INVERSE_MAP);
// Print the homography found:
ROS_INFO_STREAM("Homography = " << H);
}
else
{
cv::drawMatches(
image_0_->image, keypoints_0_,
image_1_->image, keypoints_1, matches,
draw_image);
image_1_->image.copyTo(warped_image_->image);
ROS_WARN_STREAM("No homography transformation found!");
}
// Publish warped image (using homography):
warped_image_->header = image_1_->header;
pub_.publish(warped_image_->toImageMsg());
// Show images:
cv::imshow("correspondences", draw_image);
cv::imshow("homography", warped_image_->image);
cv::waitKey(3);
}
}
int main(int argc, char** argv)
{
ofstream f1;
f1.open("result.txt");
size_t i,j;
Point2f cp;
cv::initModule_nonfree();
vector<Point2f> MP1,MP2;
vector<int> trainIdxs, queryIdxs;
//Read Video File
VideoCapture cap("video1.avi");
if( !cap.isOpened() )
{ cout << "Could not initialize capturing...\n"; return 0;}
VideoWriter writer("ms_tracking.avi",CV_FOURCC('D','I','V','3'),
10,cvSize(640,480),1);
cv::SURF mySURF; mySURF.extended = 0;
Ptr<DescriptorMatcher> descriptorMatcher = DescriptorMatcher::create( "FlannBased" );
int mactherFilterType = getMatcherFilterType( "CrossCheckFilter" );
Mat frame,img1,img2;
cap >> frame;
if( frame.empty() )
return -1;
img1 = frame.clone() ;
Mat temp,temp1;
if(img1.empty())
cout << "Exiting as the input image is empty" << endl;
const char* name = "Initiate_ROI";
box = cvRect(-1,-1,0,0);
cvNamedWindow( name,1);
cvSetMouseCallback( name, my_mouse_callback2);
// Main loop
while( 1 )
{
img1.copyTo(temp);
if( drawing_poly)
{
for ( i=0; i < polyPoints.size(); i++)
circle(temp, polyPoints[i], 2, Scalar(0,255,0), -1,8);
}
cv::imshow(name,temp) ;
char c = (char)waitKey(10);
if( c == '\x1b' ) // esc
break;
if(poly_drawn)
break;
}
//Read the polygon points from a text file
FILE *f11;
polyPoints.clear();
IpolyPoints.clear();
f11 = fopen("points.txt","r");
Point a;
for(int j=0;j<37;j++)
{
fscanf(f11,"%d",&(a.x));
fscanf(f11,"%d",&(a.y));
polyPoints.push_back(a);
IpolyPoints.push_back(a);
}
fclose(f11);
// Drawing Polygon
Point pointArr[polyPoints.size()];
for (i=0; i< polyPoints.size(); i++)
pointArr[i] = polyPoints[i];
const Point* pointsArray[1] = {pointArr};
int nCurvePts[1] = { polyPoints.size() };
polylines(temp, pointsArray, nCurvePts, 1, 1, Scalar(0,255,0), 1);
cout << polyPoints.size() << endl;
box= boundingRect(polyPoints);
//boxOrg = Rect(box.x-15, box.y-15, box.width+30, box.height+30);
boxOuter = Rect(box.x-30, box.y-30, box.width+60, box.height+60);
//box =boxOrg; // storing the initial selected Box, as "box" variable changes in consecutive matching
boxP=box;
Mat img1ROI, labels1, clusters1, descriptors,roidescriptors, descriptors1,bdescriptors, bmdescriptors;
vector<int> reprojections; // number of reprojections per KP, size same as KP(incresing)
vector<Point2f> points,points1,points2, Mpoints1,Mpoints2,bpoints,npoints1,npoints2; //bmpoints,tpoints;
vector<KeyPoint> roikeypoints, bkeypoints,keypoints,keypoints1, keypoints2;
draw_box(temp, box ); //Show InnerBox - This is used by the Mean-Shift Tracker
draw_box(temp,boxOuter); //Show OuterBox - This is used for removing background points
bpoints.clear();
//calculating keypoints and descriptors of the selected polygon in image roi
//==============================================================================================//
for(i=0;i<polyPoints.size();i++)
{
// cout << polyPoints[i] << endl; //
polyPoints[i].x = polyPoints[i].x -boxOuter.x;
polyPoints[i].y = polyPoints[i].y- boxOuter.y;
}
img1ROI = img1(boxOuter);
points1.clear();
mySURF.detect(img1ROI, roikeypoints);
KeyPoint::convert(roikeypoints, points);
mySURF.compute(img1ROI, roikeypoints, roidescriptors);
bdescriptors.release();bkeypoints.clear();
bcategorizePoints( points, bpoints,polyPoints, roikeypoints, roidescriptors, bkeypoints, bdescriptors);
shiftPoints(bpoints,boxOuter);
for(i=0;i<bpoints.size();i++)
circle(temp, bpoints[i], 2, Scalar(0,255,0),2);
vector<KeyPoint> tpkeypoints; Mat tpdescriptors;
categorizePoints( points, points1,polyPoints, roikeypoints, roidescriptors, tpkeypoints, tpdescriptors);
shiftPoints(points1, boxOuter);
for(i=0;i<points1.size();i++)
circle(temp, points1[i], 2, Scalar(0,0,255),2);
//====================================================================================================//
points1.clear();
Mat img2ROI;
// tpkeypoints = keypoints1; tpdescriptors = descriptors1;
cv::imshow(name,temp) ;
imwrite("a.jpg",temp);
cout << "BD_SIZE \t" << bdescriptors.rows << "\t" << "FD_SIZE \t" << tpdescriptors.rows << endl;
// Mat newimg = img1ROI.clone();
// KeyPoint::convert(tpkeypoints, points1);
// for(size_t i=0;i<points1.size();i++)
// circle(newimg, points1[i], 2, Scalar(255,0,255),2);
// imshow( "newimg", newimg );
// points1.clear();
waitKey(0);
cvDestroyWindow( name );
int FG_mp, FG, BG_mp, BG, FG_BG, msI ; //Foreground matching points
struct timeval t1, t2;
for(int l=0;;l++)
{
gettimeofday(&t1, NULL);
cv::kmeans(tpdescriptors, NOC, labels1, TermCriteria( CV_TERMCRIT_ITER + CV_TERMCRIT_EPS, 50, 1.0 ), 1,
KMEANS_RANDOM_CENTERS, clusters1);
cap >> frame;
img2 = frame.clone() ;
temp1 =frame.clone() ;
if(img2.empty() )
{
cout<< "Could not open image: " << endl ;
break;}
int flag=1;
Mpoints1.clear();
Mat descriptors2;
msI=0;
meanShift(img1, img2, descriptorMatcher, mactherFilterType, tpkeypoints, tpdescriptors,keypoints2,descriptors2,
clusters1, cp, flag, MP1,img2ROI,bkeypoints, bdescriptors, temp1,FG_mp, FG, BG_mp, BG, FG_BG,msI);
//==========scaling=================
float scale=1;
// cout <<"MP1size \t" << MP1.size() <<endl;
if(APPLY_SCALING)
{
vector<DMatch> filteredMatches;
if(descriptors1.rows > 4 && descriptors2.rows > 4)
{
crossCheckMatching( descriptorMatcher, descriptors1, descriptors2, filteredMatches, 1 );
trainIdxs.clear(); queryIdxs.clear();
for( i = 0; i < filteredMatches.size(); i++ )
{
queryIdxs.push_back(filteredMatches[i].queryIdx);
trainIdxs.push_back(filteredMatches[i].trainIdx);
}
points1.clear(); points2.clear();
KeyPoint::convert(keypoints1, points1, queryIdxs);
KeyPoint::convert(keypoints2, points2, trainIdxs);
// cout << "point2size" << points2.size() << endl;
//homography
npoints1.clear();npoints2.clear();
Mpoints1.clear();Mpoints2.clear();
Mat H12, points1t;
double ransacReprojThreshold = 10;
if( ransacReprojThreshold >= 0 && points1.size() > 4)
H12 = findHomography( Mat(points1), Mat(points2), CV_RANSAC, ransacReprojThreshold );
vector<char> matchesMask( filteredMatches.size(), 0 );// NONmatchesMask( filteredMatches.size(), 0 );
if( !H12.empty() )
{
perspectiveTransform(Mat(points1), points1t, H12);
double maxInlierDist = 10;//ransacReprojThreshold < 0 ? 3 : ransacReprojThreshold;
for(i = 0; i < points1.size(); i++ )
{
if( norm(points2[i] - points1t.at<Point2f>((int)i,0)) <= 5)// maxInlierDist ) // inlier
{
matchesMask[i] = 1;
npoints2.push_back(points2[i]);
npoints1.push_back(points1[i]);
}
}
for(i=0; i<npoints2.size();i++)
{
for(j=0;j<MP1.size();j++)
{
double dist = norm(npoints2[i]-MP1[j]);
// cout <<"dist \t" <<dist << endl;
// waitKey(0);
if(dist < 0.1)
{
Mpoints2.push_back(npoints2[i]);
Mpoints1.push_back(npoints1[i]);
break;
}
}
}
}
Mat drawImg;
drawMatches( img1ROI, keypoints1, img2ROI, keypoints2, filteredMatches, drawImg, CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask
#if DRAW_RICH_KEYPOINTS_MODE
, DrawMatchesFlags::DRAW_RICH_KEYPOINTS
#endif
);
imshow( "correspondance", drawImg );
cout << "npoints1.size \t" << Mpoints1.size() << "\t" << Mpoints2.size() << endl;
if(Mpoints1.size() > 8)
weightScalingAspect(Mpoints1,Mpoints2,&scale);
}
}
img1=img2;
img1ROI = img2ROI;
boxOrg =box;
keypoints1 = keypoints2;
descriptors1 =descriptors2;
box.x += box.width/2;
box.y += box.height/2;
box.height = round(boxOrg.height *scale);
box.width = round(( float(boxOrg.width)/float(boxOrg.height) ) * box.height);
box.x -= box.width/2;
box.y -= box.height/2;
boundaryCheckRect(box);
cout <<"SCALE \t" << scale << endl;
gettimeofday(&t2, NULL);
double diff = (float)((t2.tv_sec * 1000000 + t2.tv_usec) - (t1.tv_sec * 1000000 + t1.tv_usec));
diff = diff/1000;
cout <<"Time taken in mili sec \t" << diff<< endl;
// cout << tpdescriptors.rows << endl;
//cout <<"BD \t" << bdescriptors.rows << endl;
f1 << l << "\t" << FG_mp << "\t" << BG_mp << "\t" << FG << "\t"<< msI << "\n";
cout << "l \t" << l << "\t" <<" msI \t"<< msI << endl;
imshow("img2",temp1);
writer << temp1;
waitKey(0);
// boxOrg = eBox;
char c = (char)waitKey(10);
if( c == '\x1b' ) // esc
{
cout << "Exiting ..." << endl;
break;
}
}
trajectory.close();
return 0;
}
static void doIteration(Mat& img1, Mat& img2, vector<int>& queryIdxs, vector<int>& trainIdxs, Ptr<DescriptorMatcher>& descriptorMatcher,
int matcherFilter,vector<KeyPoint>& keypoints1, Mat& descriptors1, vector<KeyPoint>& keypoints2,Mat& descriptors2,
Mat& matchedDesc1, Mat& matchedDesc2, vector<Point2f>& matchedPoints1, vector<Point2f>& matchedPoints2,
vector<Point2f>& MP1, vector<KeyPoint>& tempkey)
{
assert( !img2.empty());
cv::SURF mySURF; mySURF.extended = 0;
Mat H12;
mySURF.detect(img2, keypoints2); mySURF.compute(img2, keypoints2, descriptors2);
vector<DMatch> filteredMatches;
switch( matcherFilter )
{
case CROSS_CHECK_FILTER :
crossCheckMatching( descriptorMatcher, descriptors1, descriptors2, filteredMatches, 1 );
break;
default :
simpleMatching( descriptorMatcher, descriptors1, descriptors2, filteredMatches );
}
trainIdxs.clear(); queryIdxs.clear();
for( size_t i = 0; i < filteredMatches.size(); i++ )
{
queryIdxs.push_back(filteredMatches[i].queryIdx);
trainIdxs.push_back(filteredMatches[i].trainIdx);
}
//////
Mat mDesc1, mDesc2;
for(size_t i=0; i<queryIdxs.size(); i++)
{
mDesc1.push_back(descriptors1.row(queryIdxs[i]));
mDesc2.push_back(descriptors2.row(trainIdxs[i]));
}
vector<Point2f> points1; KeyPoint::convert(keypoints1, points1, queryIdxs);
vector<Point2f> points2; KeyPoint::convert(keypoints2, points2, trainIdxs);
vector<char> matchesMask( filteredMatches.size(), 0 );//, matchesMask2( filteredMatches.size(), 1 );;
Mat drawImg;// drawImg2;
cout << "points2.size \t" << points2.size() << endl;
cout <<"HELLO \t" << endl;
if( RANSAC_THREHOLD >= 0 )
{
if (points2.size() < 4 )
{
cout << "matchedPoints1 less than 4, hence prev ROI is retained" << endl;
for(size_t i1=0;i1<points2.size();i1++)
{
matchesMask[i1] = 1;
matchedPoints1.push_back( points1[i1]);
matchedPoints2.push_back( points2[i1]);
matchedDesc1.push_back(descriptors1.row(queryIdxs[i1]));
matchedDesc2.push_back(descriptors2.row(trainIdxs[i1]));
tempkey.push_back(keypoints2[trainIdxs[i1]]);
MP1.push_back(points2[i1]);
}
}
else
{
H12 = findHomography( Mat(points1), Mat(points2), CV_RANSAC, RANSAC_THREHOLD );
if( !H12.empty() )
{
Mat points1t; perspectiveTransform(Mat(points1), points1t, H12);
vector<Point2f> points2Shift(points2.size());
points2Shift = points2;
shiftPoints(points2Shift, box);
Point2f boxCenter;
boxCenter.x = box.x + box.width/2;
boxCenter.y = box.y + box.height/2;
for( size_t i1 = 0; i1 < points1.size(); i1++ )
{
double descDiff = pow(norm(mDesc1.row(i1) - mDesc2.row(i1)) , 2);
// if(descDiff < 0.08)
{
double diff = norm(points2[i1] - points1t.at<Point2f>((int)i1,0));
if(diff <= 30)
{
// cout << diff << endl;
matchesMask[i1] = 1;
matchedPoints1.push_back( points1[i1]);
matchedPoints2.push_back( points2[i1]);
matchedDesc1.push_back(descriptors1.row(queryIdxs[i1]));
matchedDesc2.push_back(descriptors2.row(trainIdxs[i1]));
tempkey.push_back(keypoints2[trainIdxs[i1]]);
MP1.push_back(points2[i1]);
}
}
}
}
// drawMatches( img1, keypoints1, img2, keypoints2, filteredMatches, drawImg2, CV_RGB(255, 255, 0), CV_RGB(255,255, 255), matchesMask2
// #if DRAW_RICH_KEYPOINTS_MODE
// , DrawMatchesFlags::DRAW_RICH_KEYPOINTS
// #endif
// );
drawMatches( img1, keypoints1, img2, keypoints2, filteredMatches, drawImg, CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask
#if DRAW_RICH_KEYPOINTS_MODE
, DrawMatchesFlags::DRAW_RICH_KEYPOINTS
#endif
);
cout << endl;
imshow( "doiter", drawImg );
// Mat newimg = img1.clone();
// KeyPoint::convert(keypoints1, points1);
// for(size_t i=0;i<points1.size();i++)
// circle(newimg, points1[i], 2, Scalar(255,0,255),2);
// imshow( "doimg", newimg );
// points1.clear();
// waitKey(0);
}
}
// waitKey(0);
}
// Mean Shift Algorithm
void meanShift(Mat& img1, Mat& img2, Ptr<DescriptorMatcher>& descriptorMatcher, int matcherFilterType, vector<KeyPoint>& tpkeypoints,
Mat& tpdescriptors, vector<KeyPoint>& keypoints2, Mat& descriptors2, Mat& clusters1, Point2f &cp, int& flag,
vector<Point2f>& MP1, Mat& img2ROI, vector<KeyPoint>& bkeypoints, Mat& bdescriptors, Mat& temp,
int& FG_mp, int&FG, int& BG_mp, int& BG, int& FG_BG, int& msI)
{
size_t i,j;
// Mat temp=img2.clone();
int converged = 0, semiConverged = 0;
Mat img1ROI, labels1_;
Point2f lastCenter(box.x+box.width/2,box.y+box.height/2);
float scale = 1;
img1ROI = img1(boxOrg);
vector<Point2f> points1, bmp;
KeyPoint::convert(tpkeypoints, points1);
searchBin(tpdescriptors, clusters1, labels1_); // clustering based on Kmeans centers obatined earlier
//vector<Point2f> np;
// Mat newimg = img1ROI.clone();
// KeyPoint::convert(tpkeypoints, np);
// for(size_t i=0;i<np.size();i++)
// circle(newimg, np[i], 2, Scalar(255,0,255),2);
// imshow( "msimg", newimg );
// np.clear();
// waitKey(0);
vector<float> prevPDF(NOC); // pdf of source
weightedPDF( points1, boxOrg, labels1_, NOC, prevPDF); // Making histogram/pdf by normalizing the data and applying weights based on positions
// Iterations for finding the object
Point2f zBCmax(0,0); // center corspndng to the iteration with max BC
float BC, BCmax=0, BCprev=0, BCnow=0; // Bhattachrya coefficient, max val for an image, previous and current val for an iteration
int stopCount=0; // MS iterations must converege for stopCount < ManualSetThreshold
vector<Point2f> matchedPoints1, matchedPoints2;
while ( !converged )
{
// ofstream tempF;
//tempF.open("tempF.txt", ios::out);
matchedPoints1.clear(); matchedPoints2.clear();
Mat matchedDesc1, matchedDesc2;
vector<int> queryIdxs, trainIdxs;
cv::Rect expandedBox;
#ifdef DEBUG
cout << "iteration in while = \t" << ++iter << endl;
#endif
if (EXPANDED_BOX)
{
expandedBox = Rect(box.x-25, box.y-25, box.width+50, box.height+50);
boundaryCheckRect(expandedBox);
img2ROI = img2(expandedBox);
}
else
{
// cout << box.br() << "\t" << box.tl() << "\t" << img2.cols << endl;
img2ROI = img2(box);
}
vector<KeyPoint> tempkey;
// Mat pointsTransed21;
MP1.clear();
doIteration(img1ROI, img2ROI, queryIdxs, trainIdxs,descriptorMatcher, matcherFilterType, tpkeypoints,tpdescriptors,
keypoints2,descriptors2, matchedDesc1, matchedDesc2, matchedPoints1, matchedPoints2, MP1,tempkey);
if(matchedPoints2.size() < 1)
{
FG=0; BG=0;FG_mp=0;BG_mp=0;FG_BG=0; msI=0;
break;
}
// mdescriptors = matchedDesc2;
// KeyPoint::convert(keypoints2, points2);
if (EXPANDED_BOX)
shiftPoints(matchedPoints2, expandedBox);
else
shiftPoints(matchedPoints2, box);
// shiftPoints(matchedPoints1,boxOrg);
vector<float> predPDF(NOC,0);
Mat labels2, labels2_; // depending on PDF_OF_WHOLE
Point2f z(0,0);
//==================== Edited at 8th april =======================//
bmp.clear();
Mat tmatchedDesc2, tmatchedDesc1;
vector<Point2f> tmatchedPoints2;
msI = stopCount;
FG_mp = matchedPoints2.size();
vector<KeyPoint> tempbk;
Mat tempBd;
vector<DMatch> filteredMatches;
crossCheckMatching( descriptorMatcher, bdescriptors, descriptors2, filteredMatches, 1 );
trainIdxs.clear(); queryIdxs.clear();
for( i = 0; i < filteredMatches.size(); i++ )
{
queryIdxs.push_back(filteredMatches[i].queryIdx);
trainIdxs.push_back(filteredMatches[i].trainIdx);
}
vector<Point2f> points1; KeyPoint::convert(bkeypoints, points1, queryIdxs);
vector<Point2f> points2; KeyPoint::convert(keypoints2, points2, trainIdxs);
vector<char> matchesMask( filteredMatches.size(), 0 );
/////
Mat H12;
Mat drawImg;
if (points2.size() < 4 )
{
cout << "backpoints less than 4, hence prev ROI is retained" << endl;
return;
for(i=0;i<points2.size();i++)
{
bmp.push_back( points2[i]);
tempBd.push_back(bdescriptors.row(queryIdxs[i]));
tempbk.push_back(keypoints2[trainIdxs[i]]);
tempBd.push_back(descriptors2.row(trainIdxs[i]));
tempbk.push_back(keypoints2[trainIdxs[i]]);
}
}
else
{
H12 = findHomography( Mat(points1), Mat(points2), CV_RANSAC, RANSAC_THREHOLD );
if( !H12.empty() )
{
Mat points1t; perspectiveTransform(Mat(points1), points1t, H12);
for( size_t i1 = 0; i1 < points1.size(); i1++ )
{
double diff = norm(points2[i1] - points1t.at<Point2f>((int)i1,0));
if(diff <= 20)
{
matchesMask[i1]=1;
bmp.push_back( points2[i1]);
tempBd.push_back(bdescriptors.row(queryIdxs[i1]));
tempbk.push_back(keypoints2[trainIdxs[i1]]);
tempBd.push_back(descriptors2.row(trainIdxs[i1]));
tempbk.push_back(keypoints2[trainIdxs[i1]]);
}
}
drawMatches( img1ROI, bkeypoints, img2ROI, keypoints2, filteredMatches, drawImg, CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask
#if DRAW_RICH_KEYPOINTS_MODE
, DrawMatchesFlags::DRAW_RICH_KEYPOINTS
#endif
);
}
}
imshow("bm",drawImg);
//============edit part ====
shiftPoints(bmp, box);
vector<int> bflag(bmp.size(),0);
for(i=0;i<bmp.size();i++)
bflag[i]=0;
vector<int> ft(matchedPoints2.size(),0);
for(i=0;i<matchedPoints2.size();i++)
{
ft[i]=0;
for(j=0; j< bmp.size(); j++)
{
double diff = norm (matchedPoints2[i] - bmp[j]);
// cout << diff << endl;
if(diff < 0.5)
{
bflag[j]=1;
ft[i]=1;
break;
}
}
if(ft[i]==0)
{
tmatchedPoints2.push_back(matchedPoints2[i]);
tmatchedDesc1.push_back(matchedDesc1.row(i));
tmatchedDesc2.push_back(matchedDesc2.row(i));
}
}
//=================================================================//
// allot descriptors to the clusters to make histogram
searchBin(tmatchedDesc1, clusters1, labels1_);
searchBin( tmatchedDesc2, clusters1, labels2);
if (PDF_OF_WHOLE)
searchBin( descriptors2, clusters1, labels2_);
// find the PDF for the above histogram as per weights
if (PDF_OF_WHOLE)
weightedPDF( points2, box, labels2_, NOC, predPDF);
else
weightedPDF( tmatchedPoints2, box, labels2, NOC, predPDF);
// find weights for each IPoint as per the values of weighted PDFs
vector<float> weights(labels2.rows,0);
Mat imgTemp = img2.clone();
findWeights( prevPDF, predPDF, labels1_, labels2, weights, queryIdxs, tmatchedDesc1, tmatchedDesc2);
// find new ROI center as per above weights
findNewCenter(tmatchedPoints2, weights, box, z);
lastCenter = Point2f (box.x+box.width/2, box.y+box.height/2);
// if current BC is less than previous BC, then take mean of the prev and current centers
BCnow = findBC(predPDF,prevPDF);
if (BCnow < BCprev)
z = 0.5*(z+lastCenter);
BCprev = BCnow;
// check if ROI centers converge to same pixel
if ( (norm(z - lastCenter) < 3))
{
semiConverged = 1;
if (!SHOW_FINAL_ROI)
rectangle(temp, box, Scalar(0,0,255),2);
}
else
{
// keep iterating
stopCount++;
if (stopCount >= MAX_MS_ITER)
{
semiConverged = 1;
flag = 0;
if (!SHOW_FINAL_ROI)
rectangle(temp, box, Scalar(0,0,255),2);
z = zBCmax;
}
box.x = z.x - box.width/2;
box.y = z.y - box.height/2;
boundaryCheckRect(box);
if (stopCount < MAX_MS_ITER)
if (!SHOW_FINAL_ROI)
;// rectangle(temp, box, Scalar(0,255,0), 2);
}
// store values of max BC and corresponding center z
if ( BCnow > BCmax)
{
BCmax = BC;
zBCmax = z;
}
if (semiConverged)
{
converged = 1;
// FG_mp, FG, BG_mp, BG, FG_BG, msI ;
//==========edited on 5april ========
bdescriptors.release();
bkeypoints.clear();
for(i=0;i<tempBd.rows;i++)
{
bdescriptors.push_back(tempBd.row(i));
bkeypoints.push_back(tempbk[i]);
}
tpdescriptors.release();
tpkeypoints.clear();
//============================================//
for(i=0;i<matchedPoints2.size();i++)
{
if(ft[i]==0)
{
tpdescriptors.push_back(matchedDesc1.row(i));
tpkeypoints.push_back(tempkey[i]);
tpdescriptors.push_back(matchedDesc2.row(i));
tpkeypoints.push_back(tempkey[i]);
}
}
//=================================
box.x = z.x - box.width/2;
box.y = z.y - box.height/2;
// imgTemp.release();
trajectory << z.x << "\t" << z.y << "\t" << box.width << "\t" << box.height << endl;
cp =z;
cv::circle(temp, z, 3, Scalar(0,255,255), 3);
cv::rectangle(temp, box, Scalar(255,0,0),2);
cout << "MP1 \t" << MP1.size() <<"\t" << "bmp \t" <<bmp.size() << endl;
for(size_t i=0;i<MP1.size();i++)
{//circle(temp, MP1[i], 3, Scalar(255,255,255),3);
circle(temp, matchedPoints2[i], 3, Scalar(255,0,255),3);
}
// shiftPoints(bmp,box);
for(size_t i=0;i<bmp.size();i++)
{ circle(temp, bmp[i], 2, Scalar(0,0,0),2);
// cout << bmp[i] << endl;
}
}
char c = (char)waitKey(10);
if( c == '\x1b' ) // esc
{
cout << "Exiting from while iterator..." << endl;
break;
}
}
cv::imshow("Iter", temp);
// waitKey(0);
eachIter.close();
}
/**
* Get the depths of the features and eliminate those who are not matched.
* Optionally, get the 3D pts of
* with the one given to this->fromImage() method or constructor.
*
* Image is not saved to class by default, it will consume lots
* of memory! This method is only used for DEBUGGING
*
* @param string imPath
* @return void
*/
std::map<int, cv::Point3d> Observation::get3DPts(const cv::Mat& imL, const cv::Mat& imR, const StereoRig& rig)
{
cv::Ptr<cv::FeatureDetector> detector = new cv::SurfFeatureDetector(config::HESSIAN_THRESHOLD);
cv::Ptr<cv::DescriptorExtractor> descriptorExtractor = new cv::SurfDescriptorExtractor(4, 2, true);
std::map<int, cv::Point3d> tmpPoints3D;
std::vector<cv::KeyPoint> keyPointsR;
cv::Mat descriptorsR;
// detect keypoints and extract feaures
detector->detect(imR, keyPointsR);
descriptorExtractor->compute(imR, keyPointsR, descriptorsR);
// use a new matcher to match features from left im to right im
cv::FlannBasedMatcher matcher;
std::vector<cv::DMatch> matches, matchesCrossCheck;
// match in both directions: left image to right vice versa
matcher.match(descriptors, descriptorsR, matches);
matcher.clear();
matcher.match(descriptorsR, descriptors, matchesCrossCheck);
std::vector<char> matchesMask(matches.size(),0);
std::map<int,int> matchesLTR, matchesRTL;
for (size_t i=0; i<matches.size(); ++i)
matchesLTR[matches[i].queryIdx] = matches[i].trainIdx;
for (size_t i=0; i<matchesCrossCheck.size(); ++i)
matchesRTL[matchesCrossCheck[i].queryIdx] = matchesCrossCheck[i].trainIdx;
size_t cnt=0;
typedef std::map<int,int>::const_iterator It_ii;
for (It_ii it=matchesLTR.begin(); it != matchesLTR.end(); ++it, cnt++)
{
It_ii searchResult = matchesRTL.find(it->second);
if (matchesRTL.end() == searchResult) // continue if feat does not exist in 2nd im
continue;
if (searchResult->second != it->first)
continue;
// else
matchesMask[cnt] = 1;
}
// keep only matches on the same row (epipolar constraint) and having a positive disparity value
for (int i=matches.size()-1; i>=0; --i)
{
if (matchesMask[i]==0)
continue;
double disparity = keyPoints[matches[i].queryIdx].pt.x-keyPointsR[matches[i].trainIdx].pt.x;
if (disparity < 0 || 10 < std::abs(keyPoints[matches[i].queryIdx].pt.y-keyPointsR[matches[i].trainIdx].pt.y))
matchesMask[i] = 0;
else
tmpPoints3D[matches[i].queryIdx] = rig.get3DPoint(keyPoints[matches[i].queryIdx].pt.x, // negate the negative sign, we want
keyPoints[matches[i].queryIdx].pt.y, // positive depth values
disparity);
}
#ifdef INSPECTION_MODE
// buralar derinligi gostermek icin, gecici
cv::Mat di = imL.clone();
typedef std::map<int, cv::Point3d>::const_iterator It;
for (It it=tmpPoints3D.begin(); it != tmpPoints3D.end(); ++it)
{
std::stringstream ss;
ss << std::setprecision(2) << -it->second.z/1000.;
cv::Point pt(keyPoints[it->first].pt.x, keyPoints[it->first].pt.y);
cv::circle(di, pt, 2, cv::Scalar::all(255), 1);
cv::putText(di, ss.str(), pt, cv::FONT_HERSHEY_PLAIN, 0.5, cv::Scalar::all(255));
}
cv::imshow("disparities", di);
cv::Mat drawImg;
cv::drawMatches(imL, keyPoints, imR, keyPointsR, matches, drawImg, CV_RGB(0, 255, 0), CV_RGB(0, 0, 255), matchesMask);
cv::imshow("matches", drawImg);
/**/
#endif
return tmpPoints3D;
}