essential matrix scale estimation

raphaelchang · Oct 10, 2019 · 35a62a3 · 35a62a3
1 parent 858f569
commit 35a62a3
Show file tree

Hide file tree

Showing 19 changed files with 181 additions and 69 deletions.
diff --git a/launch/default_camera.yaml b/launch/default_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 294.912
+    fy: 294.912
+    cx: 512
+    cy: 512
+    chi: 0.3
+    alpha: 0.6666667
+
+stereo_camera_parameters:
+    fx: 294.912
+    fy: 294.912
+    cx: 512
+    cy: 512
+    chi: 0.3
+    alpha: 0.6666667
+    tf_t: [2.0, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/default_camera_60.yaml b/launch/default_camera_60.yaml
@@ -0,0 +1,15 @@
+camera_model: perspective
+
+camera_parameters:
+    fx: 1024
+    fy: 1024
+    cx: 512
+    cy: 512
+
+stereo_camera_parameters:
+    fx: 1024
+    fy: 1024
+    cx: 512
+    cy: 512
+    tf_t: [2.0, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/kitti_camera.yaml b/launch/kitti_camera.yaml
@@ -0,0 +1,15 @@
+camera_model: perspective
+
+camera_parameters:
+    fx: 721.5377
+    fy: 721.5377
+    cx: 609.5593
+    cy: 172.854
+
+stereo_camera_parameters:
+    fx: 721.5377
+    fy: 721.5377
+    cx: 609.5593
+    cy: 172.854
+    tf_t: [0.533, 0.005, -0.001]
+    tf_r: [0, 0, 0, 1.0]
diff --git a/launch/matching_eval.launch b/launch/matching_eval.launch
@@ -2,8 +2,7 @@
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).matching.hdf5" />
     <arg name="rate" default="1" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 1024, fy: 1024, cx: 512, cy: 512, chi: 0.0, alpha: 0.0}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="detector_type" default="ORB" />
     <arg name="detector_params" default="{nfeatures: 100}" />
     <arg name="descriptor_type" default="ORB" />
@@ -17,9 +16,8 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="matched_image_topic" value="/omni_slam/matched" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: '$(arg detector_type)'
             detector_parameters: $(arg detector_params)
             descriptor_type: '$(arg descriptor_type)'

diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).odometry.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_odometry_eval_node" name="omni_slam_odometry_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -19,9 +18,8 @@
         <param name="path_ground_truth_topic" value="/omni_slam/odometry_path_truth" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 2000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1

diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).reconstruction.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_reconstruction_eval_node" name="omni_slam_reconstruction_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -14,9 +13,8 @@
         <param name="point_cloud_topic" value="/omni_slam/reconstructed" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1

diff --git a/launch/slam_eval.launch b/launch/slam_eval.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
-    <arg name="stereo_camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [2.0, 0.0, 0.0]
-            stereo_tf_r: [0.0, 0.0, 0.0, 1.0]
         </rosparam>
     </node>
 </launch>

diff --git a/launch/slam_eval_kitti.launch b/launch/slam_eval_kitti.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="perspective" />
-    <arg name="camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
-    <arg name="stereo_camera_params" default="{fx: 721.5377, fy: 721.5377, cx: 609.5593, cy: 172.854}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/kitti_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="imu" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.533, 0.005, -0.001]
-            stereo_tf_r: [0, 0, 0, 1.0]
         </rosparam>
     </node>
 </launch>

diff --git a/launch/slam_eval_t265.launch b/launch/slam_eval_t265.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
-    <arg name="stereo_camera_params" default="{fx: 287.405, fy: 287.092, cx: 418.432, cy: 399.257, chi: -0.0000588, alpha: 0.675621}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/t265_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
             keyframe_interval: 1
@@ -54,8 +50,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.06410918, 0.0, 0.0]
-            stereo_tf_r: [0.0, 0.0, 0.0, 1.0]
         </rosparam>
     </node>
 </launch>

diff --git a/launch/slam_eval_tum.launch b/launch/slam_eval_tum.launch
@@ -1,9 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).slam.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 313.533725, fy: 313.51138991, cx: 510.37735, cy: 514.419593, chi: -0.18065, alpha: 0.59186286}" />
-    <arg name="stereo_camera_params" default="{fx: 313.4386621, fy: 313.359903, cx: 505.72413717, cy: 510.5791992, chi: -0.17829397, alpha: 0.591774861}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/tum_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_slam_eval_node" name="omni_slam_slam_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -23,10 +21,8 @@
         <param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
         <param name="output_frame" value="map" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
-            stereo_camera_parameters: $(arg stereo_camera_params)
             detector_type: 'ORB'
             detector_parameters: {nfeatures: 1000}
             descriptor_type: 'ORB'
@@ -55,8 +51,6 @@
             stereo_matcher_num_scales: 5
             stereo_matcher_error_threshold: 20
             stereo_matcher_epipolar_threshold: 0.008
-            stereo_tf_t: [0.1010976, 0.001878, 0.00117]
-            stereo_tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177]
         </rosparam>
     </node>
 </launch>

diff --git a/launch/t265_camera.yaml b/launch/t265_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 287.405
+    fy: 287.092
+    cx: 418.432
+    cy: 399.257
+    chi: -0.0000588
+    alpha: 0.675621
+
+stereo_camera_parameters:
+    fx: 287.405
+    fy: 287.092
+    cx: 418.432
+    cy: 399.257
+    chi: -0.0000588
+    alpha: 0.675621
+    tf_t: [0.06410918, 0.0, 0.0]
+    tf_r: [0.0, 0.0, 0.0, 1.0]
diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch
@@ -1,8 +1,7 @@
 <launch>
     <arg name="bag_file" default="" />
     <arg name="results_file" default="$(arg bag_file).tracking.hdf5" />
-    <arg name="camera_model" default="double_sphere" />
-    <arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
+    <arg name="camera_file" default="$(find omni_slam_eval)/launch/default_camera.yaml" />
     <arg name="rate" default="1" />
     <node pkg="omni_slam_eval" type="omni_slam_tracking_eval_node" name="omni_slam_tracking_eval_node" required="true" output="screen">
         <param name="bag_file" value="$(arg bag_file)" />
@@ -12,9 +11,8 @@
         <param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
         <param name="tracked_image_topic" value="/omni_slam/tracked" />
         <param name="rate" value="$(arg rate)" />
+        <rosparam command="load" file="$(arg camera_file)" />
         <rosparam subst_value="true">
-            camera_model: '$(arg camera_model)'
-            camera_parameters: $(arg camera_params)
             detector_type: 'GFTT'
             detector_parameters: {maxCorners: 100, qualityLevel: 0.01, minDistance: 5, blockSize: 5}
             tracker_type: 'lk'

diff --git a/launch/tum_camera.yaml b/launch/tum_camera.yaml
@@ -0,0 +1,19 @@
+camera_model: double_sphere
+
+camera_parameters:
+    fx: 313.533725
+    fy: 313.51138991
+    cx: 510.37735
+    cy: 514.419593
+    chi: -0.18065
+    alpha: 0.59186286
+
+stereo_camera_parameters:
+    fx: 313.4386621
+    fy: 313.359903
+    cx: 505.72413717
+    cy: 510.5791992
+    chi: -0.17829397
+    alpha: 0.591774861
+    tf_t: [0.1010976, 0.001878, 0.00117]
+    tf_r: [-0.0237542, 0.0002876, -0.0004555, 0.9997177]