re-calibrated the camera

willdzeng · Jul 20, 2016 · 52cf805 · 52cf805
1 parent d7ba5d0
commit 52cf805
Show file tree

Hide file tree

Showing 7 changed files with 51 additions and 23 deletions.
diff --git a/config/left.yaml b/config/left.yaml
@@ -1,20 +1,20 @@
 image_width: 1280
 image_height: 720
-camera_name: camera
+camera_name: narrow_stereo/left
 camera_matrix:
   rows: 3
   cols: 3
-  data: [713.929340, 0.000000, 679.052285, 0.000000, 715.011005, 328.170747, 0.000000, 0.000000, 1.000000]
+  data: [696.262741, 0.000000, 664.582790, 0.000000, 696.715205, 331.199610, 0.000000, 0.000000, 1.000000]
 distortion_model: plumb_bob
 distortion_coefficients:
   rows: 1
   cols: 5
-  data: [-0.173005, 0.017162, 0.000777, -0.002202, 0.000000]
+  data: [-0.174138, 0.025148, -0.000023, 0.001127, 0.000000]
 rectification_matrix:
   rows: 3
   cols: 3
-  data: [0.996855, -0.010879, 0.078501, 0.011292, 0.999925, -0.004824, -0.078442, 0.005695, 0.996902]
+  data: [0.999987, 0.000814, 0.005020, -0.000801, 0.999996, -0.002658, -0.005022, 0.002654, 0.999984]
 projection_matrix:
   rows: 3
   cols: 4
