Depth odometry

astrobee/config/graph_localizer.config

@@ -145,6 +145,15 @@ handrail_adder_min_num_plane_matches = 1
 handrail_adder_point_to_line_stddev = 0.1
 handrail_adder_point_to_plane_stddev = 0.1
 handrail_adder_use_silu_for_point_to_line_segment_factor = false
+-- Depth Odometry Factor
+depth_odometry_adder_enabled = true 
+depth_odometry_adder_noise_scale = 10
+depth_odometry_adder_use_points_between_factor = false 
+depth_odometry_adder_point_to_point_error_threshold = 10.0
+depth_odometry_adder_position_covariance_threshold = 1
+depth_odometry_adder_orientation_covariance_threshold = 1
+depth_odometry_adder_pose_translation_norm_threshold = 3.0
+depth_odometry_adder_max_num_points_between_factors = 10
 -- IMU integration
 -- none, ButterOxSxLpxNx, where O is order, S is sample rate, Lp is low pass cutoff, N is notch 
 -- Note that notch depends on the sample rate and aliasing
@@ -171,6 +180,8 @@ max_optical_flow_buffer_size = 20
 max_vl_buffer_size = 10
 -- AR ~1 Hz
 max_ar_buffer_size = 10
+-- Depth Odometry ~2 Hz
+max_depth_odometry_buffer_size = 10
 -- DL ~1 Hz
 max_dl_buffer_size = 10
 -- Other

astrobee/config/localization/depth_odometry.config

@@ -0,0 +1,122 @@
+-- Copyright (c) 2017, United States Government, as represented by the
+-- Administrator of the National Aeronautics and Space Administration.
+--
+-- All rights reserved.
+--
+-- The Astrobee platform is licensed under the Apache License, Version 2.0
+-- (the "License"); you may not use this file except in compliance with the
+-- License. You may obtain a copy of the License at
+--
+--     http://www.apache.org/licenses/LICENSE-2.0
+--
+-- Unless required by applicable law or agreed to in writing, software
+-- distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+-- WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+-- License for the specific language governing permissions and limitations
+-- under the License.
+
+require "context"
+
+-- Max time diff between successive depth measurements 
+max_time_diff = 10
+
+-- Max time diff for correlating image and point cloud measurements to create a depth measurement
+-- This ideally have the same timestamp but sometimes vary slightly even though they are from 
+-- the same measurement
+max_image_and_point_cloud_time_diff = 0.05
+
+-- Covariance thresholds for rejecting relative odometry result 
+position_covariance_threshold = 100
+orientation_covariance_threshold = 100
+
+-- icp or image_feature
+depth_odometry_method = "image_feature"
+
+-- ICP options
+
+-- Search radius for each point in ICP 
+search_radius = 0.04
+
+symmetric_objective = true
+-- Only applicable for symmetric objective
+enforce_same_direction_normals = true
+
+-- ICP fitness score threshold for rejecting ICP result
+fitness_threshold = 1
+max_iterations = 10
+
+-- Use RANSAC AI algorithm to compute initial estimate
+inital_estimate_with_ransac_ia = false 
+
+correspondence_rejector_surface_normal = true
+-- Threshold for cos(theta) where theta is the angle between two normals. Range from [0,1]
+-- The closer to 1, the more aligned the normals are
+correspondence_rejector_surface_normal_threshold = 0.75 
+
+
+-- Downsample options 
+downsample = true 
+downsample_leaf_size = 0.02
+
+-- coarse to fine options for ICP
+coarse_to_fine = false 
+num_coarse_to_fine_levels = 1
+coarse_to_fine_downsample_ratio = 0.5
+-- Final (smallest) leaf size
+coarse_to_fine_final_leaf_size = 0.02
+downsample_last_coarse_to_fine_iteration = true
+
+-- Depth image aligner options
+
+-- brisk, surf, lk_optical_flow
+detector = "lk_optical_flow"
+-- Brisk detector options
+brisk_threshold = 120
+brisk_octaves = 4
+brisk_float_pattern_scale = 1
+-- Brisk matching options
+brisk_max_match_hamming_distance = 100 
+brisk_flann_table_number = 3 
+brisk_flann_key_size = 18
+brisk_flann_multi_probe_level = 2
+-- Surf options
+surf_threshold = 1000
+surf_max_match_distance = 0.25 
+-- LK optical flow options
+lk_max_iterations = 10
+lk_termination_epsilon = 0.03
+lk_window_width = 10
+lk_window_height = 10
+lk_max_level = 3
+lk_min_eigen_threshold = 0.2
+lk_max_flow_distance = 50
+lk_max_backward_match_distance = 0.1
+-- Good features to track options
+lk_max_corners = 100
+lk_quality_level = 0.01
+lk_min_distance = 10
+lk_block_size = 5 
+lk_use_harris_detector = false
+lk_k = 0.04
+
+-- CLAHE histogram equalization options
+use_clahe = true 
+clahe_grid_length = 8 
+clahe_clip_limit = 40
+-- Other
+min_x_distance_to_border = 10 
+min_y_distance_to_border = 10
+min_num_inliers = 5
+
+-- Point cloud with known correspondences aligner
+pcwkca_max_num_iterations = 100
+pcwkca_function_tolerance = 1e-6 
+pcwkca_max_num_match_sets = 10000000 
+pcwkca_normal_search_radius = 0.03 
+pcwkca_use_umeyama_initial_guess = false 
+pcwkca_use_single_iteration_umeyama = false 
+pcwkca_use_point_to_plane_cost = false 
+pcwkca_use_symmetric_point_to_plane_cost = false 
+pcwkca_verbose_optimization = false 
+
+publish_point_clouds = false 

