Stereo Calibration for Low resolution

This commit is contained in:
AhmedElsafy 2019-12-20 16:49:49 -04:00
parent 6615e8d36c
commit 40663b2f5d
7 changed files with 102 additions and 9 deletions

View File

@ -1,5 +1,5 @@
image_width: 1920 image_width: 320
image_height: 1080 image_height: 240
camera_name: left camera_name: left
camera_matrix: camera_matrix:
rows: 3 rows: 3

View File

@ -1,5 +1,5 @@
image_width: 1920 image_width: 320
image_height: 1080 image_height: 240
camera_name: right camera_name: right
camera_matrix: camera_matrix:
rows: 3 rows: 3

BIN
calibrationdata.tar.gz Normal file

Binary file not shown.

View File

@ -0,0 +1,53 @@
<launch>
<!-- Command Line Arguments -->
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
<arg name="width" default="320" /> <!-- Image Width -->
<arg name="height" default="240" /> <!-- Image Height -->
<arg name="fps" default="30" /> <!-- Desired framerate -->
<!-- Make arguments available to parameter server -->
<param name="image_width" type="int" value="$(arg width)" />
<param name="image_height" type="int" value="$(arg height)" />
<param name="target_fps" type="int" value="$(arg fps)" />
<group ns="stereo">
<node name="right" pkg="gscam" type="gscam" output='screen' >
<param name="camera_name" value="right" />
<param name="gscam_config" value="nvcamerasrc sensor-id=0 auto-exposure=1 aeLock=1 contrast=0.7 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420,framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="camera_id" value="0" />
<param name="frame_id" value="right_link" />
<param name="sync_sink" value="true" />
<param name="right/image_width" type="int" value="$(arg width)" />
<param name="right/image_height" type="int" value="$(arg height)" />
<param name="right/target_fps" type="int" value="$(arg fps)" />
<param name="camera_info_url" value="package://jetson_csi_cam/right_320x240.yaml" />
<remap from="camera/image_raw" to="right/image_raw" />
<remap from="camera/camera_info" to="right/camera_info" />
<remap from="set_camera_info" to="camera_info" />
</node>
<node name="left" pkg="gscam" type="gscam" output='screen' >
<param name="camera_name" value="left" />
<param name="gscam_config" value="nvcamerasrc sensor-id=1 auto-exposure=1 aeLock=1 contrast=0.7 ! video/x-raw(memory:NVMM), width=(int)$(arg width), height=(int)$(arg height), format=(string)I420, framerate=(fraction)$(arg fps)/1 ! nvvidconv flip-method=2 ! video/x-raw, format=(string)BGRx ! videoconvert ! video/x-raw, format=(string)BGR" />
<param name="frame_id" value="left_link" />
<param name="sync_sink" value="true" />
<param name="left/image_width" type="int" value="$(arg width)" />
<param name="left/image_height" type="int" value="$(arg height)" />
<param name="left/target_fps" type="int" value="$(arg fps)" />
<param name="camera_info_url" value="package://jetson_csi_cam/left_320x240.yaml" />
<remap from="camera/image_raw" to="left/image_raw" />
<remap from="camera/camera_info" to="left/camera_info" />
<remap from="/set_camera_info" to="/left/set_camera_info" />
</node>
<!--<node name="both" pkg="stereo_image_proc" type="stereo_image_proc" output="screen" >
<param name="stereo" value="stereo"/>
<param name="image" value="image_rect"/>
<param name="_approximate_sync" value="true"/>
<param name="queue_size" value="10000"/>
</node>-->
</group>
</launch>

