Matching GetAlignedMatching(size_t size) {
Matching match;
for (size_t i = 0; i < size; i++) {
match.push_back(DMatch(i, i, 0));
}
return match;
}
// Draws matches of keypints from two images on output image.
void ICLASS_API DrawMatches(
TMat* img1, TCVectorKeyPoint* keypoints1,
TMat* img2, TCVectorKeyPoint* keypoints2,
TCVectorDMatch* matches1to2, TMat** outImg)
{
vector<KeyPoint> k1, k2;
for (size_t i = 0; i < keypoints1->size(); i++)
{
TKeyPoint K = *keypoints1->at(i);
k1.push_back(KeyPoint(K.x, K.y, K.size, K.angle, K.response, K.octave, K.class_id));
}
for (size_t i = 0; i < keypoints2->size(); i++)
{
TKeyPoint K = *keypoints2->at(i);
k2.push_back(KeyPoint(K.x, K.y, K.size, K.angle, K.response, K.octave, K.class_id));
}
vector<DMatch> m1to2;
for (size_t i = 0; i < matches1to2->size(); i++)
{
TDMatch K = *matches1to2->at(i);
m1to2.push_back(DMatch(K.queryIdx, K.trainIdx, K.imgIdx, K.distance));
}
Mat oImg;
drawMatches(*img1->Mat(), k1, *img2->Mat(), k2, m1to2, oImg);
*outImg = new TMat(oImg);
};
vector<DMatch> points(
vector<pair<int, int> > edge_matches,
Mat &desc1, Mat &desc2,
vector<vector<int> > &edges1, vector<vector<int> > &edges2,
double th, double sigma = 0.5
) {
vector<DMatch> matches;
set<pair<int, int> > S;
for (size_t i = 0; i < edge_matches.size(); i++) {
int base_edge_idx = edge_matches[i].first;
int ref_edge_idx = edge_matches[i].second;
vector<Mat> des_1(3), des_2(3);
for (int k = 0; k < 3; k++) {
des_1[k] = desc1.row(edges1[base_edge_idx][k]);
des_2[k] = desc2.row(edges2[ref_edge_idx][k]);
}
vector<int> best_match(3);
vector<double> best_sim(3, -1E30);
for (int j = 0; j < 3; j++) {
for (int k = 0; k < 3; k++) {
double _sim = exp(-sum(abs(des_1[j] - des_2[k]))[0] / sigma);
if (_sim > best_sim[j]) {
best_sim[j] = _sim;
best_match[j] = k;
}
}
}
for (int j = 0; j < 3; j++) {
int k = best_match[j];
int qI = edges1[base_edge_idx][j];
int tI = edges2[ref_edge_idx][k];
if (!S.count(make_pair(qI, tI)) && best_sim[j] > th) {
double _dist = -sigma * log(best_sim[j]);
matches.push_back(DMatch(qI, tI, _dist));
S.insert(make_pair(qI, tI));
}
}
}
return matches;
}
void DisplayCorrespondence::onNewImage() {
int wrongmatch_counter=0;
int goodmatch_counter=0;
Mat image = in_img.read();
std::string path = in_path.read();
std::vector<int> image_params = in_image_params.read();
std::vector<std::vector<int> > MatchedSourceForTile =in_MatchedSourceForTile.read();
std::vector<int> PositionOfPatchesInImages =in_PositionOfPatchesInImages.read();
std::vector<std::vector<int> > MatchedPatchInMatcher =in_MatchedPatchInMatcher.read();
std::vector<std::vector<double> > DistanceMap = in_DistanceMap.read();
std::vector<std::string> all_file_paths = in_all_file_paths.read();
int files_number = all_file_paths.size();
std::vector<double> QueryMatchMap = in_match_map.read();
double match_quality = in_match_quality.read();
patches_cols = image_params[0];
patches_rows = image_params[1];
int patchsize = image_params[2];
int bd_cols = image_params[3];
int bd_rows = image_params[4];
int bd_patch_size = image_params[5];
int queue_size = image_params[6];
int BestMatchingImage = image_params[7];
int image_cols = image_params[8];
int image_rows = image_params[9];
double threshold = (double)image_params[10]/10000;
std::vector<std::vector<int> > BDMatchMap;
BDMatchMap.resize(bd_cols * bd_rows);
//for (int q = 0; q < bd_cols * bd_rows; q++) {
// BDMatchMap[q] = -1;
//}
int query_col;
int query_row;
int fn;
std::cout << "qs:" << queue_size << " thre:"<<threshold<<" p:"<<image_params[10]<<std::endl;
/*=======================================================================*/
/*************************************************************************/
/* create corresspondence map for DB image */
/*************************************************************************/
if (mode == 3) {
for (int k = 0; k < patches_rows * patches_cols; k++) {
int flag = 0;
int zzz;
//check if there is match of current patch to the best image
for (zzz = 0; zzz < queue_size; zzz++) {
if (DistanceMap[k][zzz]>threshold || DistanceMap[k][zzz]<0){
//std::cout<<"k:"<<k<<" zzz:"<<zzz<<" dmap"<<DistanceMap[k][zzz]<<" th:"<<threshold<<std::endl;
//break;
}
if (MatchedSourceForTile[k][zzz] == BestMatchingImage) {
flag = 1;
break;
}
}
if (flag) {
//where in the image is the patch (all patches are numbered linearly in 1D. Like in progressive scan in TV from left to right and to next line)
fn = PositionOfPatchesInImages[MatchedPatchInMatcher[k][zzz]];
BDMatchMap[fn].push_back(k);
}
}
}
if (mode==4){
//!!!!!!!!!!!!!!!!!!!!!!!
Mat matchedim = cv::imread(path, -1);
//!!!!!!!!!!!!!!!!!!
double nx, ny, mx, my;
double angle_rad=M_PI*angle/180;
//scalefactor=1.0;
int basex=bd_patch_size*bd_cols;
int basey=bd_patch_size*bd_rows;
//int imgx=patchsize*patches_cols;
//int imgy=patchsize*patches_rows;
int imgx=image_cols;
int imgy=image_rows;
//circle(matchedim, Point(basex/2.0,basey/2.0), 20, Scalar(0,255,0),-1);
//circle(image, Point(imgx/2.0,imgy/2.0), 20, Scalar(0,255,0),-1);
//float px=-100, py=-100;
//float px2=px*scalefactor;
//float py2=px*scalefactor;
//float qx=px2*cos(angle_rad)-py2*sin(angle_rad);
//float qy=px2*sin(angle_rad)+py2*cos(angle_rad);
//std::cout<<qx<<" "<<qy<<std::endl;
//circle(matchedim, Point(basex/2.0+px,basey/2.0+py), 10, Scalar(255,0,0),-1);
//circle(image, Point(basex/2.0+qx,basey/2.0+qy), 10, Scalar(255,0,0),-1);
for (int k = 0; k < patches_rows * patches_cols; k++) {
int flag = 0;
int zzz;
//check if there is match of current patch to the best image
for (zzz = 0; zzz < queue_size; zzz++) {
if (DistanceMap[k][zzz]>threshold || DistanceMap[k][zzz]<0.0){
//std::cout<<"k:"<<k<<" zzz:"<<zzz<<" dmap"<<DistanceMap[k][zzz]<<" th:"<<threshold<<std::endl;
break;
}
if (MatchedSourceForTile[k][zzz] == BestMatchingImage) {
flag = 1;
break;
}
}
if (flag) {
fn = PositionOfPatchesInImages[MatchedPatchInMatcher[k][zzz]];
//float p1x=base_keypoints[matches[i].queryIdx].pt.x-basex/2;
//float p1y=base_keypoints[matches[i].queryIdx].pt.y-basey/2;
//float p2x=keypoints[matches[i].trainIdx].pt.x-imgx/2;
//float p2y=keypoints[matches[i].trainIdx].pt.y-imgy/2;
//cout<<"loop i2: "<<i<<std::endl;
//float bigx=p1x*(cos(angle_rad)-sin(angle_rad));
//float bigy=p1y*(sin(angle_rad)+cos(angle_rad));
//float bigx=p1x*cos(angle_rad)-p1y*sin(angle_rad);
//float bigy=p1x*sin(angle_rad)+p1y*cos(angle_rad);
//cout<<"loop i3: "<<i<<std::endl;
//if (sqrt((bigx-p2x)*(bigx-p2x)+(bigy-p2y)*(bigy-p2y))<=threshold){
nx = bd_patch_size * (fn % bd_cols) + bd_patch_size * 0.5;
ny = bd_patch_size * (fn / bd_cols) + bd_patch_size * 0.5;
//nx*=scalefactor;
//ny*=scalefactor;
//which tile is matched
query_col = k % patches_cols;
query_row = k / patches_cols;
//location of the center
mx = patchsize * (query_col) + 0.5 * patchsize;
my = patchsize * (query_row) + 0.5 * patchsize;
float p1x=nx-basex/2.0;
float p1y=ny-basey/2.0;
float p2x=mx-imgx/2.0;
float p2y=my-imgy/2.0;
p1x*=scalefactor;
p1y*=scalefactor;
//circle(image, Point(mx,my), 10, Scalar(0,0,255), -1);
//circle(matchedim, Point(nx,ny), 10, Scalar(0,0,255),-1);
float bigx=p1x*cos(angle_rad)-p1y*sin(angle_rad);
float bigy=p1x*sin(angle_rad)+p1y*cos(angle_rad); //<<" basex"<<basex<<" basey"<<basey<<" imgx"<<imgx<<" imgy"<<imgy
//std::cout<<"scale:"<<scalefactor<<std::endl;
if (sqrt((bigx-p2x)*(bigx-p2x)+(bigy-p2y)*(bigy-p2y))<=(sqrt(2)*bd_patch_size*scalefactor/2+3)){
//where in the image is the patch (all patches are numbered linearly in 1D. Like in progressive scan in TV from left to right and to next line)
//std::cout<<"p1x:"<<p1x<<" p1y:"<<p1y<<" p2x:"<<p2x<<" p2y:"<<p2y<<" bx:"<<bigx<<" by:"<<bigy<<" scale:"<<scalefactor<<std::endl;
//std::cout<<sqrt((bigx-p2x)*(bigx-p2x)+(bigy-p2y)*(bigy-p2y))<<std::endl;
BDMatchMap[fn].push_back(k);
goodmatch_counter++;
}
else{
wrongmatch_counter++;
}
}
}
wrongmatches.push_back(wrongmatch_counter);
goodmatches.push_back(goodmatch_counter);
similarity.push_back(match_quality);
}
/*=======================================================================*/
/*************************************************************************/
/* simple correspondence drawing */
/*************************************************************************/
if (mode == 0) {
float nx, ny, mx, my;
Mat outimg;
//int query_col;
//int query_row;
std::vector<cv::KeyPoint> im1kp;
std::vector<cv::KeyPoint> im2kp;
std::vector<cv::DMatch> immatches;
int tempc = 0;
int cant = 0, cant2 = 0;
//read query image
Mat matchedim = cv::imread(path, -1);
double color;
for (int k = 0; k < patches_rows * patches_cols; k++) {
int flag = 0;
int zzz;
//check if there is match of current patch to the best image
for (zzz = 0; zzz < queue_size; zzz++) {
if (MatchedSourceForTile[k][zzz] == BestMatchingImage) {
flag = 1;
//cant++;
break;
}
}
if (flag) {
//where in the image is the patch (all patches are numbered linearly in 1D. Like in progressive scan in TV from left to right and to next line)
fn = PositionOfPatchesInImages[MatchedPatchInMatcher[k][zzz]];
//BDMatchMap[fn] = k;
//error?
if (fn < 0) {
return;
}
//location of the center tile in the image#include <time.h>
nx = bd_patch_size * (fn % bd_cols) + bd_patch_size * 0.5;
ny = bd_patch_size * (fn / bd_cols) + bd_patch_size * 0.5;
//which tile is matched
query_col = k % patches_cols;
query_row = k / patches_cols;
//location of the center
mx = patchsize * (query_col) + 0.5 * patchsize;
my = patchsize * (query_row) + 0.5 * patchsize;
//Is it not a bad match?
if (DistanceMap[k][0] < 0 || DistanceMap[k][0] > 0.3) {
cant2++;
continue;
}
//choose colour
//color = (1 - 10 * DistanceMap[k][0] * DistanceMap[k][0]) * 255.0;
int cb = 0, cg = 0, cr = 255;
int qLineW = 2, bdLineW = 2;
//cb=rand() % 255;
//cg=rand() % 255;
//cr=rand() % 255;
//draw patches in query img
rectangle(image, Point(mx - 0.5 * patchsize, my - 0.5
* patchsize), Point(mx + 0.5 * patchsize, my + 0.5
* patchsize), Scalar(cb, cg, cr), qLineW);
//draw patches in BD img
rectangle(matchedim, Point(nx - 0.5 * bd_patch_size, ny - 0.5
* bd_patch_size), Point(nx + 0.5 * bd_patch_size, ny
+ 0.5 * bd_patch_size), Scalar(cb, cg, cr), bdLineW);
//each tile center as a KP
im1kp.push_back(KeyPoint(Point2f(nx, ny), 5.0));
im2kp.push_back(KeyPoint(Point2f(mx, my), 5.0));
std::ostringstream q;
q.precision(2);
q << QueryMatchMap[k];
//putText(image, q.str(), cvPoint(mx-0.25*patchsize,my-0.125*patchsize), FONT_HERSHEY_SIMPLEX, 0.5, cvScalar(0,0,0), 1.5, CV_AA);
//and now set correspondence
immatches.push_back(DMatch(tempc, tempc, 0.0));
tempc++;
}
}
drawMatches(image, im2kp, matchedim, im1kp, immatches, outimg,
Scalar::all(-1));
out_image.write(outimg);
}
/*=======================================================================*/
/*************************************************************************/
/* corresponding tiles marked with the same color */
/*************************************************************************/
if (mode == 3 || mode==4) {
std::cout << "mode:" << mode << " s" << BDMatchMap.size() << std::endl;
//!!!!!!!!!!!!!!!!!!!
//!!!!!!!!!!!!!!!!
//int counter=0;
Mat matchedim = cv::imread(path, -1);
float nx, ny, mx, my;
Mat outimg;
std::vector<cv::KeyPoint> im1kp;
std::vector<cv::KeyPoint> im2kp;
std::vector<cv::DMatch> immatches;
int k;
for (int m = 0; m < BDMatchMap.size(); m++) {
int cb = 0, cg = 0, cr = 255;
int qLineW = -1, bdLineW = -1;
cb = rand() % 255;
cg = rand() % 255;
cr = rand() % 255;
if (BDMatchMap[m].size() > 0) {
for (int n = 0; n < BDMatchMap[m].size(); n++) {
k = BDMatchMap[m][n];
fn = n;
query_col = k % patches_cols;
query_row = k / patches_cols;
//std::cout<<query_col<<" "<<query_row<<" "<<k<<std::endl;
//location of the center
mx = patchsize * (query_col) + 0.5 * patchsize;
my = patchsize * (query_row) + 0.5 * patchsize;
//Is it not a bad match?
if (DistanceMap[k][0] < 0.0 || DistanceMap[k][0] > threshold) {
continue;
}
//counter++;
//draw patches in query img
rectangle(image, Point(mx - 0.5 * patchsize, my - 0.5
* patchsize), Point(mx + 0.5 * patchsize, my + 0.5
* patchsize), Scalar(cb, cg, cr), qLineW);
std::ostringstream q;
q.precision(2);
q << QueryMatchMap[k];
putText(image, q.str(), cvPoint(mx-0.25*patchsize,my-0.125*patchsize), FONT_HERSHEY_SIMPLEX, 0.3, cvScalar(0,0,0), 1.5, CV_AA);
}
nx = bd_patch_size * (m % bd_cols) + bd_patch_size * 0.5;
ny = bd_patch_size * (m / bd_cols) + bd_patch_size * 0.5;
//draw patches in BD img
rectangle(matchedim, Point(nx - 0.5 * bd_patch_size, ny - 0.5
* bd_patch_size), Point(nx + 0.5 * bd_patch_size, ny
+ 0.5 * bd_patch_size), Scalar(cb, cg, cr), bdLineW);
}
}
drawMatches(image, im2kp, matchedim, im1kp, immatches, outimg,
Scalar::all(-1));
out_image.write(outimg);
//goodmatches_q.push_back(counter);
}
/*=======================================================================*/
/*************************************************************************/
/* experimental mode to reconstruct image from matched patches */
/* possible only when patches have the same size */
/*************************************************************************/
//
if ((mode == 1 && patchsize == bd_patch_size) || mode == 2) {
int nx, ny, mx, my;
//int fn;
//int query_col;
//int query_row;
int ax, bx, ay, by;
int fx;
int r, g, b;
Mat outimage(image_rows, image_cols, CV_8UC3,
Scalar(0.0, 0.0, 0.0, 1.0));
int counter = 0;
int zzz = 0, flag = 0;
//scan through all the files
for (int l = 0; l < files_number; l++) {
Mat matchedim = cv::imread(all_file_paths[l], -1);
unsigned char *input = (unsigned char*) (matchedim.data);
for (int k = 0; k < patches_rows * patches_cols; k++) {
//matches only to the best image
if (mode == 1) {
flag = 0;
//find if an image has some corresponging match on the list
for (zzz = 0; zzz < queue_size; zzz++) {
if (MatchedSourceForTile[k][zzz] == BestMatchingImage) {
flag = 1;
break;
}
}
}
//matches to all img in the DB
if (flag || mode == 2) {
query_col = k % patches_cols;
query_row = k / patches_cols;
mx = patchsize * (query_col);
my = patchsize * (query_row);
fn
= PositionOfPatchesInImages[MatchedPatchInMatcher[k][zzz]];
nx = bd_patch_size * (fn % bd_cols);
ny = bd_patch_size * (fn / bd_cols);
//if the tile is matched to the current image l
if (MatchedSourceForTile[k][zzz] == l) {
//then copy image pixel-by-pixel
for (int ax = 0; ax < patchsize; ax++) {
for (int ay = 0; ay < patchsize; ay++) {
//outimage[mx+ax][mx+ay]=matchedim[nx+ax][nx+ay];
//if ((ax+nx)>512||(ay+ny))
b = matchedim.data[matchedim.step[0]
* (ay + ny) + matchedim.step[1] * (ax
+ nx)];
g = matchedim.data[matchedim.step[0]
* (ay + ny) + matchedim.step[1] * (ax
+ nx) + 1];
r = matchedim.data[matchedim.step[0]
* (ay + ny) + matchedim.step[1] * (ax
+ nx) + 2];
outimage.data[outimage.step[0] * (my + ay)
+ outimage.step[1] * (mx + ax) + 0] = b;
outimage.data[outimage.step[0] * (my + ay)
+ outimage.step[1] * (mx + ax) + 1] = g;
outimage.data[outimage.step[0] * (my + ay)
+ outimage.step[1] * (mx + ax) + 2] = r;
}
}
}
}
}
}
std::cout << "done" << std::endl;
out_image.write(outimage);
}
/*************************************************************************/
std::cout << "raise" << std::endl;
matched->raise();
}
void TestCube::setMatches(int num, vector<DMatch> &matches) {
for (int i = 0; i < num; i++) {
matches.push_back(DMatch(i, i, 0));
}
}
int test_multitypestorage()
{
// init some variables:
Mat a(10,10,CV_32F, Scalar(0.3));
Mat b(20,20,CV_32F, Scalar(1.));
Mat c(10,10,CV_32FC3, Scalar(120.));
double v(9.);
double w(19.);
vector<KeyPoint> kp, kp1;
kp.push_back(KeyPoint(1.0,-1.0,20));
kp.push_back(KeyPoint(2.0,21.0,10));
kp1.push_back(KeyPoint(11.0,-1.0,20));
kp1.push_back(KeyPoint(2.0,1.0,10));
vector<vector<DMatch> > m;
vector<DMatch> _m, _n;
_m.push_back(DMatch(1,2,10.));
_m.push_back(DMatch(3,4,4.));
_n.push_back(DMatch(6,2,1.));
_n.push_back(DMatch(5,4,41.));
m.push_back(_m);
m.push_back(_n);
// 0-test:
//cerr << "test_compare(1,2) = " << test_compare(1,2) << " = 0 (false) " << endl;
//cerr << "test_compare(1.,1.) = " << test_compare(1.,1.) << " = 1 (true) " << endl;
//cerr << "test_compare(a,a) = " << test_compare(a,a) << " = 1 " << endl;
//cerr << "test_compare(a,b) = " << test_compare(a,b) << " = 0 "<< endl;
//cerr << "test_compare(c,c) = " << test_compare(c,c) << " = 1 "<< endl;
//cerr << "test_compare(a,c) = " << test_compare(a,c) << " = 0 "<< endl;
//cerr << "test_compare(v,v) = " << test_compare(v,v) << " = 1 "<< endl;
//cerr << "test_compare(v,w) = " << test_compare(v,w) << " = 0 "<< endl;
//cerr << "test_compare(kp,kp) = " << test_compare<KeyPoint>(kp,kp) << " = 1 "<< endl;
//cerr << "test_compare(kp,kp1) = " << test_compare<KeyPoint>(kp,kp1) << " = 0 "<< endl;
//cerr << "test_compare(m,m) = " << test_compare<vector<DMatch> >(m,m) << " = 1 "<< endl;
//cerr << "test_compare(_m,_n) = " << test_compare<DMatch>(_m,_n) << " = 0 "<< endl;
// init MTStorage:
MultiTypeStorage storage;
// add elements:
storage.setElement<Mat>("a", a);
storage.setElement<Mat>("b", b);
storage.setElement<Mat>("c", c);
storage.setElement<double>("v", v);
storage.setElement<double>("w", w);
storage.setElement<vector<KeyPoint> >("kp", kp);
storage.setElement<vector<vector<DMatch> > >("m", m);
// get elements:
Mat A;
if (storage.getElement<Mat>("a") != NULL)
A = *(storage.getElement<Mat>("a"));
else{
cerr << "a is not found !" << endl;
return -1;
}
Mat B;
if (storage.getElement<Mat>("b")!=NULL)
B = *(storage.getElement<Mat>("b"));
else{
cerr << "b is not found !" << endl;
return -1;
}
Mat C = *(storage.getElement<Mat>("c"));
double V = *(storage.getElement<double>("v"));
double W = *(storage.getElement<double>("w"));
vector<KeyPoint> KP = *(storage.getElement<vector<KeyPoint> >("kp"));
vector<vector<DMatch> > M = *(storage.getElement<vector<vector<DMatch> > >("m"));
// compare:
int s1 = test_compare(A,a) + test_compare(B,b) + test_compare(C,c) + test_compare(V,v) + test_compare(W,w) + test_compare(KP,kp) + test_compare(M,m);
if (s1 == 7)
cerr << "test OK" << endl;
else
cerr << "test failed" << endl;
}
Mat Tracker::process(const Mat frame, Stats& stats)
{
TickMeter tm;
vector<KeyPoint> kp;
Mat desc;
tm.start();
detector->detectAndCompute(frame, noArray(), kp, desc);
stats.keypoints = (int)kp.size();
vector< vector<DMatch> > matches;
vector<KeyPoint> matched1, matched2;
matcher->knnMatch(first_desc, desc, matches, 2);
for(unsigned i = 0; i < matches.size(); i++) {
if(matches[i][0].distance < nn_match_ratio * matches[i][1].distance) {
matched1.push_back(first_kp[matches[i][0].queryIdx]);
matched2.push_back( kp[matches[i][0].trainIdx]);
}
}
stats.matches = (int)matched1.size();
Mat inlier_mask, homography;
vector<KeyPoint> inliers1, inliers2;
vector<DMatch> inlier_matches;
if(matched1.size() >= 4) {
homography = findHomography(Points(matched1), Points(matched2),
RANSAC, ransac_thresh, inlier_mask);
}
tm.stop();
stats.fps = 1. / tm.getTimeSec();
if(matched1.size() < 4 || homography.empty()) {
Mat res;
hconcat(first_frame, frame, res);
stats.inliers = 0;
stats.ratio = 0;
return res;
}
for(unsigned i = 0; i < matched1.size(); i++) {
if(inlier_mask.at<uchar>(i)) {
int new_i = static_cast<int>(inliers1.size());
inliers1.push_back(matched1[i]);
inliers2.push_back(matched2[i]);
inlier_matches.push_back(DMatch(new_i, new_i, 0));
}
}
stats.inliers = (int)inliers1.size();
stats.ratio = stats.inliers * 1.0 / stats.matches;
vector<Point2f> new_bb;
perspectiveTransform(object_bb, new_bb, homography);
Mat frame_with_bb = frame.clone();
if(stats.inliers >= bb_min_inliers) {
drawBoundingBox(frame_with_bb, new_bb);
}
Mat res;
drawMatches(first_frame, inliers1, frame_with_bb, inliers2,
inlier_matches, res,
Scalar(255, 0, 0), Scalar(255, 0, 0));
return res;
}