astrobee/config/tools/graph_bag.config

@@ -21,6 +21,9 @@ imu_min_msg_spacing = 0
 -- flight_mode 
 flight_mode_msg_delay = 0
 flight_mode_min_msg_spacing = 0
+-- depth odometry
+depth_odometry_msg_delay = 0
+depth_odometry_min_msg_spacing = 0
 -- optical flow
 of_msg_delay = 0
 of_min_msg_spacing = 0

astrobee/launch/astrobee.launch

@@ -30,6 +30,7 @@
   <!-- Remaining options -->
   <arg name="output" default="log" />         <!-- Output to screen or log   -->
   <arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer      -->
+  <arg name="depth_odometry" default="false" /> <!-- Run depth odometry node -->
   <arg name="drivers" default="true" />       <!-- Should we launch drivers? -->
   <arg name="sim" default="local" />          <!-- SIM IP address            -->
   <arg name="llp" default="local" />          <!-- LLP IP address            -->
@@ -154,6 +155,7 @@
         <arg name="debug" value="$(arg debug)" />      <!-- Debug node group     -->
         <arg name="output" value="$(arg output)" />
         <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" />
+        <arg name="depth_odometry" value="$(arg depth_odometry)" />
       </include>
 
       <!-- If we have specified a bag, then play it back into flight software-->

astrobee/launch/offline_localization/loc_rviz.launch

@@ -31,8 +31,6 @@
   <env name="ASTROBEE_CONFIG_DIR" value="$(find astrobee)/config" />
   <env name="ASTROBEE_RESOURCE_DIR" value="$(find astrobee)/resources" />
 
-  <param name="robot_description" command="$(find xacro)/xacro --inorder $(find rviz_visualizer)/model/astrobee_model.urdf" />  
-
   <group if="$(eval arg('world') == 'iss')">
     <node type="rviz" name="rviz" pkg="rviz" args="-d $(find astrobee)/resources/rviz/loc_iss.rviz" />
     <param name="/iss/robot_description" textfile="$(find astrobee_iss)/urdf/model.urdf" />

astrobee/launch/offline_localization/localization_from_bag.launch

@@ -24,13 +24,14 @@
   <!-- Command line arguments -->
   <arg name="bagfile"/>             
   <arg name="record" default="false" />
-  <arg name="imu_augmentor" default="false" />
+  <arg name="imu_augmentor" default="true" />
+  <arg name="depth_odometry" default="false" />
   <arg name="robot" default="bumble" />
   <arg name="world" default="iss" />
   <arg name="localization_manager" default="false" />
   <arg name="service_calls" default="true" unless="$(arg localization_manager)"/>     
   <arg name="service_calls" value="false" if="$(arg localization_manager)" />   
-  <arg name="image_features" default="false" />
+  <arg name="image_features" default="true" />
 
 
   <!-- Set environment configs that are required to run nodes -->
@@ -79,6 +80,13 @@
     </include>
   </group>
 
+ <!-- Run depth odometry if required -->
+  <group if="$(arg depth_odometry)">
+    <include file="$(find depth_odometry)/launch/depth_odometry.launch">
+      <arg name="terminal" value="terminator -x"/>
+    </include>
+  </group>
+
   <!-- Play bagfile -->
   <include file="$(find astrobee)/launch/offline_localization/replay_localization.launch">
     <arg name="bagfile" value="$(arg bagfile)"/>

astrobee/launch/offline_localization/record_localization.launch

@@ -18,5 +18,5 @@
 <launch>
   <arg name="output_bagfile"/>             
   <arg name="terminal" default="" />
-  <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)"/>
+  <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)"/>
 </launch>

astrobee/launch/offline_localization/replay_localization.launch

@@ -29,5 +29,5 @@
   <remap from="/mgt/img_sampler/nav_cam/image_record" to="/hw/cam_nav"/>
   <remap from="/gnc/ekf" to="/bag/gnc/ekf"/>
 
-  <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)" />
+  <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)" />
 </launch>

astrobee/launch/robot/MLP.launch

@@ -26,6 +26,7 @@
   <arg name="dds" default="true"/>               <!-- Should DDS be started     -->
   <arg name="output" default="log"/>             <!-- Where nodes should log    -->
   <arg name="ground_truth_localizer" default="false"/>   <!-- Runs ground_truth localizer -->
