ROS matcher v1

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

ASIFT_tests/demo_ASIFT_src/results/matching.txt

@@ -1,34 +1,36 @@
-40  2
+33  2
 0
-322.192  279.122  521.25  53.2794
-291.509  169.803  321.857  265.435
-361.771  216.725  6.50275e+18  1.3905e+18
-361.959  175.732  1.17666e-38  6.33158e-39
-261.936  171.287  0  0
-322.959  119.184  0  0
-374.229  132.773  4.44369e-36  6.60253e-36
-310.273  236.617  0  0
-230.152  191.051  2.42542e-35  1.79523e-35
-363.462  146.017  0  0
-377.929  162.983  1.17672e-34  7.60752e-35
-367.28  179.248  0  0
-354.104  176.741  1.47763e-35  8.19376e-36
-223.012  231.746  0  0
-317.773  232.706  2.49846e-34  7.0199e-35
-355.697  190.284  0  0
-362.208  184.382  0  0
-324.178  230.519  0  0
-368.527  140.516  0  0
-231.499  206.978  9506.8  5965.8
-371.585  152.449  31258.7  26473.7
-343.099  119.621  0  0
-300.647  157.174  0  0
-315.991  235.875  0  0
-221.354  204.377  0  0
-277.903  162.656  0  0
-341.004  112.32  0  0
-335.039  109.815  2063.98  6047.64
-294.302  268.305  37080.1  7738.15
-307.57  255.971  1473.99  304.215
-324.178  230.519  0  0
+133.472  77.1922  348.289  309.874
+202.823  132.712  160.121  83.1512
+175.339  82.9587  -3.74428  -4.10178
+129.237  138.035  1.70623e-38  5.60948e-39
+207.079  55.0473  0  0
+101.407  146.016  0  0
+71.1489  165.504  1.63422e-35  2.50249e-36
+208.451  55.0875  0  0
+112.768  136.519  1.29621e-35  3.96182e-36
+146.747  114.172  0  0
+134.927  116.608  1.29591e-34  6.45121e-35
+165.643  122.045  0  0
+204.456  133.983  5.02353e-36  2.5039e-36
+198.185  99.5892  0  0
+173.849  83.076  9.02463e-35  9.65457e-35
+185.841  120.421  0  0
+92.4551  132.039  0  0
+137.484  85.9481  0  0
+193.252  108.485  0  0
+200.814  86.5181  3305.1  4582.94
+209.967  62.4535  23424.9  49425
+148.299  78.2075  0  0
+173.672  83.9415  0  0
+187.827  102.638  0  0
+221.133  66.2506  0  0
+216.155  98.7816  0  0
+132.791  108.658  0  0
+198.88  106.059  3382.74  3213.08
+192.889  119.165  13061.8  9455.39
+227.783  47.7612  276.539  1016.59
+200.226  122.554  0  0
+187.17  103.672  0  0
+204.456  133.983  0  0
 1

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);
 
+				// 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);
 
 
-				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);
+					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();

rviz_interface/rviz_interface/launch/interface_camera.launch~

@@ -1,4 +1,5 @@
 <launch>
+	<node name="rviz" pkg="rviz" type="rviz"/>
 	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
-	<node name="plannar_seg" pkg="pcl_tests" type="plannar_seg"/>
+	<!--node name="plannar_seg" pkg="pcl_tests" type="plannar_seg"/-->
 </launch>

rviz_interface/rviz_interface/launch/interface_matching.launch

@@ -0,0 +1,5 @@
+<launch>
+	<node name="rviz" pkg="rviz" type="rviz"/>
+	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
+	<!--node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/-->
+</launch>

rviz_interface/rviz_interface/launch/interface_matching.launch~

@@ -0,0 +1,5 @@
+<launch>
+	<node name="rviz" pkg="rviz" type="rviz"/>
+	<node name="RvizInterface" pkg="rviz_interface" type="RvizInterface"/>
+	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher"/>
+</launch>