/* +---------------------------------------------------------------------------+
| The Mobile Robot Programming Toolkit (MRPT) C++ library |
| |
| http://www.mrpt.org/ |
| |
| Copyright (C) 2005-2011 University of Malaga |
| |
| This software was written by the Machine Perception and Intelligent |
| Robotics Lab, University of Malaga (Spain). |
| Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
| |
| This file is part of the MRPT project. |
| |
| MRPT is free software: you can redistribute it and/or modify |
| it under the terms of the GNU General Public License as published by |
| the Free Software Foundation, either version 3 of the License, or |
| (at your option) any later version. |
| |
| MRPT is distributed in the hope that it will be useful, |
| but WITHOUT ANY WARRANTY; without even the implied warranty of |
| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
| GNU General Public License for more details. |
| |
| You should have received a copy of the GNU General Public License |
| along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
| |
+---------------------------------------------------------------------------+ */
#include <mrpt/hwdrivers/CRovio.h>
#include <mrpt/hwdrivers/CJoystick.h>
#include <mrpt/utils/utils_defs.h>
#include <mrpt/slam.h>
#include <fstream>
#include <cmath>
using namespace mrpt;
using namespace mrpt::hwdrivers;
using namespace mrpt::utils;
using namespace mrpt::gui;
using namespace std;
/*--------------------------------
| VARIABLES |
---------------------------------*/
struct data_st
{
CRovio Robot;
bool showvideo, features_taken, matched, matching_done, fm_calculated;
};
CDisplayWindow win("Video"),wind1,wind2; //wind3,wind4,wind5,wind6; //open a window to show the video
/*--------------------------------
| FUNCTIONS |
---------------------------------*/
// Int to binary and print.
void binary(int number) {
int remainder;
if(number <= 1) {
cout << number;
return;
}
remainder = number%2;
binary(number >> 1);
cout << remainder;
}
//Print the encoders readed from the CRovio Class
void print_Encoders(int *arr)
{
string field_name[12]={"Packet length","Not Used","Left Wheel:Dir Rotation","Left Wheel:Ticks","Right Wheel:Dir Rotation","Right Wheel:Ticks","Rear Wheel:Dir Rotation","Rear Wheel:Ticks","Not used","Head Position","Batery","Config Status"};
//Code
cout << "\n-----------------ENCODERS------------------\n";
for(int i=0;i<=11;i++)
{
cout << field_name[i] <<" -> ";
if(i==10) cout <<hex << arr[i] <<dec;
else cout << arr[i];
if (i==11)
{
cout <<" = "; binary(arr[i]); //print binary value
}
cout<<endl;
}
cout << "\n-------------------------------------------\n";
}
//Show the Position given by the NorthStar system
void showPosition(data_st &Pdata)
{
mrpt::math::TPose2D pose_out; //Pose sent to the console
if (Pdata.Robot.getPosition(pose_out))
{
cout << "\n pose: " << CPose2D(pose_out) << endl;
}
else cerr << "Error\n";
}
void goThere(data_st &Pdata, string& response, string& errormsg)
{
mrpt::math::TPose2D pose;
int x_t, y_t; float theta_t; //Target Position
float x_d, y_d, theta_d, distance, angle; //Distance to the Target
bool target=false;
do{
cout<<"\nEnter the target (x, y & theta)\n";
cin >> x_t >>y_t >>theta_t;
Pdata.Robot.getPosition(pose);
x_d=x_t-pose.x;
y_d=y_t-pose.y;
distance=sqrt(x_d*x_d+y_d*y_d);
while(distance>500)
{
theta_d=atan2(x_d,(-y_d));
angle=theta_d-pose.phi;
if(angle>M_PI)
angle-=2*M_PI;
if(angle<(-M_PI))
angle+=2*M_PI;
while(angle>0.6 || angle<(-0.6))
{
if(angle>0.6)
Pdata.Robot.Moving(18, 5, 10, response, errormsg);
if(angle<(-0.6))
Pdata.Robot.Moving(18, 6, 10, response, errormsg);
Pdata.Robot.getPosition();
angle=theta_d-pose.phi;
if(angle>M_PI)
angle-=2*M_PI;
if(angle<(-M_PI))
angle+=2*M_PI;
}
Pdata.Robot.Moving(18, 1, 5, response, errormsg);
Pdata.Robot.getPosition();
x_d=x_t-pose.x;
y_d=y_t-pose.y;
distance=sqrt(x_d*x_d+y_d*y_d);
}
angle=theta_t-pose.phi;
if(angle>M_PI)
angle-=2*M_PI;
if(angle<(-M_PI))
angle+=2*M_PI;
while(angle>0.6 || angle<(-0.6))
{
if(angle>0.6)
Pdata.Robot.Moving(18, 6, 10, response, errormsg);
if(angle<(-0.6))
Pdata.Robot.Moving(18, 5, 10, response, errormsg);
Pdata.Robot.getPosition(pose);
angle=theta_t-pose.phi;
if(angle>M_PI)
angle-=2*M_PI;
if(angle<(-M_PI))
angle+=2*M_PI;
}
if(distance<500 && theta_d<0.6 && theta_d>(-0.6))
target=true;
}while(!target);
}
//Shows the video in the "win" window.
void videowindow(data_st* data)
{
CObservationImagePtr lastimg;
while(data->showvideo)
{
data->Robot.getLastImage2(lastimg);
if(lastimg) //Avoid NULL image
win.showImage(lastimg->image);
mrpt::system::sleep(50);
}
}
/*--------------------------------
| JOYSTICK CONTROLS |
---------------------------------*/
void JoystickControl(data_st *data)
{
CJoystick joy;
float x,y,z;
vector <bool> buttons;
string response, errormsg;
int action, direction;
//CTicTac tictac;
while(joy.getJoystickPosition(0,x,y,z,buttons))
{
//printf("Joystick readings: %.03f, %.03f, %.03f (", x, y, z);
int speed=(int)(10-(x*x+y*y)*9);
if(x<-0.2 || x>0.2 || y <-0.2 || y>0.2)
{
if(y<-0.2)
{
if(x<0.2 && x>-0.2){//Move Forward
action=18; direction=1;}
if(x<-0.2){//Move Forward & Left
action=18; direction=7;}
if(x>0.2){//Move Forward & Right
action=18; direction=8;}
}
if(y>0.2)
{
if(x<0.2 && x>-0.2){ //Move Backward
action=18; direction=2;}
if(x>0.2){ //Move Backward & Right
action=18; direction=10;}
if(x<-0.2){ //Move Backward & Left
action=18; direction=9;}
}
if(y>-0.2 && y<0.2)
{
if(x<-0.2){ //Move Left
action=18; direction=3;}
if(x>0.2){ //Move Right
action=18; direction=4;}
}
data->Robot.Moving(action, direction, speed, response, errormsg);
}
if((buttons[3] && buttons[2])||(buttons[0] && buttons[1])||(y<-0.5 && buttons[2])||(buttons[0] && buttons[2]))
{
if(buttons[3] && buttons[2]) //Take head up
data->Robot.Moving(18, 11, 5, response, errormsg);
if(buttons[0] && buttons[1]) //Go home
data->Robot.Moving(12, 12, 5, response, errormsg);
if(y<-0.5 && buttons[2]) //Go home & dock
data->Robot.Moving(13, 12, 5, response, errormsg);
if(buttons[0] && buttons[2]) //Get Report
{
data->Robot.Moving(1, 12, 5, response, errormsg);
cout<<"Response:\n"<<response;
}
}
else
{
if(buttons[0]) //Turn to the left
data->Robot.Moving(18, 5, 5, response, errormsg);
if(buttons[1]) //Turn to the right
data->Robot.Moving(18, 6, 5, response, errormsg);
if(buttons[2]) //Take head to the middle
data->Robot.Moving(18, 13, 5, response, errormsg);
if(buttons[3]) //Take head down
data->Robot.Moving(18, 12, 5, response, errormsg);
}
fflush(stdout);
mrpt::system::sleep(20);
}
cout<<"No Joystick connected...\n\n\n"<<endl;
}
/*--------------------------------
| MATCHING |
---------------------------------*/
//Method to compare two matches list
/*
CMatchedFeatureList comp2Matched_lists2(CMatchedFeatureList &list1, CMatchedFeatureList &list2)
{
CMatchedFeatureList final_list;
CMatchedFeatureList::iterator itVec1, itVec2; // We set an iterator for the vector
for( itVec1 = list1.begin(); itVec1 != list1.end(); itVec1++) // ... we search it into the vector
{
for( itVec2 = list2.begin(); itVec2 != list2.end(); itVec2++)
{
if( itVec1->second->ID == itVec2->first->ID )
{
std::pair<CFeaturePtr,CFeaturePtr> mPair( itVec1, itVec2 );
final_list.insert( mPair );
}
} // end for itVec2
} // end for itVec1
return final_list;
}
*/
CMatchedFeatureList comp2Matched_lists(CMatchedFeatureList &list1, CMatchedFeatureList &list2)
{
CMatchedFeatureList final_list=list1;
CMatchedFeatureList::iterator itVec1, itVec2; // We set an iterator for the vector
for( itVec1 = final_list.begin(); itVec1 != final_list.end();) // ... we search it into the vector
{
for( itVec2 = list2.begin(); itVec2 != list2.end(); itVec2++)
{
if( itVec1->second->ID == itVec2->first->ID )
break;
} // end for itVec2
if(itVec2 == list2.end())
itVec1 = final_list.erase( itVec1 );
else
itVec1++;
} // end for itVec1
return final_list;
}
void matching(data_st *data)
{
CTicTac tictac;
TMatchingOptions opt;
opt.useXRestriction=false;
if(data->matching_type==epipolar_match)
{
opt.useEpipolarRestriction=true;
opt.parallelOpticalAxis=false;
opt.epipolar_TH=2.0;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
opt.F.loadFromTextFile("FM_Harris2ways.txt");
}
else
{
opt.useEpipolarRestriction=false;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
}
cout<<"\nCalculating matches...\n";
//opt.F=data->FM_Harris2ways;
opt.matching_method=TMatchingOptions::mmDescriptorSURF;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris, data->featsHarris2, data->Harris_matched, opt);
cout << "Detected " << data->Harris_matched.size() << " Harris matches in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched.saveToTextFile("Harris_matches.txt");
//Calculate matches in the inverse way
//opt.F=(~opt.F);
//opt.F=(~data->FM_Harris2ways);
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris2, data->featsHarris, data->Harris_matched_inv, opt);
cout << "Detected " << data->Harris_matched_inv.size() << " Harris matches 'INV' in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched_inv.saveToTextFile("Harris_matches_inv.txt");
// Only matches found in both ways are kept in a new file (Detect matches in only one way and delete it)
data->Harris_matched2ways = comp2Matched_lists(data->Harris_matched, data->Harris_matched_inv);
cout << data->Harris_matched2ways.size() << " Harris matches in both ways\n" << endl;
data->matching_done=true;
cout<<"\nMatching finished\n";
}
void matching2(data_st* data)
{
//data_st *data=(data_st*) Pdata; //structure cast
CTicTac tictac;
TMatchingOptions opt;
opt.useXRestriction=false;
if(data->matching_type==epipolar_match)
{
opt.useEpipolarRestriction=true;
opt.parallelOpticalAxis=false;
opt.epipolar_TH=2.0;
}
else
opt.useEpipolarRestriction=false;
cout<<"\nCalculating matches...\n";
//HARRIS
if(data->matching_type==epipolar_match)
{
//data->FM_Harris.loadFromTextFile("FM_Harris.txt");
opt.F=data->FM_Harris;
//opt.minCC_TH=0.94999999;
//opt.rCC_TH=0.920;
//opt.maxEDD_TH=0.2;
//opt.EDD_RATIO=0.5;
opt.maxEDSD_TH=0.18;
opt.EDSD_RATIO=0.6;
}
else
{
//opt.minCC_TH=0.94999999;
//opt.rCC_TH=0.92;
//opt.maxEDD_TH=0.2;
//opt.EDD_RATIO=0.5;
opt.maxEDSD_TH=0.16;
opt.EDSD_RATIO=0.55;
}
opt.matching_method=TMatchingOptions::mmDescriptorSURF;
//opt.matching_method=TMatchingOptions::mmCorrelation;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris, data->featsHarris2, data->Harris_matched, opt);
cout << "Detected " << data->Harris_matched.size() << " Harris matches in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched.saveToTextFile("Harris_matches.txt");
opt.F=data->FM_Harris_inv;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsHarris2, data->featsHarris, data->Harris_matched_inv, opt);
cout << "Detected " << data->Harris_matched_inv.size() << " Harris matches 'INV' in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->Harris_matched_inv.saveToTextFile("Harris_matches_inv.txt");
//Detect matches in only one way and delete it (Only matches found in both ways are kept in a new file)
data->Harris_matched2ways = comp2Matched_lists(data->Harris_matched, data->Harris_matched_inv);
cout << data->Harris_matched2ways.size() << " Harris matches in both ways\n" << endl;
data->Harris_matched2ways.saveToTextFile("Harris_matches2ways.txt");
/*
//SIFT
if(data->matching_type==epipolar_match)
{
opt.F=data->FM_SIFT;
opt.maxEDD_TH=0.2;
opt.EDD_RATIO=0.5;
}
else
{
opt.maxEDD_TH=0.2;
opt.EDD_RATIO=0.4;
}
opt.matching_method=TMatchingOptions::mmDescriptorSIFT;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsSIFT_Hess, data->featsSIFT_Hess2, data->SIFT_matched, opt);
cout << "Detected " << data->SIFT_matched.size() << " SIFT matches in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->SIFT_matched.saveToTextFile("SIFT_matches.txt");
//SURF
if(data->matching_type==epipolar_match)
{
opt.F=data->FM_SURF;
//opt.maxEDSD_TH=0.15;
//opt.EDSD_RATIO=0.6;
}
else
{
opt.maxEDSD_TH=0.12;
opt.EDSD_RATIO=0.5;
}
opt.matching_method=TMatchingOptions::mmDescriptorSURF;
tictac.Tic();
mrpt::vision::utils::matchFeatures2(data->featsSURF, data->featsSURF2, data->SURF_matched, opt);
cout << "Detected " << data->SURF_matched.size() << " SURF matches in " << endl;
cout << format(" %.03fms",tictac.Tac()*1000) << endl;
data->SURF_matched.saveToTextFile("SURF_matches.txt");
*/
data->matching_done=true;
cout<<"\nMatching finished\n";
}
//GETTING FUNDAMENTAL MATRIX
void getFMat(data_st *data)
{
CvMat* points1;
CvMat* points2;
CvMat* status;
CvMat* fundamental_matrix;
//ofstream mat;
// HARRIS
if(data->Harris_matched.size()<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
int point_count = data->Harris_matched.size();
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
CMatchedFeatureList::iterator it;
unsigned int i, j;
/* Fill the points here ... */
for(i=0, it=data->Harris_matched.begin(); it!=data->Harris_matched.end(); it++,i++)
{
points1->data.fl[i*2] = it->first->x;
points1->data.fl[i*2+1] = it->first->y;
points2->data.fl[i*2] = it->second->x;
points2->data.fl[i*2+1] = it->second->y;
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 1, 0.99, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX HARRIS:\n\n";
for(i=0; i<data->Harris_matched.size();i++)
cout<< (status->data.ptr[i] == 0 ? "0" : "1");
cout<<"\n";
//mat.open("FM_Harris.txt");
//if(!mat.is_open())
// cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_Harris(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_Harris(i,j)<<"\t";
//mat<<data->FM_Harris(i,j)<<"\t";
}
cout<<"\n";
//mat<<"\n";
}
data->FM_Harris.saveToTextFile("FM_Harris.txt");
//mat.close();
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
//inv
if(data->Harris_matched_inv.size()<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
int point_count = data->Harris_matched_inv.size();
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
CMatchedFeatureList::iterator it;
unsigned int i, j;
/* Fill the points here ... */
for(i=0, it=data->Harris_matched_inv.begin(); it!=data->Harris_matched_inv.end(); it++,i++)
{
points1->data.fl[i*2] = it->first->x;
points1->data.fl[i*2+1] = it->first->y;
points2->data.fl[i*2] = it->second->x;
points2->data.fl[i*2+1] = it->second->y;
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 1, 0.99, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX HARRIS 'INV':\n\n";
for(i=0; i<data->Harris_matched_inv.size();i++)
cout<< status->data.ptr[i] == 0 ? "0" : "1";
cout<<"\n";
//mat.open("FM_Harris_inv.txt");
//if(!mat.is_open())
// cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_Harris_inv(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_Harris_inv(i,j)<<"\t";
//mat<<data->FM_Harris_inv(i,j)<<"\t";
}
cout<<"\n";
//mat<<"\n";
}
//mat.close();
}
else
cout<<"\nNo Fundamental Matrix 'INV' found\n";
}
//2ways
if(data->Harris_matched2ways.size()<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
int point_count = data->Harris_matched2ways.size();
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
CMatchedFeatureList::iterator it;
unsigned int i, j;
/* Fill the points here ... */
for(i=0, it=data->Harris_matched2ways.begin(); it!=data->Harris_matched2ways.end(); it++,i++)
{
points1->data.fl[i*2] = it->first->x;
points1->data.fl[i*2+1] = it->first->y;
points2->data.fl[i*2] = it->second->x;
points2->data.fl[i*2+1] = it->second->y;
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 1, 0.99, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX HARRIS '2WAYS':\n\n";
for(i=0; i<data->Harris_matched2ways.size();i++)
cout<< status->data.ptr[i] == 0 ? "0" : "1";
cout<<"\n";
//mat.open("FM_Harris2ways.txt");
//if(!mat.is_open())
// cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_Harris2ways(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_Harris2ways(i,j)<<"\t";
//mat<<data->FM_Harris2ways(i,j)<<"\t";
}
cout<<"\n";
//mat<<"\n";
}
//mat.close();
}
else
cout<<"\nNo Fundamental Matrix 'INV' found\n";
}
/*
// SIFT
if(data->SIFT_matched.size()<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
int point_count = data->SIFT_matched.size();
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
CMatchedFeatureList::iterator it;
unsigned int i, j;
// Fill the points here ...
for(i=0, it=data->SIFT_matched.begin(); it!=data->SIFT_matched.end(); it++,i++)
{
points1->data.fl[i*2] = it->first->x;
points1->data.fl[i*2+1] = it->first->y;
points2->data.fl[i*2] = it->second->x;
points2->data.fl[i*2+1] = it->second->y;
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 1, 0.99, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX - SIFT:\n\n";
for(i=0; i<data->SIFT_matched.size();i++)
cout<<status->data.ptr[i];
cout<<"\n";
mat.open("FM_SIFT.txt");
if(!mat.is_open())
cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_SIFT(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_SIFT(i,j)<<"\t";
mat<<data->FM_SIFT(i,j)<<"\t";
}
cout<<"\n";
mat<<"\n";
}
mat.close();
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
// SURF
if(data->SURF_matched.size()<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
int point_count = data->SURF_matched.size();
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
CMatchedFeatureList::iterator it;
unsigned int i, j;
// Fill the points here ...
for(i=0, it=data->SURF_matched.begin(); it!=data->SURF_matched.end(); it++,i++)
{
points1->data.fl[i*2] = it->first->x;
points1->data.fl[i*2+1] = it->first->y;
points2->data.fl[i*2] = it->second->x;
points2->data.fl[i*2+1] = it->second->y;
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 1, 0.99, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX SURF:\n\n";
for(i=0; i<data->SURF_matched.size();i++)
cout<<status->data.ptr[i];
cout<<"\n";
mat.open("FM_SURF.txt");
if(!mat.is_open())
cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_SURF(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_SURF(i,j)<<"\t";
mat<<data->FM_SURF(i,j)<<"\t";
}
cout<<"\n";
mat<<"\n";
}
mat.close();
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
*/
cout<<"\nFundamental Matrix calculation finished\n";
data->fm_calculated=true;
}
void getFMat_from_txt(data_st *data)
{
CvMat* points1;
CvMat* points2;
CvMat* status;
CvMat* fundamental_matrix;
ofstream mat;
CMatrix A;
int point_count;
int i,j;
// HARRIS
A.loadFromTextFile("Harris_matches.txt");
point_count = A.getRowCount();
if(point_count<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
/* Fill the points here ... */
for(i=0; i<point_count; i++)
{
points1->data.fl[i*2] = A.get_unsafe(i,1);
points1->data.fl[i*2+1] = A.get_unsafe(i,2);
points2->data.fl[i*2] = A.get_unsafe(i,4);
points2->data.fl[i*2+1] = A.get_unsafe(i,5);
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 2, 0.8, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX HARRIS:\n\n";
for(i=0; i<point_count;i++)
cout<< (status->data.ptr[i]==0 ? "A" : "B");
cout<<"\n";
mat.open("FM_Harris.txt");
if(!mat.is_open())
cout<<"Fundamental Matrix file not opened";
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_Harris(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_Harris(i,j)<<"\t";
mat<<data->FM_Harris(i,j)<<"\t";
}
cout<<"\n";
mat<<"\n";
}
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
// SIFT
A.loadFromTextFile("SIFT_matches.txt");
point_count = A.getRowCount();
if(point_count<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
/* Fill the points here ... */
for(i=0; i<point_count; i++)
{
points1->data.fl[i*2] = A.get_unsafe(i,1);
points1->data.fl[i*2+1] = A.get_unsafe(i,2);
points2->data.fl[i*2] = A.get_unsafe(i,4);
points2->data.fl[i*2+1] = A.get_unsafe(i,5);
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 2, 0.8, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX SIFT:\n\n";
for(i=0; i<point_count;i++)
cout<<status->data.ptr[i];
cout<<"\n";
mat.open("FM_SIFT.txt");
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_SIFT(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_SIFT(i,j)<<"\t";
mat<<data->FM_SIFT(i,j)<<"\t";
}
cout<<"\n";
mat<<"\n";
}
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
// SURF
A.loadFromTextFile("SURF_matches.txt");
point_count = A.getRowCount();
if(point_count<8)
cout<<"WARNING->Less than 8 elements...\n";
else
{
points1 = cvCreateMat(1,point_count,CV_32FC2);
points2 = cvCreateMat(1,point_count,CV_32FC2);
status = cvCreateMat(1,point_count,CV_8UC1);
/* Fill the points here ... */
for(i=0; i<point_count; i++)
{
points1->data.fl[i*2] = A.get_unsafe(i,1);
points1->data.fl[i*2+1] = A.get_unsafe(i,2);
points2->data.fl[i*2] = A.get_unsafe(i,4);
points2->data.fl[i*2+1] = A.get_unsafe(i,5);
}
fundamental_matrix = cvCreateMat(3,3,CV_32FC1);
int fm_count = cvFindFundamentalMat( points1,points2,fundamental_matrix,
CV_FM_RANSAC, 2, 0.8, status );
if(fm_count!=0)
{
cout<<"\n\nFUNDAMENTAL MATRIX SURF:\n\n";
for(i=0; i<point_count;i++)
cout<<status->data.ptr[i];
cout<<"\n";
mat.open("FM_SURF.txt");
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)
{
data->FM_SURF(i,j)=cvGetReal2D(fundamental_matrix,i,j);
cout<<data->FM_SURF(i,j)<<"\t";
mat<<data->FM_SURF(i,j)<<"\t";
}
cout<<"\n";
mat<<"\n";
}
}
else
cout<<"\nNo Fundamental Matrix found\n";
}
cout<<"\nFundamental Matrix calculation finished\n";
data->fm_calculated=true;
}
//void getAffineMat(CMatrixDouble16 first, CMatrixDouble16 second, CvMat affine)
//{
// //CvMat* warp_mat = cvCreateMat(2,3,CV_32FC1);
// CvPoint2D32f srcTri[3], dstTri[3];
// // Compute warp matrix
// srcTri[0].x = first.get_unsafe(1,1);
// srcTri[0].y = first.get_unsafe(1,2);
// srcTri[1].x = first.get_unsafe(1,3);
// srcTri[1].y = first.get_unsafe(1,4);
// srcTri[2].x = first.get_unsafe(1,5);
// srcTri[2].y = first.get_unsafe(1,6);
//
// dstTri[0].x = second.get_unsafe(1,1);
// dstTri[0].y = second.get_unsafe(1,2);
// dstTri[1].x = second.get_unsafe(1,3);
// dstTri[1].y = second.get_unsafe(1,4);
// dstTri[2].x = second.get_unsafe(1,5);
// dstTri[2].y = second.get_unsafe(1,6);
//
// cvGetAffineTransform( srcTri, dstTri, warp_mat );
//
//}
/*--------------------------------
| Features & Match STREAM |
---------------------------------*/
void FeaturesStream(void* Pdata)
{
data_st *data=(data_st*) Pdata; //structure cast
TestExtractFeatures2(Pdata, "img1.jpg", "f_harris1.txt");
int k = 2;
string pictname, fharrisname, fharrismatch, fharrismatch_inv, fharrismatch2ways, FM1, FM2, FM_;
pictname = format("img%i.jpg",k);
while(data->pict2.loadFromFile(pictname))
{
fharrisname = format("f_harris%i.txt",k);
data->featsHarris = data->featsHarris2;
TestExtractFeatures2(Pdata, pictname, fharrisname);
data->matching_type=complete_match;
matching(data);
getFMat(data);
FM1 = format("FM_Harris%i.txt",k);
FM2 = format("FM_Harris_inv%i.txt",k);
FM_ = format("FM_Harris2ways%i.txt",k);
data->FM_Harris.saveToTextFile(FM1);
data->FM_Harris_inv.saveToTextFile(FM2);
data->FM_Harris2ways.saveToTextFile(FM_);
data->matching_type=epipolar_match;
matching(data);
fharrismatch = format("Harris_matches%i.txt",k);
fharrismatch_inv = format("Harris_matches_inv%i.txt",k);
fharrismatch2ways = format("Harris_matches2ways%i.txt",k);
data->Harris_matched.saveToTextFile(fharrismatch);
data->Harris_matched_inv.saveToTextFile(fharrismatch_inv);
data->Harris_matched2ways.saveToTextFile(fharrismatch2ways);
k++;
pictname = format("img%i.jpg",k);
data->featsHarris=data->featsHarris2;
}
cout<<"Last Image analysed: " <<(k-1) <<endl;
}
void Tracking(void* Pdata)
{
data_st *data=(data_st*) Pdata; //structure cast
string pict1name, pict2name, featsname;
int k = 1;
TestExtractFeatures2(Pdata, "img1.jpg", "harris1.txt");
data->pict1 = data->pict2;
while(data->pict2.loadFromFile(format("img%i.jpg",(k+1))))
{
featsname = format("harris%i.txt",(k+1));
data->featsHarris = data->featsHarris2;
trackFeatures(data->pict1, data->pict2, data->featsHarris2);
// CFeatureList::iterator it;
// for(it=data->featsHarris2.begin(); it!=data->featsHarris2.end(); it++)
// {
// if((*it)->x < 0)
// (*it).
// }
data->featsHarris2.saveToTextFile(featsname);
data->pict1 = data->pict2;
k++;
}
}
/*--------------------------------
| TRANSLATION & ROTATION |
---------------------------------*/
void getRTMat(void* Pdata)
{
data_st *data=(data_st*) Pdata; //structure cast
CMatrix K, F;
K=data->intrinsic_matrix;
//F=data->FM_SURF;
F.loadFromTextFile("FM_Harris.txt");
data->RT=(~K)*F*K;
//RT.multiply(data->FM_Harris,data->intrinsic_matrix);
}
/*--------------------------------
| MAIN |
---------------------------------*/
int main()
{
//VAR
char decision; //to read from keyboard
data_st data;
bool output=false;
data.showvideo=false;
string response, errormsg;
mrpt::system::TThreadHandle screen_hd, featuring_hd, matching_hd, fm_hd;
int *a_enc;
int act=0; //Action
int dir=0; //Direction
int speed=3; //Speed
data.matched=false; //this var checks if there are extracted features
data.features_taken=false, data.matching_done=false, data.fm_calculated=false;
//string line;
//fstream f_harris, f_harris_b;
ofstream matRT;
CImage Picture;
std::vector<double> distortion2;
CMatrix distortion_matrix2(1,4), intrinsic_matrix2(3,3);
string imgname;
string MenuJoystick=
"---------------------------Joystick Controls---------------------------\n\n"
"|1->Turn to the left 2->Turn to the right |\n"
"|3->Head to the middle 4->Head down 3&4->Head up |\n"
"|1&2->Go Home Up&3->Go Home and Dock 1&3->Get Report |\n"
"-----------------------------------------------------------------------\n";
string MenuKeyboard=
"---------------------------Keyboard Controls---------------------------\n"
"|MOVEMENT CAMERA PATHS |\n"
"|'w'->up 'c'->Middle 'r'->Start |\n"
"|'s'->down 'z'->Up 't'->Stop/Save |\n"
"|'a'->left 'x'->Down 'y'->Show |\n"
"|'d'->up 'u'->Delete |\n"
"|'q'->Move left 'i'->Run Ford |\n"
"|'e'->move Right 'o'->Run Back |\n"
"|'g'->Move to target position |\n"
"| |\n"
"|EXTRA |\n"
"|'-'->-Speed '+'->+Speed 'v'->Video |\n"
"|'h'->Gohome 'j'->menuJY 'r'->menuKB |\n"
"|'l'->ShowPosition 'n'->ShowEnc 'k'->Get Report |\n"
"|'f'->Ext Features 'b'->Match Feat '*'->FMatrix |\n"
"| '.'->EXIT |\n"
"-----------------------------------------------------------------------\n\n";
//Code START here
try
{
//initialization of Rovio Values (Ip, user and password)
data.Robot.Initialize(errormsg);
if(errormsg.empty())
{
cout<<"What do you want from the robot?\n\n";
cout<<MenuKeyboard<<endl;
//cout<<MenuJoystick<<endl;
//Calling Joystick in other thread
//createThread(JoystickControl,&data);
do{
decision=mrpt::system::os::getch();
switch(decision)
{
//Movement control
case 'w':
act=18;dir=1;break;
case 's':
act=18;dir=2;break;
case 'q':
act=18;dir=3;break;
case 'e':
act=18;dir=4;break;
case 'a':
act=18;dir=5;break;
case 'd':
act=18;dir=6;break;
//camera head movement
case 'z':
act=18;dir=11;break;
case 'x':
act=18;dir=12;break;
case 'c':
act=18;dir=13;break;
//Paths
case 'r': //start
act=2;
cout << "Recording Path" << endl; break;
case 't': //stop and save path
act=4;
cout << "Path Saved" << endl; break;
case 'y': //GetPath
act=6;
cout << "Getting saved Paths" << endl;
output=true; break;
case 'u': //delete path
act=5;
cout << "Path deleted" << endl; break;
case 'i': //fordward
act=7;
cout << "Running path Fordward mode" << endl; break;
case 'o': //backward
act=8;
cout << "Running path Backward mode" << endl; break;
//Speed control
case '-':
act=0;
if (speed<=7)
speed+=2;
cout << "Speed: "<< speed <<endl;
break;
case '+':
act=0;
if(speed>=3)
speed-=2;
cout << "Speed: "<< speed <<endl;
break;
//Camera On/Off
case 'v':
act=0;
if(!data.showvideo) //open the window
{
cout<<"VIDEO ON"<<endl;
data.showvideo=!data.showvideo;
screen_hd=createThread(videowindow, &data);
}
else //close the window
{
cout<<"VIDEO OFF"<<endl;
data.showvideo=!data.showvideo; // this kill the thread
joinThread(screen_hd); //Wait till thread finish
win.~CDisplayWindow();
}
break;
case 'h':
cout << "GOING HOME" << endl;
act=13;break;
case 'j':
act=0;
cout<<MenuJoystick<<endl;break;
case 'm':
act=0;
cout<<MenuKeyboard<<endl;break;
case 'k':
act=1; output=true;
cout <<"--------------------------REPORT------------------------" << endl;
break;
case 'l': //Print the position
act=0; output=false;
showPosition(data);
break;
case 'g': //Go to Target Position
goThere(data, response, errormsg);
break;
case 'n': //Print the encoder's values.
act=0; output=false;
a_enc=data.Robot.getEncoders();
print_Encoders(a_enc);
break;
case 'f': //Take Features
act=0; output=false;
data.features_taken=false; //this var checks if the function TestExtractFeatures has finished
featuring_hd=createThread(TestExtractFeatures, &data);
break;
case 'b': //Match Features without epipolar restriction
act=0; output=false;
data.matching_done=false; //this var checks if the function TestExtractFeatures has finished
data.matching_type=complete_match;
matching_hd=createThread(matching, &data);
break;
case '1': //Change the matching to complete matching, get all the correspondences using epipolar restriction
act=0; output=false;
data.matching_done=false; //this var checks if the function TestExtractFeatures has finished
data.matching_type=epipolar_match;
matching_hd=createThread(matching, &data);
break;
case '*': //Calculate Fundamental Matrix
act=0; output=false;
data.fm_calculated=false; //this var checks if the function TestExtractFeatures has finished
fm_hd=createThread(getFMat, &data);
break;
case '/': //Calculate Fundamental Matrix
act=0; output=false;
data.fm_calculated=false; //this var checks if the function TestExtractFeatures has finished
fm_hd=createThread(getFMat_from_txt, &data);
break;
case '2': //Calculate the Translation & Rotation Matrix for the camera
act=0; output=false;
getRTMat(&data);
matRT.open("RT.txt");
cout<<"\nRT-Matrix\n\n";
cout<<data.RT<< endl;
matRT << data.RT.inMatlabFormat() << endl;
matRT.close();
break;
case '3': //Get the camera's intrinsic matrix
act=0; output=false;
data.intrinsic_matrix.loadFromTextFile("intrinsic_matrix.txt");
cout<<data.intrinsic_matrix;
break;
case 'p': //Take Picture and show it in a window
act=0; output=false;
data.Robot.captureImageRect(Picture);
win.showImage(Picture);
break;
case '9': //Extract features from a stream and match them
act=0; output=false;
FeaturesStream(&data);
break;
case '8': //Track features
act=0; output=false;
Tracking(&data);
break;
case '7': //Rectify images
intrinsic_matrix2.loadFromTextFile("intrinsic_matrix.txt");
distortion_matrix2.loadFromTextFile("distortion_matrix.txt");
distortion2.resize(4);
distortion2[0] = distortion_matrix2.get_unsafe(0,0);
distortion2[1] = distortion_matrix2.get_unsafe(0,1);
distortion2[2] = distortion_matrix2.get_unsafe(0,2);
distortion2[3] = distortion_matrix2.get_unsafe(0,3);
imgname="1.jpg";
for(int i=1;Picture.loadFromFile(imgname);i++)
{
Picture.rectifyImageInPlace(intrinsic_matrix2, distortion2);
Picture.saveToFile(imgname);
imgname=format("%i.jpg",i);
}
break;
default:
act=0; //No action is executed
}//end switch
//EXECUTION
if (act!=0)
{
data.Robot.Moving(act,dir,speed, response, errormsg);
//a_enc=data.Robot.getEncoders();
//print_Encoders(a_enc);
}
if(output)
{
cout << "RESPONSE->" << response <<endl;
output=false;
}
if (!errormsg.empty()){
cout <<"---------------------------------------------------" << endl;
cout << "ERROR->" <<errormsg <<endl;
}
if(data.features_taken) //End features thread
joinThread(featuring_hd);
if(data.matching_done) //End matching thread
joinThread(matching_hd);
if(data.fm_calculated) //End getFM thread
joinThread(fm_hd);
mrpt::system::sleep(10);
}while(decision!='.');
}//end if
else
mrpt::system::pause();
}//end try
catch(std::exception &e)
{
cerr << e.what() << endl;
return -1;
}
return 0;
}