-  data: [716.527831, 0.000000, 664.431450, 0.000000, 0.000000, 716.527831, 342.078526, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+  data: [696.388858, 0.000000, 648.123726, 0.000000, 0.000000, 696.388858, 345.289268, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/config/right.yaml b/config/right.yaml
@@ -1,20 +1,20 @@
 image_width: 1280
 image_height: 720
-camera_name: camera
+camera_name: narrow_stereo/right
 camera_matrix:
   rows: 3
   cols: 3
-  data: [719.216628, 0.000000, 632.224929, 0.000000, 719.553447, 362.122099, 0.000000, 0.000000, 1.000000]
+  data: [697.561169, 0.000000, 619.174651, 0.000000, 697.422254, 364.054801, 0.000000, 0.000000, 1.000000]
 distortion_model: plumb_bob
 distortion_coefficients:
   rows: 1
   cols: 5
-  data: [-0.178709, 0.026960, 0.001302, -0.002090, 0.000000]
+  data: [-0.167196, 0.022090, 0.000330, -0.000437, 0.000000]
 rectification_matrix:
   rows: 3
   cols: 3
-  data: [0.996074, -0.013859, -0.087430, 0.014320, 0.999887, 0.004641, 0.087356, -0.005875, 0.996160]
+  data: [0.999946, -0.000419, -0.010401, 0.000447, 0.999996, 0.002653, 0.010400, -0.002658, 0.999942]
 projection_matrix:
   rows: 3
   cols: 4
-  data: [716.527831, 0.000000, 664.431450, -432.282493, 0.000000, 716.527831, 342.078526, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
+  data: [696.388858, 0.000000, 648.123726, -359.466975, 0.000000, 696.388858, 345.289268, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]
diff --git a/launch/camera_calibration.launch b/launch/camera_calibration.launch
@@ -1,11 +1,14 @@
 <launch>
-	<arg name="size" default="8x6"/>
-	<arg name="square" default="0.108"/>
-	<arg name="no-service-check" default="true"/>
-
-	<node pkg="camera_calibration" type="cameracalibrator.py" name="camera_calibration" output="screen" args="--size $(arg size) --square $(arg square) --no-service-check">
-		<remap from="right" to="/camera/right/image_raw"/>
-		<remap from="left" to="/camera/left/image_raw"/>
-		<param name="size" value="8x6"/>
-	</node>
+
+    <include file="$(find zed_cpu_ros)/launch/zed_cpu_ros.launch"/>
+
+    <arg name="size" default="8x6"/>
+    <arg name="square" default="0.108"/>
+    <arg name="no-service-check" default="true"/>
+
+    <node pkg="camera_calibration" type="cameracalibrator.py" name="camera_calibration" output="screen" args="">
+        <remap from="right" to="/camera/right/image_raw"/>
+        <remap from="left" to="/camera/left/image_raw"/>
+    </node>
+
 </launch>
diff --git a/launch/depth.launch b/launch/depth.launch
@@ -0,0 +1,9 @@
+<launch>
+	<include file="$(find zed_cpu_ros)/launch/image_proc.launch"/>
+	<group ns="camera">
+		<node pkg="nodelet" type="nodelet" name="depth_nodelet_manager"  args="manager"/>
+	    <node pkg="nodelet" type="nodelet" name="disparity" args="load stereo_image_proc/disparity depth_nodelet_manager"/>
+	    <node pkg="nodelet" type="nodelet" name="depth" args="load rtabmap_ros/disparity_to_depth depth_nodelet_manager"/>
+	</group>
+
+</launch>
diff --git a/launch/image_proc.launch b/launch/image_proc.launch
@@ -7,6 +7,14 @@
   	<remap from="left/camera_info" to="/camera/left/camera_info"/>
   	<remap from="right/image_raw" to="/camera/right/image_raw"/>
   	<remap from="right/camera_info" to="/camera/right/camera_info"/>
+  	<param name="min_disparity" value="1"/>
+  	<param name="correlation_window_size" value="20"/>
   </node>
 
+	<group ns="camera">
+		<node pkg="nodelet" type="nodelet" name="depth_nodelet_manager"  args="manager"/>
+		<node pkg="nodelet" type="nodelet" name="depth" args="load rtabmap_ros/disparity_to_depth depth_nodelet_manager"/>
+	</group>
+
+
 </launch>
diff --git a/launch/zed_cpu_ros.launch b/launch/zed_cpu_ros.launch
@@ -7,7 +7,11 @@
 	<param name="frame_rate" value="30"/>
 	<param name="config_file_location" value="$(arg config_file_location)"/>
 	<param name="show_image" value="false"/>
-	<param name="left_frame_id" value="left_camera"/>
-	<param name="right_frame_id" value="right_camera"/>
+	<param name="left_frame_id" value="left_frame"/>
+	<param name="right_frame_id" value="left_frame"/>
 </node>
+
+<node pkg="tf" type="static_transform_publisher" name="static_tf_1" args="0.25 0 0.4 0 0 0 1 base_link left_frame 30"/>
+<!-- <node pkg="tf" type="static_transform_publisher" name="static_tf_2" args="0 0.120 0 0 0 0 1 left_frame right_frame 30"/> -->
+
 </launch>
diff --git a/src/zed_cpu_ros.cpp b/src/zed_cpu_ros.cpp
@@ -149,9 +149,10 @@ class ZedCameraROS {
 
 		sensor_msgs::CameraInfo left_info, right_info;
 
+
 		ROS_INFO("Try load camera calibration files");
 		if (load_zed_config_) {
-			// get camera info from zed 
+			// get camera info from zed
 			try {
 				getZedCameraInfo(config_file_location_, resolution_, left_info, right_info);
 			}
@@ -160,7 +161,7 @@ class ZedCameraROS {
 				throw e;
 			}
 		} else {
-			// get config from the left, right.yaml in config 
+			// get config from the left, right.yaml in config
 			camera_info_manager::CameraInfoManager info_manager(nh);
 			info_manager.setCameraName("zed/left");
 			info_manager.loadCameraInfo( "package://zed_cpu_ros/config/left.yaml");
@@ -169,6 +170,9 @@ class ZedCameraROS {
 			info_manager.setCameraName("zed/right");
 			info_manager.loadCameraInfo( "package://zed_cpu_ros/config/right.yaml");
 			right_info = info_manager.getCameraInfo();
+
+			left_info.header.frame_id = left_frame_id_;
+			right_info.header.frame_id = right_frame_id_;
 		}
 
 		// std::cout << left_info << std::endl;