void alignFrames(Mat *colorA, Mat *colorB, Mat *depthA, Mat *depthB, Transformation *inverse)
{
// Prepare the matcher
Mat grayA, grayB;
cvtColor(*colorA, grayA, CV_RGB2GRAY);
cvtColor(*colorB, grayB, CV_RGB2GRAY);
RobustMatcher rmatcher;
Ptr<FeatureDetector> pfd = new SurfFeatureDetector(10);
rmatcher.setFeatureDetector(pfd);
// Match the two images
vector<DMatch> matches;
vector<KeyPoint> keypointsA, keypointsB;
Mat fundemental= rmatcher.match(grayA, grayB, matches, keypointsA, keypointsB);
Mat img_matches;
drawMatches( grayA, keypointsA, grayB, keypointsB,
matches, img_matches, Scalar::all(-1), Scalar::all(-1),
vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );
//-- Show detected matches
imshow( "Good Matches", img_matches );
vector<Point2f> selPoints1, selPoints2;
KeyPoint::convert(keypointsA, selPoints1);
KeyPoint::convert(keypointsB, selPoints2);
vector<Point3f *> setA, setB;
for(int i = 0; i < matches.size(); i++)
{
Point2f point2D = selPoints1.at(matches.at(i).queryIdx);
int bit0 = depthA->at<Vec3b>(point2D.y, point2D.x)[0];
int bit1 = depthA->at<Vec3b>(point2D.y, point2D.x)[1];
double depth = (bit0 | bit1 << 8 );
Point3f *point3D = new Point3f(point2D.x, point2D.y, depth / 100);
setA.push_back(point3D);
point2D = selPoints2.at(matches.at(i).trainIdx);
bit0 = depthB->at<Vec3b>(point2D.y, point2D.x)[0];
bit1 = depthB->at<Vec3b>(point2D.y, point2D.x)[1];
depth = (bit0 | bit1 << 8 );
Point3f *point3DB = new Point3f(point2D.x, point2D.y, depth / 100);
setB.push_back(point3DB);
}
Transformation result(false);
HornMethod hornMethod;
hornMethod.getTransformation(setA, setB, &result);
inverse->invert(&result);
// clean vectors SetA and SetB
}
bool recognize(const cv::Mat &in, cv::Rect roi) {
bool result= false;
cv::Mat image1 = cv::imread("data/sim.png",0);
// Prepare the matcher
RobustMatcher rmatcher;
rmatcher.setConfidenceLevel(0.98);
rmatcher.setMinDistanceToEpipolar(1.0);
rmatcher.setRatio(0.65f);
cv::Ptr<cv::FeatureDetector> pfd = new cv::SurfFeatureDetector(10);
rmatcher.setFeatureDetector(pfd);
cv::Mat face = in(roi);
// Match the two images
std::vector<cv::DMatch> matches;
std::vector<cv::KeyPoint> keypoints1, keypoints2;
rmatcher.match(image1, face ,matches, keypoints1, keypoints2);
// draw the matches
cv::Mat imageMatches;
cv::drawMatches(image1,keypoints1, // 1st image and its keypoints
face,keypoints2, // 2nd image and its keypoints
matches, // the matches
imageMatches, // the image produced
cv::Scalar(255,255,255)); // color of the lines
if (rmatcher.getRecognition()){
result=true;
}
return result;
}
int main(int argc,char **argv)
{
// Read input images
char* str1;
char* str2;
char* str3;
char* str4;
str1 = argv[1]; str2 = argv[2];
str3 = argv[3]; str4 = argv[4];
cv::Mat image1 = cv::imread(str1,0);
cv::Mat image2 = cv::imread(str2,0);
cv::Mat imageD1 = cv::imread(str3,CV_LOAD_IMAGE_ANYDEPTH);
cv::Mat imageD2 = cv::imread(str4,CV_LOAD_IMAGE_ANYDEPTH);
// cv::Mat image1= cv::imread("/home/tyu/ubuntu_opencv/image/1.png",0);
// cv::Mat image2= cv::imread("/home/tyu/ubuntu_opencv/image/2.png",0);
// cv::Mat imageD1 = cv::imread("/home/tyu/ubuntu_opencv/image/1d.png",CV_LOAD_IMAGE_ANYDEPTH);
// cv::Mat imageD2 = cv::imread("/home/tyu/ubuntu_opencv/image/2d.png",CV_LOAD_IMAGE_ANYDEPTH);
if (!image1.data || !image2.data)
{
std::cout<<"Cannt load the image data!\n";
return 0;
}
// Display the images
// cv::namedWindow("Right Image");
// cv::imshow("Right Image",image1);
// cv::namedWindow("Left Image");
// cv::imshow("Left Image",image2);
// Prepare the matcher
RobustMatcher rmatcher;
rmatcher.setConfidenceLevel(0.98);
rmatcher.setMinDistanceToEpipolar(1.0);
rmatcher.setRatio(0.65f);
cv::Ptr<cv::FeatureDetector> pfd= new cv::SurfFeatureDetector(15000);
rmatcher.setFeatureDetector(pfd);
// Match the two images
std::vector<cv::DMatch> matches;
std::vector<cv::KeyPoint> keypoints1, keypoints2;
cv::Mat fundemental= rmatcher.match(image1,image2,matches, keypoints1, keypoints2);
std::cout<<fundemental<<std::endl;
std::cout<<keypoints1.size()<<std::endl;
// draw the matches
// cv::Mat imageMatches;
/* cv::drawMatches(image1,keypoints1, // 1st image and its keypoints
image2,keypoints2, // 2nd image and its keypoints
matches, // the matches
imageMatches, // the image produced
cv::Scalar(1,255,255)); // color of the lines*/
// std::cout<<"imageMatches\'s size is :"<<imageMatches.size()<<std::endl;
/* /////draw lines///////
line(image1,
cv::Point(0,255.3),
cv::Point(640,255.3),
cv::Scalar(0,0,0)
);
);*/
// cv::namedWindow("Matches");
// cv::imshow("Matches",imageMatches);
/////////////////////
// Convert keypoints into Point2f
// std::vector<cv::Point2f> points1, points2;
std::vector<cv::Point3f> points_xyz1,points_xyz2;
compute3dPosition(imageD1,imageD2,keypoints1,keypoints2,points_xyz1,points_xyz2);
std::cout<<points_xyz1.size()<<std::endl;
std::cout<<points_xyz2.size()<<std::endl;
cv::Mat p1 = cv::Mat(points_xyz1).t();
cv::Mat p2 = cv::Mat(points_xyz2).t();
cv::Mat mat;
// mat = p1;
// std::cout<<mat<<std::endl;
int iterations = 0;
double distance = 100.0;
double Error = 0.0;
while(iterations < 0)
{
cv::Mat np1 , np2, np1_, np2_;
int num = 0;
for(int i=0; i<points_xyz1.size(); i++)
{
num++;
cv::Mat temp = p1.col(i).clone();
// temp.copyTo(np1.col(i));
np1.push_back(temp);
temp = p2.col(i).clone();
np2.push_back(temp);
}
iterations++;
}
/* for (std::vector<cv::DMatch>::const_iterator it= matches.begin();
it!= matches.end();
++it) {
// Get the position of left keypoints
float xpoint1= keypoints1[it->queryIdx].pt.x;
float ypoint1= keypoints1[it->queryIdx].pt.y;
double z = imageD1.at<ushort>(ypoint1,xpoint1);
// Get the position of right keypoints
float xpoint2= keypoints2[it->trainIdx].pt.x;
float ypoint2= keypoints2[it->trainIdx].pt.y;
double z1 = imageD2.at<ushort>(ypoint2,xpoint2);
if(z != 0 &&z1 != 0)
{
points_xyz1.push_back(cv::Point3f(xpoint1,ypoint1,z/5000));
points_xyz2.push_back(cv::Point3f(xpoint2,ypoint2,z1/5000));
points1.push_back(cv::Point2f(xpoint1,ypoint1));
points2.push_back(cv::Point2f(xpoint2,ypoint2));
// cv::circle(image1,cv::Point(xpoint1,ypoint1),5,cv::Scalar(1,1,255),3);
// cv::circle(image2,cv::Point(xpoint2,ypoint2),5,cv::Scalar(1,1,255),3);
}
else
continue;
}*/
std::ofstream p1out("p1.txt"), p2out("p2.txt");
if(!p1out||!p2out)
std::cout<<"Can not open the file!"<<std::endl;
// out<<"fundemental = "<<std::endl<<std::endl;
// out<<fundemental<<std::endl<<std::endl;
// out<<"p1 = "<<std::endl<<std::endl;
//true location
//write p1 location to out.txt
for(int i = 0 ; i < points_xyz1.size(); i++)
p1out<<points_xyz1[i].x<<" "<<points_xyz1[i].y<<" "<<points_xyz1[i].z<<std::endl;
// out<<std::endl;
//write p2 location to out.txt
// out<<"p2 = "<<std::endl<<std::endl;
for(int i = 0 ; i < points_xyz2.size(); i++)
p2out<<points_xyz2[i].x<<" "<<points_xyz2[i].y<<" "<<points_xyz2[i].z<<std::endl;
// out<<"p1 = "<<std::endl<<std::endl<<points_xyz1<<std::endl;
// out<<"p2 = "<<std::endl<<std::endl<<points_xyz2<<std::endl;
// out.close();
p1out.close();p2out.close();
//////////////////////////centroid/////////
// std::cout<<"The centroid is :"<<std::endl<<centroid(points_xyz1)<<std::endl;
// std::cout<<"The distance between each points to the centroid: "<<std::endl<<centroid_dist(points_xyz1,centroid(points_xyz1))<<std::endl;
/* // Draw the epipolar lines
std::vector<cv::Vec3f> lines1;
cv::computeCorrespondEpilines(cv::Mat(points1),1,fundemental,lines1);
for (std::vector<cv::Vec3f>::const_iterator it= lines1.begin();
it!=lines1.end(); ++it) {
cv::line(image2,cv::Point(0,-(*it)[2]/(*it)[1]),
cv::Point(image2.cols,-((*it)[2]+(*it)[0]*image2.cols)/(*it)[1]),
cv::Scalar(255,255,255));
}
std::vector<cv::Vec3f> lines2;
cv::computeCorrespondEpilines(cv::Mat(points2),2,fundemental,lines2);
for (std::vector<cv::Vec3f>::const_iterator it= lines2.begin();
it!=lines2.end(); ++it) {
cv::line(image1,cv::Point(0,-(*it)[2]/(*it)[1]),
cv::Point(image1.cols,-((*it)[2]+(*it)[0]*image1.cols)/(*it)[1]),
cv::Scalar(255,255,255));
}
// Display the images with epipolar lines
cv::namedWindow("Right Image Epilines (RANSAC)");
cv::imshow("Right Image Epilines (RANSAC)",image1);
cv::namedWindow("Left Image Epilines (RANSAC)");
cv::imshow("Left Image Epilines (RANSAC)",image2);
cv::waitKey();*/
return 0;
}
void ImageRecognizer::queryBoWFeature(std::vector<std::string>* pReturnArray,
const std::string& imagePath) {
pReturnArray->clear();
boost::shared_ptr<DBAdapter> dbAdapter(new HbaseAdapter);
cv::Mat image = cv::imread(imagePath, 1);
LocalFeature localFeature;
LocalFeatureExtractor localFeatureExtractor;
localFeatureExtractor.extractFeature(image, localFeature.keypoint,
localFeature.descriptor);
BoWHistogram histogram;
ANNVocabulary::instance()->quantizeFeature(localFeature, &histogram);
const std::vector<int64_t>& visualwordIdArray =
histogram.visualwordIdArray();
const std::vector<double>& frequencyArray = histogram.frequencyArray();
std::cout << "histogram size: " << histogram.size() << std::endl;
// TODO: modify the query pipeline
boost::unordered_map<int64_t, double> candidateScoreMap;
Ticker ticker;
ticker.start();
for (size_t i = 0; i < visualwordIdArray.size(); ++i) {
std::vector<Posting> postingArray;
InvertedIndexClient invertedIndexClient;
invertedIndexClient.loadPostingList(&postingArray,
visualwordIdArray[i]);
for (size_t j = 0; j < postingArray.size(); ++j) {
candidateScoreMap[postingArray[j].imageId] += postingArray[j].score
* frequencyArray[i];
}
}
ticker.stop();
ticker.start();
std::vector<RankItem<int64_t, double> > candidateRankList;
candidateRankList.reserve(candidateScoreMap.size());
for (boost::unordered_map<int64_t, double>::iterator iter =
candidateScoreMap.begin(); iter != candidateScoreMap.end();
++iter) {
RankItem<int64_t, double> item(iter->first, iter->second);
candidateRankList.push_back(item);
}
size_t candidateSize = (size_t) GlobalConfig::CANDIDATE_COUNT;
if (candidateRankList.size() > candidateSize) {
std::nth_element(candidateRankList.begin(),
candidateRankList.begin() + candidateSize,
candidateRankList.end());
}
ticker.stop();
ticker.start();
std::vector<double> queryVector;
histogram.flatten(&queryVector, ANNVocabulary::instance()->size());
std::vector<RankItem<int64_t, double> > rankList;
for (size_t i = 0; i < candidateRankList.size() && i < candidateSize; ++i) {
int64_t otherImageId = candidateRankList[i].index;
BoWHistogram otherHistogram;
otherHistogram.load(otherImageId);
double score = otherHistogram.innerProduct(queryVector);
RankItem<int64_t, double> item(otherImageId, -1.0 * score);
rankList.push_back(item);
}
std::sort(rankList.begin(), rankList.end());
ticker.stop();
if (GlobalConfig::USE_VERIFICATION) {
for (int i = 0;
i < GlobalConfig::VERIFICATION_COUNT
&& i < (int) rankList.size(); ++i) {
RobustMatcher rmatcher;
LocalFeature otherLocalFeature;
otherLocalFeature.load(rankList[i].index);
std::vector<cv::DMatch> matches;
cv::Mat homography = rmatcher.match(localFeature, otherLocalFeature,
&matches);
if (matches.size() >= 7) {
std::string path;
dbAdapter->loadCell(&path, GlobalConfig::IMAGE_TABLE,
rankList[i].index, GlobalConfig::IMAGE_HASH_COLUMN);
pReturnArray->push_back(path);
}
}
} else {
for (size_t i = 0; i < rankList.size(); ++i) {
std::string path;
dbAdapter->loadCell(&path, GlobalConfig::IMAGE_TABLE,
rankList[i].index, GlobalConfig::IMAGE_HASH_COLUMN);
pReturnArray->push_back(path);
}
}
}
void disjunkt_image_test() {
vector<string>* dateien = list_directory(
"/Users/christian/Pictures/Referenzbilder/");
RobustMatcher* rmatcher = new RobustMatcher();
for (vector<string>::iterator it = dateien->begin(); it != dateien->end();
it++) {
cout << *it << endl;
// Lade das Bild aus der Datei
cv::Mat image = cv::imread(*it, 0);
;
unsigned int most_matches = 0; // speicher die Anzahl der meisten Matches
string image2_filename; // speichert den Dateinamen von Bild 2 mit den meisten Matches
string second_best_match_image_name;
unsigned int second_most_matches = 0;
std::vector<cv::KeyPoint> keypoints1;
std::vector<cv::KeyPoint> keypoints2;
std::vector<cv::DMatch> matches;
std::list<ImageMatch*>* allImageMatches = new std::list<ImageMatch*>();
for (vector<string>::iterator it2 = dateien->begin();
it2 != dateien->end(); it2++) {
// Lade ebenfalls das Bild
cv::Mat image2 = cv::imread(*it2, 0);
// Berechne SchlŸsselpunkte und Deskriptoren auf beiden Bildern und Teste mit dem RM ob das Bild erkannt wurde.
rmatcher->match(image, image2, matches, keypoints1, keypoints2);
if (matches.size() > 0) {
// Sammle alle Matches > 0 in einem Vector
ImageMatch* imageMatch = new ImageMatch(*it2, matches.size());
allImageMatches->push_back(imageMatch);
}
//cout << *it << " matches " << *it2 << " with " << matches.size() << " points" << endl;
/*int matches_size = matches.size();
if (matches_size < most_matches) {
if (matches_size > second_most_matches) {
second_most_matches = matches_size;
second_best_match_image_name = *it2;
}
}
if (matches_size > most_matches) {
most_matches = matches_size;
image2_filename = *it2;
}*/
}
// Wenn allImagesMatches nicht leer ist, dann sortiere diese
allImageMatches->sort(ImageMatch::compareImageMatch);
// Speicher den dateiname von Bild 1 als Key in einer Map.
// Der Dateiname von Bild 2 mit den meisten Matches wird als Value in der Map gespeichert.
/*
cout << "Ergebnis: " << *it << " == " << image2_filename << " matches: " << most_matches << endl;
double similarity;
// €hnliChkeit zum zweitbesten Ermitteln
if (most_matches > 0) {
similarity = (double) second_most_matches / (double) most_matches;
} else {
similarity = 0;
}
cout << "€hnlichkeit zum zweitbesten Bild: " << similarity << " Name: " << second_best_match_image_name << endl;
*/
cout << "sortierte liste mit ImageMatches: " << endl;
for (std::list<ImageMatch*>::iterator list_it = allImageMatches->begin(); list_it != allImageMatches->end(); list_it++) {
ImageMatch* imageMatch = *list_it;
cout << "Matches: " << imageMatch->getMatchesSize() << endl;
}
// Nun kann die €hnlichkeit zu den anderen Bildern ermittelt werden.
// gehe die list mit den matches durch
double similarity = 1;
int firstValue = 0;
for (std::list<ImageMatch*>::iterator list_it = allImageMatches->begin(); list_it != allImageMatches->end(); list_it++) {
ImageMatch* imageMatch = *list_it;
if (firstValue == 0) {
firstValue = imageMatch->getMatchesSize();
} else {
int otherValue = imageMatch->getMatchesSize();
similarity -= (double) otherValue / (double) firstValue;
}
}
cout << "End-€hnlichkeit des besten Matches: " << similarity << endl;
}
}
void method1() {
// vector of keypoints
std::vector<cv::KeyPoint> refKeypoints;
std::vector<cv::KeyPoint> keypoints1;
std::vector<cv::KeyPoint> keypoints2;
//std::vector<cv::KeyPoint> keypoints3;
// Construct the SURF feature detector object
//cv::SurfFeatureDetector surf(5000.0);
// Referenzbild
cv::Mat referenz =
cv::imread(
"/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192758.jpg",
0); // open in b&w
// Andere Bilder
cv::Mat image1 =
cv::imread(
"/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192836.jpg",
0);
//cv::Mat image2 = cv::imread("/Users/christian/Dropbox/Photos/SURF/blume2.jpg");
Helper* helper = new Helper();
// Keypoints berechnen
double t = (double) getTickCount();
helper->detectKeypoints(referenz, refKeypoints);
t = ((double) getTickCount() - t) / getTickFrequency();
cout << "Times passed compute keypoints ref image: " << t << endl;
cv::Mat out;
drawKeypoints(referenz, refKeypoints, out, cv::Scalar(0, 255, 255),
DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
//cv::imwrite("/Users/christian/Desktop/Master_LaTex/Implementierung/Bilder/Bild_Keypoints.png", out);
cv::namedWindow("Color Reduces Image");
cv::imshow("Color Reduces Image", out);
cv::waitKey(0);
cout << "anzahl der keypoints: " << refKeypoints.size() << endl;
// do something ...
helper->detectKeypoints(image1, keypoints1);
//surf.detect(image2,keypoints2);
// cout << "size keypoints image1: " << keypoints1.size() << endl;
// cout << "size keypoints image2: " << keypoints2.size() << endl;
//cout << "size keypoints image3: " << keypoints3.size() << endl;
// Construction of the SURF descriptor extractor+
//cv::SurfDescriptorExtractor surfDesc;
// Extraction of the SURF descriptors
cv::Mat refDescriptors;
cv::Mat descriptors1;
cv::Mat descriptors2;
// Berechnung der Discriptoren auf allen Bildern.
t = (double) getTickCount();
helper->extractDescriptors(referenz, refKeypoints, refDescriptors);
t = ((double) getTickCount() - t) / getTickFrequency();
cout << "Times passed compute descriptir ref image: " << t << endl;
cout << "Anzahl der descriptoren: " << *refDescriptors.size << endl;
helper->extractDescriptors(image1, keypoints1, descriptors1);
//surfDesc.compute(image2, keypoints2, descriptors2);
// Draw Keypoints
cv::Mat image_out;
/*cv::drawKeypoints(referenz, // original image
refKeypoints, // vector of keypoints
image_out, // the output image
cv::Scalar(255,255,255), // keypoint color
cv::DrawMatchesFlags::DEFAULT); // drawing flag
*/
//cv::waitKey(0);
// Construction of the matcher
cv::BruteForceMatcher<cv::L2<float> > matcher;
// Match the two image descriptors
std::vector<cv::DMatch> matches_image1;
//std::vector<cv::DMatch> matches_image2;
// do something ...
//Helper *helper = new Helper();
Helper::saveDescriptorsInFile("/Users/christian/keypoints.xml",
descriptors1);
cv::Mat descriptors_loaded;
t = (double) getTickCount();
Helper::loadDescriptorsFromFile("/Users/christian/keypoints.xml",
descriptors_loaded);
t = ((double) getTickCount() - t) / getTickFrequency();
cout << "Times passed to load descriptors: " << t << endl;
t = (double) getTickCount();
// 1. Alle Bilder mit Referenzbild matchen
matcher.match(refDescriptors, descriptors_loaded, matches_image1);
//matcher.match(refDescriptors, descriptors2, matches_image2);
t = ((double) getTickCount() - t) / getTickFrequency();
cout << "Times passed to COMPUTE matches ref images: " << t << endl;
Helper::saveKeypointsInFile("/Users/christian/keypoints.xml", keypoints1);
//helper->saveDescriptorsInFile("/Users/christian/descriptors.xml", descriptors1);
cout << "keypoints1 size: " << keypoints1.size() << endl;
std::vector<cv::KeyPoint> keypoints_loaded;
t = (double) getTickCount();
Helper::loadKeyPointsFromFile("/Users/christian/keypoints.xml",
keypoints_loaded);
t = ((double) getTickCount() - t) / getTickFrequency();
cout << "Times passed to load keypoints: " << t << endl;
cout << "keypoints loaded size: " << keypoints_loaded.size() << endl;
//cv::drawMatches(referenz, refKeypoints, image1, keypoints_loaded, matches_image1, image_out, cv::Scalar(0,255,0), cv::Scalar(0,0,255), new vector<vector<char> >(), cv::DrawMatchesFlags::DEFAULT);
cv::drawMatches(referenz, refKeypoints, image1, keypoints_loaded,
matches_image1, image_out, cv::Scalar(0, 255, 255),
cv::Scalar(0, 0, 0));
//cv::imwrite("/Users/christian/Desktop/Master_LaTex/Implementierung/Bilder/bruteforce.png", image_out);
/*
if (matches_image1 < matches_image2) {
cout << "matches_image1 kleiner" << endl;
} else {
cout << "matches_image1 groe§er" << endl;
}*/
//cout << "distance image1 and image2: " << sum_distance(matches) << endl;
//cv::waitKey(0);*/
/*
cv::Mat readDescriptors_ref, readDescriptors_image1;
cout << "size descriptors1: " << *descriptors1.size << endl;
t = (double)getTickCount();
cv::FileStorage fs("test.xml", FileStorage::WRITE);
fs << "descriptors_ref_image" << refDescriptors;
fs << "image1" << descriptors1;
fs.release();
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "Times passed to write descritpors into file: " << t << endl;
cv::Mat descriptors4;
t = (double)getTickCount();
cv::FileStorage fsRead("test.xml", FileStorage::READ);
fsRead["descriptors_ref_image"] >> readDescriptors_ref;
fsRead["image1"] >> readDescriptors_image1;
fsRead.release();
t = ((double)getTickCount() - t)/getTickFrequency();
cout << "Times passed to read descritpors from file: " << t << endl;
cout << "imag1e descriptors size: " << *readDescriptors_image1.size << endl;
*/
cv::namedWindow("Image");
cv::imshow("Image", image_out);
//cv::imwrite("keypoints_plain_gray.jpg",image_out);
cv::waitKey(0);
// Robust Matcher
// prepare the matcher
RobustMatcher rmatcher;
/*rmatcher.setConfidenceLevel(0.98);
rmatcher.setMinDistanceToEpipolar(1.0);
rmatcher.setRatio(0.65f);*/
//cv::Ptr<cv::FeatureDetector> pfd = new cv::SurfFeatureDetector(5000);
//rmatcher.setFeatureDetector(pfd);
// Get the images
// Referenzbild
//image1 = cv::imread("/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192758.jpg");
// Testbild 1
//cv::Mat image2 = cv::imread("/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192836.jpg");
// Testbild 2:
image1 =
cv::imread(
"/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192758.jpg",
0);
// Testbild 3:
cv::Mat image2 =
cv::imread(
"/Users/christian/SecureDropBox/Photos/SURF/IMG_20120228_192836.jpg",
0);
// Match the two images;
std::vector<cv::DMatch> matches;
std::vector<cv::KeyPoint> keypoints3, keypoints4;
cv::Mat fundemental = rmatcher.match(image1, image2, matches, keypoints1,
keypoints2);
/*cv::drawKeypoints(, // original image
keypoints2, // vector of keypoints
image_out, // the output image
cv::Scalar(255,255,255), // keypoint color
cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); // drawing flag
*/
cv::drawMatches(image1, keypoints1, image2, keypoints2, matches, image_out,
cv::Scalar(0, 255, 255), cv::Scalar(0, 0, 0));
/*
// Gebe die Summe der Distancen aus:
cout << "sumDistance image1 and image 2: " << sum_distance(matches) << endl;
fundemental = rmatcher.match(image1, image3, matches, keypoints1, keypoints3);
//cv::drawMatches(image1, keypoints1, image3, keypoints3, matches, image_out, cv::Scalar(255,255,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cout << "sumDistance image1 and image 3: " << sum_distance(matches) << endl;
fundemental = rmatcher.match(image1, image4, matches, keypoints1, keypoints4);
//cv::drawMatches(image1, keypoints1, image4, keypoints4, matches, image_out, cv::Scalar(255,255,255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
cout << "sumDistance image1 and image 4: " << sum_distance(matches) << endl;
*/
cv::namedWindow("RobustMatches");
cv::imshow("Color Reduces Image", image_out);
cv::waitKey(0);
//cv::imwrite("/Users/christian/Desktop/Master_LaTex/Implementierung/Bilder/robust.png",image_out);
}
void ComputePnP(const char* folder_name,int start_idx,int total_num,int jump_num){//Compute two-view PnP
int img_idx = start_idx;
int img_num = total_num;
int img_num2 = jump_num;
char imgname[64];
char pcdname[64];
char imgname2[64];
char pcdname2[64];
ofstream out_pose,out_pose2;
out_pose.open("RT.txt");
out_pose2.open("invRT.txt");
cv::Mat colorImage_1(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_8UC4);
cv::Mat pointCloud_XYZforRGB_1(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_32FC3,cv::Scalar::all(0));
cv::Mat colorImage_2(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_8UC4);
cv::Mat pointCloud_XYZforRGB_2(KINECT_IMAGE_HEGIHT,KINECT_IMAGE_WIDTH,CV_32FC3,cv::Scalar::all(0));
cv::Mat img1,img2;
/////////////////////PCL objects//////////////////////////
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_1 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_1f (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_2f (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::visualization::PCLVisualizer vislm;
////////////////////Find cv::Matched points between two RGB images////////////////////////
ifstream cam_in("CamPara.txt");
cam_in>>CamIntrinsic[0][0]>>CamIntrinsic[0][1]>>CamIntrinsic[0][2]
>>CamIntrinsic[1][0]>>CamIntrinsic[1][1]>>CamIntrinsic[1][2]
>>CamIntrinsic[2][0]>>CamIntrinsic[2][1]>>CamIntrinsic[2][2];
cam_in.close();
const cv::Mat Camera_Matrix(3,3,CV_64F,CamIntrinsic);
const cv::Mat disCoef(1,5,CV_64F,DisCoef);
cv::Mat img_Matches;
int numKeyPoints = 400;
int numKeyPoints2 = 400;
RobustMatcher rMatcher;
cv::Ptr<cv::FeatureDetector> detector = new cv::FastFeatureDetector(0);
cv::Ptr<cv::FeatureDetector> detector2 = new cv::FastFeatureDetector(0);
cv::Ptr<cv::DescriptorExtractor> extractor = new cv::OrbDescriptorExtractor();
cv::Ptr<cv::DescriptorMatcher> Matcher= new cv::BFMatcher(cv::NORM_HAMMING);
rMatcher.setFeatureDetector(detector);
rMatcher.setDescriptorExtractor(extractor);
rMatcher.setDescriptorMatcher(Matcher);
std::vector<cv::KeyPoint> img1_keypoints;
cv::Mat img1_descriptors;
std::vector<cv::KeyPoint> img2_keypoints;
cv::Mat img2_descriptors;
std::vector<cv::DMatch > Matches;
//////////////////////PCL rigid motion estimation///////////////////////////
Eigen::Matrix4f TotaltransforMation = Eigen::Matrix4f::Identity();
Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity();
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Transcloud_2 (new pcl::PointCloud<pcl::PointXYZRGBA>);
//////////////////////////////////////////////////////////////////////////////
printf("From %d to %d",img_idx,img_idx+(img_num-1)*img_num2);
//Camera position
pcl::PointCloud<pcl::PointXYZ> Camera_pose0;
pcl::PointCloud<pcl::PointXYZ> Camera_pose;
Camera_pose0.push_back(pcl::PointXYZ(0,0,0));
Camera_pose0.push_back(pcl::PointXYZ(0.2,0,0));
Camera_pose0.push_back(pcl::PointXYZ(0,0.2,0));
Camera_pose0.push_back(pcl::PointXYZ(0,0,0.2));
/////////////Looping///////////////////////////
int d = 0;
for(int i=0; i<img_num-1; ++i, d+=img_num2){
sprintf(imgname,"%s/rgb_%d.jpg",folder_name,img_idx+d);
sprintf(pcdname,"%s/pcd_%d",folder_name,img_idx+d);
sprintf(imgname2,"%s/rgb_%d.jpg",folder_name,img_idx+d+img_num2);
sprintf(pcdname2,"%s/pcd_%d",folder_name,img_idx+d+img_num2);
printf("comparing %s & %s\n",imgname,imgname2);
////////////Reading data/////////////////////
img1 = cv::imread(imgname,CV_LOAD_IMAGE_GRAYSCALE);
colorImage_1 = cv::imread(imgname,CV_LOAD_IMAGE_COLOR);
printf("reading\n");
FILE* fin = fopen(pcdname,"rb");
fread(pointCloud_XYZforRGB_1.data,1,pointCloud_XYZforRGB_1.step*pointCloud_XYZforRGB_1.rows,fin);
fclose(fin);
img2 = cv::imread(imgname2,CV_LOAD_IMAGE_GRAYSCALE);
colorImage_2 = cv::imread(imgname2,CV_LOAD_IMAGE_COLOR);
printf("reading\n");
fin = fopen(pcdname2,"rb");
fread(pointCloud_XYZforRGB_2.data,1,pointCloud_XYZforRGB_2.step*pointCloud_XYZforRGB_2.rows,fin);
fclose(fin);
cloud_1->clear();
cloud_2->clear();
Mat2PCL_XYZRGB_ALL(colorImage_1,pointCloud_XYZforRGB_1,*cloud_1);
Mat2PCL_XYZRGB_ALL(colorImage_2,pointCloud_XYZforRGB_2,*cloud_2);
///////////////////Finding 2D features//////////////////////////
img1_keypoints.clear();
img2_keypoints.clear();
detector->detect(img1,img1_keypoints);
extractor->compute(img1,img1_keypoints,img1_descriptors);
detector2->detect(img2,img2_keypoints);
extractor->compute(img2,img2_keypoints,img2_descriptors);
Matches.clear();
printf("cv::Matching\n");
rMatcher.match(Matches, img1_keypoints, img2_keypoints,img1_descriptors,img2_descriptors);
//printf("drawing\n");
drawMatches(img1, img1_keypoints, img2, img2_keypoints,Matches, img_Matches, cv::Scalar::all(-1), cv::Scalar::all(-1),vector<char>(), cv::DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS);
cv::imshow("Good matches",img_Matches);
cv::waitKey(10);
///////////////////Find corresponding 3D pints////////////////////////////////////////
//We should draw point cloud from the Matches...
//////////////////3D geometry///////////////////////////////////////////////////////
/////////////////////////
//Compute PnP
////////////////////////
cv::Mat rvec(1,3,cv::DataType<double>::type);
cv::Mat tvec(1,3,cv::DataType<double>::type);
cv::Mat rotationMatrix(3,3,cv::DataType<double>::type);
vector<cv::Point2f> p2ds;
vector<cv::Point3f> p3ds;
Mat2PCL_XYZRGB_MATCH_PnP(pointCloud_XYZforRGB_1,p3ds,p2ds,img1_keypoints,img2_keypoints,Matches);
printf("3D/2D points:%d,%d\n",p3ds.size(),p2ds.size());
if(p3ds.size() > 5){
cv::solvePnPRansac(p3ds,p2ds,Camera_Matrix,disCoef,rvec,tvec);
cv::Rodrigues(rvec,rotationMatrix);
PnPret2Mat4f(rotationMatrix,tvec,Ti);
Ti = Ti.inverse();
}
///////////////////////Compose the translation, and transform the point cloud////////////////
Transcloud_2->clear();
std::cout<<"\nLocal motion from PnP is \n"<<Ti;
TotaltransforMation = TotaltransforMation*Ti;
pcl::transformPointCloud(*cloud_2,*Transcloud_2,TotaltransforMation);
std::cout<<"\nTotal motion from PnP is \n"<<TotaltransforMation<<endl;
//Add camera coordinate
pcl::transformPointCloud(Camera_pose0,Camera_pose,TotaltransforMation);
sprintf(pcdname2,"X%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[1],255,0,0,pcdname2);
sprintf(pcdname2,"Y%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[2],0,255,0,pcdname2);
sprintf(pcdname2,"Z%d",img_idx+d+img_num2);
vislm.addLine(Camera_pose[0],Camera_pose[3],0,0,255,pcdname2);
if(i==0){
vislm.addPointCloud(cloud_1->makeShared(),pcdname);
//PCLviewer.showCloud(cloud_1,"init");
out_pose<<Eigen::Matrix4f::Identity()<<endl;
}
vislm.addPointCloud(Transcloud_2->makeShared(),pcdname2);
out_pose<<TotaltransforMation<<endl;
out_pose2<<TotaltransforMation.inverse()<<endl;
}
vislm.resetCamera();
vislm.spin();
out_pose.close();
out_pose2.close();
}