diff --git a/calibration/cal.yaml b/calibration/cal.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5c091b531a7cf533cc70dd494becb28abf649757
--- /dev/null
+++ b/calibration/cal.yaml
@@ -0,0 +1,20 @@
+image_width: 1920
+image_height: 1080
+camera_name: narrow_stereo
+camera_matrix:
+ rows: 3
+ cols: 3
+ data: [1443.836053, 0, 700.72761, 0, 1462.939972, 473.741164, 0, 0, 1]
+distortion_model: plumb_bob
+distortion_coefficients:
+ rows: 1
+ cols: 5
+ data: [0.153747, -0.347937, -0.01007, 0.029807, 0]
+rectification_matrix:
+ rows: 3
+ cols: 3
+ data: [1, 0, 0, 0, 1, 0, 0, 0, 1]
+projection_matrix:
+ rows: 3
+ cols: 4
+ data: [1460.615234, 0, 728.385954, 0, 0, 1515.619141, 465.957632, 0, 0, 0, 1, 0]
diff --git a/launch/usb_cam_tag.launch b/launch/usb_cam_tag.launch
index 0393844d763a9f61fb337b426757450016a97ce8..80f9af394c211f2c23c1c72d2dd3f8215ab161b9 100644
--- a/launch/usb_cam_tag.launch
+++ b/launch/usb_cam_tag.launch
@@ -11,6 +11,7 @@
<param name="io_method" type="string" value="mmap"/>
<param name="image_width" type="int" value="1920"/>
<param name="image_height" type="int" value="1080"/>
+ <param name="framerate" type="int" value="5"/>
<param name="pixel_format" type="string" value="yuyv"/>
<param name="framerate" type="int" value="3"/>
<param name="camera_info_url" type="string" value="file://$(find iri_image_local_binarization)/calibration/usb_cam.yaml" />
@@ -48,13 +49,18 @@
to="/camera_out/image_raw"/>
</node>
- <node name="image_th"
- pkg="image_view"
- type="image_view"
- output="screen">
- <remap from="/image"
- to="/local_th/image_out/image_raw"/>
- </node>
+
+ <group ns="cam1">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam1/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam1/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
<!-- <node name="ar_pose"
pkg ="ar_pose"
@@ -70,15 +76,201 @@
to="/local_th/image_out/camera_info"/>
</node>-->
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam1/camera/image_raw"
+ to="/cam1/local_th/image_out/image_raw"/>
+ <remap from="/cam1/camera/camera_info"
+ to="/cam1/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
+ <group ns="cam2">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam2/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam2/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
+
+<!-- <node name="image_th"
+ pkg="image_view"
+ type="image_view"
+ output="screen">
+ <remap from="/cam2/image"
+ to="/cam2/local_th/image_out/image_raw"/>
+ </node>-->
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam2/camera/image_raw"
+ to="/cam2/local_th/image_out/image_raw"/>
+ <remap from="/cam2/camera/camera_info"
+ to="/cam2/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
+ <group ns="cam3">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam3/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam3/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
+
+<!-- <node name="image_th"
+ pkg="image_view"
+ type="image_view"
+ output="screen">
+ <remap from="/cam3/image"
+ to="/cam3/local_th/image_out/image_raw"/>
+ </node>-->
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam3/camera/image_raw"
+ to="/cam3/local_th/image_out/image_raw"/>
+ <remap from="/cam3/camera/camera_info"
+ to="/cam3/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
+ <group ns="cam4">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam4/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam4/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
+
+<!-- <node name="image_th"
+ pkg="image_view"
+ type="image_view"
+ output="screen">
+ <remap from="/cam4/image"
+ to="/cam4/local_th/image_out/image_raw"/>
+ </node>-->
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam4/camera/image_raw"
+ to="/cam4/local_th/image_out/image_raw"/>
+ <remap from="/cam4/camera/camera_info"
+ to="/cam4/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
+ <group ns="cam5">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam5/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam5/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
+
+<!-- <node name="image_th"
+ pkg="image_view"
+ type="image_view"
+ output="screen">
+ <remap from="/cam5/image"
+ to="/cam5/local_th/image_out/image_raw"/>
+ </node>-->
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam5/camera/image_raw"
+ to="/cam5/local_th/image_out/image_raw"/>
+ <remap from="/cam5/camera/camera_info"
+ to="/cam5/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
+ <group ns="cam6">
+ <node name="local_th"
+ pkg="iri_image_local_binarization"
+ type="iri_image_local_binarization"
+ output="screen">
+ <param name="window_size" value="15"/>
+ <remap from="/cam6/local_th/image_in/camera_info"
+ to="/camera_out/camera_info"/>
+ <remap from="/cam6/local_th/image_in/image_raw"
+ to="/camera_out/image_raw"/>
+ </node>
+
+<!-- <node name="image_th"
+ pkg="image_view"
+ type="image_view"
+ output="screen">
+ <remap from="/cam6/image"
+ to="/cam6/local_th/image_out/image_raw"/>
+ </node>-->
+
+ <node name="ar_pose"
+ pkg ="ar_pose"
+ type="ar_multi"
+ respawn="false"
+ output="screen">
+ <param name="marker_pattern_list" type="string" value="$(find ar_pose)/data/object_4x4"/>
+ <param name="threshold" type="int" value="100"/>
+ <param name="use_history" type="bool" value="true"/>
+ <remap from="/cam6/camera/image_raw"
+ to="/cam6/local_th/image_out/image_raw"/>
+ <remap from="/cam6/camera/camera_info"
+ to="/cam6/local_th/image_out/camera_info"/>
+ </node>
+ </group>
+
<!-- <node name="world2camera"
pkg ="tf"
type="static_transform_publisher"
args="0 0 0.0 -1.57 0 -1.57 world camera 1" />-->
- <node name="dynamic_reconfigure"
- pkg ="dynamic_reconfigure"
- type="reconfigure_gui"/>
-
<!-- <node name="rviz"
pkg="rviz"
type="rviz"
diff --git a/src/image_local_binarization_alg_node.cpp b/src/image_local_binarization_alg_node.cpp
index 7de20444143c7681ddd92221bf9f2292b6479b20..e7dd33900b1b929317bf705808cc9551cacc9b05 100644
--- a/src/image_local_binarization_alg_node.cpp
+++ b/src/image_local_binarization_alg_node.cpp
@@ -7,11 +7,11 @@ ImageLocalBinarizationAlgNode::ImageLocalBinarizationAlgNode(void) :
this->loop_rate_ = 1;//in [Hz]
// [init publishers]
- this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 10);
+ this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
this->histogram_publisher_ = this->it.advertise("hystogram", 1);
// [init subscribers]
- this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 10, &ImageLocalBinarizationAlgNode::image_in_callback,this);
+ this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 1, &ImageLocalBinarizationAlgNode::image_in_callback,this);
pthread_mutex_init(&this->image_in_mutex_,NULL);
diff --git a/src/image_local_binarization_alg_nodelet.cpp b/src/image_local_binarization_alg_nodelet.cpp
index a1e193c3c36292c7b14680a43fbc530bc3fb4568..165e9e1acf25e999161e129de41d83f7749e468e 100644
--- a/src/image_local_binarization_alg_nodelet.cpp
+++ b/src/image_local_binarization_alg_nodelet.cpp
@@ -7,11 +7,11 @@ ImageLocalBinarizationAlgNodelet::ImageLocalBinarizationAlgNodelet(ros::NodeHand
this->loop_rate_ = 1;//in [Hz]
// [init publishers]
- this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 10);
+ this->image_out_publisher_ = this->it.advertiseCamera("image_out/image_raw", 1);
this->histogram_publisher_ = this->it.advertise("hystogram", 1);
// [init subscribers]
- this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 10, &ImageLocalBinarizationAlgNodelet::image_in_callback,this);
+ this->image_in_subscriber_ = this->it.subscribeCamera("image_in/image_raw", 1, &ImageLocalBinarizationAlgNodelet::image_in_callback,this);
pthread_mutex_init(&this->image_in_mutex_,NULL);
this->image_out_=cv_bridge::CvImagePtr(new cv_bridge::CvImage);