void FeatureMatchThread::run()
{
Mat resultImg;
Mat grayImg;
cvtColor(cropImg,grayImg,CV_BGR2GRAY);
featureDetector.detect(grayImg,keyPoints);
featureExtractor.compute(grayImg,keyPoints,descriptors);
flannIndex.build(descriptors,flann::LshIndexParams(12,20,2),cvflann::FLANN_DIST_HAMMING);
while(true)
{
Mat captureImage_gray;
vector<KeyPoint> captureKeyPoints;
Mat captureDescription;
vector<DMatch> goodMatches;
cap >> captureImage;
if(captureImage.empty())
continue;
cvtColor(captureImage,captureImage_gray,CV_BGR2GRAY);
featureDetector.detect(captureImage_gray,captureKeyPoints);
featureExtractor.compute(captureImage_gray,captureKeyPoints,captureDescription);
Mat matchIndex(captureDescription.rows,2,CV_32SC1);
Mat matchDistance(captureDescription.rows,2,CV_32FC1);
flannIndex.knnSearch(captureDescription,matchIndex,matchDistance,2,flann::SearchParams());
for(int i=0;i<matchDistance.rows;i++)
{
if(matchDistance.at<float>(i,0) < 0.6 * matchDistance.at<float>(i,1))
{
DMatch dmatches(i,matchIndex.at<int>(i,0),matchDistance.at<float>(i,0));
goodMatches.push_back(dmatches);
}
}
drawMatches(captureImage,captureKeyPoints,cropImg,keyPoints,goodMatches,resultImg);
emit NewFeatureMatch(&resultImg);
imshow(WindowName,captureImage);
cv::setMouseCallback(WindowName,mouse_callback);
captureKeyPoints.clear();
goodMatches.clear();
waitKey(30);
}
return;
}
void mouse_callback(int event,int x,int y,int flags, void* param)
{
switch(event)
{
case EVENT_MOUSEMOVE:
{
if(drawing_box)
{
box.width = x-box.x;
box.height = y-box.y;
Mat tmp = captureImage.clone();
cv::rectangle(tmp,Point(box.x,box.y),Point(box.x + box.width , box.y + box.height) , Scalar(0,255,0) , 3);
imshow(WindowName,tmp);
}
}
break;
case EVENT_LBUTTONDOWN:
{
drawing_box = true;
box = Rect(x,y,0,0);
}
break;
case EVENT_LBUTTONUP:
{
drawing_box = false;
if(box.width < 0)
{
box.x += box.width;
box.width *= -1;
}
if(box.height < 0)
{
box.y += box.height;
box.height *= -1;
}
//Â^¨ú¹Ï¹³
Mat copyCapture = captureImage.clone();
Mat tmp(copyCapture,box);
Mat grayImg;
cropImg = tmp;
cvtColor(cropImg,grayImg,CV_BGR2GRAY);
featureDetector.detect(grayImg,keyPoints);
featureExtractor.compute(grayImg,keyPoints,descriptors);
flannIndex.build(descriptors,flann::LshIndexParams(12,20,2),cvflann::FLANN_DIST_HAMMING);
}
break;
}
return;
}
int main(int argc, char** argv)
{
if( argc < 2 )
{
printPrompt( argv[0] );
return -1;
}
initModule_nonfree();
// Get Input Data
ifstream file(argv[1]);
if ( !file.is_open() )
return false;
string str;
// Image Name
getline( file, str ); getline( file, str );
string image_name = str;
// Cloud Name
getline( file, str ); getline( file, str );
string cloud_name = str;
// width of images to be created.
getline( file, str ); getline( file, str );
int w = atoi(str.c_str());
// height of images to be created
getline( file, str ); getline( file, str );
int h = atoi(str.c_str());
// resolution of voxel grids
getline( file, str ); getline( file, str );
float r = atof(str.c_str());
// f (distance from pinhole)
getline( file, str ); getline( file, str );
float f = atof(str.c_str());
// thetax (initial rotation about X Axis of map)
getline( file, str ); getline( file, str );
float thetaX = atof(str.c_str());
// thetay (initial rotation about Y Axis of map)
getline( file, str ); getline( file, str );
float thetaY = atof(str.c_str());
// number of points to go to
getline( file, str ); getline( file, str );
float nop = atoi(str.c_str());
// Number of divisions
getline( file, str ); getline( file, str );
float divs = atoi(str.c_str());
// Number of images to return
getline( file, str ); getline( file, str );
int numtoreturn = atoi(str.c_str());
// Should we load or create photos?
getline( file, str ); getline( file, str );
string lorc =str.c_str();
// Directory to look for photos
getline( file, str ); getline( file, str );
string dir =str.c_str();
// Directory to look for kp and descriptors
getline( file, str ); getline( file, str );
string kdir =str.c_str();
// save photos?
getline( file, str ); getline( file, str );
string savePhotos =str.c_str();
file.close();
// Done Getting Input Data
map<vector<float>, Mat> imagemap;
map<vector<float>, Mat> surfmap;
map<vector<float>, Mat> siftmap;
map<vector<float>, Mat> orbmap;
map<vector<float>, Mat> fastmap;
imagemap.clear();
vector<KeyPoint> SurfKeypoints;
vector<KeyPoint> SiftKeypoints;
vector<KeyPoint> OrbKeypoints;
vector<KeyPoint> FastKeypoints;
Mat SurfDescriptors;
Mat SiftDescriptors;
Mat OrbDescriptors;
Mat FastDescriptors;
int minHessian = 300;
SurfFeatureDetector SurfDetector (minHessian);
SiftFeatureDetector SiftDetector (minHessian);
OrbFeatureDetector OrbDetector (minHessian);
FastFeatureDetector FastDetector (minHessian);
SurfDescriptorExtractor SurfExtractor;
SiftDescriptorExtractor SiftExtractor;
OrbDescriptorExtractor OrbExtractor;
if ( !fs::exists( dir ) || lorc == "c" )
{ // Load Point Cloud and render images
PointCloud<PT>::Ptr cloud (new pcl::PointCloud<PT>);
io::loadPCDFile<PT>(cloud_name, *cloud);
Eigen::Affine3f tf = Eigen::Affine3f::Identity();
tf.rotate (Eigen::AngleAxisf (thetaX, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud (*cloud, *cloud, tf);
tf = Eigen::Affine3f::Identity();
tf.rotate (Eigen::AngleAxisf (thetaY, Eigen::Vector3f::UnitY()));
pcl::transformPointCloud (*cloud, *cloud, tf);
// Create images from point cloud
imagemap = render::createImages(cloud, nop, w, h, r, f);
if (savePhotos == "y")
{
for (map<vector<float>, Mat>::iterator i = imagemap.begin(); i != imagemap.end(); ++i)
{
// Create image name and storagename
string imfn = dir + "/";
string kpfn = kdir + "/";
for (int j = 0; j < i->first.size(); j++)
{
imfn += boost::to_string(i->first[j]) + " ";
kpfn += boost::to_string(i->first[j]) + " ";
}
imfn += ".jpg";
imwrite(imfn, i->second);
// Detect keypoints, add to keypoint map. Same with descriptors
SurfDetector.detect(i->second, SurfKeypoints);
SiftDetector.detect(i->second, SiftKeypoints);
OrbDetector.detect(i->second, OrbKeypoints);
FastDetector.detect(i->second, FastKeypoints);
SurfExtractor.compute(i->second, SurfKeypoints, SurfDescriptors);
SiftExtractor.compute(i->second, SiftKeypoints, SiftDescriptors);
OrbExtractor.compute(i->second, OrbKeypoints, OrbDescriptors);
SiftExtractor.compute(i->second, FastKeypoints, FastDescriptors);
// Store KP and Descriptors in yaml file.
kpfn += ".yml";
FileStorage store(kpfn, cv::FileStorage::WRITE);
write(store,"SurfKeypoints",SurfKeypoints);
write(store,"SiftKeypoints",SiftKeypoints);
write(store,"OrbKeypoints", OrbKeypoints);
write(store,"FastKeypoints",FastKeypoints);
write(store,"SurfDescriptors",SurfDescriptors);
write(store,"SiftDescriptors",SiftDescriptors);
write(store,"OrbDescriptors", OrbDescriptors);
write(store,"FastDescriptors",FastDescriptors);
store.release();
surfmap[i->first] = SurfDescriptors;
siftmap[i->first] = SiftDescriptors;
orbmap[i->first] = OrbDescriptors;
fastmap[i->first] = FastDescriptors;
}
}
}
else
{ // load images from the folder dir
// First look into the folder to get a list of filenames
vector<fs::path> ret;
const char * pstr = dir.c_str();
fs::path p(pstr);
get_all(pstr, ret);
for (int i = 0; i < ret.size(); i++)
{
// Load Image via filename
string fn = ret[i].string();
istringstream iss(fn);
vector<string> tokens;
copy(istream_iterator<string>(iss), istream_iterator<string>(), back_inserter<vector<string> >(tokens));
// Construct ID from filename
vector<float> ID;
for (int i = 0; i < 6; i++) // 6 because there are three location floats and three direction floats
ID.push_back(::atof(tokens[i].c_str()));
string imfn = dir + "/" + fn;
// Read image and add to imagemap.
Mat m = imread(imfn);
imagemap[ID] = m;
// Create Filename for loading Keypoints and descriptors
string kpfn = kdir + "/";
for (int j = 0; j < ID.size(); j++)
{
kpfn += boost::to_string(ID[j]) + " ";
}
kpfn = kpfn+ ".yml";
// Create filestorage item to read from and add to map.
FileStorage store(kpfn, cv::FileStorage::READ);
FileNode n1 = store["SurfKeypoints"];
read(n1,SurfKeypoints);
FileNode n2 = store["SiftKeypoints"];
read(n2,SiftKeypoints);
FileNode n3 = store["OrbKeypoints"];
read(n3,OrbKeypoints);
FileNode n4 = store["FastKeypoints"];
read(n4,FastKeypoints);
FileNode n5 = store["SurfDescriptors"];
read(n5,SurfDescriptors);
FileNode n6 = store["SiftDescriptors"];
read(n6,SiftDescriptors);
FileNode n7 = store["OrbDescriptors"];
read(n7,OrbDescriptors);
FileNode n8 = store["FastDescriptors"];
read(n8,FastDescriptors);
store.release();
surfmap[ID] = SurfDescriptors;
siftmap[ID] = SiftDescriptors;
orbmap[ID] = OrbDescriptors;
fastmap[ID] = FastDescriptors;
}
}
TickMeter tm;
tm.reset();
cout << "<\n Analyzing Images ..." << endl;
// We have a bunch of images, now we compute their grayscale and black and white.
map<vector<float>, Mat> gsmap;
map<vector<float>, Mat> bwmap;
for (map<vector<float>, Mat>::iterator i = imagemap.begin(); i != imagemap.end(); ++i)
{
vector<float> ID = i->first;
Mat Image = i-> second;
GaussianBlur( Image, Image, Size(5,5), 0, 0, BORDER_DEFAULT );
gsmap[ID] = averageImage::getPixSumFromImage(Image, divs);
bwmap[ID] = averageImage::aboveBelow(gsmap[ID]);
}
Mat image = imread(image_name);
Mat gsimage = averageImage::getPixSumFromImage(image, divs);
Mat bwimage = averageImage::aboveBelow(gsimage);
// cout << gsimage <<endl;
imwrite("GS.png", gsimage);
namedWindow("GSIMAGE (Line 319)");
imshow("GSIMAGE (Line 319)", gsimage);
waitKey(0);
vector<KeyPoint> imgSurfKeypoints;
vector<KeyPoint> imgSiftKeypoints;
vector<KeyPoint> imgOrbKeypoints;
vector<KeyPoint> imgFastKeypoints;
Mat imgSurfDescriptors;
Mat imgSiftDescriptors;
Mat imgOrbDescriptors;
Mat imgFastDescriptors;
SurfDetector.detect(image, imgSurfKeypoints);
SiftDetector.detect(image, imgSiftKeypoints);
OrbDetector.detect(image, imgOrbKeypoints);
FastDetector.detect(image, imgFastKeypoints);
SurfExtractor.compute(image, imgSurfKeypoints, imgSurfDescriptors);
SiftExtractor.compute(image, imgSiftKeypoints, imgSiftDescriptors);
OrbExtractor.compute(image, imgOrbKeypoints, imgOrbDescriptors);
SiftExtractor.compute(image, imgFastKeypoints, imgFastDescriptors);
tm.start();
cout << ">\n<\n Comparing Images ..." << endl;
// We have their features, now compare them!
map<vector<float>, float> gssim; // Gray Scale Similarity
map<vector<float>, float> bwsim; // Above Below Similarity
map<vector<float>, float> surfsim;
map<vector<float>, float> siftsim;
map<vector<float>, float> orbsim;
map<vector<float>, float> fastsim;
for (map<vector<float>, Mat>::iterator i = gsmap.begin(); i != gsmap.end(); ++i)
{
vector<float> ID = i->first;
gssim[ID] = similarities::getSimilarity(i->second, gsimage);
bwsim[ID] = similarities::getSimilarity(bwmap[ID], bwimage);
surfsim[ID] = similarities::compareDescriptors(surfmap[ID], imgSurfDescriptors);
siftsim[ID] = similarities::compareDescriptors(siftmap[ID], imgSiftDescriptors);
orbsim[ID] = 0;//similarities::compareDescriptors(orbmap[ID], imgOrbDescriptors);
fastsim[ID] = 0;//similarities::compareDescriptors(fastmap[ID], imgFastDescriptors);
}
map<vector<float>, int> top;
bool gotone = false;
typedef map<vector<float>, int>::iterator iter;
// Choose the best ones!
for (map<vector<float>, Mat>::iterator i = imagemap.begin(); i != imagemap.end(); ++i)
{
vector<float> ID = i->first;
int sim = /* gssim[ID] + 0.5*bwsim[ID] + */ 5*surfsim[ID] + 0.3*siftsim[ID] + orbsim[ID] + fastsim[ID];
// cout << surfsim[ID] << "\t";
// cout << siftsim[ID] << "\t";
// cout << orbsim[ID] << "\t";
// cout << fastsim[ID] << endl;
if (!gotone)
{
top[ID] = sim;
gotone = true;
}
iter it = top.begin();
iter end = top.end();
int max_value = it->second;
vector<float> max_ID = it->first;
for( ; it != end; ++it)
{
int current = it->second;
if(current > max_value)
{
max_value = it->second;
max_ID = it->first;
}
}
// cout << "Sim: " << sim << "\tmax_value: " << max_value << endl;
if (top.size() < numtoreturn)
top[ID] = sim;
else
{
if (sim < max_value)
{
top[ID] = sim;
top.erase(max_ID);
}
}
}
tm.stop();
double s = tm.getTimeSec();
cout << ">\n<\n Writing top " << numtoreturn << " images ..." << endl;
int count = 1;
namedWindow("Image");
namedWindow("Match");
namedWindow("ImageBW");
namedWindow("MatchBW");
namedWindow("ImageGS");
namedWindow("MatchGS");
imshow("Image", image);
imshow("ImageBW", bwimage);
imshow("ImageGS", gsimage);
vector<KeyPoint> currentPoints;
for (iter i = top.begin(); i != top.end(); ++i)
{
vector<float> ID = i->first;
cout << " Score: "<< i->second << "\tGrayScale: " << gssim[ID] << "\tBW: " << bwsim[ID] << " \tSURF: " << surfsim[ID] << "\tSIFT: " << siftsim[ID] << endl;
string fn = "Sim_" + boost::to_string(count) + "_" + boost::to_string(i->second) + ".png";
imwrite(fn, imagemap[ID]);
count++;
normalize(bwmap[ID], bwmap[ID], 0, 255, NORM_MINMAX, CV_64F);
normalize(gsmap[ID], gsmap[ID], 0, 255, NORM_MINMAX, CV_64F);
imshow("Match", imagemap[ID]);
imshow("MatchBW", bwmap[ID]);
imshow("MatchGS", gsmap[ID]);
waitKey(0);
}
cout << ">\nComparisons took " << s << " seconds for " << imagemap.size() << " images ("
<< (int) imagemap.size()/s << " images per second)." << endl;
return 0;
}
int main() {
initModule_features2d();
bool die(false);
int iter(0);
int i_snap(0);
string filename("snapshot");
string suffix(".png");
Freenect::Freenect freenect;
vector<KeyPoint> Keypoints;
MyFreenectDevice& device = freenect.createDevice<MyFreenectDevice>(0);
OrbFeatureDetector detector;
Mat depthMat(Size(640, 480), CV_16UC1);
Mat bgrMat(Size(640, 480), CV_8UC3, Scalar(0));
Mat depthf(Size(640, 480), CV_8UC1);
Mat ownMat(Size(640, 480), CV_8UC3, Scalar(0));
Mat inFrame(Size(640, 480), CV_8UC3, Scalar(0));
Mat outFrame(Size(640, 480), CV_8UC3, Scalar(0));
Mat postProc(Size(640, 480), CV_8UC3, Scalar(0));
Mat hsvMat(Size(640, 480), CV_8UC3, Scalar(0));
Mat rgbMat(Size(640, 480), CV_8UC3, Scalar(0));
Mat threshMat(Size(640, 480), CV_8U, Scalar(0));
Mat bgrThresh(Size(640, 480), CV_8UC1, Scalar(0));
Mat greyMat(Size(640, 480), CV_8UC1, Scalar(0));
//HSV
//Scalar min(0, 32, 0);
//Scalar max(30, 255, 255);
//RGB
//Scalar min(101, 63, 21);
//Scalar max(153, 75, 33);
//BGR
//Scalar min(0, 0, 0);
//Scalar max(255, 0, 0);
namedWindow("Output", CV_WINDOW_AUTOSIZE);
namedWindow("depth", CV_WINDOW_AUTOSIZE);
namedWindow("Post", CV_WINDOW_AUTOSIZE);
device.startVideo();
device.startDepth();
while (die == false) {
device.getVideo(bgrMat);
device.getDepth(depthMat);
cv::imshow("Output", bgrMat);
cvtColor(bgrMat, hsvMat, CV_BGR2HSV);
inRange(hsvMat, Scalar(100, 75, 35), Scalar(140, 255, 255), threshMat);
morphOps(threshMat);
adaptiveThreshold( threshMat, bgrThresh, 255, CV_ADAPTIVE_THRESH_MEAN_C, CV_THRESH_BINARY, 3, 1);
detector.detect(bgrThresh, Keypoints);
drawKeypoints(bgrMat, Keypoints, postProc, Scalar(0, 0, 255)/*, DrawMatchesFlags::DRAW_RICH_KEYPOINTS*/);
cv::imshow("Post", postProc);
//FOR DEBUGGING
//cv::imshow("Post", bgrThresh);
//cv::imshow("Post", threshMat);
//depthMat.convertTo(depthf, CV_8UC1, 255.0 / 2048.0);
//cv::imshow("depth", depthf);
char k = cvWaitKey(5);
if (k == 27) {
cvDestroyWindow("rgb");
cvDestroyWindow("depth");
cvDestroyWindow("Post");
break;
}
if (k == 8) {
std::ostringstream file;
file << filename << i_snap << suffix;
cv::imwrite(file.str(), bgrMat);
i_snap++;
}
if (iter >= 10000)
break;
iter++;
}
device.stopVideo();
device.stopDepth();
return 0;
}
int main(int argc, char** argv)
{
int flag_use_image = 0;
if( argc != 2 )
{
std::cout<< "Usage: ./init num" << std::endl;
std::cout<< "num: 0 - image" << std::endl
<< " 1 - video" << std::endl
<< " 2 - dataset" << std::endl;
return -1;
}
else
{
std::string val = argv[1];
if(val == "0")
{
}
else if(val == "1")
{
flag_use_image = 1;
}
else if(val == "2")
{
flag_use_image = 2;
}
else
{
std::cout<< "num error" << std::endl;
}
}
std::string winName = "Image";
namedWindow(winName, WINDOW_NORMAL);
mat_canvas = imread( "data/book.jpg");
if(mat_canvas.data == NULL)
{
std::cout<< "Image is not opened." << std::endl;
return -1;
}
if(flag_use_image == 0)
{
setMouseCallback(winName, mouseEvent);
// // write mat to file
// std::string fileName = "mat_descriptors.yml";
// FileStorage fs(fileName, FileStorage::WRITE);
// fs << "descriptors" << mat_descriptors;
// fs.release();
// std::cout<< fileName << " is generated." << std::endl;
// Mat copy;
// FileStorage fs2("mat_descriptors.yml", FileStorage::READ);
// fs2["descriptors"] >> copy;
// fs2.release();
// FileStorage fs3("test.yml", FileStorage::WRITE);
// fs3 << "descriptors" << copy;
// fs3.release();
//////////////////////////////////////////////////////////
// std::vector<cv::Point3f> vec_pois;
// vec_pois.push_back(Point3f(0, 0, 0));
// vec_pois.push_back(Point3f(1.1, 0.1, 0));
// vec_pois.push_back(Point3f(0.3, 2.1, 0));
// vec_pois.push_back(Point3f(7.3, 2, 0));
// vec_pois.push_back(Point3f(1.3, 4.1, 0));
// FileStorage fs3("POIs.yml", FileStorage::WRITE);
// fs3 << "POIs" << vec_pois;
// fs3.release();
//////////////////////////////////////////////////////////
while(1)
{
imshow(winName, mat_canvas );
waitKey(30);
}
}
//-- use dataset
else if(flag_use_image == 2)
{
useDataset();
while(1)
{
imshow(winName, mat_canvas );
waitKey(30);
}
}
else // video input: tracking features
{
VideoCapture cap;
cap.open(1);
if(!cap.isOpened()) // check if we succeeded
return -1;
cap.set(CV_CAP_PROP_FRAME_WIDTH, 800);
cap.set(CV_CAP_PROP_FRAME_HEIGHT, 600);
namedWindow("Keypoints", WINDOW_NORMAL);
Mat mat_image;
int num_vecKeypoints;
int num_trackingPoints = 50;
Mat mat_descriptors;
char keyInput;
//-- Step 1: Detect the keypoints using Detector
// int minHessian = 400;
OrbFeatureDetector detector;
FREAK extractor;
while(1)
{
cap >> mat_image;
std::vector<KeyPoint> vec_keypoints, vec_goodKeypoints;
detector.detect( mat_image, vec_keypoints );
num_vecKeypoints = vec_keypoints.size();
std::sort(vec_keypoints.begin(), vec_keypoints.end(),
jlUtilities::sort_feature_response);
if(num_vecKeypoints > num_trackingPoints)
{
num_vecKeypoints = num_trackingPoints;
vec_keypoints.erase(vec_keypoints.begin() + num_vecKeypoints,
vec_keypoints.end());
}
extractor.compute( mat_image, vec_keypoints, mat_descriptors );
// write mat to file
std::string fileName = "mat_descriptors.yml";
FileStorage fs(fileName, FileStorage::WRITE);
fs << "descriptors" << mat_descriptors;
fs.release();
std::cout<< fileName << " is generated." << std::endl;
// Mat copy;
// FileStorage fs2("mat_descriptors.yml", FileStorage::READ);
// fs2["descriptors"] >> copy;
// fs2.release();
// FileStorage fs3("test.yml", FileStorage::WRITE);
// fs3 << "descriptors" << copy;
// fs3.release();
//////////////////////////////////////////////////////////
// std::vector<cv::Point3f> vec_pois;
// vec_pois.push_back(Point3f(0, 0, 0));
// vec_pois.push_back(Point3f(1.1, 0.1, 0));
// vec_pois.push_back(Point3f(0.3, 2.1, 0));
// vec_pois.push_back(Point3f(7.3, 2, 0));
// vec_pois.push_back(Point3f(1.3, 4.1, 0));
// FileStorage fs3("POIs.yml", FileStorage::WRITE);
// fs3 << "POIs" << vec_pois;
// fs3.release();
//////////////////////////////////////////////////////////
//-- Draw keypoints
Mat mat_kpImage;
drawKeypoints( mat_image, vec_keypoints, mat_kpImage,
Scalar::all(-1), DrawMatchesFlags::DEFAULT );
for (int i=0; i<num_trackingPoints; i++) {
cv::circle(mat_kpImage,
vec_keypoints[i].pt, // center
3, // radius
cv::Scalar(0,0,255), // color
-1); // negative thickness=filled
char szLabel[50];
sprintf(szLabel, "%d", i);
putText (mat_kpImage, szLabel, vec_keypoints[i].pt,
cv::FONT_HERSHEY_PLAIN, // font face
1.0, // font scale
cv::Scalar(255,0,0), // font color
1); // thickness
}
//-- Show detected (drawn) keypoints
imshow("Keypoints", mat_kpImage );
waitKey(30);
}
}
return 0;
}
StitchedMap::StitchedMap(Mat &img1, Mat &img2, int max_trans, int max_rotation, float max_pairwise_distance, cv::Mat oldTransform)
{
// load images, TODO: check that they're grayscale
image1 = img1.clone();
image2 = img2.clone();
if(image1.size != image2.size)
cv::resize(image2,image2,image1.size());
works = true;
// create feature detector set.
OrbFeatureDetector detector;
OrbDescriptorExtractor dexc;
BFMatcher dematc(NORM_HAMMING, false);
// 1. extract keypoint s
detector.detect(image1, kpv1);
detector.detect(image2, kpv2);
// 2. extract descriptors
dexc.compute(image1, kpv1, dscv1);
dexc.compute(image2, kpv2, dscv2);
// 3. match keypoints
if(kpv1.size() == 0|| kpv2.size() == 0)
{
ROS_WARN("No KPV");
works = false;
return;
}
// ROS_INFO("Kpv1:%i entries\t Kpv2:%i entries",kpv1.size(),kpv2.size());
dematc.match(dscv1, dscv2, matches);
// 4. find matching point pairs with same distance in both images
for (size_t i=0; i<matches.size(); i++) {
KeyPoint a1 = kpv1[matches[i].queryIdx],
b1 = kpv2[matches[i].trainIdx];
if (matches[i].distance > 30)
continue;
for (size_t j=0; j<matches.size(); j++) {
KeyPoint a2 = kpv1[matches[j].queryIdx],
b2 = kpv2[matches[j].trainIdx];
if (matches[j].distance > 30)
continue;
if ( fabs(norm(a1.pt-a2.pt) - norm(b1.pt-b2.pt)) > max_pairwise_distance ||
fabs(norm(a1.pt-a2.pt) - norm(b1.pt-b2.pt)) == 0)
continue;
coord1.push_back(a1.pt);
coord1.push_back(a2.pt);
coord2.push_back(b1.pt);
coord2.push_back(b2.pt);
fil1.push_back(a1);
fil1.push_back(a2);
fil2.push_back(b1);
fil2.push_back(b2);
}
}
// cv::imwrite("img1.pgm",image1);
// cv::imwrite("img2.pgm",image2);
// 5. find homography
// ROS_INFO("Found %i matches",matches.size());
if(coord1.size() < 1)
{
ROS_WARN("Problem by transforming map,this migth just an start up problem \n Coord1:%lu",coord1.size());
works = false;
return;
}
ROS_DEBUG("Compute estimateRigid");
H = estimateRigidTransform(coord2, coord1,false);
if(H.empty())
{
ROS_WARN("H contain no data, cannot find valid transformation");
works = false;
return;
}
//ROS_DEBUG("H: size:%lu|empty:%i",H.size,H.empty());
rotation = 180./M_PI*atan2(H.at<double>(0,1),H.at<double>(1,1));
transx = H.at<double>(0,2);
transy = H.at<double>(1,2);
scalex = sqrt(pow(H.at<double>(0,0),2)+pow(H.at<double>(0,1),2));
scaley = sqrt(pow(H.at<double>(1,0),2)+pow(H.at<double>(1,1),2));
ROS_DEBUG("H: transx:%f|transy%f|scalex:%f,scaley:%f|rotation:%f",transx,transy,scalex,scaley,rotation);
//first_x_trans = transx;
//first_y_trans = transy;
float scale_change = 0.05;
if(scalex > 1 + scale_change || scaley > 1 + scale_change)
{
ROS_WARN("Map should not scale change is to lagre");
works = false;
return;
}
if(scalex < 1 - scale_change|| scaley < 1 - scale_change)
{
ROS_WARN("Map should not scale change is to small");
works = false;
return;
}
if(max_trans != -1)
{
if(transx > max_trans || transy > max_trans)
{
ROS_WARN("Map should not trans so strong");
works = false;
return;
}
}
if(max_rotation != -1)
{
if(rotation > max_rotation || rotation < -1 * max_rotation)
{
ROS_WARN("Map should not rotate so strong");
works = false;
return;
}
}
cur_trans = H;
ROS_DEBUG("Finished estimateRigid");
//evaluade transformation
//evaluate
if(works)
{
Mat tmp (image2.size(),image2.type());
Mat thr;
Mat image(image2.size(), image2.type());
warpAffine(image2,image,H,image.size());
addWeighted(image,.5,image1,.5,0.0,image);
warpAffine(image2,tmp,H,image2.size());
addWeighted(tmp,.5,image1,.5,0.0,image);
//cvtColor(image1,tmp,CV_BGR2GRAY);
threshold(tmp,thr,0,255,THRESH_BINARY);
Mat K=(Mat_<uchar>(5,5)<< 0, 0, 1, 0, 0,\
0, 0, 1, 0, 0,\
1, 1, 1, 1, 1,\
0, 0, 1, 0, 0,\
0, 0, 1, 0, 0);
erode(thr,thr,K,Point(-1,-1),1,BORDER_CONSTANT);
vector< vector <Point> > contours; // Vector for storing contour
vector< Vec4i > hierarchy;
findContours( thr, contours, hierarchy,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
for( int i = 0; i< contours.size(); i++ )
{
Rect r= boundingRect(contours[i]);
rects2.push_back(r);
rectangle(tmp,Point(r.x,r.y), Point(r.x+r.width,r.y+r.height), Scalar(0,0,255),2,8,0);
}//Opened contour
Mat thr1;
//cvtColor(image1,tmp,CV_BGR2GRAY);
threshold(image1,thr1,0,255,THRESH_BINARY);
erode(thr1,thr1,K,Point(-1,-1),1,BORDER_CONSTANT);
vector< vector <Point> > contours1; // Vector for storing contour
vector< Vec4i > hierarchy1;
findContours( thr1, contours1, hierarchy1,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
for( int i = 0; i< contours1.size(); i++ ){
Rect r= boundingRect(contours1[i]);
rects1.push_back(r);
//rectangle(image1,Point(r.x,r.y), Point(r.x+r.width,r.y+r.height), Scalar(0,0,255),2,8,0);
}//Opened contour
vector<Rect> near1,near2;
int offset = 5;
for(int i = 0; i < rects1.size(); i++)
{
Rect ri = rects1.at(i);
if(ri.x == 1 && ri.y == 1)
continue;
for(int j = 0; j < rects2.size();j++)
{
Rect rj = rects2.at(j);
if(ri.x < rj.x + offset && ri.x > rj.x -offset)
{
if(ri.y < rj.y + offset && ri.y > rj.y -offset)
{
near1.push_back(ri);
near2.push_back(rj);
}
}
}
}
double eudis = 0;
double disX,disY;
for(int i = 0; i < near1.size(); i++)
{
Rect ri = near1.at(i);
Rect rj = near2.at(i);
disX = ri.x - rj.x;
disY = ri.y - rj.y;
if(disX < 0)
disX = disX * (-1);
if(disY < 0)
disY = disY * (-1);
eudis += sqrt((disX*disX)+(disY*disY));
}
if(near1.size() < 2)
eudis = -1;
else
eudis = eudis / near1.size();
ROS_DEBUG("EudisNew:%f\t near1Size:%lu:\toldTran:%i",eudis,near1.size(),oldTransform.empty());
//calc EudisOld
Mat thr3,tmp1;
//cvtColor(image1,tmp,CV_BGR2GRAY);
if(oldTransform.empty())
return;
warpAffine(image2,tmp1,oldTransform,image2.size());
threshold(tmp1,thr3,0,255,THRESH_BINARY);
erode(thr3,thr3,K,Point(-1,-1),1,BORDER_CONSTANT);
vector< vector <Point> > contours3; // Vector for storing contour
vector< Vec4i > hierarchy3;
findContours( thr3, contours3, hierarchy3,CV_RETR_CCOMP, CV_CHAIN_APPROX_SIMPLE );
for( int i = 0; i< contours3.size(); i++ ){
Rect r= boundingRect(contours3[i]);
rects3.push_back(r);
}//Opened contour
near1.clear();
near2.clear();
for(int i = 0; i < rects1.size(); i++)
{
Rect ri = rects1.at(i);
if(ri.x == 1 && ri.y == 1)
continue;
for(int j = 0; j < rects3.size();j++)
{
Rect rj = rects3.at(j);
if(ri.x < rj.x + offset && ri.x > rj.x -offset)
{
if(ri.y < rj.y + offset && ri.y > rj.y -offset)
{
near1.push_back(ri);
near2.push_back(rj);
}
}
}
}
double eudisOLD = 0;
for(int i = 0; i < near1.size(); i++)
{
Rect ri = near1.at(i);
Rect rj = near2.at(i);
disX = ri.x - rj.x;
disY = ri.y - rj.y;
if(disX < 0)
disX = disX * (-1);
if(disY < 0)
disY = disY * (-1);
eudisOLD += sqrt((disX*disX)+(disY*disY));
}
if(near1.size() < 2)
eudis = -1;
else
eudisOLD = eudisOLD / near1.size();
//if(eudisOLD < eudis)
// works = false;
ROS_WARN("EudisOLD:%f\t near1Size:%lu:|works:%i",eudis,near1.size(),works);
//calc EudisOld
/* for(int i = 0; i < rects1.size(); i++)
{
Rect r = rects1.at(i);
rectangle(image1,Point(r.x,r.y), Point(r.x+r.width,r.y+r.height), Scalar(0,0,255),2,8,0);
}*/
}
return;
}
