ROS matcher v1

A debugger (Problème de référence)

asift_match/CMakeLists.txt

@@ -132,6 +132,10 @@ target_link_libraries(ASIFT_matcher
  X11
 )
 
+#add_custom_command(TARGET ASIFT_matcher PRE_BUILD
+#                   COMMAND ${CMAKE_COMMAND} -E copy_directory
+#                       ${CMAKE_SOURCE_DIR}/book_training $<TARGET_FILE_DIR:ASIFT_matcher>)
+
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

asift_match/CMakeLists.txt~

@@ -96,7 +96,7 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O2 -ftree-vectorize -funroll-loops -L/u
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
   #CATKIN_DEPENDS roscpp tf message_runtime
-  INCLUDE_DIRS book_training
+  #INCLUDE_DIRS include
 )
 
 ###########
@@ -132,6 +132,10 @@ target_link_libraries(ASIFT_matcher
  X11
 )
 
+add_custom_command(TARGET ASIFT_matcher PRE_BUILD
+                   COMMAND ${CMAKE_COMMAND} -E copy_directory
+                       ${CMAKE_SOURCE_DIR}/book_training $<TARGET_FILE_DIR:ASIFT_matcher>)
+
 ## Add cmake target dependencies of the executable
 ## same as for the library above
 # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})

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)
+ASIFT_matcher::ASIFT_matcher(): _nb_refs(0), _total_num_matchings(0), _resize_imgs(false), _showDebug(false), _showInfo(false)
 {
 	default_sift_parameters(_siftParam);
 }
@@ -63,7 +63,8 @@ bool ASIFT_matcher::addReference(const char* image_path, unsigned int num_tilts)
     size_t w1=gray.width(), h1=gray.height();
     ipixels1.assign(gray.begin(), gray.end());
 
-    std::cout<<"Building reference from "<< image_path << std::endl;
+    if(_showInfo)
+    	std::cout<<"Building reference from "<< image_path << std::endl;
 
     return addReference(ipixels1, w1, h1, num_tilts);
 }
