Amélioration Save/Load + Tests Robobo + modif gitIgnore

asift_match/launch/asift_match.launch~

@@ -1,6 +1,7 @@
 <launch>
 	<param name="tracked_object" value="6DOF"/>
 	<param name="num_tilt" type="int" value="2"/>
+	<param name="std_dev_filter_coeff" type="int" value="2"/>
 	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/book_references.txt"/>
 	<!--rosparam param="reference_data">[
 	"$(find asift_matching)/reference_data/train_image_000.png", 

asift_match/src/ASIFT_matcher.cpp

@@ -1,6 +1,6 @@
 #include "ASIFT_matcher.hpp"
 
-ASIFT_matcher::ASIFT_matcher(): _nb_refs(0), _total_num_matchings(0), _resize_imgs(false), _showDebug(false), _showInfo(false)
+ASIFT_matcher::ASIFT_matcher(): _nb_refs(0), _total_num_matchings(0), _resize_imgs(false), _showDebug(false), _showInfo(true)
 {
 	default_sift_parameters(_siftParam);
 }
@@ -64,7 +64,7 @@ bool ASIFT_matcher::addReference(const char* image_path, unsigned int num_tilts)
     ipixels1.assign(gray.begin(), gray.end());
 
     if(_showInfo)
-    	std::cout<<"Building reference from "<< image_path << std::endl;
+   		std::cout<<"Building reference from "<< image_path << std::endl;
 
     return addReference(ipixels1, w1, h1, num_tilts);
 }
@@ -174,10 +174,8 @@ bool ASIFT_matcher::addReference(const vector<float>& image, unsigned int w, uns
 	_nb_refs++;
 
 	if(_showInfo)
-	{
-		cout<<"Reference built in "<< difftime(tend, tstart) << " seconds." << endl;
-		cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
-	}
+		cout<<"Reference built in "<< difftime(tend, tstart) << " seconds." << endl
+		<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
 
 	return true;
 }