+  <arg name="depth_odometry" default="false"/>   <!-- Runs depth odometry node -->
 
   <!-- Setup nodelet managers -->
   <group if="$(eval optenv('ASTROBEE_NODEGRAPH','')=='')">
@@ -86,6 +87,18 @@
     <arg name="debug" value="$(arg debug)" />
     <arg name="default" value="true" />
   </include>
+  <group if="$(arg depth_odometry)">
+    <include file="$(find ff_util)/launch/ff_nodelet.launch">
+      <arg name="class" value="depth_odometry/DepthOdometryNodelet" />
+      <arg name="name" value="depth_odometry_nodelet" />
+      <arg name="manager" value="mlp_vision" />
+      <arg name="spurn" value="$(arg spurn)" />
+      <arg name="nodes" value="$(arg nodes)" />
+      <arg name="extra" value="$(arg extra)" />
+      <arg name="debug" value="$(arg debug)" />
+      <arg name="default" value="true" />
+    </include>
+  </group>
 
   <!-- Run ground_truth_localizer, otherwise run graph localizer -->
   <group unless="$(arg ground_truth_localizer)">

astrobee/launch/sim.launch

@@ -34,6 +34,7 @@
   <arg name="rec" default="" />                 <!-- Record local data       -->
   <arg name="dds" default="true" />             <!-- Enable DDS              -->
   <arg name="ground_truth_localizer" default="false" />  <!-- Use Ground Truth Localizer      -->
+  <arg name="depth_odometry" default="false" />  <!-- Run depth odometry node-->
 
   <!-- General options -->
   <arg name="gviz" default="false" />           <!-- Start GNC visualizer    -->
@@ -158,6 +159,7 @@
       <arg name="mlp" value="$(arg mlp)" />          <!-- MLP IP address     -->
       <arg name="dds" value="$(arg dds)" />          <!-- Enable DDS         -->
       <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" /> <!-- Use Ground Truth Localizer    -->
+      <arg name="depth_odometry" value="$(arg depth_odometry)" /> <!-- Run depth odometry node -->
     </include>
   </group>
 

astrobee/launch/spawn.launch

@@ -32,6 +32,7 @@
   <arg name="mlp" default="local" />            <!-- MLP IP address          -->
   <arg name="dds" default="true" />             <!-- Enable DDS              -->
   <arg name="ground_truth_localizer" default="false" /> <!-- Use Ground Truth Localizer      -->
+  <arg name="depth_odometry" default="false" /> <!-- Run depth odometry node -->
 
   <!-- General options -->
   <arg name="gviz" default="false" />           <!-- Start GNC visualizer    -->
@@ -87,6 +88,7 @@
     <arg name="mlp" value="$(arg mlp)" />      <!-- MLP IP address           -->
     <arg name="dds" value="$(arg dds)" />      <!-- Enable DDS               -->
     <arg name="ground_truth_localizer" value="$(arg ground_truth_localizer)" />      <!-- Use Ground Truth Localizer -->
+    <arg name="depth_odometry" value="$(arg depth_odometry)" /> <!-- Run depth odometry node -->
   </include>
 
 </launch>

communications/ff_msgs/msg/DepthCorrespondence.msg

@@ -0,0 +1,21 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+# 
+#     http://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+ImagePoint source_image_point
+ImagePoint target_image_point
+geometry_msgs/Point source_3d_point
+geometry_msgs/Point target_3d_point

communications/ff_msgs/msg/DepthOdometry.msg

@@ -0,0 +1,24 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+
+Header header
+ff_msgs/Odometry odometry
+ff_msgs/DepthCorrespondence[] correspondences
+bool valid_image_points
+bool valid_points_3d
+float32 runtime

communications/ff_msgs/msg/ImagePoint.msg

@@ -0,0 +1,20 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+# 
+# All rights reserved.
+# 
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+# 
+#     http://www.apache.org/licenses/LICENSE-2.0
+# 
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+
+float32 x
+float32 y

communications/ff_msgs/msg/Odometry.msg

@@ -0,0 +1,22 @@
+# Copyright (c) 2017, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The Astrobee platform is licensed under the Apache License, Version 2.0
+# (the "License"); you may not use this file except in compliance with the
+# License. You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+# WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
+# License for the specific language governing permissions and limitations
+# under the License.
+#
+
+time source_time 
+time target_time
+geometry_msgs/PoseWithCovariance sensor_F_source_T_target
+geometry_msgs/PoseWithCovariance body_F_source_T_target

localization/CMakeLists.txt

@@ -25,6 +25,7 @@ add_subdirectory(optimization_common)
 add_subdirectory(sparse_mapping)
 
 if (USE_ROS)
+  add_subdirectory(depth_odometry)
   add_subdirectory(graph_localizer)
   add_subdirectory(ground_truth_localizer)
   add_subdirectory(imu_augmentor)