@@ -91,8 +92,9 @@ bool ASIFT_matcher::addReference(const vector<float>& image, unsigned int w, uns
 
 	if(_resize_imgs)
 	{
-		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;
+		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;
 		
@@ -171,8 +173,11 @@ bool ASIFT_matcher::addReference(const vector<float>& image, unsigned int w, uns
 
 	_nb_refs++;
 
-	cout<<"Reference built in "<< difftime(tend, tstart) << " seconds." << endl;
-	cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
+	if(_showInfo)
+	{
+		cout<<"Reference built in "<< difftime(tend, tstart) << " seconds." << endl;
+		cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
+	}
 
 	return true;
 }
@@ -225,7 +230,8 @@ unsigned int ASIFT_matcher::match(const char* image_path, unsigned int num_tilts
     size_t w1=gray.width(), h1=gray.height();
     ipixels1.assign(gray.begin(), gray.end());
 
-    std::cout<<"Matching from "<<image_path<<std::endl;
+    if(_showInfo)
+    	std::cout<<"Matching from "<<image_path<<std::endl;
 
     return match(ipixels1, w1, h1, num_tilts);
 }
@@ -254,8 +260,11 @@ unsigned int ASIFT_matcher::match(const vector<float>& image, unsigned int w, un
 
 	if(_resize_imgs)
 	{
-		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;
+		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;
 		
@@ -322,8 +331,11 @@ 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);
-	cout<< "Keypoints computation accomplished in " << difftime(tend, tstart) << " seconds." << endl;
-	cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
+	if(_showInfo)
+	{
+		cout<< "Keypoints computation accomplished in " << difftime(tend, tstart) << " seconds." << endl;
+		cout<<"	"<< num_keys <<" ASIFT keypoints found."<< endl;
+	}
 
 	//// Match ASIFT keypoints
 	_total_num_matchings=0;
@@ -332,8 +344,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;
 
-		cout << "Matching the keypoints..." << endl;
 		tstart = time(0);
 		try
 		{
@@ -347,7 +360,9 @@ 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);
-		cout << "Keypoints matching accomplished in " << difftime(tend, tstart) << " seconds." << endl;
+
+		if(_showInfo)
+			cout << "Keypoints matching accomplished in " << difftime(tend, tstart) << " seconds." << endl;
 
 		_num_matchings.push_back(num_matchings);
 		_total_num_matchings += num_matchings;
@@ -434,7 +449,9 @@ bool ASIFT_matcher::computeCenter(int& cx, int& cy) const
 //Return true if successfull 
 bool ASIFT_matcher::distFilter(int threshold=2)
 {
-	cout<<"filtering keypoint..."<<endl;
+	if(_showInfo)
+		cout<<"filtering keypoint..."<<endl;
+	
 	if(getNbMatch()==0)
 	{
 		cerr<<"Error : cannot filter points without matchs"<<endl;

asift_match/src/ASIFT_matcher.hpp

@@ -69,6 +69,8 @@ public:
 	void setResizeImg(bool resize_imgs){ _resize_imgs=resize_imgs;}
 	bool isShowingDebug() const{ return _showDebug;}
 	void showDebug(bool showDebug){ _showDebug=showDebug;}
+	bool isShowingInfo() const{ return _showInfo;}
+	void showInfo(bool showInfo){ _showInfo=showInfo;}
 
 	void print() const; //Debugging function
 
@@ -96,6 +98,7 @@ protected:
 	//Flags
 	bool _resize_imgs;// = false; //Resize images to IM_X/IM_Y ?
 	bool _showDebug;// = 0; //Show debugging messages ?
+	bool _showInfo; //Show info messages
 };
 
 #endif

asift_match/src/ROS_matcher.cpp

@@ -1,8 +1,9 @@
 #include "ROS_matcher.hpp"
 
-ROS_matcher::ROS_matcher(): _num_tilt(1), _status(MATCHER_STATUS_IDLE)
+ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_IDLE)
 {
-	_center_pub = _nh.advertise<geometry_msgs::PointStamped>("/ROS_matcher/center", 10);
+	// _center_pub = _nh.advertise<geometry_msgs::PointStamped>("/ROS_matcher/center", 10);
+	_center_pub = _nh.advertise<rviz_interface::NamedPoint>("/object_center", 1);
 
 	info_sub = new message_filters::Subscriber<sensor_msgs::CameraInfo>(_nh, "/camera/rgb/camera_info", 1);
 	image_sub = new message_filters::Subscriber<sensor_msgs::Image>(_nh, "/camera/rgb/image_raw", 1);
@@ -12,8 +13,8 @@ ROS_matcher::ROS_matcher(): _num_tilt(1), _status(MATCHER_STATUS_IDLE)
 
 	// unsigned int nb_ref =2;
 	// std::string refData[] = {
- //      "book_training/train_image_000.png", 
- //      "book_training/train_image_001.png"};
+ //      "train_image_000.png", 
+ //      "train_image_001.png"};
 
  //    for(unsigned int i=0; i<nb_ref;i++)
 	// {
@@ -32,14 +33,14 @@ ROS_matcher::~ROS_matcher()
 
 void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_msg, const sensor_msgs::Image::ConstPtr& image_msg, const sensor_msgs::PointCloud2::ConstPtr& pointcloud_msg)
 {
-	ROS_INFO("Callback");
+	// ROS_INFO("Callback");
 
 	if(_status == MATCHER_STATUS_IDLE)
 	{
 		unsigned int width = image_msg->width, height = image_msg->height;
 		std::vector<float> image(height*width);
 
-		// ROS_INFO("Size : %d - %d", height*width, height * image_msg->step);
+		// ROS_INFO("Image size : %d - %d", height, width);
 		//Conversion en niveau de gris
 		if(image_msg->encoding == "yuv422")
 		{
@@ -55,15 +56,17 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m
 		}
 
 		//Matching
-		if(matcher.getNbRef()<4)
+		if(matcher.getNbRef()<1)
 		{
+			ROS_INFO("Building reference...");
 			matcher.addReference(image, width, height, _num_tilt);
 		}
 		else
 		{
-		ROS_INFO("Matching..."); 
+		// ROS_INFO("Matching..."); 
 		_status = MATCHER_STATUS_PROCESSING;
 
+		ROS_INFO("Matching...");
 		int nb_match=0;
 		nb_match = matcher.match(image, width, height, _num_tilt);
 
@@ -74,8 +77,10 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m
 		{
 			//Publish 3D point
 			int cx, cy;
-			geometry_msgs::PointStamped center_msg;
-			//Provisoire
+			// geometry_msgs::PointStamped center_msg;
+			rviz_interface::NamedPoint center_msg;
+			matcher.distFilter(2);
+
 			if(matcher.computeCenter(cx, cy))
 			{
 				//Conversions des donnée d'entrée en PCL
@@ -84,12 +89,37 @@ void ROS_matcher::cameraCallback(const sensor_msgs::CameraInfo::ConstPtr& info_m
 				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
 				pcl::fromPCLPointCloud2(pcl_pc2,*cloud);
 
-				
-				
-				center_msg.header.frame_id = image_msg->header.frame_id;
-				center_msg.point.x=cx;
-				center_msg.point.y=cy;
-				_center_pub.publish(center_msg);
+				// ROS_INFO("PointCloud Size : %d / %d", cloud->height, cloud->width);
+				ROS_INFO("Center %f / %f",(float)cx/width,(float)cy/height);
+
+				if(cloud->isOrganized())
+				{
+					pcl::PointXYZ center = cloud->at(cx,cy);
+
+
+					if(!isnan(center.x) && !isnan(center.y) && !isnan(center.z))
+					{
+						center_msg.name = "6DOF";
+
+						center_msg.header.frame_id = image_msg->header.frame_id;
+						center_msg.point.x=center.x;
+						center_msg.point.y=center.y;
+						center_msg.point.z=center.z;
+						_center_pub.publish(center_msg);
+					}
+					else
+					{
+						ROS_WARN("NaN Values");
+					}
+				}
+				else
+				{
+					ROS_WARN("Pointcloud isn't organized");
+				}
+			}
+			else
+			{
+				ROS_WARN("Failed to compute center");
 			}
 		}
 

asift_match/src/ROS_matcher.hpp

@@ -12,8 +12,10 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/Image.h>
 #include <geometry_msgs/PointStamped.h>
+#include <rviz_interface/NamedPoint.h>
 
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl/impl/point_types.hpp>
 
 #include "ASIFT_matcher.hpp"
 

asift_match/src/book_training/train_image_005.yml~

@@ -0,0 +1,8 @@
+Book cMo for training image 005
+rows: 4
+cols: 4
+data:
+  - [-0.2314201876, -0.9583649151, 0.1672763771, 0.09835545579]
+  - [0.7484075924, -0.06552319445, 0.6599945353, -0.0974700766]
+  - [-0.6215551242, 0.2779269699, 0.7324109687, 0.5499983612]
+  - [0; 0; 0; 1]

asift_match/src/book_training/train_image_006.yml~

@@ -0,0 +1,8 @@
+Book cMo for training image 008
+rows: 4
+cols: 4
+data:
+  - [0.02063568325, -0.5653102458, -0.8246202123, 0.0403687505]
+  - [0.8210674394, 0.4801939642, -0.3086454546, -0.1745029756]
+  - [0.5704580865, -0.6706996964, 0.4740669666, 0.4630312508]
+  - [0, 0, 0, 1]

asift_match/src/book_training/train_image_007.yml~

@@ -0,0 +1,9 @@
+Book cMo for training image 007
+rows: 4
+cols: 4
+data:
+  - [-0.03609085509, -0.3148440768, 0.9484569877, 0.04713881051]
+  - [-0.8006242946, 0.5771011583, 0.1611055304, 0.02971868344]
+  - [-0.5980787482, -0.7535432704, -0.2728998912, 0.6240615433]
+  - [0, 0, 0, 1]
+

asift_match/src/compute_asift_keypoints.cpp

@@ -562,7 +562,7 @@ int compute_asift_keypoints(vector<float>& image, int width, int height, int num
       {
         num_keys_total += (int) keys_all[tt][rr].size();
       }				
-      printf("%d ASIFT keypoints are detected. \n", num_keys_total);
+      // printf("%d ASIFT keypoints are detected. \n", num_keys_total);
   }
 
   return num_keys_total;

asift_match/src/compute_asift_matches.cpp

@@ -764,25 +764,25 @@ int compute_asift_matches(int num_of_tilts1, int num_of_tilts2, int w1, int h1,
 				matchings = matchings_unique;
 				Minfoall = Minfoall_unique;     
 				
-				cout << "The two images match! " << matchings.size() << " matchings are identified. log(nfa)=" << nfa << "." << endl;
+				// cout << "The two images match! " << matchings.size() << " matchings are identified. log(nfa)=" << nfa << "." << endl;
 			}
 			else 
 			{
 				matchings.clear();
 				Minfoall.clear();
-				cout << "The two images do not match. The matching is not significant: log(nfa)=" << nfa << "." << endl;
+				// cout << "The two images do not match. The matching is not significant: log(nfa)=" << nfa << "." << endl;
 			}
 		}
 		else 
 		{
 			matchings.clear();
 			Minfoall.clear();
-			cout << "The two images do not match. Not enough matches to do epipolar filtering." << endl;		
+			// cout << "The two images do not match. Not enough matches to do epipolar filtering." << endl;		
 		}
 	}
 	else 
 	{
-		cout << "The two images do not match.\n" << endl;
+		// cout << "The two images do not match.\n" << endl;
 	}
 	
 	return matchings.size();