@@ -187,7 +185,7 @@ unsigned int ASIFT_matcher::match(const char* image_path, unsigned int num_tilts
 {
 	if(_nb_refs<=0)
 	{
-		cout<<"ASIFT_matcher Error : Trying to match without reference"<<endl;
+		std::cerr<<"ASIFT_matcher Error : Trying to match without reference"<<std::endl;
 		return 0;
 	}
 
@@ -261,10 +259,8 @@ unsigned int ASIFT_matcher::match(const vector<float>& image, unsigned int w, un
 	if(_resize_imgs)
 	{
 		if(_showInfo)
-		{
 			cout << "WARNING: The input image is resized to " << wS << "x" << hS << " for ASIFT. " << endl 
 			<< "         But the results will be normalized to the original image size." << endl << endl;
-		}
 		
 		float InitSigma_aa = 1.6;
 		
@@ -331,11 +327,10 @@ unsigned int ASIFT_matcher::match(const vector<float>& image, unsigned int w, un
 	num_keys = compute_asift_keypoints(ipixels1_zoom, wS1, hS1, num_tilts, _showDebug, keys, _siftParam);
 
 	tend = time(0);
+
 	if(_showInfo)
-	{
-		cout<< "Keypoints computation accomplished in " << difftime(tend, tstart) << " seconds." << endl;
-		cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
-	}
+		cout<< "Keypoints computation accomplished in " << difftime(tend, tstart) << " seconds." << endl
+		<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
 
 	//// Match ASIFT keypoints
 	_total_num_matchings=0;
@@ -344,9 +339,9 @@ unsigned int ASIFT_matcher::match(const vector<float>& image, unsigned int w, un
 	{
 		int num_matchings = 0;
 		matchingslist matchings;
+
 		if(_showInfo)
 			cout << "Matching the keypoints..." << endl;
-
 		tstart = time(0);
 		try
 		{
@@ -360,7 +355,6 @@ unsigned int ASIFT_matcher::match(const vector<float>& image, unsigned int w, un
 		// cout<< _keys[i].size()<< " " << _keys[i][0].size() <<" "<< _keys[i][0][0].size()<<endl;
 
 		tend = time(0);
-
 		if(_showInfo)
 			cout << "Keypoints matching accomplished in " << difftime(tend, tstart) << " seconds." << endl;
 
@@ -451,7 +445,6 @@ bool ASIFT_matcher::distFilter(int threshold=2)
 {
 	if(_showInfo)
 		cout<<"filtering keypoint..."<<endl;
-	
 	if(getNbMatch()==0)
 	{
 		cerr<<"Error : cannot filter points without matchs"<<endl;
@@ -542,12 +535,16 @@ bool ASIFT_matcher::saveReferences(const char* ref_path) const
 			// Follow the same convention of David Lowe: 
 			// the first line contains the number of keypoints and the length of the desciptors (128) 
 			// Added number of tilts
-			// Added sizes
-			file_key1 << _num_keys[j] << " " << VecLength << " " <<_size_refs[j].first<<" "<<_size_refs[j].second<<" "<<std::endl; //<<_num_tilts[j] << " "
-			for (int tt = 0; tt < (int) kps.size(); tt++)
+			// Added sizes (* zoom_ref useful ?)
+			file_key1 << _num_keys[j] << " " << VecLength << " " <<_size_refs[j].first*_zoom_refs[j]<<" "<<_size_refs[j].second*_zoom_refs[j]<<" "<<_num_tilts[j] << " "<<std::endl; //<<_num_tilts[j] << " "
+			for (int tt = 0; tt < (int) kps.size(); tt++) //kps.size = num_tilt
 			{
+				file_key1<<kps[tt].size()<<" "<<std::endl;
+
 				for (int rr = 0; rr < (int) kps[tt].size(); rr++)
 				{
+					file_key1<<kps[tt][rr].size()<<" "<<std::endl;
+
 					keypointslist::iterator ptr = kps[tt][rr].begin();
 					for(int i=0; i < (int) kps[tt][rr].size(); i++, ptr++)	
 					{
@@ -577,13 +574,12 @@ bool ASIFT_matcher::saveReferences(const char* ref_path) const
 
 //Load reference data necessary for the matching 
 //Doesn't load references images or Sift parameters
-//TODO : split data between different tilts
 bool ASIFT_matcher::loadReferences(const char* ref_path)
 {
 	std::ifstream ref_file(ref_path);
 	std::string line, tmp;
 	std::stringstream iss;
-	pair<int,int> size_tmp;
+	pair<int,int> img_size_tmp;
 	if (ref_file.is_open())
 	{
 		std::getline(ref_file, line);
@@ -593,7 +589,7 @@ bool ASIFT_matcher::loadReferences(const char* ref_path)
 		_keys = std::vector<asift_keypoints>(_nb_refs);
 		_num_keys = std::vector< int >(_nb_refs);
 		_size_refs= std::vector< pair<int,int> >(_nb_refs);
-		_num_tilts = std::vector< int >(_nb_refs,1);
+		_num_tilts = std::vector< int >(_nb_refs);
 		_zoom_refs = std::vector<float>(_nb_refs,1);
 		for(unsigned int i = 0; i<_nb_refs;i++)
 		{
@@ -610,61 +606,75 @@ bool ASIFT_matcher::loadReferences(const char* ref_path)
 				return false;
 			}
 
-			// std::getline(iss,tmp,' ');
-			// _num_tilts[i]=atoi(tmp.c_str());
+			std::getline(iss,tmp,' ');
+			img_size_tmp.first=atoi(tmp.c_str());
 
 			std::getline(iss,tmp,' ');
-			size_tmp.first=atoi(tmp.c_str());
+			img_size_tmp.second=atoi(tmp.c_str());
+
+			_size_refs[i]=img_size_tmp;
 
 			std::getline(iss,tmp,' ');
-			size_tmp.second=atoi(tmp.c_str());
+			_num_tilts[i]=atoi(tmp.c_str());
 
-			_size_refs[i]=size_tmp;
-
-			keypointslist list;
-			for(unsigned int j =0; j<(unsigned int)_num_keys[i];j++)
+			asift_keypoints nakp(_num_tilts[i]);
+			
+			for(unsigned int j =0; j<(unsigned int)_num_tilts[i];j++)
 			{
-				keypoint nkp;
-
 				std::getline(ref_file, line);
 				std::stringstream iss(line);
-				// if(j==0)
-				// 	cout<<line<<endl;
 
-				std::getline(iss,tmp,' ');
-				// std::stof(nb_ref, nkp.x); //C++11
-				nkp.x=atof(tmp.c_str());
-				// if(j<5)
-				// 	cout<<"x : "<<nkp.x<<endl;
-				std::getline(iss,tmp,' ');
-				nkp.y=atof(tmp.c_str());
-				// if(j<5)
-				// 	cout<<"y : "<<nkp.y<<endl;
-				std::getline(iss,tmp,' ');
-				nkp.scale=atof(tmp.c_str());
-				// if(j<5)
-				// 	cout<<"Scale : "<<nkp.scale<<endl;
-				std::getline(iss,tmp,' ');
-				nkp.angle=atof(tmp.c_str());
+				int veclist_size_tmp = atoi(line.c_str());
 
-				for(unsigned int k=0; k<(int) VecLength; k++)
+				std::vector< keypointslist > vkpl(veclist_size_tmp);
+
+				for(unsigned int k =0; k<(unsigned int)veclist_size_tmp;k++)
 				{
-					std::getline(iss,tmp,' ');
-					nkp.vec[k]=atof(tmp.c_str());
+					std::getline(ref_file, line);
+					std::stringstream iss(line);
+
+					int list_size_tmp = atoi(line.c_str());
+
+					keypointslist list(list_size_tmp);
+
+					for(unsigned int l =0; l<(unsigned int)list_size_tmp;l++)
+					{
+						keypoint nkp;
+
+						std::getline(ref_file, line);
+						std::stringstream iss(line);
+						// if(j==0)
+						// 	cout<<line<<endl;
+
+						std::getline(iss,tmp,' ');
+						// std::stof(nb_ref, nkp.x); //C++11
+						nkp.x=atof(tmp.c_str());
+						// if(j<5)
+						// 	cout<<"x : "<<nkp.x<<endl;
+						std::getline(iss,tmp,' ');
+						nkp.y=atof(tmp.c_str());
+						// if(j<5)
+						// 	cout<<"y : "<<nkp.y<<endl;
+						std::getline(iss,tmp,' ');
+						nkp.scale=atof(tmp.c_str());
+						// if(j<5)
+						// 	cout<<"Scale : "<<nkp.scale<<endl;
+						std::getline(iss,tmp,' ');
+						nkp.angle=atof(tmp.c_str());
+
+						for(unsigned int m=0; m<(int) VecLength; m++)
+						{
+							std::getline(iss,tmp,' ');
+							nkp.vec[m]=atof(tmp.c_str());
+						}
+
+						list[l]=nkp;
+					}
+					vkpl[k]=list;
 				}
-
-				list.push_back(nkp);
-				// if(j<5)
-				// {
-				// 	cout<<"x : "<<list[j].x<<endl;
-				// 	cout<<"y : "<<list[j].y<<endl;
-				// 	cout<<"Scale : "<<list[j].scale<<endl;
-				// }
-
+				nakp[j]=vkpl;
 			}
-			std::vector< keypointslist > vkps(1,list);
-			asift_keypoints akps(1,vkps);
-			_keys[i]=akps;
+			_keys[i]=nakp;
 			// std::getline(ref_file, line);
 		}
 	}
@@ -678,6 +688,156 @@ bool ASIFT_matcher::loadReferences(const char* ref_path)
 	return true;
 }
 
+// bool ASIFT_matcher::saveReferences(const char* ref_path) const
+// {
+// 	// Write all the keypoints (row, col, scale, orientation, desciptor (128 integers))
+// 	std::ofstream file_key1(ref_path);
+// 	if (file_key1.is_open())
+// 	{
+// 		file_key1<<_nb_refs<<" "<<std::endl;
+// 		for(unsigned int j=0; j<_keys.size();j++)
+// 		{
+// 			asift_keypoints kps =_keys[j];
+// 			// Follow the same convention of David Lowe: 
+// 			// the first line contains the number of keypoints and the length of the desciptors (128) 
+// 			// Added number of tilts
+// 			// Added sizes
+// 			file_key1 << _num_keys[j] << " " << VecLength << " " <<_size_refs[j].first<<" "<<_size_refs[j].second<<" "<<std::endl; //<<_num_tilts[j] << " "
+// 			for (int tt = 0; tt < (int) kps.size(); tt++) //kps.size = num_tilt
+// 			{
+// 				for (int rr = 0; rr < (int) kps[tt].size(); rr++)
+// 				{
+// 					keypointslist::iterator ptr = kps[tt][rr].begin();
+// 					for(int i=0; i < (int) kps[tt][rr].size(); i++, ptr++)	
+// 					{
+// 						file_key1 << _zoom_refs[j]*ptr->x << " " << _zoom_refs[j]*ptr->y << " " << _zoom_refs[j]*ptr->scale << " " << ptr->angle;
+
+// 						for (int ii = 0; ii < (int) VecLength; ii++)
+// 						{
+// 							file_key1 << " " << ptr->vec[ii];
+// 						}
+						
+// 						file_key1 << std::endl;
+// 					}
+// 				}	
+// 			}
+// 			// file_key1<<std::endl;
+// 		}
+// 	}
+// 	else 
+// 	{
+// 		std::cerr << "Unable to open the file :"<<ref_path;
+// 		return false;
+// 	}
+
+// 	file_key1.close();
+// 	return true;
+// }
+
+
+//Load reference data necessary for the matching 
+//Doesn't load references images or Sift parameters
+//TODO : split data between different tilts
+// bool ASIFT_matcher::loadReferences(const char* ref_path)
+// {
+// 	std::ifstream ref_file(ref_path);
+// 	std::string line, tmp;
+// 	std::stringstream iss;
+// 	pair<int,int> size_tmp;
+// 	if (ref_file.is_open())
+// 	{
+// 		std::getline(ref_file, line);
+// 		std::string::size_type sz;
+// 		// _nb_refs = std::stoi(line, &sz); //C++11
+// 		_nb_refs = atoi(line.c_str());
+// 		_keys = std::vector<asift_keypoints>(_nb_refs);
+// 		_num_keys = std::vector< int >(_nb_refs);
+// 		_size_refs= std::vector< pair<int,int> >(_nb_refs);
+// 		_num_tilts = std::vector< int >(_nb_refs,1);
+// 		_zoom_refs = std::vector<float>(_nb_refs,1);
+// 		for(unsigned int i = 0; i<_nb_refs;i++)
+// 		{
+// 			std::getline(ref_file, line);
+// 			std::stringstream iss(line);
+
+// 			std::getline(iss,tmp,' ');
+// 			_num_keys[i]=atoi(tmp.c_str());
+
+// 			std::getline(iss,tmp,' ');
+// 			if(VecLength!=atoi(tmp.c_str()))
+// 			{
+// 				std::cerr<<"Error VecLength doesn't correspond..."<<std::endl;
+// 				return false;
+// 			}
+
+// 			std::getline(iss,tmp,' ');
+// 			size_tmp.first=atoi(tmp.c_str());
+
+// 			std::getline(iss,tmp,' ');
+// 			size_tmp.second=atoi(tmp.c_str());
+
+// 			_size_refs[i]=size_tmp;
+
+// 			std::getline(iss,tmp,' ');
+// 			_num_tilts[i]=atoi(tmp.c_str());
+
+// 			keypointslist list;
+// 			for(unsigned int j =0; j<(unsigned int)_num_keys[i];j++)
+// 			{
+// 				keypoint nkp;
+
+// 				std::getline(ref_file, line);
+// 				std::stringstream iss(line);
+// 				// if(j==0)
+// 				// 	cout<<line<<endl;
+
+// 				std::getline(iss,tmp,' ');
+// 				// std::stof(nb_ref, nkp.x); //C++11
+// 				nkp.x=atof(tmp.c_str());
+// 				// if(j<5)
+// 				// 	cout<<"x : "<<nkp.x<<endl;
+// 				std::getline(iss,tmp,' ');
+// 				nkp.y=atof(tmp.c_str());
+// 				// if(j<5)
+// 				// 	cout<<"y : "<<nkp.y<<endl;
+// 				std::getline(iss,tmp,' ');
+// 				nkp.scale=atof(tmp.c_str());
+// 				// if(j<5)
+// 				// 	cout<<"Scale : "<<nkp.scale<<endl;
+// 				std::getline(iss,tmp,' ');
+// 				nkp.angle=atof(tmp.c_str());
+
+// 				for(unsigned int k=0; k<(int) VecLength; k++)
+// 				{
+// 					std::getline(iss,tmp,' ');
+// 					nkp.vec[k]=atof(tmp.c_str());
+// 				}
+
+// 				list.push_back(nkp);
+// 				// if(j<5)
+// 				// {
+// 				// 	cout<<"x : "<<list[j].x<<endl;
+// 				// 	cout<<"y : "<<list[j].y<<endl;
+// 				// 	cout<<"Scale : "<<list[j].scale<<endl;
+// 				// }
+
+// 			}
+// 			std::vector< keypointslist > vkps(1,list);
+// 			asift_keypoints akps(1,vkps);
+// 			_keys[i]=akps;
+// 			// std::getline(ref_file, line);
+// 		}
+// 	}
+// 	else 
+// 	{
+// 		std::cerr << "Unable to open the file :"<<ref_path;
+// 		return false;
+// 	}
+
+// 	ref_file.close();
+// 	return true;
+// }
+
 ASIFT_matcher& ASIFT_matcher::operator=(const ASIFT_matcher& m)
 {
 
@@ -707,10 +867,10 @@ void ASIFT_matcher::print() const
 {
 	for(unsigned int i=0; i< _keys.size();i++)
 	{
-		cout<<"Ref size:"<<i<<" - size :"<<_keys[i].size()<<endl;
+		cout<<"Ref :"<<i<<" - Nb tilts : "<<_num_tilts[i]<< " - Nb keys : "<< _num_keys[i]<<endl;
 		for(unsigned int j=0; j<_keys[i].size();j++)
 		{
-			cout<<"	"<<j<<" - size :"<<_keys[i][j].size()<<endl;
+			cout<<"	Tilt "<<j<<" - size :"<<_keys[i][j].size()<<endl;
 			for(unsigned int k=0; k<_keys[i][j].size();k++)
 			{
 				cout<<"		"<<k<<" - size :"<<_keys[i][j][k].size()<<endl;
@@ -731,4 +891,14 @@ void ASIFT_matcher::print() const
 			}
 		}
 	}
-}
+}
+
+// unsigned int ASIFT_matcher::getNbMatch() const
+// {
+// 	unsigned int res = 0;
+// 	for (unsigned int i=0;i<_num_matchings.size();i++)
+// 	{
+// 		res+=_num_matchings[i];
+// 	} 
+// 	return res;
+// }

asift_match/src/ASIFT_matcher.hpp

@@ -66,7 +66,7 @@ public:
 	const siftPar& getSiftPar() const{ return _siftParam;}
 	void setSiftPar(const siftPar &newSiftPar){ _siftParam = newSiftPar;}
 	bool isResizingImg() const{ return _resize_imgs;}
-	void setResizeImg(bool resize_imgs){ _resize_imgs=resize_imgs;}
+	void resizeImg(bool resize_imgs){ _resize_imgs=resize_imgs;}
 	bool isShowingDebug() const{ return _showDebug;}
 	void showDebug(bool showDebug){ _showDebug=showDebug;}
 	bool isShowingInfo() const{ return _showInfo;}
@@ -77,7 +77,6 @@ public:
 protected:
 
 	//Reference Images
-	// vector< image > _im_refs;
 	unsigned int _nb_refs;// = 0; //Number of reference images
 	vector< vector< float > > _im_refs; //Reference images used for matching
 	vector< pair<int,int> > _size_refs; //Width/Height

asift_match/src/ROS_matcher.cpp

@@ -25,6 +25,7 @@ ROS_matcher::ROS_matcher(): _status(MATCHER_STATUS_WAITING_INIT)
 	Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(2), *_image_sub, *_pointcloud_sub);
 	Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2));
 
+	matcher.showInfo(false);
 	//Load References
 	if(_nh.getParam("reference_txt_path", reference_txt_path))
 	{

asift_match/src/ROS_matcher.hpp

@@ -1,3 +1,10 @@
+/*
+ * ROS wrapper for the ASIFT_matcher object.
+ * Track an object described in the references in a RGBD stream and publish it's center.
+ * @author : antoine.harle@etu.upmc.Fr
+ * @see : ASIFT_matcher.cpp/.hpp, asift_match.launch
+ */
+
 #ifndef ROSMATCHER_HPP
 #define ROSMATCHER_HPP
 
@@ -39,16 +46,16 @@ protected:
 	// message_filters::Subscriber<sensor_msgs::CameraInfo>* info_sub;
 	message_filters::Subscriber<sensor_msgs::Image>* _image_sub;
 	message_filters::Subscriber<sensor_msgs::PointCloud2>* _pointcloud_sub;
-	
-	//Matcher
-	int _num_tilt, _filter_coeff;
-	ASIFT_matcher matcher;
-
-	MATCHER_STATUS _status;
 
 	message_filters::Synchronizer<MySyncPolicy>* Timesync;
+	
+	//Matcher
+	int _num_tilt, _filter_coeff; //Parameters of the ASIFT_matcher
+	ASIFT_matcher matcher; //Matcher
 
-	std::string tracked_object;
+	MATCHER_STATUS _status; //Matcher status
+
+	std::string tracked_object; //Name of the tracked object.
 
 public:
 	ROS_matcher();