20
left_320x240.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 320
image_height: 240
camera_name: left
camera_matrix:
rows: 3
cols: 3
data: [244.429325, 0.000000, 165.897763, 0.000000, 325.915464, 123.820184, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.412729, 0.153568, -0.000334, -0.003140, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [0.920705, 0.002779, 0.390251, -0.008226, 0.999891, 0.012287, -0.390174, -0.014523, 0.920627]
projection_matrix:
rows: 3
cols: 4
data: [343.984335, 0.000000, -82.478912, 0.000000, 0.000000, 343.984335, 132.359158, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

20
right_320x240.yaml Normal file
View File

@ -0,0 +1,20 @@
image_width: 320
image_height: 240
camera_name: right
camera_matrix:
rows: 3
cols: 3
data: [231.318831, 0.000000, 152.641652, 0.000000, 308.097601, 127.750465, 0.000000, 0.000000, 1.000000]
distortion_model: plumb_bob
distortion_coefficients:
rows: 1
cols: 5
data: [-0.373994, 0.139767, 0.000333, -0.001028, 0.000000]
rectification_matrix:
rows: 3
cols: 3
data: [0.918165, 0.031804, 0.394919, -0.026285, 0.999467, -0.019379, -0.395324, 0.007413, 0.918512]
projection_matrix:
rows: 3
cols: 4
data: [343.984335, 0.000000, -82.478912, -22.005200, 0.000000, 343.984335, 132.359158, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000]

View File

@ -7,9 +7,9 @@
<!-- Command Line Arguments --> <!-- Command Line Arguments -->
<arg name="sync_sink" default="true" /> <!-- Synchronize the app sink --> <arg name="sync_sink" default="true" /> <!-- Synchronize the app sink -->
<arg name="width" default="1280" /> <!-- Image Width --> <arg name="width" default="320" /> <!-- Image Width -->
<arg name="height" default="720" /> <!-- Image Height --> <arg name="height" default="240" /> <!-- Image Height -->
<arg name="fps" default="60" /> <!-- Desired framerate --> <arg name="fps" default="30" /> <!-- Desired framerate -->
<!-- Make arguments available to parameter server --> <!-- Make arguments available to parameter server -->
<param name="image_width" type="int" value="$(arg width)" /> <param name="image_width" type="int" value="$(arg width)" />
@ -27,7 +27,7 @@
<param name="right/image_width" type="int" value="$(arg width)" /> <param name="right/image_width" type="int" value="$(arg width)" />
<param name="right/image_height" type="int" value="$(arg height)" /> <param name="right/image_height" type="int" value="$(arg height)" />
<param name="right/target_fps" type="int" value="$(arg fps)" /> <param name="right/target_fps" type="int" value="$(arg fps)" />
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-right.yaml" /> <param name="camera_info_url" value="package://jetson_csi_cam/right_320x240.yaml" />
<!--<param name="image_encoding_" value="jpeg" />--> <!--<param name="image_encoding_" value="jpeg" />-->
<remap from="camera/image_raw" to="right/image_raw" /> <remap from="camera/image_raw" to="right/image_raw" />
<remap from="camera/camera_info" to="right/camera_info" /> <remap from="camera/camera_info" to="right/camera_info" />
@ -43,7 +43,7 @@
<param name="left/image_width" type="int" value="$(arg width)" /> <param name="left/image_width" type="int" value="$(arg width)" />
<param name="left/image_height" type="int" value="$(arg height)" /> <param name="left/image_height" type="int" value="$(arg height)" />
<param name="left/target_fps" type="int" value="$(arg fps)" /> <param name="left/target_fps" type="int" value="$(arg fps)" />
<param name="camera_info_url" value="package://jetson_csi_cam/calibration-left.yaml" /> <param name="camera_info_url" value="package://jetson_csi_cam/left_320x240.yaml" />
<!--<param name="image_encoding_" value="jpeg" />--> <!--<param name="image_encoding_" value="jpeg" />-->
<remap from="camera/image_raw" to="left/image_raw" /> <remap from="camera/image_raw" to="left/image_raw" />
<remap from="camera/camera_info" to="left/camera_info" /> <remap from="camera/camera_info" to="left/camera_info" />