Skip to content

Commit 2ffae0b

Browse files
authored
Depth odometry (#361)
Added depth_odometry package that computes ICP and image feature based depth odometry as well as a point_cloud_common package with point to plane ICP and registration with known correspondences classes and a vision_common package with several feature tracking approaches. Also added a new depth odometry factor adder using either relative poses or corresponding points.
1 parent 4af88c0 commit 2ffae0b

File tree

211 files changed

+11107
-677
lines changed

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

211 files changed

+11107
-677
lines changed

astrobee/config/graph_localizer.config

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -145,6 +145,15 @@ handrail_adder_min_num_plane_matches = 1
145145
handrail_adder_point_to_line_stddev = 0.1
146146
handrail_adder_point_to_plane_stddev = 0.1
147147
handrail_adder_use_silu_for_point_to_line_segment_factor = false
148+
-- Depth Odometry Factor
149+
depth_odometry_adder_enabled = true
150+
depth_odometry_adder_noise_scale = 10
151+
depth_odometry_adder_use_points_between_factor = false
152+
depth_odometry_adder_point_to_point_error_threshold = 10.0
153+
depth_odometry_adder_position_covariance_threshold = 1
154+
depth_odometry_adder_orientation_covariance_threshold = 1
155+
depth_odometry_adder_pose_translation_norm_threshold = 3.0
156+
depth_odometry_adder_max_num_points_between_factors = 10
148157
-- IMU integration
149158
-- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch
150159
-- Note that notch depends on the sample rate and aliasing
@@ -171,6 +180,8 @@ max_optical_flow_buffer_size = 20
171180
max_vl_buffer_size = 10
172181
-- AR ~1 Hz
173182
max_ar_buffer_size = 10
183+
-- Depth Odometry ~2 Hz
184+
max_depth_odometry_buffer_size = 10
174185
-- DL ~1 Hz
175186
max_dl_buffer_size = 10
176187
-- Other
Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
-- Copyright (c) 2017, United States Government, as represented by the
2+
-- Administrator of the National Aeronautics and Space Administration.
3+
--
4+
-- All rights reserved.
5+
--
6+
-- The Astrobee platform is licensed under the Apache License, Version 2.0
7+
-- (the "License"); you may not use this file except in compliance with the
8+
-- License. You may obtain a copy of the License at
9+
--
10+
-- http://www.apache.org/licenses/LICENSE-2.0
11+
--
12+
-- Unless required by applicable law or agreed to in writing, software
13+
-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
14+
-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
15+
-- License for the specific language governing permissions and limitations
16+
-- under the License.
17+
18+
require "context"
19+
20+
-- Max time diff between successive depth measurements
21+
max_time_diff = 10
22+
23+
-- Max time diff for correlating image and point cloud measurements to create a depth measurement
24+
-- This ideally have the same timestamp but sometimes vary slightly even though they are from
25+
-- the same measurement
26+
max_image_and_point_cloud_time_diff = 0.05
27+
28+
-- Covariance thresholds for rejecting relative odometry result
29+
position_covariance_threshold = 100
30+
orientation_covariance_threshold = 100
31+
32+
-- icp or image_feature
33+
depth_odometry_method = "image_feature"
34+
35+
-- ICP options
36+
37+
-- Search radius for each point in ICP
38+
search_radius = 0.04
39+
40+
symmetric_objective = true
41+
-- Only applicable for symmetric objective
42+
enforce_same_direction_normals = true
43+
44+
-- ICP fitness score threshold for rejecting ICP result
45+
fitness_threshold = 1
46+
max_iterations = 10
47+
48+
-- Use RANSAC AI algorithm to compute initial estimate
49+
inital_estimate_with_ransac_ia = false
50+
51+
correspondence_rejector_surface_normal = true
52+
-- Threshold for cos(theta) where theta is the angle between two normals. Range from [0,1]
53+
-- The closer to 1, the more aligned the normals are
54+
correspondence_rejector_surface_normal_threshold = 0.75
55+
56+
57+
-- Downsample options
58+
downsample = true
59+
downsample_leaf_size = 0.02
60+
61+
-- coarse to fine options for ICP
62+
coarse_to_fine = false
63+
num_coarse_to_fine_levels = 1
64+
coarse_to_fine_downsample_ratio = 0.5
65+
-- Final (smallest) leaf size
66+
coarse_to_fine_final_leaf_size = 0.02
67+
downsample_last_coarse_to_fine_iteration = true
68+
69+
-- Depth image aligner options
70+
71+
-- brisk, surf, lk_optical_flow
72+
detector = "lk_optical_flow"
73+
-- Brisk detector options
74+
brisk_threshold = 120
75+
brisk_octaves = 4
76+
brisk_float_pattern_scale = 1
77+
-- Brisk matching options
78+
brisk_max_match_hamming_distance = 100
79+
brisk_flann_table_number = 3
80+
brisk_flann_key_size = 18
81+
brisk_flann_multi_probe_level = 2
82+
-- Surf options
83+
surf_threshold = 1000
84+
surf_max_match_distance = 0.25
85+
-- LK optical flow options
86+
lk_max_iterations = 10
87+
lk_termination_epsilon = 0.03
88+
lk_window_width = 10
89+
lk_window_height = 10
90+
lk_max_level = 3
91+
lk_min_eigen_threshold = 0.2
92+
lk_max_flow_distance = 50
93+
lk_max_backward_match_distance = 0.1
94+
-- Good features to track options
95+
lk_max_corners = 100
96+
lk_quality_level = 0.01
97+
lk_min_distance = 10
98+
lk_block_size = 5
99+
lk_use_harris_detector = false
100+
lk_k = 0.04
101+
102+
-- CLAHE histogram equalization options
103+
use_clahe = true
104+
clahe_grid_length = 8
105+
clahe_clip_limit = 40
106+
-- Other
107+
min_x_distance_to_border = 10
108+
min_y_distance_to_border = 10
109+
min_num_inliers = 5
110+
111+
-- Point cloud with known correspondences aligner
112+
pcwkca_max_num_iterations = 100
113+
pcwkca_function_tolerance = 1e-6
114+
pcwkca_max_num_match_sets = 10000000
115+
pcwkca_normal_search_radius = 0.03
116+
pcwkca_use_umeyama_initial_guess = false
117+
pcwkca_use_single_iteration_umeyama = false
118+
pcwkca_use_point_to_plane_cost = false
119+
pcwkca_use_symmetric_point_to_plane_cost = false
120+
pcwkca_verbose_optimization = false
121+
122+
publish_point_clouds = false

astrobee/config/tools/graph_bag.config

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,9 @@ imu_min_msg_spacing = 0
2121
-- flight_mode
2222
flight_mode_msg_delay = 0
2323
flight_mode_min_msg_spacing = 0
24+
-- depth odometry
25+
depth_odometry_msg_delay = 0
26+
depth_odometry_min_msg_spacing = 0
2427
-- optical flow
2528
of_msg_delay = 0
2629
of_min_msg_spacing = 0

astrobee/launch/astrobee.launch

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@
3030
<!-- Remaining options -->
3131
<arg name="output" default="log" /> <!-- Output to screen or log -->
3232
<arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer -->
33+
<arg name="depth_odometry" default="false" /> <!-- Run depth odometry node -->
3334
<arg name="drivers" default="true" /> <!-- Should we launch drivers? -->
3435
<arg name="sim" default="local" /> <!-- SIM IP address -->
3536
<arg name="llp" default="local" /> <!-- LLP IP address -->
@@ -154,6 +155,7 @@
154155
<arg name="debug" value="$(arg debug)" /> <!-- Debug node group -->
155156
<arg name="output" value="$(arg output)" />
156157
<arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" />
158+
<arg name="depth_odometry" value="$(arg depth_odometry)" />
157159
</include>
158160

159161
<!-- If we have specified a bag, then play it back into flight software-->

astrobee/launch/offline_localization/loc_rviz.launch

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -31,8 +31,6 @@
3131
<env name="ASTROBEE_CONFIG_DIR" value="$(find astrobee)/config" />
3232
<env name="ASTROBEE_RESOURCE_DIR" value="$(find astrobee)/resources" />
3333

34-
<param name="robot_description" command="$(find xacro)/xacro --inorder $(find rviz_visualizer)/model/astrobee_model.urdf" />
35-
3634
<group if="$(eval arg('world') == 'iss')">
3735
<node type="rviz" name="rviz" pkg="rviz" args="-d $(find astrobee)/resources/rviz/loc_iss.rviz" />
3836
<param name="/iss/robot_description" textfile="$(find astrobee_iss)/urdf/model.urdf" />

astrobee/launch/offline_localization/localization_from_bag.launch

Lines changed: 10 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -24,13 +24,14 @@
2424
<!-- Command line arguments -->
2525
<arg name="bagfile"/>
2626
<arg name="record" default="false" />
27-
<arg name="imu_augmentor" default="false" />
27+
<arg name="imu_augmentor" default="true" />
28+
<arg name="depth_odometry" default="false" />
2829
<arg name="robot" default="bumble" />
2930
<arg name="world" default="iss" />
3031
<arg name="localization_manager" default="false" />
3132
<arg name="service_calls" default="true" unless="$(arg localization_manager)"/>
3233
<arg name="service_calls" value="false" if="$(arg localization_manager)" />
33-
<arg name="image_features" default="false" />
34+
<arg name="image_features" default="true" />
3435

3536

3637
<!-- Set environment configs that are required to run nodes -->
@@ -79,6 +80,13 @@
7980
</include>
8081
</group>
8182

83+
<!-- Run depth odometry if required -->
84+
<group if="$(arg depth_odometry)">
85+
<include file="$(find depth_odometry)/launch/depth_odometry.launch">
86+
<arg name="terminal" value="terminator -x"/>
87+
</include>
88+
</group>
89+
8290
<!-- Play bagfile -->
8391
<include file="$(find astrobee)/launch/offline_localization/replay_localization.launch">
8492
<arg name="bagfile" value="$(arg bagfile)"/>

astrobee/launch/offline_localization/record_localization.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -18,5 +18,5 @@
1818
<launch>
1919
<arg name="output_bagfile"/>
2020
<arg name="terminal" default="" />
21-
<node pkg="rosbag" type="record" name="recorder" output="screen" launch-prefix="$(arg terminal)" args="/graph_loc/state /graph_loc/graph /sparse_mapping/pose /loc/ml/features /loc/of/features /ar_tag/pose /gnc/ekf /imu_bias_tester/pose -O $(arg output_bagfile)"/>
21+
<node pkg="rosbag" type="record" name="recorder" output="screen" launch-prefix="$(arg terminal)" args="/graph_loc/state /graph_loc/graph /sparse_mapping/pose /loc/ml/features /loc/of/features /ar_tag/pose /gnc/ekf /imu_bias_tester/pose /loc/depth/odom -O $(arg output_bagfile)"/>
2222
</launch>

astrobee/launch/offline_localization/replay_localization.launch

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -29,5 +29,5 @@
2929
<remap from="/mgt/img_sampler/nav_cam/image_record" to="/hw/cam_nav"/>
3030
<remap from="/gnc/ekf" to="/bag/gnc/ekf"/>
3131

32-
<node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --topics /hw/imu /hw/cam_nav /mgt/img_sampler/nav_cam/image_record /mob/flight_mode /loc/ar/features $(arg image_feature_topics)" />
32+
<node pkg="rosbag" type="play" name="player" output="screen" launch-prefix="$(arg terminal)" args="--clock $(arg bagfile) --topics /hw/imu /hw/cam_nav /hw/depth_haz/extended/amplitude_int /hw/depth_haz/points /mgt/img_sampler/nav_cam/image_record /mob/flight_mode /loc/ar/features /loc/depth/odom $(arg image_feature_topics)" />
3333
</launch>

astrobee/launch/robot/MLP.launch

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@
2626
<arg name="dds" default="true"/> <!-- Should DDS be started -->
2727
<arg name="output" default="log"/> <!-- Where nodes should log -->
2828
<arg name="ground_truth_localizer" default="false"/> <!-- Runs ground_truth localizer -->
29+
<arg name="depth_odometry" default="false"/> <!-- Runs depth odometry node -->
2930

3031
<!-- Setup nodelet managers -->
3132
<group if="$(eval optenv('ASTROBEE_NODEGRAPH','')=='')">
@@ -86,6 +87,18 @@
8687
<arg name="debug" value="$(arg debug)" />
8788
<arg name="default" value="true" />
8889
</include>
90+
<group if="$(arg depth_odometry)">
91+
<include file="$(find ff_util)/launch/ff_nodelet.launch">
92+
<arg name="class" value="depth_odometry/DepthOdometryNodelet" />
93+
<arg name="name" value="depth_odometry_nodelet" />
94+
<arg name="manager" value="mlp_vision" />
95+
<arg name="spurn" value="$(arg spurn)" />
96+
<arg name="nodes" value="$(arg nodes)" />
97+
<arg name="extra" value="$(arg extra)" />
98+
<arg name="debug" value="$(arg debug)" />
99+
<arg name="default" value="true" />
100+
</include>
101+
</group>
89102

90103
<!-- Run ground_truth_localizer, otherwise run graph localizer -->
91104
<group unless="$(arg ground_truth_localizer)">

astrobee/launch/sim.launch

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -34,6 +34,7 @@
3434
<arg name="rec" default="" /> <!-- Record local data -->
3535
<arg name="dds" default="true" /> <!-- Enable DDS -->
3636
<arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer -->
37+
<arg name="depth_odometry" default="false" /> <!-- Run depth odometry node-->
3738

3839
<!-- General options -->
3940
<arg name="gviz" default="false" /> <!-- Start GNC visualizer -->
@@ -158,6 +159,7 @@
158159
<arg name="mlp" value="$(arg mlp)" /> <!-- MLP IP address -->
159160
<arg name="dds" value="$(arg dds)" /> <!-- Enable DDS -->
160161
<arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" /> <!-- Use Ground Truth Localizer -->
162+
<arg name="depth_odometry" value="$(arg depth_odometry)" /> <!-- Run depth odometry node -->
161163
</include>
162164
</group>
163165

0 commit comments

Comments
 (0)