Resoluation du problème de référence

ASIFT_tests/demo_ASIFT_src/results/matching.txt

@@ -1,36 +1,24 @@
-33  2
+21  2
 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
+116.024  117.743  140.946  127.067
+127.041  182.984  82.7571  322.511
+223.925  126.146  9.11888e-14  8.90962e-14
+116.572  104.098  7.31246e-39  3.79738e-39
+258.196  39.082  0  0
+164.056  165.645  0  0
+202.577  116.285  8.7283e-36  7.36299e-36
+134.788  167.73  0  0
+152.242  132.98  2.61374e-35  3.22363e-35
+174.031  99.713  0  0
+130.144  161.018  1.79235e-34  4.25297e-34
+252.616  48.1934  0  0
+189.51  74.5927  4.82302e-35  1.42279e-35
+174.216  98.0268  0  0
+145.793  140.577  2.52858e-34  3.6943e-34
+175.301  105.345  0  0
+140.726  169.022  0  0
+199.218  95.4902  0  0
+136.131  159.282  0  0
+145.05  156.893  2800.66  5619.78
+130.144  161.018  19024.1  45141.4
 1

asift_match/CMakeLists.txt

@@ -29,8 +29,11 @@ find_package(catkin REQUIRED COMPONENTS
  pcl_ros
  sensor_msgs
 )
-#find_package(Eigen3 REQUIRED)
-#find_package(cmake_modules REQUIRED)
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 #add_compile_options(-std=c++11)

asift_match/CMakeLists.txt~

@@ -132,9 +132,9 @@ 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_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

asift_match/launch/asift_match.launch

@@ -0,0 +1,12 @@
+<launch>
+	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/references.txt"/>
+	<!--rosparam param="reference_data">[
+	"$(find asift_matching)/reference_data/train_image_000.png", 
+	"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam-->
+
+	<param name="object_center_topic" value="/object_center"/>
+	<param name="image_topic" value="/camera/rgb/image_raw"/>
+	<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
+
+	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
+</launch>

asift_match/launch/asift_match.launch~

@@ -0,0 +1,12 @@
+<launch>
+	<param name="reference_txt_path" value="$(find asift_matching)/reference_data/references.txt"/>
+	<rosparam param="reference_data">[
+	"$(find asift_matching)/reference_data/train_image_000.png", 
+	"$(find asift_matching)/reference_data/train_image_001.png"]</rosparam>
+
+	<param name="object_center_topic" value="/object_center"/>
+	<param name="image_topic" value="/camera/rgb/image_raw"/>
+	<param name="pointcloud_topic" value="/camera/depth_registered/points"/>
+
+	<node name="ASIFT_matching" pkg="asift_matching" type="ASIFT_matcher" output="screen"/>
+</launch>

asift_match/package.xml

@@ -7,7 +7,7 @@
   <!-- One maintainer tag required, multiple allowed, one person per tag -->
   <!-- Example:  -->
   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
-  <maintainer email="blue@todo.todo">blue</maintainer>
+  <maintainer email="antoineharle@etu.upmc.fr">Antoine Harle</maintainer>
 
 
   <!-- One license tag required, multiple allowed, one license per tag -->

asift_match/package.xml~

@@ -56,19 +56,13 @@
   <build_depend>pcl_conversions</build_depend>
   <build_depend>pcl_ros</build_depend>
   <build_depend>sensor_msgs</build_depend>
-
-  <build_export_depend>pcl_conversions</build_export_depend>
-  <build_export_depend>pcl_ros</build_export_depend>
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>sensor_msgs</build_export_depend>
-
-  <exec_depend>pcl_conversions</exec_depend>
-  <exec_depend>pcl_ros</exec_depend>
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>sensor_msgs</exec_depend>
-
   <build_depend>libpcl-all-dev</build_depend>
-  <exec_depend>libpcl-all</exec_depend>
+
+  <run_depend>libpcl-all</run_depend>
+  <run_depend>pcl_conversions</run_depend>
+  <run_depend>pcl_ros</run_depend>
+  <run_depend>roscpp</run_depend>
+  <run_depend>sensor_msgs</run_depend>
   <run_depend>roscpp</run_depend>
   <run_depend>tf</run_depend>
   <!--run_depend>message_runtime</run_depend-->

asift_match/setup.py

@@ -0,0 +1,17 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+	packages=['asift_matching'],
+	package_dir={'': 'src'},
+	)
+
+setup(**setup_args)
+
+

asift_match/setup.py~

@@ -0,0 +1,17 @@
+#!/usr/bin/python
+# -*- coding: utf-8 -*-
+
+## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+# fetch values from package.xml
+setup_args = generate_distutils_setup(
+	packages=['babbling_modules_emulator'],
+	package_dir={'': 'src'},
+	)
+
+setup(**setup_args)
+
+

asift_match/src/ROS_matcher.cpp

@@ -1,26 +1,43 @@
 #include "ROS_matcher.hpp"
 
-ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_IDLE)
+ROS_matcher::ROS_matcher(): _num_tilt(8), _status(MATCHER_STATUS_WAITING_INIT)
 {
-	// _center_pub = _nh.advertise<geometry_msgs::PointStamped>("/ROS_matcher/center", 10);
-	_center_pub = _nh.advertise<rviz_interface::NamedPoint>("/object_center", 1);
+	std::string center_topic, image_topic, pointcloud_topic, reference_txt_path;
+	std::vector<std::string> reference_data_paths;
 
-	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);
-	pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, "/camera/depth_registered/points", 1);
+	_nh.param<std::string>("object_center_topic", center_topic,"/ASIFT_matcher/object_center");
+	_nh.param<std::string>("image_topic", image_topic,"/camera/rgb/image_raw");
+	_nh.param<std::string>("pointcloud_topic", pointcloud_topic,"/camera/depth_registered/points");
+	
+	_center_pub = _nh.advertise<rviz_interface::NamedPoint>(center_topic, 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, image_topic, 1);
+	pointcloud_sub= new message_filters::Subscriber<sensor_msgs::PointCloud2>(_nh, pointcloud_topic, 1);
+	
 	Timesync = new message_filters::Synchronizer<MySyncPolicy>(MySyncPolicy(10),*info_sub, *image_sub, *pointcloud_sub);
 	Timesync-> registerCallback(boost::bind(&ROS_matcher::cameraCallback, this, _1, _2, _3));
 
-	// unsigned int nb_ref =2;
-	// std::string refData[] = {
- //      "train_image_000.png", 
- //      "train_image_001.png"};
+	if(_nh.getParam("reference_txt_path", reference_txt_path))
+	{
+		if(matcher.loadReferences(reference_txt_path.c_str()))
+			_status = MATCHER_STATUS_IDLE;
+	}
+	else if(_nh.getParam("reference_data", reference_data_paths))
+	{
+		for(unsigned int i=0; i<reference_data_paths.size(); i++)
+			matcher.addReference(reference_data_paths[i].c_str(),_num_tilt);
+		
+		if(matcher.getNbRef()>0)
+			_status = MATCHER_STATUS_IDLE;
+	}
+	else
+	{
+		ROS_WARN("No reference data to initialize matcher.");
+	}
 
- //    for(unsigned int i=0; i<nb_ref;i++)
-	// {
-	// 	matcher.addReference(refData[i].c_str(), _num_tilt);
-	// }
-	ROS_INFO("Matcher Ready !");
+	if(_status == MATCHER_STATUS_IDLE)
+		ROS_INFO("Matcher Ready ! (%d references)",matcher.getNbRef());
 }
 
 ROS_matcher::~ROS_matcher()