Merge pull request nasa#396 from nasa/develop

Release 0.16.1

.dockerignore

@@ -1,4 +1,4 @@
-.git/
+# .git/ This entry was unignored for scripts/docker/astrobee_quick.Dockerfile to retain git history
 scripts/docker/
 submodules/android/
 submodules/avionics/

.github/workflows/ci_pr.yml

@@ -12,6 +12,8 @@ jobs:
 
     steps:
     - uses: actions/checkout@v2
+      with:
+        fetch-depth: 0
 
     - name: Checkout submodule
       run: git submodule update --init --depth 1 description/media
@@ -35,6 +37,8 @@ jobs:
 
     steps:
     - uses: actions/checkout@v2
+      with:
+        fetch-depth: 0
 
     - name: Checkout submodule
       run: git submodule update --init --depth 1 description/media
@@ -58,6 +62,8 @@ jobs:
 
     steps:
     - uses: actions/checkout@v2
+      with:
+        fetch-depth: 0
 
     - name: Checkout submodule
       run: git submodule update --init --depth 1 description/media

.github/workflows/ci_release.yml

@@ -21,7 +21,7 @@ jobs:
                           --build-arg UBUNTU_VERSION=16.04
                           --build-arg ROS_VERSION=kinetic
                           --build-arg PYTHON=''
-                          -t astrobee/astrobee:base-latest-ubuntu16.04
+                          -t astrobee/astrobee:latest-base-ubuntu16.04
 
     - name: Build code for Ubuntu 16
       run: docker build . -f ./scripts/docker/astrobee.Dockerfile
@@ -61,7 +61,7 @@ jobs:
                           --build-arg UBUNTU_VERSION=18.04
                           --build-arg ROS_VERSION=melodic
                           --build-arg PYTHON=''
-                          -t astrobee/astrobee:base-latest-ubuntu18.04
+                          -t astrobee/astrobee:latest-base-ubuntu18.04
 
     - name: Build code for Ubuntu 18
       run: docker build . -f ./scripts/docker/astrobee.Dockerfile
@@ -101,7 +101,7 @@ jobs:
                           --build-arg UBUNTU_VERSION=20.04
                           --build-arg ROS_VERSION=noetic
                           --build-arg PYTHON=3
-                          -t astrobee/astrobee:base-latest-ubuntu20.04
+                          -t astrobee/astrobee:latest-base-ubuntu20.04
 
     - name: Build code for Ubuntu 20
       run: docker build . -f ./scripts/docker/astrobee.Dockerfile

README.md

@@ -42,6 +42,8 @@ due to NASA legal requirements. Thank you for your understanding.
 
 ["A Brief Guide to Astrobee’s Flight Software"](https://github.com/albee/a-brief-guide-to-astrobee/raw/master/a_brief_guide_to_astrobee_latest.pdf) is a good tutorial, with a particular emphasis on the advanced topic of modifying Astrobee's flight software to enable Guidance, Navigation, & Control (GN&C) research. (Note that most guest science can be implemented as an app that uses the [Astrobee Command API](https://nasa.github.io/astrobee/html/command_dictionary.html) without modifying the flight software.)
 
+The ["Astrobee ROS Demo"](https://github.com/Pedro-Roque/astrobee_ros_demo) package provides a C++/Python interface to implement custom control methods in ROS.
+
 For more information, read [Astrobee-related publications](https://www.nasa.gov/content/research-publications-0).
 Learning about the Astrobee [platform](https://www.nasa.gov/sites/default/files/atoms/files/bualat_spaceops_2018_paper.pdf),
 [software](https://www.nasa.gov/sites/default/files/atoms/files/fluckiger2018astrobee.pdf),

RELEASE.md

@@ -1,5 +1,11 @@
 # Releases
 
+## Release 0.16.1
+
+  * depth odometry
+  * better performance evaluation
+  * multiple other fixes and improvements
+
 ## Release 0.16.0
 
   * project compiles with catkin

astrobee.doxyfile

@@ -39,7 +39,7 @@ PROJECT_NAME           = "NASA Astrobee Robot Software"
 # control system is used.
 
 
-PROJECT_NUMBER         = 0.16.0
+PROJECT_NUMBER         = 0.16.1
 
 # Using the PROJECT_BRIEF tag one can provide an optional one line description
 # for a project that appears at the top of each page and should give viewer a
@@ -789,7 +789,7 @@ INPUT                  =  README.md \
                           astrobee \
                           simulation \
                           tools \
-                          description \
+                          description/description \
                           licenses.csv
 
 

astrobee/CMakeLists.txt

@@ -18,7 +18,7 @@
 cmake_minimum_required(VERSION 3.0)
 project(astrobee)
 
-set(ASTROBEE_VERSION 0.16.0)
+set(ASTROBEE_VERSION 0.16.1)
 
 ## Compile as C++14, supported in ROS Kinetic and newer
 add_compile_options(-std=c++14)

astrobee/config/cameras.config

@@ -56,13 +56,11 @@ dock_cam = {
   bayer_throttle_ratio = 3
 };
 
--- The haz and perch cam params below are ignored for the time being.
--- The distortion_coeff, etc, must be set in each bot's config file.
 haz_cam = {
   width                = 224,
   height               = 171,
-  undistorted_width    = 600,
-  undistorted_height   = 500,
+  undistorted_width    = 250,
+  undistorted_height   = 200,
   distortion_coeff     = robot_camera_calibrations.haz_cam.distortion_coeff,
   intrinsic_matrix     = robot_camera_calibrations.haz_cam.intrinsic_matrix,
   device               = "/dev/haz_cam",
@@ -73,15 +71,15 @@ haz_cam = {
 };
 
 perch_cam = {
-  width=224,
-  height=171,
-  undistorted_width=600,
-  undistorted_height=500,
-  distortion_coeff=robot_camera_calibrations.perch_cam.distortion_coeff,
-  intrinsic_matrix=robot_camera_calibrations.perch_cam.intrinsic_matrix,
-  device="/dev/perch_cam",
-  gain=            robot_camera_calibrations.perch_cam.gain,
-  exposure=        robot_camera_calibrations.perch_cam.exposure,
+  width                = 224,
+  height               = 171,
+  undistorted_width    = 250,
+  undistorted_height   = 200,
+  distortion_coeff     = robot_camera_calibrations.perch_cam.distortion_coeff,
+  intrinsic_matrix     = robot_camera_calibrations.perch_cam.intrinsic_matrix,
+  device               = "/dev/perch_cam",
+  gain                 = robot_camera_calibrations.perch_cam.gain,
+  exposure             = robot_camera_calibrations.perch_cam.exposure,
   calibration_gain     = 105,
   calibration_exposure = 30
 };
@@ -100,6 +98,9 @@ sci_cam = {
   calibration_exposure = 30
 };
 
+nav_cam_to_haz_cam_timestamp_offset = robot_camera_calibrations.nav_cam_to_haz_cam_timestamp_offset;
+nav_cam_to_sci_cam_timestamp_offset = robot_camera_calibrations.nav_cam_to_sci_cam_timestamp_offset;
+
 -----------------------------------------------------
 --  PicoFlexx cameras support the following modes
 -----------------------------------------------------
@@ -120,21 +121,24 @@ picoflexx = {
       device = robot_perch_cam_device,  -- camera uuid ("" : automatic)
       exposure = 0,                     -- exposure time (0: automatic)
       mode = "MODE_9_5FPS_2000",        -- use case
-      required = true                   -- is camera required
+      required = true,                  -- is camera required
+      amplitude_factor = 100.0
     },{
       name = "haz_cam",                 -- frame
       topic = "haz",                    -- frame
       device = robot_haz_cam_device,    -- camera uuid ("" : automatic)
       exposure = 0,                     -- exposure time (0: automatic)
       mode = "MODE_9_5FPS_2000",        -- use case
-      required = true                   -- is camera required
+      required = true,                  -- is camera required
+      amplitude_factor = 100.0
     },{
       name = "test_cam",                -- special name
       topic = "test",                   -- frame
       device = "0005-4805-0050-1520",   -- camera uuid ("" : automatic)
       exposure = 0,                     -- exposure time (0: automatic)
       mode = "MODE_5_45FPS_500",        -- use case
-      required = false                  -- is camera required
+      required = false,                 -- is camera required
+      amplitude_factor = 100.0
     }
   }
 }

astrobee/config/geometry.config

@@ -34,11 +34,10 @@ nav_cam_transform = robot_geometry.nav_cam_transform;
 dock_cam_transform = robot_geometry.dock_cam_transform;
 sci_cam_transform = robot_geometry.sci_cam_transform;
 imu_transform = robot_geometry.imu_transform;
-hazcam_to_navcam_transform = robot_geometry.hazcam_to_navcam_transform;
-scicam_to_hazcam_transform = robot_geometry.scicam_to_hazcam_transform;
-navcam_to_hazcam_timestamp_offset = robot_geometry.navcam_to_hazcam_timestamp_offset;
-scicam_to_hazcam_timestamp_offset = robot_geometry.scicam_to_hazcam_timestamp_offset;
-hazcam_depth_to_image_transform = robot_geometry.hazcam_depth_to_image_transform;
+
+nav_cam_to_haz_cam_transform = robot_geometry.nav_cam_to_haz_cam_transform;
+nav_cam_to_sci_cam_transform = robot_geometry.nav_cam_to_sci_cam_transform;
+haz_cam_depth_to_image_transform = robot_geometry.haz_cam_depth_to_image_transform;
 
 -- These will maybe be robot-specific post-calibration
 flashlight_front_transform = transform(vec3(0.1345, 0, .0369), quat4(0.0, 0.0, 0.0, 1.0));

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,140 @@
+-- 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 = 0.7 
+
+-- 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
+use_fitness_threshold_rejection = false
+fitness_threshold = 1
+max_iterations = 1
+
+-- 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 
+
+correspondence_rejector_median_distance = false 
+-- Correspondences with distance > median_distance*factor are rejected
+correspondence_rejector_median_distance_factor = 1.0
+
+-- Organized normal estimation
+use_organized_normal_estimation = true 
+-- avg_3d_gradient, covariance, avg_depth_change
+organized_normal_method = "avg_3d_gradient"
+use_depth_dependent_smoothing = false
+max_depth_change_factor = 0.02
+normal_smoothing_size = 10.0
+
+-- Normal space sampling
+use_normal_space_sampling = true
+bins_per_axis = 12 
+num_samples = 3500
+
+-- Downsample options 
+downsample = false 
+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_length = 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
+-- Refine estimate with PointToPlaneICP
+refine_estimate = true
+
+-- 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